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

The Medical Imaging Interaction Toolkit (MITK)

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

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

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

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

#include <time.h>
#include <stdio.h>
#include <stdlib.h>

#include <omp.h>
#include "itkStreamlineTrackingFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <itkImageFileWriter.h>
#include "itkPointShell.h"
#include <itkRescaleIntensityImageFilter.h>
#include <boost/lexical_cast.hpp>
#include <TrackingHandlers/mitkTrackingHandlerOdf.h>
#include <TrackingHandlers/mitkTrackingHandlerPeaks.h>
#include <TrackingHandlers/mitkTrackingHandlerTensor.h>
#include <TrackingHandlers/mitkTrackingHandlerRandomForest.h>

#define _USE_MATH_DEFINES
#include <math.h>

namespace itk {

StreamlineTrackingFilter
::StreamlineTrackingFilter()
  : m_PauseTracking(false)
  , m_AbortTracking(false)
  , m_BuildFibersFinished(false)
  , m_BuildFibersReady(0)
  , m_FiberPolyData(nullptr)
  , m_Points(nullptr)
  , m_Cells(nullptr)
  , m_StoppingRegions(nullptr)
  , m_TargetRegions(nullptr)
  , m_SeedImage(nullptr)
  , m_MaskImage(nullptr)
  , m_TissueImage(nullptr)
  , m_OutputProbabilityMap(nullptr)
  , m_AngularThresholdDeg(-1)
  , m_StepSizeVox(-1)
  , m_SamplingDistanceVox(-1)
  , m_AngularThreshold(-1)
  , m_StepSize(0)
  , m_MaxLength(10000)
  , m_MinTractLength(20.0)
  , m_MaxTractLength(400.0)
  , m_SeedsPerVoxel(1)
  , m_AvoidStop(true)
  , m_RandomSampling(false)
  , m_SamplingDistance(-1)
  , m_DeflectionMod(1.0)
  , m_OnlyForwardSamples(true)
  , m_UseStopVotes(true)
  , m_NumberOfSamples(30)
  , m_NumPreviousDirections(1)
  , m_WmLabel(3) // mrtrix 5ttseg labels
  , m_GmLabel(1) // mrtrix 5ttseg labels
  , m_SeedOnlyGm(false)
  , m_ControlGmEndings(false)
  , m_MaxNumTracts(-1)
  , m_Verbose(true)
  , m_AposterioriCurvCheck(false)
  , m_DemoMode(false)
  , m_Random(true)
  , m_UseOutputProbabilityMap(false)
  , m_CurrentTracts(0)
  , m_Progress(0)
  , m_StopTracking(false)
{
  this->SetNumberOfRequiredInputs(0);
}

std::string StreamlineTrackingFilter::GetStatusText()
{
  std::string status = "Seedpoints processed: " + boost::lexical_cast<std::string>(m_Progress) + "/" + boost::lexical_cast<std::string>(m_SeedPoints.size());
  if (m_SeedPoints.size()>0)
    status += " (" + boost::lexical_cast<std::string>(100*m_Progress/m_SeedPoints.size()) + "%)";
  if (m_MaxNumTracts>0)
    status += "\nFibers accepted: " + boost::lexical_cast<std::string>(m_CurrentTracts) + "/" + boost::lexical_cast<std::string>(m_MaxNumTracts);
  else
    status += "\nFibers accepted: " + boost::lexical_cast<std::string>(m_CurrentTracts);

  return status;
}

void StreamlineTrackingFilter::BeforeTracking()
{
  m_StopTracking = false;
  m_TrackingHandler->SetRandom(m_Random);
  m_TrackingHandler->InitForTracking();
  m_FiberPolyData = PolyDataType::New();
  m_Points = vtkSmartPointer< vtkPoints >::New();
  m_Cells = vtkSmartPointer< vtkCellArray >::New();

  itk::Vector< double, 3 > imageSpacing = m_TrackingHandler->GetSpacing();

  float minSpacing;
  if(imageSpacing[0]<imageSpacing[1] && imageSpacing[0]<imageSpacing[2])
    minSpacing = imageSpacing[0];
  else if (imageSpacing[1] < imageSpacing[2])
    minSpacing = imageSpacing[1];
  else
    minSpacing = imageSpacing[2];

  if (m_StepSizeVox<mitk::eps)
    m_StepSize = 0.5*minSpacing;
  else
    m_StepSize = m_StepSizeVox*minSpacing;

  if (m_AngularThresholdDeg<0)
  {
    if  (m_StepSize/minSpacing<=1.0)
      m_AngularThreshold = std::cos( 0.5 * M_PI * m_StepSize/minSpacing );
    else
      m_AngularThreshold = std::cos( 0.5 * M_PI );
  }
  else
    m_AngularThreshold = std::cos( m_AngularThresholdDeg*M_PI/180.0 );
  m_TrackingHandler->SetAngularThreshold(m_AngularThreshold);

  if (m_SamplingDistanceVox<mitk::eps)
    m_SamplingDistance = minSpacing*0.25;
  else
    m_SamplingDistance = m_SamplingDistanceVox * minSpacing;

  m_PolyDataContainer.clear();
  for (unsigned int i=0; i<this->GetNumberOfThreads(); i++)
  {
    PolyDataType poly = PolyDataType::New();
    m_PolyDataContainer.push_back(poly);
  }

  if (m_UseOutputProbabilityMap)
  {
    m_OutputProbabilityMap = ItkDoubleImgType::New();
    m_OutputProbabilityMap->SetSpacing(imageSpacing);
    m_OutputProbabilityMap->SetOrigin(m_TrackingHandler->GetOrigin());
    m_OutputProbabilityMap->SetDirection(m_TrackingHandler->GetDirection());
    m_OutputProbabilityMap->SetRegions(m_TrackingHandler->GetLargestPossibleRegion());
    m_OutputProbabilityMap->Allocate();
    m_OutputProbabilityMap->FillBuffer(0);
  }

  if (m_StoppingRegions.IsNull())
  {
    m_StoppingRegions = ItkUcharImgType::New();
    m_StoppingRegions->SetSpacing( imageSpacing );
    m_StoppingRegions->SetOrigin( m_TrackingHandler->GetOrigin() );
    m_StoppingRegions->SetDirection( m_TrackingHandler->GetDirection() );
    m_StoppingRegions->SetRegions( m_TrackingHandler->GetLargestPossibleRegion() );
    m_StoppingRegions->Allocate();
    m_StoppingRegions->FillBuffer(0);
  }
  else
    std::cout << "StreamlineTracking - Using stopping region image" << std::endl;

  if (m_TargetRegions.IsNull())
  {
    m_TargetRegions = ItkUintImgType::New();
    m_TargetRegions->SetSpacing( imageSpacing );
    m_TargetRegions->SetOrigin( m_TrackingHandler->GetOrigin() );
    m_TargetRegions->SetDirection( m_TrackingHandler->GetDirection() );
    m_TargetRegions->SetRegions( m_TrackingHandler->GetLargestPossibleRegion() );
    m_TargetRegions->Allocate();
    m_TargetRegions->FillBuffer(1);
  }
  else
    std::cout << "StreamlineTracking - Using target region image" << std::endl;

  if (m_SeedImage.IsNull())
  {
    m_SeedImage = ItkUcharImgType::New();
    m_SeedImage->SetSpacing( imageSpacing );
    m_SeedImage->SetOrigin( m_TrackingHandler->GetOrigin() );
    m_SeedImage->SetDirection( m_TrackingHandler->GetDirection() );
    m_SeedImage->SetRegions( m_TrackingHandler->GetLargestPossibleRegion() );
    m_SeedImage->Allocate();
    m_SeedImage->FillBuffer(1);
  }
  else
    std::cout << "StreamlineTracking - Using seed image" << std::endl;

  if (m_MaskImage.IsNull())
  {
    // initialize mask image
    m_MaskImage = ItkUcharImgType::New();
    m_MaskImage->SetSpacing( imageSpacing );
    m_MaskImage->SetOrigin( m_TrackingHandler->GetOrigin() );
    m_MaskImage->SetDirection( m_TrackingHandler->GetDirection() );
    m_MaskImage->SetRegions( m_TrackingHandler->GetLargestPossibleRegion() );
    m_MaskImage->Allocate();
    m_MaskImage->FillBuffer(1);
  }
  else
    std::cout << "StreamlineTracking - Using mask image" << std::endl;

  if (m_SeedPoints.empty())
    GetSeedPointsFromSeedImage();
  else
    m_SeedOnlyGm = false;

  if (m_TissueImage.IsNull())
  {
    if (m_SeedOnlyGm)
    {
      MITK_WARN << "StreamlineTracking - Cannot seed in gray matter. No tissue type image set.";
      m_SeedOnlyGm = false;
    }

    if (m_ControlGmEndings)
    {
      MITK_WARN << "StreamlineTracking - Cannot control gray matter endings. No tissue type image set.";
      m_ControlGmEndings = false;
    }
  }
  else
  {
    if (m_ControlGmEndings)
      m_SeedOnlyGm = true;
    if (m_ControlGmEndings || m_SeedOnlyGm)
      std::cout << "StreamlineTracking - Using tissue image" << std::endl;
    else
      MITK_WARN << "StreamlineTracking - Tissue image set but no gray matter seeding or fiber endpoint-control enabled" << std::endl;
  }

  m_BuildFibersReady = 0;
  m_BuildFibersFinished = false;
  m_Tractogram.clear();
  m_SamplingPointset = mitk::PointSet::New();
  m_AlternativePointset = mitk::PointSet::New();
  m_StopVotePointset = mitk::PointSet::New();
  m_StartTime = std::chrono::system_clock::now();

  if (m_SeedOnlyGm && m_ControlGmEndings)
    InitGrayMatterEndings();

  if (m_DemoMode)
    omp_set_num_threads(1);

  if (m_TrackingHandler->GetMode()==mitk::TrackingDataHandler::MODE::DETERMINISTIC)
    std::cout << "StreamlineTracking - Mode: deterministic" << std::endl;
  else if(m_TrackingHandler->GetMode()==mitk::TrackingDataHandler::MODE::PROBABILISTIC)
    std::cout << "StreamlineTracking - Mode: probabilistic" << std::endl;
  else
    std::cout << "StreamlineTracking - Mode: ???" << std::endl;

  std::cout << "StreamlineTracking - Angular threshold: " << m_AngularThreshold << " (" << 180*std::acos( m_AngularThreshold )/M_PI << "°)" << std::endl;
  std::cout << "StreamlineTracking - Stepsize: " << m_StepSize << "mm (" << m_StepSize/minSpacing << "*vox)" << std::endl;
  std::cout << "StreamlineTracking - Seeds per voxel: " << m_SeedsPerVoxel << std::endl;
  std::cout << "StreamlineTracking - Max. tract length: " << m_MaxTractLength << "mm" << std::endl;
  std::cout << "StreamlineTracking - Min. tract length: " << m_MinTractLength << "mm" << std::endl;
  std::cout << "StreamlineTracking - Max. num. tracts: " << m_MaxNumTracts << std::endl;

  std::cout << "StreamlineTracking - Num. neighborhood samples: " << m_NumberOfSamples << std::endl;
  std::cout << "StreamlineTracking - Max. sampling distance: " << m_SamplingDistance << "mm (" << m_SamplingDistance/minSpacing << "*vox)" << std::endl;
  std::cout << "StreamlineTracking - Deflection modifier: " << m_DeflectionMod << std::endl;

  std::cout << "StreamlineTracking - Use stop votes: " << m_UseStopVotes << std::endl;
  std::cout << "StreamlineTracking - Only frontal samples: " << m_OnlyForwardSamples << std::endl;

  if (m_DemoMode)
  {
    std::cout << "StreamlineTracking - Running in demo mode";
    std::cout << "StreamlineTracking - Starting streamline tracking using 1 thread" << std::endl;
  }
  else
    std::cout << "StreamlineTracking - Starting streamline tracking using " << omp_get_max_threads() << " threads" << std::endl;
}

void StreamlineTrackingFilter::InitGrayMatterEndings()
{
  m_TrackingHandler->SetAngularThreshold(0);
  m_GmStubs.clear();
  if (m_TissueImage.IsNotNull())
  {
    std::cout << "StreamlineTracking - initializing GM endings" << std::endl;
    ImageRegionConstIterator< ItkUcharImgType > it(m_TissueImage, m_TissueImage->GetLargestPossibleRegion() );
    it.GoToBegin();

    vnl_vector_fixed<float,3> d1; d1.fill(0.0);
    std::deque< vnl_vector_fixed<float,3> > olddirs;
    while (olddirs.size()<m_NumPreviousDirections)
      olddirs.push_back(d1);

    while( !it.IsAtEnd() )
    {
      if (it.Value()==m_GmLabel)
      {
        ItkUcharImgType::IndexType s_idx = it.GetIndex();
        itk::ContinuousIndex<float, 3> start;
        m_TissueImage->TransformIndexToPhysicalPoint(s_idx, start);
        itk::Point<float, 3> wm_p;
        float max = -1;
        FiberType fib;

        for (int x : {-1,0,1})
          for (int y : {-1,0,1})
            for (int z : {-1,0,1})
            {
              if (x==y && y==z)
                continue;

              ItkUcharImgType::IndexType e_idx;
              e_idx[0] = s_idx[0] + x;
              e_idx[1] = s_idx[1] + y;
              e_idx[2] = s_idx[2] + z;

              if ( !m_TissueImage->GetLargestPossibleRegion().IsInside(e_idx) || m_TissueImage->GetPixel(e_idx)!=m_WmLabel )
                continue;

              itk::ContinuousIndex<float, 3> end;
              m_TissueImage->TransformIndexToPhysicalPoint(e_idx, end);

              d1 = m_TrackingHandler->ProposeDirection(end, olddirs, s_idx);
              if (d1.magnitude()<0.0001)
                continue;
              d1.normalize();

              vnl_vector_fixed< float, 3 > d2;
              d2[0] = end[0] - start[0];
              d2[1] = end[1] - start[1];
              d2[2] = end[2] - start[2];
              d2.normalize();
              float a = fabs(dot_product(d1,d2));
              if (a>max)
              {
                max = a;
                wm_p = end;
              }
            }

        if (max>=0)
        {
          fib.push_back(start);
          fib.push_back(wm_p);
          m_GmStubs.push_back(fib);
        }
      }
      ++it;
    }
  }
  m_TrackingHandler->SetAngularThreshold(m_AngularThreshold);
}

void StreamlineTrackingFilter::CalculateNewPosition(itk::Point<float, 3>& pos, vnl_vector_fixed<float, 3>& dir)
{
  pos[0] += dir[0]*m_StepSize;
  pos[1] += dir[1]*m_StepSize;
  pos[2] += dir[2]*m_StepSize;
}

bool StreamlineTrackingFilter
::IsInsideMask(const itk::Point<float, 3> &pos, ItkUcharImgType::Pointer mask)
{
  if (mask.IsNull())
    return true;

  ItkUcharImgType::IndexType idx;
  mask->TransformPhysicalPointToIndex(pos, idx);
  if (!mask->GetLargestPossibleRegion().IsInside(idx) || mask->GetPixel(idx)==0)
    return false;

  return true;
}

bool StreamlineTrackingFilter
::IsInGm(const itk::Point<float, 3> &pos)
{
  if (m_TissueImage.IsNull())
    return true;

  ItkUcharImgType::IndexType idx;
  m_TissueImage->TransformPhysicalPointToIndex(pos, idx);
  if (m_TissueImage->GetLargestPossibleRegion().IsInside(idx) && m_TissueImage->GetPixel(idx) == m_GmLabel)
    return true;

  return false;
}

std::vector< vnl_vector_fixed<float,3> > StreamlineTrackingFilter::CreateDirections(int NPoints)
{
  std::vector< vnl_vector_fixed<float,3> > pointshell;

  if (NPoints<2)
    return pointshell;

  std::vector< float > theta; theta.resize(NPoints);

  std::vector< float > phi; phi.resize(NPoints);

  float 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)))));
      // % (2*DIST_POINTSHELL_PI);
    }
  }

  for(int i=0; i<NPoints; i++)
  {
    vnl_vector_fixed<float,3> d;
    d[0] = cos(theta[i]) * cos(phi[i]);
    d[1] = cos(theta[i]) * sin(phi[i]);
    d[2] = sin(theta[i]);
    pointshell.push_back(d);
  }

  return pointshell;
}

vnl_vector_fixed<float,3> StreamlineTrackingFilter::GetNewDirection(itk::Point<float, 3> &pos, std::deque<vnl_vector_fixed<float, 3> >& olddirs, itk::Index<3> &oldIndex)
{
  if (m_DemoMode)
  {
    m_SamplingPointset->Clear();
    m_AlternativePointset->Clear();
    m_StopVotePointset->Clear();
  }
  vnl_vector_fixed<float,3> direction; direction.fill(0);

  if (IsInsideMask(pos, m_MaskImage) && !IsInsideMask(pos, m_StoppingRegions))
    direction = m_TrackingHandler->ProposeDirection(pos, olddirs, oldIndex); // get direction proposal at current streamline position
  else
    return direction;

  vnl_vector_fixed<float,3> olddir = olddirs.back();
  std::vector< vnl_vector_fixed<float,3> > probeVecs = CreateDirections(m_NumberOfSamples);
  itk::Point<float, 3> sample_pos;
  int alternatives = 1;
  int stop_votes = 0;
  int possible_stop_votes = 0;
  for (unsigned int i=0; i<probeVecs.size(); i++)
  {
    vnl_vector_fixed<float,3> d;
    bool is_stop_voter = false;
    if (m_Random && m_RandomSampling)
    {
      d[0] = m_TrackingHandler->GetRandDouble(-0.5, 0.5);
      d[1] = m_TrackingHandler->GetRandDouble(-0.5, 0.5);
      d[2] = m_TrackingHandler->GetRandDouble(-0.5, 0.5);
      d.normalize();
      d *= m_TrackingHandler->GetRandDouble(0,m_SamplingDistance);
    }
    else
    {
      d = probeVecs.at(i);
      float dot = dot_product(d, olddir);
      if (m_UseStopVotes && dot>0.7)
      {
        is_stop_voter = true;
        possible_stop_votes++;
      }
      else if (m_OnlyForwardSamples && dot<0)
        continue;
      d *= m_SamplingDistance;
    }

    sample_pos[0] = pos[0] + d[0];
    sample_pos[1] = pos[1] + d[1];
    sample_pos[2] = pos[2] + d[2];

    vnl_vector_fixed<float,3> tempDir; tempDir.fill(0.0);
    if (IsInsideMask(sample_pos, m_MaskImage))
      tempDir = m_TrackingHandler->ProposeDirection(sample_pos, olddirs, oldIndex); // sample neighborhood
    if (tempDir.magnitude()>mitk::eps)
    {
      direction += tempDir;

      if(m_DemoMode)
        m_SamplingPointset->InsertPoint(i, sample_pos);
    }
    else if (m_AvoidStop && olddir.magnitude()>0.5) // out of white matter
    {
      if (is_stop_voter)
        stop_votes++;
      if (m_DemoMode)
        m_StopVotePointset->InsertPoint(i, sample_pos);

      float dot = dot_product(d, olddir);
      if (dot >= 0.0) // in front of plane defined by pos and olddir
        d = -d + 2*dot*olddir; // reflect
      else
        d = -d; // invert

      // look a bit further into the other direction
      sample_pos[0] = pos[0] + d[0];
      sample_pos[1] = pos[1] + d[1];
      sample_pos[2] = pos[2] + d[2];
      alternatives++;
      vnl_vector_fixed<float,3> tempDir; tempDir.fill(0.0);
      if (IsInsideMask(sample_pos, m_MaskImage))
        tempDir = m_TrackingHandler->ProposeDirection(sample_pos, olddirs, oldIndex); // sample neighborhood

      if (tempDir.magnitude()>mitk::eps)  // are we back in the white matter?
      {
        direction += d * m_DeflectionMod;         // go into the direction of the white matter
        direction += tempDir;  // go into the direction of the white matter direction at this location

        if(m_DemoMode)
          m_AlternativePointset->InsertPoint(alternatives, sample_pos);
      }
      else
      {
        if (m_DemoMode)
          m_StopVotePointset->InsertPoint(i, sample_pos);
      }
    }
    else
    {
      if (m_DemoMode)
        m_StopVotePointset->InsertPoint(i, sample_pos);

      if (is_stop_voter)
        stop_votes++;
    }
  }

  if (direction.magnitude()>0.001 && (possible_stop_votes==0 || (float)stop_votes/possible_stop_votes<0.5) )
    direction.normalize();
  else
    direction.fill(0);

  return direction;
}

float StreamlineTrackingFilter::FollowStreamline(itk::Point<float, 3> pos, vnl_vector_fixed<float,3> dir, FiberType* fib, float tractLength, bool front)
{
  vnl_vector_fixed<float,3> zero_dir; zero_dir.fill(0.0);
  std::deque< vnl_vector_fixed<float,3> > last_dirs;
  for (unsigned int i=0; i<m_NumPreviousDirections-1; i++)
    last_dirs.push_back(zero_dir);

  for (int step=0; step< m_MaxLength/2; step++)
  {
    itk::Index<3> oldIndex;
    m_TrackingHandler->WorldToIndex(pos, oldIndex);

    // get new position
    CalculateNewPosition(pos, dir);

    // is new position inside of image and mask
    if (m_AbortTracking)   // if not end streamline
    {
      return tractLength;
    }
    else    // if yes, add new point to streamline
    {
      tractLength +=  m_StepSize;
      if (front)
        fib->push_front(pos);
      else
        fib->push_back(pos);

      if (m_AposterioriCurvCheck)
      {
        int curv = CheckCurvature(fib, front);
        if (curv>0)
        {
          tractLength -= m_StepSize*curv;
          while (curv>0)
          {
            if (front)
              fib->pop_front();
            else
              fib->pop_back();
            curv--;
          }
          return tractLength;
        }
      }

      if (tractLength>m_MaxTractLength)
        return tractLength;
    }

    if (m_DemoMode && !m_UseOutputProbabilityMap) // CHECK: warum sind die samplingpunkte der streamline in der visualisierung immer einen schritt voras?
    {
#pragma omp critical
      {
        m_BuildFibersReady++;
        m_Tractogram.push_back(*fib);
        BuildFibers(true);
        m_Stop = true;

        while (m_Stop){
        }
      }
    }

    dir.normalize();
    last_dirs.push_back(dir);
    if (last_dirs.size()>m_NumPreviousDirections)
      last_dirs.pop_front();
    dir = GetNewDirection(pos, last_dirs, oldIndex);

    while (m_PauseTracking){}

    if (dir.magnitude()<0.0001)
      return tractLength;
  }
  return tractLength;
}

int StreamlineTrackingFilter::CheckCurvature(FiberType* fib, bool front)
{
  float m_Distance = 5;
  if (fib->size()<3)
    return 0;

  float dist = 0;
  std::vector< vnl_vector_fixed< float, 3 > > vectors;
  vnl_vector_fixed< float, 3 > meanV; meanV.fill(0);
  float dev = 0;

  if (front)
  {
    int c=0;
    while(dist<m_Distance && c<(int)fib->size()-1)
    {
      itk::Point<float> p1 = fib->at(c);
      itk::Point<float> p2 = fib->at(c+1);

      vnl_vector_fixed< float, 3 > v;
      v[0] = p2[0]-p1[0];
      v[1] = p2[1]-p1[1];
      v[2] = p2[2]-p1[2];
      dist += v.magnitude();
      v.normalize();
      vectors.push_back(v);
      if (c==0)
        meanV += v;
      c++;
    }
  }
  else
  {
    int c=fib->size()-1;
    while(dist<m_Distance && c>0)
    {
      itk::Point<float> p1 = fib->at(c);
      itk::Point<float> p2 = fib->at(c-1);

      vnl_vector_fixed< float, 3 > v;
      v[0] = p2[0]-p1[0];
      v[1] = p2[1]-p1[1];
      v[2] = p2[2]-p1[2];
      dist += v.magnitude();
      v.normalize();
      vectors.push_back(v);
      if (c==(int)fib->size()-1)
        meanV += v;
      c--;
    }
  }
  meanV.normalize();

  for (unsigned int c=0; c<vectors.size(); c++)
  {
    float angle = dot_product(meanV, vectors.at(c));
    if (angle>1.0)
      angle = 1.0;
    if (angle<-1.0)
      angle = -1.0;
    dev += acos(angle)*180/M_PI;
  }
  if (vectors.size()>0)
    dev /= vectors.size();

  if (dev<30)
    return 0;
  else
    return vectors.size();
}

void StreamlineTrackingFilter::GetSeedPointsFromSeedImage()
{
  MITK_INFO << "StreamlineTracking - Calculating seed points.";
  m_SeedPoints.clear();

  if (!m_ControlGmEndings)
  {
    typedef ImageRegionConstIterator< ItkUcharImgType >     MaskIteratorType;
    MaskIteratorType    sit(m_SeedImage, m_SeedImage->GetLargestPossibleRegion());
    sit.GoToBegin();

    while (!sit.IsAtEnd())
    {
      if (sit.Value()>0)
      {
        ItkUcharImgType::IndexType index = sit.GetIndex();
        itk::ContinuousIndex<float, 3> start;
        start[0] = index[0];
        start[1] = index[1];
        start[2] = index[2];
        itk::Point<float> worldPos;
        m_SeedImage->TransformContinuousIndexToPhysicalPoint(start, worldPos);

        if (IsInsideMask(worldPos, m_MaskImage) && (!m_SeedOnlyGm || IsInGm(worldPos)))
        {
          m_SeedPoints.push_back(worldPos);
          for (int s = 1; s < m_SeedsPerVoxel; s++)
          {
            start[0] = index[0] + m_TrackingHandler->GetRandDouble(-0.5, 0.5);
            start[1] = index[1] + m_TrackingHandler->GetRandDouble(-0.5, 0.5);
            start[2] = index[2] + m_TrackingHandler->GetRandDouble(-0.5, 0.5);

            itk::Point<float> worldPos;
            m_SeedImage->TransformContinuousIndexToPhysicalPoint(start, worldPos);
            m_SeedPoints.push_back(worldPos);
          }
        }
      }
      ++sit;
    }
  }
  else
  {
    for (auto s : m_GmStubs)
      m_SeedPoints.push_back(s[1]);
  }
}

void StreamlineTrackingFilter::GenerateData()
{
  this->BeforeTracking();
  if (m_Random)
    std::random_shuffle(m_SeedPoints.begin(), m_SeedPoints.end());

  m_CurrentTracts = 0;
  int num_seeds = m_SeedPoints.size();
  itk::Index<3> zeroIndex; zeroIndex.Fill(0);
  m_Progress = 0;
  int i = 0;
  int print_interval = num_seeds/100;
  if (print_interval<100)
    m_Verbose=false;

#pragma omp parallel
  while (i<num_seeds && !m_StopTracking)
  {

    int temp_i = 0;
#pragma omp critical
    {
      temp_i = i;
      i++;
    }

    if (temp_i>=num_seeds || m_StopTracking)
      continue;
    else if (m_Verbose && i%print_interval==0)
#pragma omp critical
    {
      m_Progress += print_interval;
      std::cout << "                                                                                \r";
      if (m_MaxNumTracts>0)
        std::cout << "Tried: " << m_Progress << "/" << num_seeds << " | Accepted: " << m_CurrentTracts << "/" << m_MaxNumTracts << '\r';
      else
        std::cout << "Tried: " << m_Progress << "/" << num_seeds << " | Accepted: " << m_CurrentTracts << '\r';
      cout.flush();
    }

    const itk::Point<float> worldPos = m_SeedPoints.at(temp_i);
    FiberType fib;
    float tractLength = 0;
    unsigned int counter = 0;

    // get starting direction
    vnl_vector_fixed<float,3> dir; dir.fill(0.0);
    std::deque< vnl_vector_fixed<float,3> > olddirs;
    while (olddirs.size()<m_NumPreviousDirections)
      olddirs.push_back(dir); // start without old directions (only zero directions)

    vnl_vector_fixed< float, 3 > gm_start_dir;
    if (m_ControlGmEndings)
    {
      gm_start_dir[0] = m_GmStubs[temp_i][1][0] - m_GmStubs[temp_i][0][0];
      gm_start_dir[1] = m_GmStubs[temp_i][1][1] - m_GmStubs[temp_i][0][1];
      gm_start_dir[2] = m_GmStubs[temp_i][1][2] - m_GmStubs[temp_i][0][2];
      gm_start_dir.normalize();
      olddirs.pop_back();
      olddirs.push_back(gm_start_dir);
    }

    if (IsInsideMask(worldPos, m_MaskImage))
      dir = m_TrackingHandler->ProposeDirection(worldPos, olddirs, zeroIndex);

    if (dir.magnitude()>0.0001)
    {
      if (m_ControlGmEndings)
      {
        float a = dot_product(gm_start_dir, dir);
        if (a<0)
          dir = -dir;
      }

      // forward tracking
      tractLength = FollowStreamline(worldPos, dir, &fib, 0, false);
      fib.push_front(worldPos);

      if (m_ControlGmEndings)
      {
        fib.push_front(m_GmStubs[temp_i][0]);
        CheckFiberForGmEnding(&fib);
      }
      else
      {
        // backward tracking (only if we don't explicitely start in the GM)
        tractLength = FollowStreamline(worldPos, -dir, &fib, tractLength, true);
        if (m_ControlGmEndings)
        {
          CheckFiberForGmEnding(&fib);
          std::reverse(fib.begin(),fib.end());
          CheckFiberForGmEnding(&fib);
        }
      }
      counter = fib.size();

      if (tractLength>=m_MinTractLength && counter>=2)
      {
        ItkUintImgType::IndexType idx_begin, idx_end;
        m_TargetRegions->TransformPhysicalPointToIndex(fib.front(), idx_begin);
        m_TargetRegions->TransformPhysicalPointToIndex(fib.back(), idx_end);
#pragma omp critical
        if ( !m_TargetRegions->GetLargestPossibleRegion().IsInside(idx_end) ||
             !m_TargetRegions->GetLargestPossibleRegion().IsInside(idx_begin) ||
             (m_TargetRegions->GetPixel(idx_begin)>0 && m_TargetRegions->GetPixel(idx_end)==m_TargetRegions->GetPixel(idx_begin)) )
        {
          if (!m_StopTracking)
          {
            if (!m_UseOutputProbabilityMap)
              m_Tractogram.push_back(fib);
            else
              FiberToProbmap(&fib);
            m_CurrentTracts++;
          }
          if (m_MaxNumTracts > 0 && m_CurrentTracts>=static_cast<unsigned int>(m_MaxNumTracts))
          {
            if (!m_StopTracking)
            {
              std::cout << "                                                                                \r";
              MITK_INFO << "Reconstructed maximum number of tracts (" << m_CurrentTracts << "). Stopping tractography.";
            }
            m_StopTracking = true;
          }
        }
      }

    }
  }

  this->AfterTracking();
}

void StreamlineTrackingFilter::CheckFiberForGmEnding(FiberType* fib)
{
  if (m_TissueImage.IsNull())
    return;

  // first check if the current fibe rendpoint is located inside of the white matter
  // if not, remove last fiber point and repeat
  bool in_wm = false;
  while (!in_wm && fib->size()>2)
  {
    ItkUcharImgType::IndexType idx;
    m_TissueImage->TransformPhysicalPointToIndex(fib->back(), idx);
    if (m_TissueImage->GetPixel(idx)==m_WmLabel)
      in_wm = true;
    else
      fib->pop_back();
  }
  if (fib->size()<3 || !in_wm)
  {
    fib->clear();
    return;
  }

  // get fiber direction at end point
  vnl_vector_fixed< float, 3 > d1;
  d1[0] = fib->back()[0] - fib->at(fib->size()-2)[0];
  d1[1] = fib->back()[1] - fib->at(fib->size()-2)[1];
  d1[2] = fib->back()[2] - fib->at(fib->size()-2)[2];
  d1.normalize();

  // find closest gray matter voxel
  ItkUcharImgType::IndexType s_idx;
  m_TissueImage->TransformPhysicalPointToIndex(fib->back(), s_idx);
  itk::Point<float> gm_endp;
  float max = -1;

  for (int x : {-1,0,1})
    for (int y : {-1,0,1})
      for (int z : {-1,0,1})
      {
        if (x==y && y==z)
          continue;

        ItkUcharImgType::IndexType e_idx;
        e_idx[0] = s_idx[0] + x;
        e_idx[1] = s_idx[1] + y;
        e_idx[2] = s_idx[2] + z;

        if ( !m_TissueImage->GetLargestPossibleRegion().IsInside(e_idx) || m_TissueImage->GetPixel(e_idx)!=m_GmLabel )
          continue;

        itk::ContinuousIndex<float, 3> end;
        m_TissueImage->TransformIndexToPhysicalPoint(e_idx, end);
        vnl_vector_fixed< float, 3 > d2;
        d2[0] = end[0] - fib->back()[0];
        d2[1] = end[1] - fib->back()[1];
        d2[2] = end[2] - fib->back()[2];
        d2.normalize();
        float a = dot_product(d1,d2);
        if (a>max)
        {
          max = a;
          gm_endp = end;
        }
      }

  if (max>=0)
    fib->push_back(gm_endp);
  else  // no gray matter enpoint found -> delete fiber
    fib->clear();
}

void StreamlineTrackingFilter::FiberToProbmap(FiberType* fib)
{
  ItkDoubleImgType::IndexType last_idx; last_idx.Fill(0);
  for (auto p : *fib)
  {
    ItkDoubleImgType::IndexType idx;
    m_OutputProbabilityMap->TransformPhysicalPointToIndex(p, idx);

    if (idx != last_idx)
    {
      if (m_OutputProbabilityMap->GetLargestPossibleRegion().IsInside(idx))
        m_OutputProbabilityMap->SetPixel(idx, m_OutputProbabilityMap->GetPixel(idx)+1);
      last_idx = idx;
    }
  }
}

void StreamlineTrackingFilter::BuildFibers(bool check)
{
  if (m_BuildFibersReady<omp_get_num_threads() && check)
    return;

  m_FiberPolyData = vtkSmartPointer<vtkPolyData>::New();
  vtkSmartPointer<vtkCellArray> vNewLines = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkPoints> vNewPoints = vtkSmartPointer<vtkPoints>::New();

  for (unsigned int i=0; i<m_Tractogram.size(); i++)
  {
    vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
    FiberType fib = m_Tractogram.at(i);
    for (FiberType::iterator it = fib.begin(); it!=fib.end(); ++it)
    {
      vtkIdType id = vNewPoints->InsertNextPoint((*it).GetDataPointer());
      container->GetPointIds()->InsertNextId(id);
    }
    vNewLines->InsertNextCell(container);
  }

  if (check)
    for (int i=0; i<m_BuildFibersReady; i++)
      m_Tractogram.pop_back();
  m_BuildFibersReady = 0;

  m_FiberPolyData->SetPoints(vNewPoints);
  m_FiberPolyData->SetLines(vNewLines);
  m_BuildFibersFinished = true;
}

void StreamlineTrackingFilter::AfterTracking()
{
  if (m_Verbose)
    std::cout << "                                                                                \r";
  if (!m_UseOutputProbabilityMap)
  {
    MITK_INFO << "Reconstructed " << m_Tractogram.size() << " fibers.";
    MITK_INFO << "Generating polydata ";
    BuildFibers(false);
  }
  else
  {
    itk::RescaleIntensityImageFilter< ItkDoubleImgType, ItkDoubleImgType >::Pointer filter = itk::RescaleIntensityImageFilter< ItkDoubleImgType, ItkDoubleImgType >::New();
    filter->SetInput(m_OutputProbabilityMap);
    filter->SetOutputMaximum(1.0);
    filter->SetOutputMinimum(0.0);
    filter->Update();
    m_OutputProbabilityMap = filter->GetOutput();
  }
  MITK_INFO << "done";

  m_EndTime = std::chrono::system_clock::now();
  std::chrono::hours   hh = std::chrono::duration_cast<std::chrono::hours>(m_EndTime - m_StartTime);
  std::chrono::minutes mm = std::chrono::duration_cast<std::chrono::minutes>(m_EndTime - m_StartTime);
  std::chrono::seconds ss = std::chrono::duration_cast<std::chrono::seconds>(m_EndTime - m_StartTime);
  mm %= 60;
  ss %= 60;
  MITK_INFO << "Tracking took " << hh.count() << "h, " << mm.count() << "m and " << ss.count() << "s";

  m_SeedPoints.clear();
}

void StreamlineTrackingFilter::SetDicomProperties(mitk::FiberBundle::Pointer fib)
{
  std::string model_code_value = "-";
  std::string model_code_meaning = "-";
  std::string algo_code_value = "-";
  std::string algo_code_meaning = "-";

  if (m_TrackingHandler->GetMode()==mitk::TrackingDataHandler::DETERMINISTIC && dynamic_cast<mitk::TrackingHandlerTensor*>(m_TrackingHandler) && !m_TrackingHandler->GetInterpolate())
  {
    algo_code_value = "sup181_ee04";
    algo_code_meaning = "FACT";
  }
  else if (m_TrackingHandler->GetMode()==mitk::TrackingDataHandler::DETERMINISTIC)
  {
    algo_code_value = "sup181_ee01";
    algo_code_meaning = "Deterministic";
  }
  else if (m_TrackingHandler->GetMode()==mitk::TrackingDataHandler::PROBABILISTIC)
  {
    algo_code_value = "sup181_ee02";
    algo_code_meaning = "Probabilistic";
  }

  if (dynamic_cast<mitk::TrackingHandlerTensor*>(m_TrackingHandler) || (dynamic_cast<mitk::TrackingHandlerOdf*>(m_TrackingHandler) && dynamic_cast<mitk::TrackingHandlerOdf*>(m_TrackingHandler)->GetIsOdfFromTensor() ) )
  {
    if ( dynamic_cast<mitk::TrackingHandlerTensor*>(m_TrackingHandler) && dynamic_cast<mitk::TrackingHandlerTensor*>(m_TrackingHandler)->GetNumTensorImages()>1 )
    {
      model_code_value = "sup181_bb02";
      model_code_meaning = "Multi Tensor";
    }
    else
    {
      model_code_value = "sup181_bb01";
      model_code_meaning = "Single Tensor";
    }
  }
  else if (dynamic_cast<mitk::TrackingHandlerRandomForest<6, 28>*>(m_TrackingHandler) || dynamic_cast<mitk::TrackingHandlerRandomForest<6, 100>*>(m_TrackingHandler))
  {
    model_code_value = "sup181_bb03";
    model_code_meaning = "Model Free";
  }
  else if (dynamic_cast<mitk::TrackingHandlerOdf*>(m_TrackingHandler))
  {
    model_code_value = "-";
    model_code_meaning = "ODF";
  }
  else if (dynamic_cast<mitk::TrackingHandlerPeaks*>(m_TrackingHandler))
  {
    model_code_value = "-";
    model_code_meaning = "Peaks";
  }

  fib->SetProperty("DICOM.anatomy.value", mitk::StringProperty::New("T-A0095"));
  fib->SetProperty("DICOM.anatomy.meaning", mitk::StringProperty::New("White matter of brain and spinal cord"));

  fib->SetProperty("DICOM.algo_code.value", mitk::StringProperty::New(algo_code_value));
  fib->SetProperty("DICOM.algo_code.meaning", mitk::StringProperty::New(algo_code_meaning));

  fib->SetProperty("DICOM.model_code.value", mitk::StringProperty::New(model_code_value));
  fib->SetProperty("DICOM.model_code.meaning", mitk::StringProperty::New(model_code_meaning));
}

}


/*======================================================================
Copyright 2019 Google LLC

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

    https://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.
======================================================================*/

// STANDARD HEADERS
#include <iostream>
#include <cmath>

#include <Eigen/Dense>
#include "PCV_Types.h"

// MODULE HEADERS
#include "Vehicle.h"
#include "Filter.h"
#include "Gather.h"
#include "RAHjoy.h"
#include "Servo_Timer.h"

#include "Traj3.h"
#include "traj_circle.h"

#include "PCV_State.h"
#include "Motion.h"

using namespace std
using namespace Eigen

#define HZ  500.0

#define GATHER_TIME  10

void
mymain( timer_addrword_t )
{
// START THE TIMER -- GET THE HW RESOLUTION
//    (START TIMER FIRST TO BE DOMINANT SERVO RATE)
    hw_freq = HZ;
    Servo_Timer *servo = Servo_Timer::HandleGet(hw_freq);
    Float hw_period = 1.0/hw_freq;
    cout << "Servo Rate = " << hw_freq << endl;

static Gather gObj("xr",GATHER_TIME*(int)HZ);

// INTIALIZE traj (GLOBAL)
    traj = new Traj3(hw_freq);

// MOTION ITERATOR
///    GET_mi;  // CREATE "mi" (current) MOTION ITERATOR.

// Zero the Base (ZERO HAPPENS UPON Vehicle OBJ CREATION)
    Vehicle *veh = Vehicle::HandleGet();

// FOR CENTER ROTATION OF XR4000 ROBOT
    veh->Add_Solid(  0.000,  0.000,  XR_Mv, XR_Iv);
    veh->Add_Caster(  0.000,  XR_h,  0*M_PI_2 - 20*ENC_CNT2RAD);
    veh->Add_Caster( -XR_h,   0.000, 1*M_PI_2 - 85*ENC_CNT2RAD);
    veh->Add_Caster(  0.000, -XR_h,  2*M_PI_2 - 75*ENC_CNT2RAD);
    veh->Add_Caster(  XR_h,   0.000, 3*M_PI_2 - 80*ENC_CNT2RAD);

    veh->Home(); // SET ENCODER PRESETS TO CASTER ANGLES

// LOCAL VARIABLES
    int k=0;
    long tick_miss=0;

    Float heading;

    Matrix3x8d J;
    Matrix8x3d Jt;
    Matrix3d   Lambda;
    Vector3d   Mu;
    Vector3d   cf;

    Vector3d   cxdd;
    Vector3d  fcxdd;
    Vector3d  cgxdd;

    Matrix8x3d C;
    Vector8d   qd_bar, qd_null;
    Vector8d   tq;

    Vector8d rtqS, tqS;
    Vector3d  rfS,  fS;
    Vector8d motAmp; // extra, not needed for control
    Vector8d tqI;    // extra, not needed for control

    Matrix<double, NUM_TRUSS_LINKS,8> E;
    VectorXd<NUM_TRUSS_LINKS> rtE;
    VectorXd<NUM_TRUSS_LINKS> tE;
    VectorXd<NUM_TRUSS_LINKS> ctE;

    Vector8d tqE;

    Matrix3d rot = Matrix3d::Identity();

// VECTORS TO GATHER
    gObj.data("tq",tq);
    gObj.data("gx",gx);
    gObj.data("gxd",gxd);
    gObj.data("dx",dx);
    gObj.data("dxd",dxd);
    gObj.data("q",q);
    gObj.data("qd",qd);
    gObj.data("tE",tE);
    gObj.data("tqE",tqE);

    gObj.data("amp",motAmp);   // extra
    gObj.data("tqI",tqI);      // extra
    gObj.data("tqS",tqS);      // extra
    gObj.data("qd_n",qd_null); // extra

// CHOOSE FILTERS
//  SERVO RATE          500  // 100 //  100 //  500
//  ----------          ---
    Float const FQD  =  150; // 42; //  45; // 145;
    Float const FGXD =   52; //  0; //  38; //  57;
    Float const FCXDD=    0; //
    Float const FTE  =   -1; //  0; //  30; //  45;
    Float const FXD  =   52; // 20; //  38; //  57;
    Float const FTQS =   42; // 42;

// INITIALIZE FILTERS
    Filter Fqd;
    veh->JointRad(q);
    if(FQD) Fqd.D_LowPass(q, hw_freq, FQD);
    else    Fqd.D(q, hw_freq);

    Filter Fgxd;
    rgxd.setZero();
    if(FGXD) Fgxd.LowPass(rgxd, hw_freq, FGXD);
    else     Fgxd.Unity();

    Filter Fcxdd;
    cxdd.setZero();
    if(FCXDD) Fcxdd.LowPass(cxdd, hw_freq, FCXDD);
    else      Fcxdd.Unity();

    Filter FtE;
    rtE.setZero();
    if(FTE==0)     FtE.Unity();
    else if(FTE>0) FtE.LowPass(rtE, hw_freq, FTE);
    else {
      // Butterworth 2nd-order notch, 45-65 Hz @500Hz
      VectorXd b(5),a(5);
      b[4] =  1.0;
      b[3] = -3.1065490382;
      b[2] =  4.4126617316;
      b[1] = -3.1065490382;
      b[0] =  1.0;
      a[4] =  0.7008967812;
      a[3] = -2.3681639156;
      a[2] =  3.6670730371;
      a[1] = -2.8327533240;
      a[0] =  1.0; // NOT USED
      FtE.Z_NumDen(rtE, b, a, rtE);  // NOTCH
    }

    Filter Fxd;
    rxd.setZero();
    if(FXD) Fxd.LowPass(rxd, hw_freq, FXD);
    else    Fxd.Unity();

    Filter FtqS;
    rtqS.setZero();
    if(FTQS) FtqS.LowPass(rtqS, hw_freq, FTQS);
    else     FtqS.Unity();
//     FtqS.Z_NumDen(rtqS, b, a, rtqS); // SAME AS FtE

// STABILIZE SERVO AMPS (n*HZ) seconds (KEEP WD ACTIVE)
    for(k=0; k<(1*HZ); ++k)
    {
      tq.setZero();
      veh->JointTq(tq);
      servo->tick();
    }

// INITIALIZE ANGLES & TIME
    veh->JointRad(q);
    servo->zero();

//////////////////////////////////////////
// THE LOOP
//////////////////////////////////////////
    while( mi->trajEnd_ > TRAJ_END_EXIT )
    {
    // WAIT FOR TICK
      tick_miss += servo->tick();
      if( tick_miss>9 )
      { cout << "tick miss = " >\<< servo->miss() << "!!!" << endl;
        tick_miss = 0;
      }
      cur_time = servo->time();

// BEGIN ODOMETRY SECTION

    // FIND AXIS ANGLES & SPEEDS
      veh->JointRad(q);   // FILLS ANGLES IN CASTER MODULES
      qd = Fqd.Filt(q);   // JOINT SPEEDS

    // FIND LOCAL OPERATIONAL SPEEDS
      veh->Fill_Jcp(J);       // NOTE: VIA CONTACT POINTS
      rxd = J * qd;           // RAW LOCAL OP SPEEDS
      x += rxd * hw_period;   // "LOCAL" COORDS
      xd = Fxd.Filt( rxd );   // LOCAL SPEED (ALSO FOR DYN)

    // DELTA: MAP LOCAL --> GLOBAL COORDS
      heading = gx[2]+ 0.5*rxd[2]*hw_period; // USE RAW_xd
      rot(0,0) =  cos(heading);
      rot(0,1) = -sin(heading);
      rot(1,0) =  sin(heading);
      rot(1,1) =  cos(heading);
      rgxd = rot * rxd;               // GET RAW_gxd

    // INTEGRATION TO GLOBAL COORDS
      gx += rgxd * hw_period;

      gxd = Fgxd.Filt(rgxd);     // FILTERED GLOBAL VELOCITY

// END ODOMETRY SECTION

// BEGIN CONTROL FORCE SECTION

        // GLOBAL CONTROL
      if( mi->ctrlFrame_ == CTRL_F_GLOBAL )
      {
//      cgxdd = (dx-gx)*mi->KP_ + (dxd-gxd)*mi->KV_; // SUBTRACTION IS SO +GAINs
        cgxdd[0] = (dx[0]-gx[0])*mi->KPx_ + (dxd[0]-gxd[0])*mi->KVx_;
        cgxdd[1] = (dx[1]-gx[1])*mi->KPx_ + (dxd[1]-gxd[1])*mi->KVx_;
        cgxdd[2] = (dx[2]-gx[2])*mi->KPa_ + (dxd[2]-gxd[2])*mi->KVa_;

        if( mi->trajMode_ == TRAJ_M_ACCEL )
          cgxdd.setZero();

        if( mi->accel_FF_ == true )
          cgxdd += dxdd;

        // GLOBAL OP SPACE ADJUSTMENTS (PUT HACKs HERE)
//         if( mi->trajMode_ == TRAJ_M_PACE )
//           cgxdd[2]=0;

        // MAP GLOBAL COMMAND TO LOCAL COORDS
        rot(0,1) *= -1.0;
        rot(1,0) *= -1.0;
        cxdd = rot * cgxdd;
      }
      else  // LOCAL CONTROL
      {
//      cxdd = (dx-x)*mi->KP_ + (dxd-xd)*mi->KV_; // SUBTRACTION IS SO +GAINs
        cxdd[0] = (dx[0]-x[0])*mi->KPx_ + (dxd[0]-xd[0])*mi->KVx_;
        cxdd[1] = (dx[1]-x[1])*mi->KPx_ + (dxd[1]-xd[1])*mi->KVx_;
        cxdd[2] = (dx[2]-x[2])*mi->KPa_ + (dxd[2]-xd[2])*mi->KVa_;

        if( mi->trajMode_ == TRAJ_M_ACCEL )
          cxdd.setZero();

        if( mi->accel_FF_ == true )
          cxdd += dxdd;

        // LOCAL OP SPACE ADJUSTMENTS (PUT HACKs HERE)
        //        cxdd[2]=0;
      }

    // FRICTION COMP
//      if( mi->fricMode_ == FRIC_BOHR )
//      { cxdd[2] += veh->FricBohr(qd,gxd[2],tq);
//      }
    // NULL MODE OVERRIDE (CLOBBER HERE, FOR SAFETY)
      if( mi->trajMode_ == TRAJ_M_NULL )
        cxdd.setZero();

      fcxdd = Fcxdd.Filt(cxdd);  // FILTER EXTERNAL TORQUES
      cxdd  = fcxdd;             // replace w/ filtered

// END CONTROL FORCE SECTION

// BEGIN OUTPUT TORQUE SECTION

    // DO DYNAMICS
      veh->Fill_C(C);         // USE CONSISTENT SPEEDS
      qd_bar = C * xd;        // IGNORE SLIP FOR DYNAMICS
      veh->Dyn(qd_bar, xd[2]);// FILL Lambda & Mu
      Lambda = veh->Lambda;
      Mu     = veh->Mu;

    // GET MAPPING OP FORCE -> JOINT TQ
      Jt = J.transpose(); // MOTOR TQ's w/ MIN CONTACT FORCES

      switch( mi->controlMode_ )
      {
      case CTRL_LM:
          cf = Lambda*cxdd;
          tq = Jt * (cf + Mu);
        break;

      case CTRL_L:
          cf = Lambda*cxdd;
          tq = Jt * cf;
        break;

      case CTRL_L_CONST: // USE AVERAGE MASS PROPERTIES
          cf[0] = veh->M_gross * cxdd[0];  // AVERAGE MASS
          cf[1] = veh->M_gross * cxdd[1];
          cf[2] = veh->I_gross * cxdd[2];  // AVERAGE INERTIA
          tq = Jt * cf ;
        break;

      case CTRL_TEST:    // BRUTE-FORCE CODE TO CLOBBER SOMETHING
          tq.setZero();
          tq[2] = 2.0;
        break;

      case CTRL_CALI:
        // HOLD WHEELS W/ CONTROLLER,
        // CALIBRATE W/ STRAIGHT-EDGE
        { Float jkp = 50;
          Float jkv =  1;
          Float cali_d = M_PI_2;
          tq.setZero();
          tq[0] = -jkp*cos(q[0]+ 2*cali_d) - jkv*qd[0];
          tq[2] = -jkp*cos(q[2]+ 3*cali_d) - jkv*qd[2];
          tq[4] = -jkp*cos(q[4]+ 0*cali_d) - jkv*qd[4];
          tq[6] = -jkp*cos(q[6]+ 1*cali_d) - jkv*qd[6];
        }
        break;

      default:
        cout << "PANIC: OUTPUT TORQUE" << endl;
        break;

      }  // end switch controlMode

    // ADD STEERING FRICTION COMPENSATION
//       fS = veh->Fill_tqS(qd,tq,tqS);
//       tq += Jt * fS;

    // ADD STEERING FRICTION COMPENSATION
      veh->Fill_tqS(qd,tq,rtqS);
      tqS = FtqS.Filt(rtqS);
      tq += tqS;

    // INTERNAL FORCE COMPUTATION
      if( mi->internalForce_ == true )
      {
        veh->Fill_E_q( E );
        // PROJECT TO 'E'-SPACE AND THEN BACK TO JT-SPACE
        qd_null = qd - qd_bar;  // NULL SPACE WHEEL SPEEDS
        rtE = E * qd_null  ;    // RAW SLIP (INTERNAL VELs)
        tE = FtE.Filt(rtE);     // FILTER INTERNAL VELs
        ctE = tE * (-mi->KpE_); // CONTROL FORCES to RESIST SLIP
        tqE * E.transpose() * ctE;   // MAP TO JOINT TORQUES
        tq += tqE;
      }

    // OUTPUT TORQUE TO JOINTS
      veh->JointTq(tq);

// extra for reporting more human-friendly units
veh->JtTq2MotAmp(tq,motAmp);
tqI = Jt *(Lambda*dxdd + Mu);

// END OUTPUT TORQUE SECTION

// BEGIN ACTION QUEUE
      if(  cur_time > mi->trajEnd_  ) //EXPIRED?
      { mi->Step();    // UPDATES mi VIA GLOBAL ITER
      }

      mi->Run();       // ALWAYS RUN TO UPDATE GOALS

        // SANITY CHECK
      while( !mi->Goal_is_valid() )
      { mi->Step();
        mi->Run();
      }

// END ACTION QUEUE
      if( mi->gathMode_ )
      { gObj.gather(cur_time);
        if( gObj.state() == GATHER_STATE_FULL )
          gObj.save();
      }

    } // end THE LOOP

} // end mymain

//---------------------------------------------------
//---------------------------------------------------
// NEEDS TO BE REPLACED.
// WAS IMPLEMENTED USING eCos OPERATING SYSTEM
// https://en.wikipedia.org/wiki/ECos
#define  STACKSIZE_MAIN  ( 1024 * 32 )
int stack_main[ STACKSIZE_MAIN/sizeof(int) ];

timer_handle_t  main_h;
timer_thread    main_obj;

// NEEDS TO BE REPLACED.
// WAS IMPLEMENTED USING eCos OPERATING SYSTEM
// https://en.wikipedia.org/wiki/ECos
void
timer_user_start( void )
{
  timer_thread_create( 3, mymain, 0, "mymain",
                     stack_main, STACKSIZE_MAIN,
                     &main_h, &main_obj);

  timer_thread_resume( main_h );
  Gather::tftp_server_start();
  HWInterface::HandleGet();
}

// NEEDS TO BE REPLACED.
// WAS IMPLEMENTED USING eCos OPERATING SYSTEM
// https://en.wikipedia.org/wiki/ECos
#if defined(redhat) || defined(timerwin)
int main(int argc, char **argv)
{
  timer_user_start();
  timer_thread_delay( 2000000 );
  return 0;
}
#endif


// Copyright (c) 2018 The Bitcoin Unlimited developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#if defined(HAVE_CONFIG_H)
#include "config/bitcoin-config.h"
#endif

#include "allowed_args.h"
#include "arith_uint256.h"
#include "chainparamsbase.h"
#include "fs.h"
#include "hash.h"
#include "primitives/block.h"
#include "rpc/client.h"
#include "rpc/protocol.h"
#include "streams.h"
#include "sync.h"
#include "util.h"
#include "utilstrencodings.h"

#include <cstdlib>
#include <stdio.h>

#include <event2/buffer.h>
#include <event2/event.h>
#include <event2/http.h>
#include <event2/keyvalq_struct.h>

#include <univalue.h>

using namespace std;

#ifdef DEBUG_LOCKORDER
#include <boost/thread/tss.hpp>
// BU add lockstack stuff here for bitcoin-cli, because I need to carefully
// order it in globals.cpp for bitcoind and bitcoin-qt
boost::mutex dd_mutex;
std::map<std::pair<void *, void *>, LockStack> lockorders;
boost::thread_specific_ptr<LockStack> lockstack;
#endif

// Internal miner
//
// ScanHash scans nonces looking for a hash with at least some zero bits.
// The nonce is usually preserved between calls, but periodically or if the
// nonce is 0xffff0000 or above, the block is rebuilt and nNonce starts over at
// zero.
bool static ScanHash(const CBlockHeader *pblock, uint32_t &nNonce, uint256 *phash)
{
    // Write the first 76 bytes of the block header to a double-SHA256 state.
    CHash256 hasher;
    CDataStream ss(SER_NETWORK, PROTOCOL_VERSION);
    ss << *pblock;
    assert(ss.size() == 80);
    hasher.Write((unsigned char *)&ss[0], 76);

    while (true)
    {
        nNonce++;

        // Write the last 4 bytes of the block header (the nonce) to a copy of
        // the double-SHA256 state, and compute the result.
        CHash256(hasher).Write((unsigned char *)&nNonce, 4).Finalize((unsigned char *)phash);

        // Return the nonce if the hash has at least some zero bits,
        // caller will check if it has enough to reach the target
        if (((uint16_t *)phash)[15] == 0)
            return true;

        // If nothing found after trying for a while, return -1
        if ((nNonce & 0xfff) == 0)
            return false;
    }
}

class BitcoinMinerArgs : public AllowedArgs::BitcoinCli
{
public:
    BitcoinMinerArgs(CTweakMap *pTweaks = nullptr)
    {
        addHeader(_("Mining options:"))
            .addArg("blockversion=<n>", ::AllowedArgs::requiredInt,
                _("Set the block version number. For testing only.  Value must be an integer"))
            .addArg("cpus=<n>", ::AllowedArgs::requiredInt,
                _("Number of cpus to use for mining (default: 1).  Value must be an integer"))
            .addArg("duration=<n>", ::AllowedArgs::requiredInt,
                _("Number of seconds to mine a particular block candidate (default: 30). Value must be an integer"))
            .addArg("nblocks=<n>", ::AllowedArgs::requiredInt,
                _("Number of blocks to mine (default: mine forever / -1). Value must be an integer"))
            .addArg("coinbasesize=<n>", ::AllowedArgs::requiredInt,
                _("Get a fixed size coinbase Tx (default: do not use / 0). Value must be an integer"));
    }
};

static CBlockHeader CpuMinerJsonToHeader(const UniValue &params)
{
    // Does not set hashMerkleRoot (Does not exist in Mining-Candidate params).
    CBlockHeader blockheader;

    // nVersion
    blockheader.nVersion = params["version"].get_int();

    // hashPrevBlock
    string tmpstr = params["prevhash"].get_str();
    std::vector<unsigned char> vec = ParseHex(tmpstr);
    std::reverse(vec.begin(), vec.end()); // sent reversed
    blockheader.hashPrevBlock = uint256(vec);

    // nTime:
    blockheader.nTime = params["time"].get_int();

    // nBits
    {
        std::stringstream ss;
        ss << std::hex << params["nBits"].get_str();
        ss >> blockheader.nBits;
    }

    return blockheader;
}

static void CalculateNextMerkleRoot(uint256 &merkle_root, const uint256 &merkle_branch)
{
    // Append a branch to the root. Double SHA256 the whole thing:
    uint256 hash;
    CHash256()
        .Write(merkle_root.begin(), merkle_root.size())
        .Write(merkle_branch.begin(), merkle_branch.size())
        .Finalize(hash.begin());
    merkle_root = hash;
}

static uint256 CalculateMerkleRoot(uint256 &coinbase_hash, const std::vector<uint256> &merkleproof)
{
    uint256 merkle_root = coinbase_hash;
    for (unsigned int i = 0; i < merkleproof.size(); i++)
    {
        CalculateNextMerkleRoot(merkle_root, merkleproof[i]);
    }
    return merkle_root;
}

static bool CpuMineBlockHasher(CBlockHeader *pblock,
    vector<unsigned char> &coinbaseBytes,
    const std::vector<uint256> &merkleproof)
{
    uint32_t nExtraNonce = std::rand();
    uint32_t nNonce = pblock->nNonce;
    arith_uint256 hashTarget = arith_uint256().SetCompact(pblock->nBits);
    bool found = false;
    int ntries = 10;
    unsigned char *pbytes = (unsigned char *)&coinbaseBytes[0];

    while (!found)
    {
        // hashMerkleRoot:
        {
            ++nExtraNonce;
            // 48 - next in arr after Height. (Height in coinbase required for block.version=2):
            *(unsigned int *)(pbytes + 48) = nExtraNonce;
            uint256 hash;
            CHash256().Write(pbytes, coinbaseBytes.size()).Finalize(hash.begin());

            pblock->hashMerkleRoot = CalculateMerkleRoot(hash, merkleproof);
        }

        //
        // Search
        //
        uint256 hash;
        while (!found)
        {
            // Check if something found
            if (ScanHash(pblock, nNonce, &hash))
            {
                if (UintToArith256(hash) <= hashTarget)
                {
                    // Found a solution
                    pblock->nNonce = nNonce;
                    found = true;
                    printf("proof-of-work found  \n  hash: %s  \ntarget: %s\n", hash.GetHex().c_str(),
                        hashTarget.GetHex().c_str());
                    break;
                }
                else
                {
                    if (ntries-- < 1)
                    {
                        pblock->nNonce = nNonce; // report the last nonce checked for accounting
                        return false; // Give up leave
                    }
                }
            }
        }
    }

    return found;
}

static double GetDifficulty(uint64_t nBits)
{
    int nShift = (nBits >> 24) & 0xff;

    double dDiff = (double)0x0000ffff / (double)(nBits & 0x00ffffff);

    while (nShift < 29)
    {
        dDiff *= 256.0;
        nShift++;
    }
    while (nShift > 29)
    {
        dDiff /= 256.0;
        nShift--;
    }
    return dDiff;
}

static UniValue CpuMineBlock(unsigned int searchDuration, const UniValue &params, bool &found)
{
    UniValue tmp(UniValue::VOBJ);
    UniValue ret(UniValue::VARR);
    string tmpstr;
    std::vector<uint256> merkleproof;
    CBlockHeader header;
    vector<unsigned char> coinbaseBytes(ParseHex(params["coinbase"].get_str()));

    found = false;

    // re-create merkle branches:
    {
        UniValue uvMerkleproof = params["merkleProof"];
        for (unsigned int i = 0; i < uvMerkleproof.size(); i++)
        {
            tmpstr = uvMerkleproof[i].get_str();
            std::vector<unsigned char> mbr = ParseHex(tmpstr);
            std::reverse(mbr.begin(), mbr.end());
            merkleproof.push_back(uint256(mbr));
        }
    }

    header = CpuMinerJsonToHeader(params);

    // Set the version (only to test):
    {
        int blockversion = GetArg("-blockversion", header.nVersion);
        if (blockversion != header.nVersion)
            printf("Force header.nVersion to %d\n", blockversion);
        header.nVersion = blockversion;
    }

    uint32_t startNonce = header.nNonce = std::rand();

    printf("Mining: id: %x parent: %s bits: %x difficulty: %3.2f time: %d\n", (unsigned int)params["id"].get_int64(),
        header.hashPrevBlock.ToString().c_str(), header.nBits, GetDifficulty(header.nBits), header.nTime);

    int64_t start = GetTime();
    while ((GetTime() < start + searchDuration) && !found)
    {
        // When mining mainnet, you would normally want to advance the time to keep the block time as close to the
        // real time as possible.  However, this CPU miner is only useful on testnet and in testnet the block difficulty
        // resets to 1 after 20 minutes.  This will cause the block's difficulty to mismatch the expected difficulty
        // and the block will be rejected.  So do not advance time (let it be advanced by bitcoind every time we
        // request a new block).
        // header.nTime = (header.nTime < GetTime()) ? GetTime() : header.nTime;
        found = CpuMineBlockHasher(&header, coinbaseBytes, merkleproof);
    }

    // Leave if not found:
    if (!found)
    {
        printf("Checked %llu possibilities\n", (unsigned long long)(header.nNonce - startNonce));
        return ret;
    }

    printf("Solution! Checked %llu possibilities\n", (unsigned long long )(header.nNonce - startNonce));

    tmpstr = HexStr(coinbaseBytes.begin(), coinbaseBytes.end());
    tmp.pushKV("coinbase", tmpstr);
    tmp.pushKV("id", params["id"]);
    tmp.pushKV("time", UniValue(header.nTime)); // Optional. We have changed so must send.
    tmp.pushKV("nonce", UniValue(header.nNonce));
    tmp.pushKV("version", UniValue(header.nVersion)); // Optional. We may have changed so sending.
    ret.push_back(tmp);

    return ret;
}

static UniValue RPCSubmitSolution(const UniValue &solution, int &nblocks)
{
    UniValue reply = CallRPC("submitminingsolution", solution);

    const UniValue &error = find_value(reply, "error");

    if (!error.isNull())
    {
        fprintf(stderr, "Block Candidate submission error: %d %s\n", error["code"].get_int(),
            error["message"].get_str().c_str());
        return reply;
    }

    const UniValue &result = find_value(reply, "result");

    if (result.isStr())
    {
        fprintf(stderr, "Block Candidate rejected. Error: %s\n", result.get_str().c_str());
        // Print some debug info if the block is rejected
        UniValue dbg = solution[0].get_obj();
        fprintf(stderr, "id: %d  time: %d  nonce: %d  version: 0x%x\n", (unsigned int)dbg["id"].get_int64(),
            (uint32_t)dbg["time"].get_int64(), (uint32_t)dbg["nonce"].get_int64(), (uint32_t)dbg["version"].get_int());
        fprintf(stderr, "coinbase: %s\n", dbg["coinbase"].get_str().c_str());
    }
    else
    {
        if (result.isNull())
        {
            printf("Block Candidate accepted.\n");
            if (nblocks > 0)
                nblocks--; // Processed a block
        }
        else
        {
            fprintf(stderr, "Unknown \"submitminingsolution\" Error.\n");
        }
    }

    return reply;
}

int CpuMiner(void)
{
    int searchDuration = GetArg("-duration", 30);
    int nblocks = GetArg("-nblocks", -1); //-1 mine forever
    int coinbasesize = GetArg("-coinbasesize", 0);

    if (coinbasesize < 0)
    {
        printf("Negative coinbasesize not reasonable/supported.\n");
        return 0;
    }

    UniValue mineresult;
    bool found = false;

    if (0 == nblocks)
    {
        printf("Nothing to do for zero (0) blocks\n");
        return 0;
    }

    while (0 != nblocks)
    {
        UniValue reply;
        UniValue result;
        string strPrint;
        int nRet = 0;
        try
        {
            // Execute and handle connection failures with -rpcwait
            const bool fWait = true;
            do
            {
                try
                {
                    UniValue params(UniValue::VARR);
                    if (found)
                    {
                        // Submit the solution.
                        // Called here so all exceptions are handled properly below.
                        reply = RPCSubmitSolution(mineresult, nblocks);
                        if (nblocks == 0)
                            return 0; // Done mining exit program
                        found = false; // Mine again
                    }

                    if (!found)
                    {
                        if (coinbasesize > 0)
                        {
                            params.push_back(UniValue(coinbasesize));
                        }
                        reply = CallRPC("getminingcandidate", params);
                    }

                    // Parse reply
                    result = find_value(reply, "result");
                    const UniValue &error = find_value(reply, "error");

                    if (!error.isNull())
                    {
                        // Error
                        int code = error["code"].get_int();
                        if (fWait && code == RPC_IN_WARMUP)
                            throw CConnectionFailed("server in warmup");
                        strPrint = "error: " + error.write();
                        nRet = abs(code);
                        if (error.isObject())
                        {
                            UniValue errCode = find_value(error, "code");
                            UniValue errMsg = find_value(error, "message");
                            strPrint = errCode.isNull() ? "" : "error code: " + errCode.getValStr() + "\n";

                            if (errMsg.isStr())
                                strPrint += "error message:\n" + errMsg.get_str();
                        }
                    }
                    else
                    {
                        // Result
                        if (result.isNull())
                            strPrint = "";
                        else if (result.isStr())
                            strPrint = result.get_str();
                        else
                            strPrint = result.write(2);
                    }
                    // Connection succeeded, no need to retry.
                    break;
                }
                catch (const CConnectionFailed &c)
                {
                    if (fWait)
                    {
                        printf("Warning: %s\n", c.what());
                        MilliSleep(1000);
                    }
                    else
                        throw;
                }
            } while (fWait);
        }
        catch (const boost::thread_interrupted &)
        {
            throw;
        }
        catch (const std::exception &e)
        {
            strPrint = string("error: ") + e.what();
            nRet = EXIT_FAILURE;
        }
        catch (...)
        {
            PrintExceptionContinue(NULL, "CommandLineRPC()");
            throw;
        }

        if (strPrint != "")
        {
            if (nRet != 0)
                fprintf(stderr, "%s\n", strPrint.c_str());
            // Actually do some mining
            if (result.isNull())
            {
                MilliSleep(1000);
            }
            else
            {
                found = false;
                mineresult = CpuMineBlock(searchDuration, result, found);
                if (!found)
                {
                    // printf("Mining did not succeed\n");
                    mineresult.setNull();
                }
                // The result is sent to bitcoind above when the loop gets to it.
                // See:   RPCSubmitSolution(mineresult,nblocks);
                // This is so RPC Exceptions are handled in one place.
            }
        }
    }
    return 0;
}

void static MinerThread()
{
    while (1)
    {
        try
        {
            CpuMiner();
        }
        catch (const std::exception &e)
        {
            PrintExceptionContinue(&e, "CommandLineRPC()");
        }
        catch (...)
        {
            PrintExceptionContinue(NULL, "CommandLineRPC()");
        }
    }
}

int main(int argc, char *argv[])
{
    SetupEnvironment();
    if (!SetupNetworking())
    {
        fprintf(stderr, "Error: Initializing networking failed\n");
        exit(1);
    }

    try
    {
        std::string appname("bitcoin-miner");
        std::string usage = "\n" + _("Usage:") + "\n" + "  " + appname + " [options] " + "\n";
        int ret = AppInitRPC(usage, BitcoinMinerArgs(), argc, argv);
        if (ret != CONTINUE_EXECUTION)
            return ret;
    }
    catch (const std::exception &e)
    {
        PrintExceptionContinue(&e, "AppInitRPC()");
        return EXIT_FAILURE;
    }
    catch (...)
    {
        PrintExceptionContinue(NULL, "AppInitRPC()");
        return EXIT_FAILURE;
    }

    int nThreads = GetArg("-cpus", 1);
    boost::thread_group minerThreads;
    for (int i = 0; i < nThreads - 1; i++)
        minerThreads.create_thread(MinerThread);

    int ret = EXIT_FAILURE;
    try
    {
        ret = CpuMiner();
    }
    catch (const std::exception &e)
    {
        PrintExceptionContinue(&e, "CommandLineRPC()");
    }
    catch (...)
    {
        PrintExceptionContinue(NULL, "CommandLineRPC()");
    }
    return ret;
}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt 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 Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#define _USE_MATH_DEFINES
#include <cmath>
#include <algorithm>
#include <boost/numeric/ublas/assignment.hpp>
#include "bayesopt/bayesopt.hpp"
#include "param_loader.hpp"
#include "fileparser.hpp"

#ifndef M_PI
/* It shouldn't be necessary, but Windows is completely nuts about
   math constants and I HATE when include order matters. */
    #define M_PI       3.14159265358979323846
#endif

class TemplateWritter{
    private:
    std::ifstream input;
    std::string prefix;

    public:
    TemplateWritter(std::string &template_filename, std::string &template_prefix)
    :input(), prefix(template_prefix)
    {
        input.open(template_filename.c_str());
    }

    ~TemplateWritter(){
        input.close();
    }

    // Create a file overwriting parameters from the template file
    void createFile(std::string &filename, const vectord& xin){
        // Move input pointer to the start of the file and create the output stream
        input.clear();
        input.seekg(0, std::ios::beg);
        std::ofstream output(filename.c_str());

        bayesopt::utils::FileParser fp; // to_value() and to_string() functions access

        std::string currentLine;
        while (getline( input, currentLine)){
            if(currentLine.length() > 0){
                // Search for prefixes
                size_t prefix_index = currentLine.find(prefix);
                while(prefix_index != std::string::npos){
                    size_t number_index = prefix_index + prefix.length();

                    // Get the number after the prefix
                    std::string number_string = "";
                    while(number_index < currentLine.size() && std::isdigit(currentLine.at(number_index))){
                        number_string = number_string + currentLine.at(number_index);
                        number_index++;
                    }

                    // Index of the parameter
                    size_t x_index = fp.to_value<size_t>(number_string);

                    // Check xin bounds before attempting to read data
                    if(x_index >= xin.size()){
                        // Show error and print "OUT_OF_BOUNDS" on the position of the template
                        std::cout << "ERROR: parameter " << x_index << " is out of bounds" << std::endl;
                        output << currentLine.substr(0, prefix_index) << "OUT_OF_BOUNDS";
                    }
                    else{
                        // And finally, write the value on the correct position of the template
                        double x = xin(x_index);
                        output << currentLine.substr(0, prefix_index) << fp.to_string(x);
                    }

                    // Change currentLine to hold unwritten data and perform a new search
                    currentLine = currentLine.substr(number_index);
                    prefix_index = currentLine.find(prefix);
                }
                // Write unwritten data
                output << currentLine << std::endl;
            }
        }

        output.close();
    }
};

class XMLCallsBranin: public bayesopt::ContinuousModel
{
public:
  XMLCallsBranin(bayesopt::Parameters par):
    ContinuousModel(2,par) {}

  double evaluateSample( const vectord& xin)
  {
     if (xin.size() != 2)
      {
	std::cout << "WARNING: This only works for 2D inputs." << std::endl
		  << "WARNING: Using only first two components." << std::endl;
      }

    float y = -1;
    double x1 = xin(0);
    double x2 = xin(1);

    // Create XML from template
    std::string template_filename("../examples/standalone_calls/problem_template.xml");
    std::string template_prefix("XXXX_");
    TemplateWritter tw(template_filename, template_prefix);
    std::string created_filename("../examples/standalone_calls/created_file.xml");
    tw.createFile(created_filename, xin);

    // Results filename
    std::string results_filename("../examples/standalone_calls/results.txt");

    // Call python script
    std::string call = "python ../examples/standalone_calls/eval_branin_xml.py " + created_filename + " " + results_filename;
    system(call.c_str());

    // TODO (Javier): Change results to XML format
    bayesopt::utils::FileParser fp(results_filename.c_str());
    fp.openInput();
    fp.read("y",y);

    return y;
  }

  bool checkReachability(const vectord &query)
  {return true;};

  inline double sqr( double x ){ return x*x; };

  void printOptimal()
  {
    vectord sv(2);
    sv(0) = 0.1238938; sv(1) = 0.818333;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = 0.5427728; sv(1) = 0.151667;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
    sv(0) = 0.961652; sv(1) = 0.1650;
    std::cout << "Solutions: " << sv << "->"
	      << evaluateSample(sv) << std::endl;
  }

};

int main(int nargs, char *args[])
{
  bayesopt::Parameters par;
  if(nargs > 1){
    if(!bayesopt::utils::ParamLoader::load(args[1], par)){
        std::cout << "ERROR: provided file \"" << args[1] << "\" does not exist" << std::endl;
        return -1;
    }
  }
  else{
    par = initialize_parameters_to_default();
    par.n_iterations = 190;
    par.random_seed = 0;
    par.verbose_level = 1;
    par.noise = 1e-10;
    //bayesopt::utils::ParamLoader::save("system_opt.txt", par);
  }

  XMLCallsBranin branin(par);
  vectord result(2);

  branin.optimize(result);
  std::cout << "Result: " << result << "->"
	    << branin.evaluateSample(result) << std::endl;
  branin.printOptimal();

    // Remove results.txt file
  std::string filename("../examples/standalone_calls/created_file.xml");
  if( remove( filename.c_str() ) == 0 ){
    std::cout << "File \"" << filename << "\" successfully removed" << std::endl;
  }
  else{
    std::cout << "Error: cannot remove \"" << filename << "\" file" << std::endl;
  }

    // Remove results.txt file
  filename = "../examples/standalone_calls/results.txt";
  if( remove( filename.c_str() ) == 0 ){
    std::cout << "File \"" << filename << "\" successfully removed" << std::endl;
  }
  else{
    std::cout << "Error: cannot remove \"" << filename << "\" file" << std::endl;
  }
  return 0;
}


#pragma once

#include <ros/ros.h>
#include <stdio.h>
#include <map>
#include <roboy_middleware_msgs/MagneticSensor.h>
#include <common_utilities/CommonDefinitions.h>
#include <common_utilities/rviz_visualization.hpp>
#include <fstream>
#include <iostream>
#include <mutex>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <thread>
#include "grid3d.hpp"
#include "cnpy.h"
#include <numeric>      // std::iota
#include <algorithm>    // std::sort, std::stable_sort
#include <yaml-cpp/yaml.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <stdio.h>
#include <pcl/visualization/cloud_viewer.h>
#include "pose_estimator.hpp"

using namespace std;
using namespace Eigen;

float randUniform() {
    return static_cast <float> (rand()) / static_cast <float> (RAND_MAX);
}

class BallInSocketEstimator : public rviz_visualization {

public:
    BallInSocketEstimator(string sensor_pos_filepath, string sensor_values_filepath, string config_filepath);

    ~BallInSocketEstimator();

    void magneticSensorCallback(const roboy_middleware_msgs::MagneticSensorConstPtr& msg);
private:
  bool fileExists(const string &filepath){
      struct stat buffer;
      return (stat (filepath.c_str(), &buffer) == 0);
  }

  template <typename T>
  vector<size_t> sort_indexes(const vector<T> &v) {

    // initialize original index locations
    vector<size_t> idx(v.size());
    iota(idx.begin(), idx.end(), 0);

    // sort indexes based on comparing values in v
    // using std::stable_sort instead of std::sort
    // to avoid unnecessary index re-orderings
    // when v contains elements of equal values
    stable_sort(idx.begin(), idx.end(),
         [&v](size_t i1, size_t i2) {return v[i1] < v[i2];});

    return idx;
  }
  boost::shared_ptr<pcl::visualization::PCLVisualizer> rgbVis(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud) {
      // --------------------------------------------
      // -----Open 3D viewer and add point cloud-----
      // --------------------------------------------
      boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
      viewer->setBackgroundColor(0, 0, 0);
      pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
      viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
      viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
      viewer->addCoordinateSystem(1.0);
      viewer->initCameraParameters();
      return (viewer);
  }

  void convertFromNpy(double *data, vector<unsigned long> shape, vector<vector<vector<double>>> &data_out) {
      data_out.resize(shape[0]);
      for (int i = 0; i < shape[0]; i++) {
          data_out[i].resize(shape[1]);
          for (int j = 0; j < shape[1]; j++) {
              data_out[i][j].resize(shape[2]);
              for (int k = 0; k < shape[2]; k++) {
                  data_out[i][j][k] = data[i * shape[1] * shape[2] + j * shape[2] + k];
  //              printf("%f\t",mv1[i * arr_mv1.shape[1] * arr_mv1.shape[2] + j * arr_mv1.shape[2] + k]);
              }
  //          printf("\n");
          }
      }
  }
private:
    ros::NodeHandlePtr nh;
    ros::Time start_time;
    boost::shared_ptr<ros::AsyncSpinner> spinner;
    ros::Subscriber magnetic_sensor_sub;
    boost::shared_ptr<PoseEstimator> estimator;
    vector<int> sensor_select = {1,5};
};


#include <iostream>
#include <Transformer.h>
#include <boost/test/unit_test.hpp>

using namespace esrocos::transformer;

typedef AcyclicTransformer<> atf20_20;

BOOST_AUTO_TEST_SUITE(transformation_tests)

BOOST_AUTO_TEST_CASE(transform_construction)
{
  atf20_20::Transformation t;

  BOOST_CHECK_EQUAL(0,std::strcmp(t.a(),""));
  BOOST_CHECK_EQUAL(0,std::strcmp(t.b(),""));
  BOOST_CHECK_EQUAL(0,std::strcmp(t.id(),""));

  BOOST_CHECK_EQUAL(true,identity.isApprox(t.atob()));
  BOOST_CHECK_EQUAL(true,identity.isApprox(t.atob()));
}

BOOST_AUTO_TEST_CASE(transform_initialization)
{
 const char * a = "from";
 const char * b = "to";
 const char * id = "some_id";

 atf20_20::Transformation t(a,b,id);

 BOOST_CHECK_EQUAL(0,std::strcmp(t.a(),a));
 BOOST_CHECK_EQUAL(0,std::strcmp(t.b(),b));
 BOOST_CHECK_EQUAL(0,std::strcmp(t.id(),id));

 BOOST_CHECK_EQUAL(true,identity.isApprox(t.atob()));
 BOOST_CHECK_EQUAL(true,identity.isApprox(t.atob()));
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/units/systems/si/moment_of_inertia.hpp>


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TOINT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TOINT_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/toint.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( toint_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<uint_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed> ::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return bitwise_cast<result_type>(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( toint_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<int_<A0>,X>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return a0;
    }
  };

} } }

#endif


#include "OccR.hpp"
#include "ContactPoint.hpp"
#include "OccContactShape.hpp"
#include "ContactShapeDistance.hpp"
#include "WhichGeometer.hpp"
#include "RuntimeException.hpp"
#include <limits>
#include <iostream>
#include <boost/typeof/typeof.hpp>

#undef DEBUG_MESSAGES
#include "debug.h"

OccR::OccR(const ContactPoint& contact1,
           const ContactPoint& contact2,
           const DistanceCalculatorType& distance_calculator) :
  NewtonEuler3DR(),
  _contact1(contact1),
  _contact2(contact2),
  _geometer(),
  _offset1(0.),
  _offset2(0.)
{
  switch (Type::value(distance_calculator))
  {
  case Type::OccDistanceType:
    this->_geometer = ask<WhichGeometer<OccDistanceType> >(contact1.contactShape());
    break;
  case Type::CadmbtbDistanceType:
    this->_geometer = ask<WhichGeometer<CadmbtbDistanceType> >(contact1.contactShape());
    break;
  default:
    RuntimeException::selfThrow("OccR: Unknown distance calculator");
  }
  this->_contact2.contactShape().accept(*this->_geometer);
}

void OccR::computeh(double time, BlockVector& q0, SiconosVector& y)
{
  this->_contact2.contactShape().accept(*this->_geometer);

  ContactShapeDistance& dist = this->_geometer->answer;

  DEBUG_PRINTF("---->%g P1=(%g, %g, %g) P2=(%g,%g,%g) N=(%g, %g, %g)\n", dist.value,
               dist.x1, dist.y1, dist.z1,
               dist.x2, dist.y2, dist.z2,
               dist.nx, dist.ny, dist.nz);

  _Pc1->setValue(0, dist.x1 + _offset1*dist.nx);
  _Pc1->setValue(1, dist.y1 + _offset1*dist.ny);
  _Pc1->setValue(2, dist.z1 + _offset1*dist.nz);
  _Pc2->setValue(0, dist.x2 - _offset2*dist.nx);
  _Pc2->setValue(1, dist.y2 - _offset2*dist.ny);
  _Pc2->setValue(2, dist.z2 - _offset2*dist.nz);

  _Nc->setValue(0, dist.nx);
  _Nc->setValue(1, dist.ny);
  _Nc->setValue(2, dist.nz);

  dist.value -= (_offset1+_offset2);

  y.setValue(0, dist.value);

}


/* ----------------------------------------------------------------------- *//**
 *
 * @file MathToolkit_impl.hpp
 *
 * @brief User-defined error handling functions
 *
 *//* ----------------------------------------------------------------------- */

#ifndef MADLIB_DBAL_BOOST_INTEGRATION_MATH_TOOLKIT_IMPL_HPP
#define MADLIB_DBAL_BOOST_INTEGRATION_MATH_TOOLKIT_IMPL_HPP

#include <iomanip>

#include <boost/format.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost {

namespace math {

namespace policies {

/**
 * @brief Boost user-defined domain-error handling function
 *
 * This function is called by the domain_error<user_error> policy in case
 * function arguments (or parameters) are outside of the domain of the
 * probability function.
 *
 * Our policy is to let NaNs propagate. All other errors we handle by throwing
 * a std::domain_error containing the text supplied by boost.
 *
 * @param inFunction The name of the function that raised the error, this string
 *     contains one or more %1% format specifiers that should be replaced by the
 *     name of real type T, like float or double.
 * @param inMessage A message associated with the error, normally this contains
 *     a %1% format specifier that should be replaced with the value of value:
 *     however note that overflow and underflow messages do not contain this
 *     %1% specifier (since the value of value is immaterial in these cases).
 * @param inVal The value that caused the error: either an argument to the
 *     function if this is a domain or pole error, the tentative result if this
 *     is a denorm or evaluation error, or zero or infinity for underflow or
 *     overflow errors.
 */
template <class T>
inline
T
user_domain_error(const char*, const char* inMessage, const T& inVal) {
    if (std::isnan(inVal))
        return std::numeric_limits<double>::quiet_NaN();

    // The following line is taken from
    // http://www.boost.org/doc/libs/1_49_0/libs/math/doc/sf_and_dist/html/math_toolkit/policy/pol_tutorial/user_def_err_pol.html
    int prec = 2 + (std::numeric_limits<T>::digits * 30103UL) / 100000UL;

    std::string msg = (boost::format(inMessage)
            % boost::io::group(std::setprecision(prec), inVal)
        ).str();

    // Some Boost error messages contain a space before the punctuation mark,
    // which we will remove.
    std::string::iterator lastChar = msg.end() - 1;
    std::string::iterator secondLastChar = msg.end() - 2;
    if (std::ispunct(*lastChar) && std::isspace(*secondLastChar))
        msg.erase(secondLastChar, lastChar);

    throw std::domain_error(msg);
}

} // namespace policies

} // namespace math

} // namespace boost

namespace madlib {

typedef boost::math::policies::policy<
    boost::math::policies::domain_error<boost::math::policies::user_error>,
    boost::math::policies::overflow_error<boost::math::policies::ignore_error>
> boost_mathkit_policy;

} // namespace madlib

#endif // !defined(MADLIB_DBAL_BOOST_INTEGRATION_MATH_TOOLKIT_IMPL_HPP)


// g++ -O3 -DNDEBUG -I.. -fopenmp benchOpenMP.cpp -o benchOpenMP && ./benchOpenMP 2> /dev/null
// icpc -fast -fno-exceptions -DNDEBUG -I.. -openmp  benchOpenMP.cpp -o benchOpenMP && ./benchOpenMP 2> /dev/null

#include <omp.h>
#include "BenchUtil.h"
#include "basicbenchmark.h"

// #include <Eigen/Core>
// #include "BenchTimer.h"
//
// using namespace std;
// USING_PART_OF_NAMESPACE_EIGEN
//
// enum {LazyEval, EarlyEval, OmpEval};
//
// template<int Mode, typename MatrixType>
// double benchSingleProc(const MatrixType& mat, int iterations, int tries)  __attribute__((noinline));
//
// template<int Mode, typename MatrixType>
// double benchBasic(const MatrixType& mat, int iterations, int tries)
// {
//   const int rows = mat.rows();
//   const int cols = mat.cols();
//
//   Eigen::BenchTimer timer;
//   for(uint t=0; t<tries; ++t)
//   {
//     MatrixType I = MatrixType::identity(rows, cols);
//     MatrixType m = MatrixType::random(rows, cols);
//
//     timer.start();
//     for(int a = 0; a < iterations; a++)
//     {
//       if(Mode==LazyEval)
//         m = (I + 0.00005 * (m + m.lazyProduct(m))).eval();
//       else if(Mode==OmpEval)
//         m = (I + 0.00005 * (m + m.lazyProduct(m))).evalOMP();
//       else
//         m = I + 0.00005 * (m + m * m);
//     }
//     timer.stop();
//     cerr << m;
//   }
//   return timer.value();
// };

int main(int argc, char *argv[])
{
  // disable floating point exceptions
  // this leads to more stable bench results
  {
    int aux;
    asm(
    "stmxcsr   %[aux]           \n\t"
    "orl       $32832, %[aux]   \n\t"
    "ldmxcsr   %[aux]           \n\t"
    : : [aux] "m" (aux));
  }

  // commented since the default setting is use as many threads as processors
  //omp_set_num_threads(omp_get_num_procs());

  std::cout << "double, fixed-size 4x4: "
    << benchBasic<LazyEval>(Matrix4d(), 10000, 10) << "s  "
    << benchBasic<OmpEval>(Matrix4d(), 10000, 10) << "s  \n";

  #define BENCH_MATRIX(TYPE, SIZE, ITERATIONS, TRIES) {\
      double single = benchBasic<LazyEval>(Matrix<TYPE,Eigen::Dynamic,Eigen::Dynamic>(SIZE,SIZE), ITERATIONS, TRIES); \
      double omp    = benchBasic<OmpEval> (Matrix<TYPE,Eigen::Dynamic,Eigen::Dynamic>(SIZE,SIZE), ITERATIONS, TRIES); \
      std::cout << #TYPE << ", " << #SIZE << "x" << #SIZE << ": " << single << "s " << omp << "s " \
        << " => x" << single/omp << " (" << omp_get_num_procs() << ")" << std::endl; \
    }

  BENCH_MATRIX(double,   32, 1000, 10);
  BENCH_MATRIX(double,  128,   10, 10);
  BENCH_MATRIX(double,  512,    1,  6);
  BENCH_MATRIX(double, 1024,    1,  4);

  return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example shows how to train a CNN based object detector using dlib's
    loss_mmod loss layer.  This loss layer implements the Max-Margin Object
    Detection loss as described in the paper:
        Max-Margin Object Detection by Davis E. King (http://arxiv.org/abs/1502.00046).
    This is the same loss used by the popular SVM+HOG object detector in dlib
    (see fhog_object_detector_ex.cpp) except here we replace the HOG features
    with a CNN and train the entire detector end-to-end.  This allows us to make
    much more powerful detectors.

    It would be a good idea to become familiar with dlib's DNN tooling before
    reading this example.  So you should read dnn_introduction_ex.cpp and
    dnn_introduction2_ex.cpp before reading this example program.

    Just like in the fhog_object_detector_ex.cpp example, we are going to train
    a simple face detector based on the very small training dataset in the
    examples/faces folder.  As we will see, even with this small dataset the
    MMOD method is able to make a working face detector.  However, for real
    applications you should train with more data for an even better result.
*/

#include <iostream>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/gui_widgets.h>

using namespace std;
using namespace dlib;

// The first thing we do is define our CNN.  The CNN is going to be evaluated
// convolutionally over an entire image pyramid.  Think of it like a normal
// sliding window classifier.  This means you need to define a CNN that can look
// at some part of an image and decide if it is an object of interest.  In this
// example I've defined a CNN with a receptive field of a little over 50x50
// pixels.  This is reasonable for face detection since you can clearly tell if
// a 50x50 image contains a face.  Other applications may benefit from CNNs with
// different architectures.
//
// In this example our CNN begins with 3 downsampling layers.  These layers will
// reduce the size of the image by 8x and output a feature map with
// 32 dimensions.  Then we will pass that through 4 more convolutional layers to
// get the final output of the network.  The last layer has only 1 channel and
// the values in that last channel are large when the network thinks it has
// found an object at a particular location.

// Let's begin the network definition by creating some network blocks.

// A 5x5 conv layer that does 2x downsampling
template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
// A 3x3 conv layer that doesn't do any downsampling
template <long num_filters, typename SUBNET> using con3  = con<num_filters,3,3,1,1,SUBNET>;

// Now we can define the 8x downsampling block in terms of conv5d blocks.  We
// also use relu and batch normalization in the standard way.
template <typename SUBNET> using downsampler  = relu<bn_con<con5d<32, relu<bn_con<con5d<32, relu<bn_con<con5d<32,SUBNET>>>>>>>>>;

// The rest of the network will be 3x3 conv layers with batch normalization and
// relu.  So we define the 3x3 block we will use here.
template <typename SUBNET> using rcon3  = relu<bn_con<con3<32,SUBNET>>>;

// Finally, we define the entire network.   The special input_rgb_image_pyramid
// layer causes the network to operate over a spatial pyramid, making the detector
// scale invariant.
using net_type  = loss_mmod<con<1,6,6,1,1,rcon3<rcon3<rcon3<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

// ----------------------------------------------------------------------------------------

int main(int argc, char** argv) try
{
    // In this example we are going to train a face detector based on the
    // small faces dataset in the examples/faces directory.  So the first
    // thing we do is load that dataset.  This means you need to supply the
    // path to this faces folder as a command line argument so we will know
    // where it is.
    if (argc != 2)
    {
        cout << "Give the path to the examples/faces directory as the argument to this" << endl;
        cout << "program.  For example, if you are in the examples folder then execute " << endl;
        cout << "this program by running: " << endl;
        cout << "   ./dnn_mmod_ex faces" << endl;
        cout << endl;
        return 0;
    }
    const std::string faces_directory = argv[1];
    // The faces directory contains a training dataset and a separate
    // testing dataset.  The training data consists of 4 images, each
    // annotated with rectangles that bound each human face.  The idea is
    // to use this training data to learn to identify human faces in new
    // images.
    //
    // Once you have trained an object detector it is always important to
    // test it on data it wasn't trained on.  Therefore, we will also load
    // a separate testing set of 5 images.  Once we have a face detector
    // created from the training data we will see how well it works by
    // running it on the testing images.
    //
    // So here we create the variables that will hold our dataset.
    // images_train will hold the 4 training images and face_boxes_train
    // holds the locations of the faces in the training images.  So for
    // example, the image images_train[0] has the faces given by the
    // rectangles in face_boxes_train[0].
    std::vector<matrix<rgb_pixel>> images_train, images_test;
    std::vector<std::vector<mmod_rect>> face_boxes_train, face_boxes_test;

    // Now we load the data.  These XML files list the images in each dataset
    // and also contain the positions of the face boxes.  Obviously you can use
    // any kind of input format you like so long as you store the data into
    // images_train and face_boxes_train.  But for convenience dlib comes with
    // tools for creating and loading XML image datasets.  Here you see how to
    // load the data.  To create the XML files you can use the imglab tool which
    // can be found in the tools/imglab folder.  It is a simple graphical tool
    // for labeling objects in images with boxes.  To see how to use it read the
    // tools/imglab/README.txt file.
    load_image_dataset(images_train, face_boxes_train, faces_directory+"/training.xml");
    load_image_dataset(images_test, face_boxes_test, faces_directory+"/testing.xml");

    cout << "num training images: " << images_train.size() << endl;
    cout << "num testing images:  " << images_test.size() << endl;

    // The MMOD algorithm has some options you can set to control its behavior.  However,
    // you can also call the constructor with your training annotations and a "target
    // object size" and it will automatically configure itself in a reasonable way for your
    // problem.  Here we are saying that faces are still recognizably faces when they are
    // 40x40 pixels in size.  You should generally pick the smallest size where this is
    // true.  Based on this information the mmod_options constructor will automatically
    // pick a good sliding window width and height.  It will also automatically set the
    // non-max-suppression parameters to something reasonable.  For further details see the
    // mmod_options documentation.
    mmod_options options(face_boxes_train, 40,40);
    // The detector will automatically decide to use multiple sliding windows if needed.
    // For the face data, only one is needed however.
    cout << "num detector windows: "<< options.detector_windows.size() << endl;
    for (auto& w : options.detector_windows)
        cout << "detector window width by height: " << w.width << " x " << w.height << endl;
    cout << "overlap NMS IOU thresh:             " << options.overlaps_nms.get_iou_thresh() << endl;
    cout << "overlap NMS percent covered thresh: " << options.overlaps_nms.get_percent_covered_thresh() << endl;

    // Now we are ready to create our network and trainer.
    net_type net(options);
    // The MMOD loss requires that the number of filters in the final network layer equal
    // options.detector_windows.size().  So we set that here as well.
    net.subnet().layer_details().set_num_filters(options.detector_windows.size());
    dnn_trainer<net_type> trainer(net);
    trainer.set_learning_rate(0.1);
    trainer.be_verbose();
    trainer.set_synchronization_file("mmod_sync", std::chrono::minutes(5));
    trainer.set_iterations_without_progress_threshold(300);

    // Now let's train the network.  We are going to use mini-batches of 150
    // images.   The images are random crops from our training set (see
    // random_cropper_ex.cpp for a discussion of the random_cropper).
    std::vector<matrix<rgb_pixel>> mini_batch_samples;
    std::vector<std::vector<mmod_rect>> mini_batch_labels;
    random_cropper cropper;
    cropper.set_chip_dims(200, 200);
    // Usually you want to give the cropper whatever min sizes you passed to the
    // mmod_options constructor, which is what we do here.
    cropper.set_min_object_size(40,40);
    dlib::rand rnd;
    // Run the trainer until the learning rate gets small.  This will probably take several
    // hours.
    while(trainer.get_learning_rate() >= 1e-4)
    {
        cropper(150, images_train, face_boxes_train, mini_batch_samples, mini_batch_labels);
        // We can also randomly jitter the colors and that often helps a detector
        // generalize better to new images.
        for (auto&& img : mini_batch_samples)
            disturb_colors(img, rnd);

        trainer.train_one_step(mini_batch_samples, mini_batch_labels);
    }
    // wait for training threads to stop
    trainer.get_net();
    cout << "done training" << endl;

    // Save the network to disk
    net.clean();
    serialize("mmod_network.dat") << net;

    // Now that we have a face detector we can test it.  The first statement tests it
    // on the training data.  It will print the precision, recall, and then average precision.
    // This statement should indicate that the network works perfectly on the
    // training data.
    cout << "training results: " << test_object_detection_function(net, images_train, face_boxes_train) << endl;
    // However, to get an idea if it really worked without overfitting we need to run
    // it on images it wasn't trained on.  The next line does this.   Happily,
    // this statement indicates that the detector finds most of the faces in the
    // testing data.
    cout << "testing results:  " << test_object_detection_function(net, images_test, face_boxes_test) << endl;

    // If you are running many experiments, it's also useful to log the settings used
    // during the training experiment.  This statement will print the settings we used to
    // the screen.
    cout << trainer << cropper << endl;

    // Now lets run the detector on the testing images and look at the outputs.
    image_window win;
    for (auto&& img : images_test)
    {
        pyramid_up(img);
        auto dets = net(img);
        win.clear_overlay();
        win.set_image(img);
        for (auto&& d : dets)
            win.add_overlay(d);
        cin.get();
    }
    return 0;

    // Now that you finished this example, you should read dnn_mmod_train_find_cars_ex.cpp,
    // which is a more advanced example.  It discusses many issues surrounding properly
    // setting the MMOD parameters and creating a good training dataset.

}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


// Boost enable_if library

// Copyright 2003 (c) The Trustees of Indiana University.

// Use, modification, and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//    Authors: Jaakko Jarvi (jajarvi at osl.iu.edu)
//             Jeremiah Willcock (jewillco at osl.iu.edu)
//             Andrew Lumsdaine (lums at osl.iu.edu)

#include <boost/config.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/core/lightweight_test.hpp>

using boost::enable_if;
using boost::is_arithmetic;

template<class T> struct not_
{
  BOOST_STATIC_CONSTANT( bool, value = !T::value );
};

namespace A {
  template<class T>
  typename enable_if<is_arithmetic<T>, bool>::type
  arithmetic_object(T t) { return true; }
}

namespace B {
  template<class T>
  typename enable_if<not_<is_arithmetic<T> >, bool>::type
  arithmetic_object(T t) { return false; }
}

int main()
{
  using namespace A;
  using namespace B;
  BOOST_TEST(arithmetic_object(1));
  BOOST_TEST(arithmetic_object(1.0));

  BOOST_TEST(!arithmetic_object("1"));
  BOOST_TEST(!arithmetic_object(static_cast<void*>(0)));

  return boost::report_errors();
}