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

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.

===================================================================*/
#ifndef __itkFiniteDiffOdfMaximaExtractionFilter_cpp
#define __itkFiniteDiffOdfMaximaExtractionFilter_cpp

#include "itkFiniteDiffOdfMaximaExtractionFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <vnl/vnl_vector.h>
#include <itkOrientationDistributionFunction.h>
#include <itkContinuousIndex.h>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>

#include <boost/math/special_functions.hpp>
#include <boost/progress.hpp>

using namespace boost::math;

namespace itk {

static bool CompareVectors(const vnl_vector_fixed< double, 3 >& v1, const vnl_vector_fixed< double, 3 >& v2)
{
  return (v1.magnitude()>v2.magnitude());
}

template< class PixelType, int ShOrder, int NrOdfDirections >
FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::FiniteDiffOdfMaximaExtractionFilter()
  : m_NormalizationMethod(MAX_VEC_NORM)
  , m_MaxNumPeaks(2)
  , m_PeakThreshold(0.4)
  , m_AbsolutePeakThreshold(0)
  , m_ClusteringThreshold(0.9)
  , m_AngularThreshold(0.7)
  , m_NumCoeffs((ShOrder*ShOrder + ShOrder + 2)/2 + ShOrder)
  , m_Toolkit(FSL)
  , m_ApplyDirectionMatrix(false)
{
  this->SetNumberOfRequiredInputs(1);
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::FindCandidatePeaks(OdfType& odf, double thr, std::vector< DirectionType >& container)
{
  double gfa = odf.GetGeneralizedFractionalAnisotropy();
  //Find the peaks using a finite difference method
  bool flag = true;
  vnl_vector_fixed< bool, NrOdfDirections > used; used.fill(false);
  //Find the peaks
  for (int i=0; i<NrOdfDirections; i++)
  {
    if (used[i])
      continue;

    double val = odf.GetElement(i);
    if (val>thr && val*gfa>m_AbsolutePeakThreshold)  // limit to one hemisphere ???
    {
      flag = true;
      std::vector< int > neighbours = odf.GetNeighbors(i);
      for (unsigned int j=0; j<neighbours.size(); j++)
        if (val<=odf.GetElement(neighbours.at(j)))
        {
          flag = false;
          break;
        }
      if (flag)   // point is a peak
      {
        container.push_back(odf.GetDirection(i).normalize());
        used[i] = true;
        for (unsigned int j=0; j<neighbours.size(); j++)
          used[neighbours.at(j)] = true;
      }
    }
  }
}

template< class PixelType, int ShOrder, int NrOdfDirections >
std::vector< vnl_vector_fixed< double, 3 > >  FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>::MeanShiftClustering(std::vector< DirectionType >& inDirs)
{
  std::vector< DirectionType > outDirs;
  if (inDirs.empty())
    return inDirs;

  DirectionType oldMean, currentMean, workingMean;
  std::vector< int > touched;

  // initialize
  touched.resize(inDirs.size(), 0);
  bool free = true;
  currentMean = inDirs[0];  // initialize first seed
  while (free)
  {
    oldMean.fill(0.0);

    // start mean-shift clustering
    float angle = 0.0;
    int counter = 0;
    while ((currentMean-oldMean).magnitude()>0.0001)
    {
      counter = 0;
      oldMean = currentMean;
      workingMean = oldMean;
      workingMean.normalize();
      currentMean.fill(0.0);
      for (unsigned int i=0; i<inDirs.size(); i++)
      {
        angle = dot_product(workingMean, inDirs[i]);
        if (angle>=m_ClusteringThreshold)
        {
          currentMean += inDirs[i];
          touched[i] = 1;
          counter++;
        }
        else if (-angle>=m_ClusteringThreshold)
        {
          currentMean -= inDirs[i];
          touched[i] = 1;
          counter++;
        }
      }
    }

    // found stable mean
    if (counter>0)
    {
      float mag = currentMean.magnitude();
      if (mag>0)
      {
        currentMean /= mag;
        outDirs.push_back(currentMean);
      }
    }

    // find next unused seed
    free = false;
    for (unsigned int i=0; i<touched.size(); i++)
      if (touched[i]==0)
      {
        currentMean = inDirs[i];
        free = true;
      }
  }

  if (inDirs.size()==outDirs.size())
  {
    return outDirs;
  }
  else
    return MeanShiftClustering(outDirs);
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::BeforeThreadedGenerateData()
{
  typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );
  itk::Vector<double,3> spacing = ShCoeffImage->GetSpacing();
  double minSpacing = spacing[0];
  if (spacing[1]<minSpacing)
    minSpacing = spacing[1];
  if (spacing[2]<minSpacing)
    minSpacing = spacing[2];

  mitk::Point3D origin = ShCoeffImage->GetOrigin();
  itk::Matrix<double, 3, 3> direction = ShCoeffImage->GetDirection();
  ImageRegion<3> imageRegion = ShCoeffImage->GetLargestPossibleRegion();

  if (m_MaskImage.IsNotNull())
  {
    origin = m_MaskImage->GetOrigin();
    direction = m_MaskImage->GetDirection();
    imageRegion = m_MaskImage->GetLargestPossibleRegion();
  }

  itk::Vector<double, 3> spacing3 = ShCoeffImage->GetSpacing();
  itk::Point<float, 3> origin3 = ShCoeffImage->GetOrigin();
  itk::Matrix<double, 3, 3> direction3 = ShCoeffImage->GetDirection();
  itk::ImageRegion<3> imageRegion3 = ShCoeffImage->GetLargestPossibleRegion();

  itk::Vector<double, 4> spacing4;
  itk::Point<float, 4> origin4;
  itk::Matrix<double, 4, 4> direction4;
  itk::ImageRegion<4> imageRegion4;

  spacing4[0] = spacing3[0]; spacing4[1] = spacing3[1]; spacing4[2] = spacing3[2]; spacing4[3] = 1;
  origin4[0] = origin3[0]; origin4[1] = origin3[1]; origin4[2] = origin3[2]; origin4[3] = 0;
  for (int r=0; r<3; r++)
    for (int c=0; c<3; c++)
      direction4[r][c] = direction3[r][c];
  direction4[3][3] = 1;
  imageRegion4.SetSize(0, imageRegion3.GetSize()[0]);
  imageRegion4.SetSize(1, imageRegion3.GetSize()[1]);
  imageRegion4.SetSize(2, imageRegion3.GetSize()[2]);
  imageRegion4.SetSize(3, m_MaxNumPeaks*3);

  m_PeakImage = PeakImageType::New();
  m_PeakImage->SetSpacing( spacing4 );
  m_PeakImage->SetOrigin( origin4 );
  m_PeakImage->SetDirection( direction4 );
  m_PeakImage->SetRegions( imageRegion4 );
  m_PeakImage->Allocate();
  m_PeakImage->FillBuffer(0.0);

  if (m_MaskImage.IsNull())
  {
    m_MaskImage = ItkUcharImgType::New();
    m_MaskImage->SetSpacing( spacing );
    m_MaskImage->SetOrigin( origin );
    m_MaskImage->SetDirection( direction );
    m_MaskImage->SetRegions( imageRegion );
    m_MaskImage->Allocate();
    m_MaskImage->FillBuffer(1);
  }
  m_NumDirectionsImage = ItkUcharImgType::New();
  m_NumDirectionsImage->SetSpacing( spacing );
  m_NumDirectionsImage->SetOrigin( origin );
  m_NumDirectionsImage->SetDirection( direction );
  m_NumDirectionsImage->SetRegions( imageRegion );
  m_NumDirectionsImage->Allocate();
  m_NumDirectionsImage->FillBuffer(0);

  // calculate SH basis
  OdfType odf;
  vnl_matrix< double > sphCoords;
  std::vector< DirectionType > dirs;
  for (int i=0; i<NrOdfDirections; i++)
  {
    DirectionType odf_dir;
    odf_dir[0] = odf.GetDirection(i)[0];
    odf_dir[1] = odf.GetDirection(i)[1];
    odf_dir[2] = odf.GetDirection(i)[2];
    dirs.push_back(odf_dir);
  }
  Cart2Sph(dirs, sphCoords);                          // convert candidate peaks to spherical angles
  m_ShBasis = CalcShBasis(sphCoords);                // evaluate spherical harmonics at each peak

  MITK_INFO << "Starting finite differences maximum extraction";
  MITK_INFO << "ODF sampling points: " << NrOdfDirections;
  MITK_INFO << "SH order: " << ShOrder;
  MITK_INFO << "Maximum peaks: " << m_MaxNumPeaks;
  MITK_INFO << "Relative threshold: " << m_PeakThreshold;
  MITK_INFO << "Absolute threshold: " << m_AbsolutePeakThreshold;
  MITK_INFO << "Clustering threshold: " << m_ClusteringThreshold;
  MITK_INFO << "Angular threshold: " << m_AngularThreshold;
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::AfterThreadedGenerateData()
{

}

template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::ThreadedGenerateData( const OutputImageRegionType& outputRegionForThread, ThreadIdType threadID )
{
  typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );

  ImageRegionConstIterator< CoefficientImageType > cit(ShCoeffImage, outputRegionForThread );

  OdfType odf;
  while( !cit.IsAtEnd() )
  {
    typename CoefficientImageType::IndexType idx3 = cit.GetIndex();
    if (m_MaskImage->GetPixel(idx3)==0)
    {
      ++cit;
      continue;
    }

    CoefficientPixelType c = cit.Get();

    // calculate ODF
    double max = 0;
    odf.Fill(0.0);
    for (int i=0; i<NrOdfDirections; i++)
    {
      for (int j=0; j<m_NumCoeffs; j++)
        odf[i] += c[j]*m_ShBasis(i,j);
      if (odf[i]>max)
        max = odf[i];
    }
    if (max<0.0001)
    {
      ++cit;
      continue;
    }

    std::vector< DirectionType > candidates, peaks, temp;
    peaks.clear();
    max *= m_PeakThreshold;                         // relative threshold
    FindCandidatePeaks(odf, max, candidates);       // find all local maxima
    candidates = MeanShiftClustering(candidates);   // cluster maxima

    vnl_matrix< double > shBasis, sphCoords;
    Cart2Sph(candidates, sphCoords);                // convert candidate peaks to spherical angles
    shBasis = CalcShBasis(sphCoords);            // evaluate spherical harmonics at each peak
    max = 0.0;
    for (unsigned int i=0; i<candidates.size(); i++)         // scale peaks according to ODF value
    {
      double val = 0;
      for (int j=0; j<m_NumCoeffs; j++)
        val += c[j]*shBasis(i,j);
      if (val>max)
        max = val;
      peaks.push_back(candidates[i]*val);
    }
    std::sort( peaks.begin(), peaks.end(), CompareVectors );  // sort peaks

    // kick out directions to close to a larger direction (too far away to cluster but too close to keep)
    unsigned int m = peaks.size();
    if ( m>m_MaxNumPeaks )
      m = m_MaxNumPeaks;
    for (unsigned int i=0; i<m; i++)
    {
      DirectionType v1 = peaks.at(i);
      double val = v1.magnitude();
      if (val<max*m_PeakThreshold || val<m_AbsolutePeakThreshold)
        break;

      bool flag = true;
      for (unsigned int j=0; j<peaks.size(); j++)
        if (i!=j)
        {
          DirectionType v2 = peaks.at(j);
          double val2 = v2.magnitude();
          double angle = fabs(dot_product(v1,v2)/(val*val2));
          if (angle>m_AngularThreshold && val<val2)
          {
            flag = false;
            break;
          }
        }

      if (flag)
        temp.push_back(v1);
    }
    peaks = temp;

    itk::Index<4> idx4; idx4[0] = idx3[0]; idx4[1] = idx3[1]; idx4[2] = idx3[2];

    // fill output image
    unsigned int num = peaks.size();
    if ( num>m_MaxNumPeaks )
      num = m_MaxNumPeaks;
    for (unsigned int i=0; i<num; i++)
    {
      DirectionType dir = peaks.at(i);
      switch (m_NormalizationMethod)
      {
      case NO_NORM:
        break;
      case SINGLE_VEC_NORM:
        dir.normalize();
        break;
      case MAX_VEC_NORM:
        dir /= max;
        break;
      }

      if (m_ApplyDirectionMatrix)
        dir = m_MaskImage->GetDirection()*dir;

      if (m_FlipX)
        dir[0] = -dir[0];
      if (m_FlipY)
        dir[1] = -dir[1];
      if (m_FlipZ)
        dir[2] = -dir[2];

      for (unsigned int j = 0; j<3; j++)
      {
        idx4[3] = i*3 + j;
        m_PeakImage->SetPixel(idx4, dir[j]);
      }
    }
    m_NumDirectionsImage->SetPixel(idx3, num);
    ++cit;
  }
  MITK_INFO << "Thread " << threadID << " finished extraction";
}

// convert cartesian to spherical coordinates
template< class PixelType, int ShOrder, int NrOdfDirections >
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::Cart2Sph(const std::vector< DirectionType >& dir, vnl_matrix<double>& sphCoords)
{
  sphCoords.set_size(dir.size(), 2);

  for (unsigned int i=0; i<dir.size(); i++)
  {
    double mag = dir[i].magnitude();

    if( mag<0.0001 )
    {
      sphCoords(i,0) = M_PI/2; // theta
      sphCoords(i,1) = M_PI/2; // phi
    }
    else
    {
      sphCoords(i,0) = acos(dir[i](2)/mag); // theta
          sphCoords(i,1) = atan2(dir[i](1), dir[i](0)); // phi
    }
  }
}

// generate spherical harmonic values of the desired order for each input direction
template< class PixelType, int ShOrder, int NrOdfDirections >
vnl_matrix<double> FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::CalcShBasis(vnl_matrix<double>& sphCoords)
{
  int M = sphCoords.rows();
  int j, m; double mag, plm;
  vnl_matrix<double> shBasis;
  shBasis.set_size(M, m_NumCoeffs);

  for (int p=0; p<M; p++)
  {
    j=0;
    for (int l=0; l<=ShOrder; l=l+2)
      for (m=-l; m<=l; m++)
      {
        switch (m_Toolkit)
        {
        case FSL:
          plm = legendre_p<double>(l,abs(m),cos(sphCoords(p,0)));
          mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;

          if (m<0)
            shBasis(p,j) = sqrt(2.0)*mag*cos(fabs((double)m)*sphCoords(p,1));
          else if (m==0)
            shBasis(p,j) = mag;
          else
            shBasis(p,j) = pow(-1.0, m)*sqrt(2.0)*mag*sin(m*sphCoords(p,1));
          break;
        case MRTRIX:

          plm = legendre_p<double>(l,abs(m),-cos(sphCoords(p,0)));
          mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;
          if (m>0)
            shBasis(p,j) = mag*cos(m*sphCoords(p,1));
          else if (m==0)
            shBasis(p,j) = mag;
          else
            shBasis(p,j) = mag*sin(-m*sphCoords(p,1));
          break;
        }

        j++;
      }
  }
  return shBasis;
}

}

#endif // __itkFiniteDiffOdfMaximaExtractionFilter_cpp


#include "teca_face_to_cell_centering.h"

#include "teca_arakawa_c_grid.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_array_attributes.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;

//#define TECA_DEBUG

namespace {

template <typename num_t>
void x_face_to_cell(unsigned long nx, unsigned long ny,
    unsigned long nz, unsigned long nxy,  const num_t *fc, num_t *cc)
{
    unsigned long nxf = nx + 1;
    unsigned long nxyf = nxf*ny;

    for (unsigned long k = 0; k < nz; ++k)
    {
        unsigned long knxy = k*nxy;
        unsigned long knxyf = k*nxyf;

        for (unsigned long j = 0; j < ny; ++j)
        {
            unsigned long jnx = j*nx;
            unsigned long jnxf = j*nxf;

            const num_t *src = fc + knxyf + jnxf;
            num_t *dest = cc + knxy + jnx;

            for (unsigned long i = 0; i < nx; ++i)
            {
                dest[i] = 0.5*(src[i] + src[i+1]);
            }
        }
    }
}

template <typename num_t>
void y_face_to_cell(unsigned long nx, unsigned long ny,
    unsigned long nz, unsigned long nxy,  const num_t *fc, num_t *cc)
{
    unsigned long nyf = ny + 1;
    unsigned long nxyf = nx*nyf;

    for (unsigned long k = 0; k < nz; ++k)
    {
        unsigned long knxy = k*nxy;
        unsigned long knxyf = k*nxyf;

        for (unsigned long j = 0; j < ny; ++j)
        {
            unsigned long jnx = j*nx;
            unsigned long j1nx = (j + 1)*nx;

            const num_t *src = fc + knxyf + jnx;
            const num_t *src1 = fc + knxyf + j1nx;

            num_t *dest = cc + knxy + jnx;

            for (unsigned long i = 0; i < nx; ++i)
            {
                dest[i] = 0.5*(src[i] + src1[i]);
            }
        }
    }
}

template <typename num_t>
void z_face_to_cell(unsigned long nx, unsigned long ny,
    unsigned long nz, unsigned long nxy,  const num_t *fc, num_t *cc)
{
    for (unsigned long k = 0; k < nz; ++k)
    {
        unsigned long knxy = k*nxy;
        unsigned long k1nxy = (k + 1)*nxy;

        for (unsigned long j = 0; j < ny; ++j)
        {
            unsigned long jnx = j*nx;

            const num_t *src = fc + knxy + jnx;
            const num_t *src1 = fc + k1nxy + jnx;

            num_t *dest = cc + knxy + jnx;

            for (unsigned long i = 0; i < nx; ++i)
            {
                dest[i] = 0.5*(src[i] + src1[i]);
            }
        }
    }
}

};

// --------------------------------------------------------------------------
teca_face_to_cell_centering::teca_face_to_cell_centering()
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_face_to_cell_centering::~teca_face_to_cell_centering()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_face_to_cell_centering::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_face_to_cell_centering":prefix));

    /*opts.add_options()
        TECA_POPTS_GET(int, prefix, mode,
            "Set the coordinate transform mode. The valid modes"
            " are: mode_wrf_v3)")
        ;*/

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_face_to_cell_centering::set_properties(
    const string &prefix, variables_map &opts)
{
    //TECA_POPTS_SET(opts, int, prefix, mode)
    this->teca_algorithm::set_properties(prefix, opts);
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_face_to_cell_centering::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_face_to_cell_centering::get_output_metadata" << endl;
#endif
    (void)port;

    teca_metadata out_md(input_md[0]);

    // get array attributes
    teca_metadata atrs;
    if (out_md.get("attributes", atrs))
    {
        TECA_FATAL_ERROR("failed to get array attributes")
        return teca_metadata();
    }

    // get the list of array names
    std::vector<std::string> arrays;
    if (out_md.get("variables", arrays))
    {
        TECA_FATAL_ERROR("failed to get array names")
        return teca_metadata();
    }

    size_t n_arrays = arrays.size();
    for (size_t i = 0; i < n_arrays; ++i)
    {
        // get the name of the ith array
        const std::string &array_name = arrays[i];

        // get the array's attributes
        teca_metadata array_atrs;
        if (atrs.get(array_name, array_atrs))
        {
            TECA_FATAL_ERROR("failed to get the attributes for array "
                << i << " \"" << array_name << "\"")
            return teca_metadata();
        }

        // get the array's centering
        int centering = teca_array_attributes::invalid_value;
        if (array_atrs.get("centering", centering))
        {
            TECA_FATAL_ERROR("failed to get the centering for array "
                << i << " \"" << array_name << "\"")
            return teca_metadata();
        }

        // if this is a face centered array change to cell centering
        if ((centering == teca_array_attributes::x_face_centering)
            || (centering == teca_array_attributes::y_face_centering)
            || (centering == teca_array_attributes::z_face_centering))
        {
            array_atrs.set("centering",
                int(teca_array_attributes::cell_centering));
        }

        // update the array's attributes
        atrs.set(array_name, array_atrs);
    }

    // update the array attributes collection
    out_md.set("attributes", atrs);

    return out_md;
}

// --------------------------------------------------------------------------
std::vector<teca_metadata>
teca_face_to_cell_centering::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    (void)port;
    (void)input_md;
    std::vector<teca_metadata> up_reqs;
    up_reqs.push_back(request);
    return up_reqs;
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_face_to_cell_centering::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    cerr << teca_parallel_id()
        << "teca_face_to_cell_centering::execute" << endl;
#endif
    (void)port;
    (void)request;

    // get the input mesh
    const_p_teca_arakawa_c_grid in_mesh
        = std::dynamic_pointer_cast<const teca_arakawa_c_grid>(input_data[0]);

    if (!in_mesh)
    {
        TECA_FATAL_ERROR("teca_arakawa_c_grid is required")
        return nullptr;
    }

    // get the mesh dimensions
    unsigned long extent[6] = {0};
    in_mesh->get_extent(extent);

    unsigned long nx = extent[1] - extent[0] + 1;
    unsigned long ny = extent[3] - extent[2] + 1;
    unsigned long nz = extent[5] - extent[4] + 1;
    unsigned long nxy = nx*ny;
    unsigned long nxyz = nxy*nz;

    // allocate the output mesh
    p_teca_arakawa_c_grid out_mesh = teca_arakawa_c_grid::New();
    out_mesh->shallow_copy(std::const_pointer_cast<teca_arakawa_c_grid>(in_mesh));

    // convert x-face centering to cell centering
    p_teca_array_collection &x_face_arrays = out_mesh->get_x_face_arrays();
    p_teca_array_collection &cell_arrays = out_mesh->get_cell_arrays();
    size_t n_arrays = x_face_arrays->size();
    for (size_t i = 0; i < n_arrays; ++i)
    {
        std::string &array_name = x_face_arrays->get_name(i);
        p_teca_variant_array fc = x_face_arrays->get(i);
        p_teca_variant_array cc = fc->new_instance(nxyz);
        TEMPLATE_DISPATCH(teca_variant_array_impl,
            fc.get(),
            const NT *pfc = static_cast<TT*>(fc.get())->get();
            NT *pcc = static_cast<TT*>(cc.get())->get();
            ::x_face_to_cell(nx, ny, nz, nxy, pfc, pcc);
            )

        x_face_arrays->remove(i);
        cell_arrays->append(array_name, cc);
    }

    // convert y-face centering to cell centering
    p_teca_array_collection &y_face_arrays = out_mesh->get_y_face_arrays();
    n_arrays = y_face_arrays->size();
    for (size_t i = 0; i < n_arrays; ++i)
    {
        std::string &array_name = y_face_arrays->get_name(i);
        p_teca_variant_array fc = y_face_arrays->get(i);
        p_teca_variant_array cc = fc->new_instance(nxyz);
        TEMPLATE_DISPATCH(teca_variant_array_impl,
            fc.get(),
            const NT *pfc = static_cast<TT*>(fc.get())->get();
            NT *pcc = static_cast<TT*>(cc.get())->get();
            ::y_face_to_cell(nx, ny, nz, nxy, pfc, pcc);
            )

        y_face_arrays->remove(i);
        cell_arrays->append(array_name, cc);
    }

    // convert z-face centering to cell centering
    p_teca_array_collection &z_face_arrays = out_mesh->get_z_face_arrays();
    n_arrays = z_face_arrays->size();
    for (size_t i = 0; i < n_arrays; ++i)
    {
        std::string &array_name = z_face_arrays->get_name(i);
        p_teca_variant_array fc = z_face_arrays->get(i);
        p_teca_variant_array cc = fc->new_instance(nxyz);
        TEMPLATE_DISPATCH(teca_variant_array_impl,
            fc.get(),
            const NT *pfc = static_cast<TT*>(fc.get())->get();
            NT *pcc = static_cast<TT*>(cc.get())->get();
            ::z_face_to_cell(nx, ny, nz, nxy, pfc, pcc);
            )

        z_face_arrays->remove(i);
        cell_arrays->append(array_name, cc);
    }

    return out_mesh;
}


/*
 *  Copyright Nick Thompson, 2019
 *  Use, modification and distribution are 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)
 */

#ifndef BOOST_MATH_INTERPOLATORS_VECTOR_BARYCENTRIC_RATIONAL_DETAIL_HPP
#define BOOST_MATH_INTERPOLATORS_VECTOR_BARYCENTRIC_RATIONAL_DETAIL_HPP

#include <vector>
#include <utility> // for std::move
#include <limits>
#include <boost/assert.hpp>

namespace boost{ namespace math{ namespace detail{

template <class TimeContainer, class SpaceContainer>
class vector_barycentric_rational_imp
{
public:
    using Real = typename TimeContainer::value_type;
    using Point = typename SpaceContainer::value_type;

    vector_barycentric_rational_imp(TimeContainer&& t, SpaceContainer&& y, size_t approximation_order);

    void operator()(Point& p, Real t) const;

    void eval_with_prime(Point& x, Point& dxdt, Real t) const;

    // The barycentric weights are only interesting to the unit tests:
    Real weight(size_t i) const { return w_[i]; }

private:

    void calculate_weights(size_t approximation_order);

    TimeContainer t_;
    SpaceContainer y_;
    TimeContainer w_;
};

template <class TimeContainer, class SpaceContainer>
vector_barycentric_rational_imp<TimeContainer, SpaceContainer>::vector_barycentric_rational_imp(TimeContainer&& t, SpaceContainer&& y, size_t approximation_order)
{
    using std::numeric_limits;
    t_ = std::move(t);
    y_ = std::move(y);

    BOOST_ASSERT_MSG(t_.size() == y_.size(), "There must be the same number of time points as space points.");
    BOOST_ASSERT_MSG(approximation_order < y_.size(), "Approximation order must be < data length.");
    for (size_t i = 1; i < t_.size(); ++i)
    {
        BOOST_ASSERT_MSG(t_[i] - t_[i-1] >  (numeric_limits<typename TimeContainer::value_type>::min)(), "The abscissas must be listed in strictly increasing order t[0] < t[1] < ... < t[n-1].");
    }
    calculate_weights(approximation_order);
}

template<class TimeContainer, class SpaceContainer>
void vector_barycentric_rational_imp<TimeContainer, SpaceContainer>::calculate_weights(size_t approximation_order)
{
    using Real = typename TimeContainer::value_type;
    using std::abs;
    int64_t n = t_.size();
    w_.resize(n, Real(0));
    for(int64_t k = 0; k < n; ++k)
    {
        int64_t i_min = (std::max)(k - (int64_t) approximation_order, (int64_t) 0);
        int64_t i_max = k;
        if (k >= n - (std::ptrdiff_t)approximation_order)
        {
            i_max = n - approximation_order - 1;
        }

        for(int64_t i = i_min; i <= i_max; ++i)
        {
            Real inv_product = 1;
            int64_t j_max = (std::min)(static_cast<int64_t>(i + approximation_order), static_cast<int64_t>(n - 1));
            for(int64_t j = i; j <= j_max; ++j)
            {
                if (j == k)
                {
                    continue;
                }
                Real diff = t_[k] - t_[j];
                inv_product *= diff;
            }
            if (i % 2 == 0)
            {
                w_[k] += 1/inv_product;
            }
            else
            {
                w_[k] -= 1/inv_product;
            }
        }
    }
}

template<class TimeContainer, class SpaceContainer>
void vector_barycentric_rational_imp<TimeContainer, SpaceContainer>::operator()(typename SpaceContainer::value_type& p, typename TimeContainer::value_type t) const
{
    using Real = typename TimeContainer::value_type;
    for (auto & x : p)
    {
        x = Real(0);
    }
    Real denominator = 0;
    for(size_t i = 0; i < t_.size(); ++i)
    {
        // See associated commentary in the scalar version of this function.
        if (t == t_[i])
        {
            p = y_[i];
            return;
        }
        Real x = w_[i]/(t - t_[i]);
        for (decltype(p.size()) j = 0; j < p.size(); ++j)
        {
            p[j] += x*y_[i][j];
        }
        denominator += x;
    }
    for (decltype(p.size()) j = 0; j < p.size(); ++j)
    {
        p[j] /= denominator;
    }
    return;
}

template<class TimeContainer, class SpaceContainer>
void vector_barycentric_rational_imp<TimeContainer, SpaceContainer>::eval_with_prime(typename SpaceContainer::value_type& x, typename SpaceContainer::value_type& dxdt, typename TimeContainer::value_type t) const
{
    using Point = typename SpaceContainer::value_type;
    using Real = typename TimeContainer::value_type;
    this->operator()(x, t);
    Point numerator;
    for (decltype(x.size()) i = 0; i < x.size(); ++i)
    {
        numerator[i] = 0;
    }
    Real denominator = 0;
    for(decltype(t_.size()) i = 0; i < t_.size(); ++i)
    {
        if (t == t_[i])
        {
            Point sum;
            for (decltype(x.size()) i = 0; i < x.size(); ++i)
            {
                sum[i] = 0;
            }

            for (decltype(t_.size()) j = 0; j < t_.size(); ++j)
            {
                if (j == i)
                {
                    continue;
                }
                for (decltype(sum.size()) k = 0; k < sum.size(); ++k)
                {
                    sum[k] += w_[j]*(y_[i][k] - y_[j][k])/(t_[i] - t_[j]);
                }
            }
            for (decltype(sum.size()) k = 0; k < sum.size(); ++k)
            {
                dxdt[k] = -sum[k]/w_[i];
            }
            return;
        }
        Real tw = w_[i]/(t - t_[i]);
        Point diff;
        for (decltype(diff.size()) j = 0; j < diff.size(); ++j)
        {
            diff[j] = (x[j] - y_[i][j])/(t-t_[i]);
        }
        for (decltype(diff.size()) j = 0; j < diff.size(); ++j)
        {
            numerator[j] += tw*diff[j];
        }
        denominator += tw;
    }

    for (decltype(dxdt.size()) j = 0; j < dxdt.size(); ++j)
    {
        dxdt[j] = numerator[j]/denominator;
    }
    return;
}

}}}
#endif


#define BOOST_TEST_MODULE TestSolvers
#include <boost/test/unit_test.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/static_matrix.hpp>

#include "test_solver.hpp"

BOOST_AUTO_TEST_SUITE( test_solvers )

BOOST_AUTO_TEST_CASE(test_nonscalar_backend)
{
    test_backend< amgcl::backend::builtin< amgcl::static_matrix<double, 2, 2> > >();
}

BOOST_AUTO_TEST_SUITE_END()


#include <observation_maneuver.h>
#include <observation_maneuver_msgs/OPPairsReply.h>
#include <observation_maneuver_msgs/OPPairsRequest.h>
#include <Eigen/Dense>
#include <visualization_msgs/Marker.h>
#include <octomap/math/Vector3.h>
#include <marker_publishing_utils.h>

namespace observation_node
{
	double distance_behind, distance_inFront;
	ros::Publisher marker_pub;

	void oppairs_callback(const observation_maneuver_msgs::OPPairsRequest::ConstPtr& opp_request)
	{
		rviz_interface::publish_deleteAll(marker_pub);
		visualization_msgs::MarkerArray waypoint_array;
		visualization_msgs::Marker marker;
		double size = 0.1f;
		int green_base = 0;
		int red_base = opp_request->header.seq;
		// Uav position
		octomath::Vector3 uav_position_octoVec (opp_request->uav_position.x,  opp_request->uav_position.y,  opp_request->uav_position.z);
		marker = visualization_msgs::Marker();
		rviz_interface::build_sphere(uav_position_octoVec, size, green_base, 0, marker, red_base, "uav_position");
		waypoint_array.markers.push_back( marker );
		// Frontier
		octomath::Vector3 frontier_octoVec (opp_request->frontier.x,  opp_request->frontier.y,  opp_request->frontier.z);
		marker = visualization_msgs::Marker();
		rviz_interface::build_sphere(frontier_octoVec, size, green_base, 0, marker, red_base, "frontier");
		waypoint_array.markers.push_back( marker );

		Eigen::Vector3d uav_position( opp_request->uav_position.x,  opp_request->uav_position.y,  opp_request->uav_position.z);
		Eigen::Vector3d frontier    ( opp_request->frontier.x, opp_request->frontier.y, opp_request->frontier.z);
		double distance_behind = opp_request->distance_behind;
		double distance_inFront = opp_request->distance_inFront;
		int circle_divisions = opp_request->circle_divisions;
		observation_lib::OPPairs oppairs (circle_divisions, opp_request->distance_toTarget, distance_inFront, distance_behind, observation_lib::translateAdjustDirection);

		int index = 0;
		rviz_interface::PublishingInput pi (marker_pub, true, "test_node", waypoint_array);
		oppairs.NewFrontier(frontier, uav_position, pi);
		while(oppairs.Next())
		{
			Eigen::Vector3d temp =  oppairs.get_current_start();
			octomath::Vector3 start_octoVec (temp(0),  temp(1),  temp(2));
			temp =  oppairs.get_current_end();
			octomath::Vector3 end_octoVec   (temp(0),  temp(1),  temp(2));
			visualization_msgs::Marker  marker;
			marker = visualization_msgs::Marker();
			int marker_id = index;
			int red_base = 1;
    		rviz_interface::build_arrow_path(start_octoVec, end_octoVec, 100+marker_id, marker, 1+0.5, "oppair_direction");
			pi.waypoint_array.markers.push_back( marker );
			pi.marker_pub.publish(pi.waypoint_array);
			index++;
		}
		marker_pub.publish(waypoint_array);
	}

}

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

	ros::init(argc, argv, "observation_node");
	ros::NodeHandle nh;

	// nh.getParam("observation/distance_behind", observation_node::distance_behind);
	// nh.getParam("observation/distance_inFront", observation_node::distance_inFront);

	observation_node::marker_pub = nh.advertise<visualization_msgs::MarkerArray>("observation_info", 1);
	ros::Subscriber opp_generation_sub = nh.subscribe<observation_maneuver_msgs::OPPairsRequest>("opp_request", 10, observation_node::oppairs_callback);

	ros::spin();
}


#ifndef FIELD_HPP_
#define FIELD_HPP_

#pragma warning (disable : 4146)
#include <NTL/ZZ_pEX.h>

#include <variant>
#include <iostream>
#include <string>
#include <sstream>

namespace gadgetlib
{

	template <uint64_t x>
	struct SignificantBits {
		static const uint64_t n = SignificantBits<x /2>::n + 1;
	};

	template <>
	struct SignificantBits<0>
	{
		static const uint64_t n = 0;
	};

	template<uint64_t characteristics>
	class Field
	{
	public:
		NTL::ZZ_p num_;
		static bool initialized_;

	private:
		void initialize()
		{
			if (!initialized_)
			{
				initialized_ = true;
				std::string str_rep = std::to_string(characteristics);
				NTL::ZZ chp = NTL::conv<NTL::ZZ>(str_rep.data());
				NTL::ZZ_p::init(chp);
			}
		}

		NTL::ZZ hexToZZ(const std::string& hexVal)
		{
			bool decimal = (hexVal[0] == 'd');

			auto convert_ch = [](char c) -> int
			{
				if (c >= '0' && c <= '9')
					return (c - '0');
				if (c >= 'a' && c <= 'f')
					return (c - 'a' + 10);

			};

			NTL::ZZ val;
			val = NTL::to_ZZ(0);	//initialise the value to zero

			for (unsigned i = (decimal ? 1: 0); i < hexVal.length(); i++)
			{
				val *= (decimal ? 10 : 16);
				val += convert_ch(hexVal[i]);

			}
			return val;
		}

	public:
		static constexpr uint64_t safe_bitsize = SignificantBits<characteristics>::n;
		static constexpr uint64_t chr = characteristics;

		Field(size_t num)
		{
			initialize();
			NTL::ZZ temp = NTL::conv<NTL::ZZ>(num);
			num_ = NTL::conv<NTL::ZZ_p>(temp);
		}

		Field()
		{
			initialize();
		}

		Field(const NTL::ZZ_p& num)
		{
			initialize();
			num_ = num;
		}

		Field(const std::variant<uint32_t, std::string>& v)
		{
			initialize();
			//uint32_t n;
			switch (v.index())
			{
			case 0:
			{
				NTL::ZZ num = NTL::conv<NTL::ZZ>(std::get<0>(v));
				num_ = NTL::conv<NTL::ZZ_p>(num);;
				break;
			}
			case 1:
				NTL::ZZ int_num = hexToZZ(std::get<1>(v));
				num_ = NTL::conv<NTL::ZZ_p>(int_num);
				break;
			};
		}

		Field& operator+=(const Field& rhs)
		{
			this->num_ += rhs.num_;
			return *this;
		}

		Field& operator-=(const Field& rhs)
		{
			this->num_ -= rhs.num_;
			return *this;
		}

		Field& operator*=(const Field& rhs)
		{
			this->num_ *= rhs.num_;
			return *this;
		}

		Field& operator-()
		{
			this->num_ = -this->num_;
			return *this;
		}

		Field& operator/=(const Field& rhs)
		{
			this->num_ = NTL::conv<NTL::ZZ_p>(NTL::rep(num_) / NTL::rep(rhs.num_));
			return *this;
		}

		static Field one()
		{
			return 1;
		}

		operator bool() const
		{
			return num_ != 0;
		}

		std::string to_string() const
		{
			std::stringstream buffer;
			buffer << num_;
			return buffer.str();
		}
	};

	template<uint64_t characteristics>
	Field<characteristics> operator+(const Field<characteristics>& left,
		const Field<characteristics>& right)
	{
		return Field<characteristics>(left.num_ + right.num_);
	}

	template<uint64_t characteristics>
	Field<characteristics> operator-(const Field<characteristics>& left,
		const Field<characteristics>& right)
	{
		return Field<characteristics>(left.num_ - right.num_);
	}

	template<uint64_t characteristics>
	Field<characteristics> operator*(const Field<characteristics>& left,
		const Field<characteristics>& right)
	{
		return Field<characteristics>(left.num_ * right.num_);
	}

	template<uint64_t characteristics>
	bool operator==(const Field<characteristics>& left,
		const Field<characteristics>& right)
	{
		return (left.num_ == right.num_);
	}

	template<uint64_t characteristics>
	bool operator!=(const Field<characteristics>& left,
		const Field<characteristics>& right)
	{
		return (left.num_ != right.num_);
	}

	template<uint64_t characteristics>
	Field<characteristics> operator%(const Field<characteristics>& left,
		int right)
	{
		assert(right == 2);
		return NTL::rem(rep(left.num_), right);
	}

	template<uint64_t characteristics>
	std::ostream& operator<< (std::ostream& stream, const Field<characteristics>& elem)
	{
		stream << elem.num_;
		return stream;
	}

	template<uint64_t characteristics>
	bool Field<characteristics>::initialized_ = false;
}

#endif


#include <boost/numeric/ublas/banded.hpp>


#ifndef ROSE499_SPLINE_HPP
#define ROSE499_SPLINE_HPP

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <cstdint>
#include <Eigen/Core>
#include "rose499/types.hpp"

namespace geom = boost::geometry;

/** 2D, 5th order spline data structure.
 *
 * Encapsulates evaluation of spline. This
 * class stores a 5th order spline.
 */
struct Spline
{
    using ValueType = SimulatorTypes::ValueType;
    using Point = geom::model::point<double, 2, geom::cs::cartesian>;
    using Line = geom::model::segment< Point >;
    using ApproximateSpline = std::vector< Line >;

    constexpr static int PolyOrder = 5;
    constexpr static int CoeffCount = PolyOrder + 1;

    /** Default Constructor for Spline
     *
     * This should only be used for testing purposes. It produces a single
     * 5th order polynomial.
     */
    Spline();
    Spline(Spline const &) = default;
    Spline(Eigen::Matrix<Spline::ValueType, 2, Eigen::Dynamic> points);
    Spline(Eigen::Matrix<Spline::ValueType, 2, Eigen::Dynamic> points, double direction);
    Eigen::Matrix<ValueType, 2, 1> operator() (ValueType parameter, uint32_t derivative = 0) const;
    Eigen::Matrix<ValueType, 2, 2> frame(ValueType parameter, uint32_t derivative = 0) const;
    ValueType nearestPoint(Eigen::Matrix<ValueType, 2, 1>, ValueType estimate) const;
    ValueType speed(ValueType parameter) const;
    int splineIndexUsed(ValueType parameter) const;
    ApproximateSpline const & approximation() const;

    Eigen::Matrix<ValueType, Eigen::Dynamic, CoeffCount> poly() const;
    Eigen::Matrix<ValueType, Eigen::Dynamic, CoeffCount> dpoly() const;
    Eigen::Matrix<ValueType, Eigen::Dynamic, CoeffCount> ddpoly() const;
    ValueType arclength() const;

    class SplineException{ };
    class InvalidParameterException : public SplineException { };
    class IncorrectSplineFormatException : public SplineException { };

private:
    void approximateSelf();

    int mSplineCount;
    Eigen::Matrix<ValueType, Eigen::Dynamic, CoeffCount> mPoly;
    Eigen::Matrix<ValueType, Eigen::Dynamic, CoeffCount> mDPoly;
    Eigen::Matrix<ValueType, Eigen::Dynamic, CoeffCount> mDDPoly;
    ApproximateSpline mApproximation;
    ValueType mArcLength;
};

#endif


#include <fstream>
#include <iostream>

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>

#include <yaml-cpp/yaml.h>

#include <libMultiRobotPlanning/sipp.hpp>

using libMultiRobotPlanning::SIPP;
using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;

struct State {
  State(int x, int y) : x(x), y(y) {}

  bool operator==(const State& other) const {
    return std::tie(x, y) == std::tie(other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << "(" << s.x << "," << s.y << ")";
  }

  unsigned int dir = 0xff;
  int x;
  int y;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

enum class Action {
  Up,
  Down,
  Left,
  Right,
  Wait,
};

std::ostream& operator<<(std::ostream& os, const Action& a) {
  switch (a) {
    case Action::Up:
      os << "Up";
      break;
    case Action::Down:
      os << "Down";
      break;
    case Action::Left:
      os << "Left";
      break;
    case Action::Right:
      os << "Right";
      break;
    case Action::Wait:
      os << "Wait";
      break;
  }
  return os;
}

class Environment {
 public:
  Environment(size_t dimx, size_t dimy, std::unordered_set<State> obstacles,
              State goal)
      : m_dimx(dimx),
        m_dimy(dimy),
        m_obstacles(std::move(obstacles)),
        m_goal(goal) {}

  float admissibleHeuristic(const State& s) {
    return std::abs(s.x - m_goal.x) + std::abs(s.y - m_goal.y);
  }

  bool isSolution(const State& s) { return s == m_goal; }

  State getLocation(const State& s) { return s; }

  void getNeighbors(const State& s,
                    std::vector<Neighbor<State, Action, int> >& neighbors) {
    neighbors.clear();

    State up(s.x, s.y + 1);
    if (stateValid(up)) {
      neighbors.emplace_back(Neighbor<State, Action, int>(up, Action::Up, 1));
    }
    State down(s.x, s.y - 1);
    if (stateValid(down)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(down, Action::Down, 1));
    }
    State left(s.x - 1, s.y);
    if (stateValid(left)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(left, Action::Left, 1));
    }
    State right(s.x + 1, s.y);
    if (stateValid(right)) {
      neighbors.emplace_back(
          Neighbor<State, Action, int>(right, Action::Right, 1));
    }
  }

  void onExpandNode(const State& /*s*/, int /*fScore*/, int /*gScore*/) {
    // std::cout << "expand: " << s << "g: " << gScore << std::endl;
  }

  void onDiscover(const State& /*s*/, int /*fScore*/, int /*gScore*/) {
    // std::cout << "  discover: " << s << std::endl;
  }

  bool isCommandValid(
      const State& /*s1*/, const State& /*s2*/, const Action& /*a*/,
      int earliestStartTime,      // can start motion at this time
      int /*latestStartTime*/,    // must have left s by this time
      int earliestArrivalTime,    // can only arrive at (s+cmd)
      int /*latestArrivalTime*/,  // needs to arrive by this time at (s+cmd)
      int& t) {
    t = std::max<int>(earliestArrivalTime, earliestStartTime + 1);

    // TODO(whoenig): need to check for swaps here...

    // return t - 1 <= latestStartTime;
    return true;
  }

 private:
  bool stateValid(const State& s) {
    return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy &&
           m_obstacles.find(s) == m_obstacles.end();
  }

 public:
  int num_generation = 0;
  int num_expansion = 0;
 private:
  int m_dimx;
  int m_dimy;
  std::unordered_set<State> m_obstacles;
  State m_goal;
};

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string inputFile;
  std::string outputFile;
  desc.add_options()("help", "produce help message")(
      "input,i", po::value<std::string>(&inputFile)->required(),
      "input file (YAML)")("output,o",
                           po::value<std::string>(&outputFile)->required(),
                           "output file (YAML)")
      // ("url",
      // po::value<std::string>(&url)->default_value("http://0.0.0.0:8080"),
      // "server URL")
      ;

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  // Configure SIPP based on config file
  YAML::Node config = YAML::LoadFile(inputFile);

  State goal(config["goal"][0].as<int>(), config["goal"][1].as<int>());
  State start(config["start"][0].as<int>(), config["start"][1].as<int>());

  std::unordered_set<State> obstacles;
  for (const auto& node : config["environment"]["obstacles"]) {
    obstacles.insert(State(node[0].as<int>(), node[1].as<int>()));
  }
  Environment env(config["environment"]["size"][0].as<int>(),
                  config["environment"]["size"][1].as<int>(), obstacles, goal);

  typedef SIPP<State, State, Action, int, Environment> sipp_t;
  sipp_t sipp(env);

  for (const auto& node : config["environment"]["collisionIntervals"]) {
    State state(node["location"][0].as<int>(), node["location"][1].as<int>());

    std::vector<sipp_t::interval> collisionIntervals;

    for (const auto& interval : node["intervals"]) {
      collisionIntervals.emplace_back(
          sipp_t::interval(interval[0].as<int>(), interval[1].as<int>()));
    }
    sipp.setCollisionIntervals(state, collisionIntervals);
  }

  // Plan
  PlanResult<State, Action, int> solution;
  bool success = sipp.search(start, Action::Wait, solution);

  if (success) {
    std::cout << "Planning successful! Total cost: " << solution.cost
              << std::endl;
    for (size_t i = 0; i < solution.actions.size(); ++i) {
      std::cout << solution.states[i].second << ": " << solution.states[i].first
                << "->" << solution.actions[i].first
                << "(cost: " << solution.actions[i].second << ")" << std::endl;
    }
    std::cout << solution.states.back().second << ": "
              << solution.states.back().first << std::endl;

    std::ofstream out(outputFile);
    out << "schedule:" << std::endl;
    out << "  agent1:" << std::endl;
    for (size_t i = 0; i < solution.states.size(); ++i) {
      out << "    - x: " << solution.states[i].first.x << std::endl
          << "      y: " << solution.states[i].first.y << std::endl
          << "      t: " << solution.states[i].second << std::endl;
    }
  } else {
    std::cout << "Planning NOT successful!" << std::endl;
  }

  return 0;
}


//
//  LinSysSolver.hpp
//  IPC
//
//  Created by Minchen Li on 6/30/18.
//

#ifndef LinSysSolver_hpp
#define LinSysSolver_hpp

#include "Types.hpp"

#include <Eigen/Eigen>
#include <Eigen/Sparse>

#include <set>
#include <map>
#include <unordered_map>
#include <iostream>

#ifdef USE_TBB
#include <tbb/tbb.h>
#endif

namespace IPC {

template <typename vectorTypeI, typename vectorTypeS>
class LinSysSolver {
protected:
    int numRows;
    Eigen::VectorXi ia, ja;
    std::vector<std::map<int, int>> IJ2aI;
    Eigen::VectorXd a;

public:
    virtual ~LinSysSolver(void){};

public:
    virtual void set_pattern(const std::vector<std::set<int>>& vNeighbor,
        const std::set<int>& fixedVert)
    {
        numRows = static_cast<int>(vNeighbor.size()) * DIM;
        ia.resize(numRows + 1);
        ia[0] = 1; // 1 + nnz above row i
        ja.resize(0); // colI of each element
        IJ2aI.resize(0); // map from matrix index to ja index
        IJ2aI.resize(numRows);

        std::vector<Eigen::VectorXi> ja_v(vNeighbor.size());
        std::vector<int> rowNNZ(numRows);
#ifdef USE_TBB
        tbb::parallel_for(0, (int)vNeighbor.size(), 1, [&](int vI)
#else
        for (int vI = 0; vI < vNeighbor.size(); ++vI)
#endif
            {
                ja_v[vI].resize((vNeighbor[vI].size() + 1) * DIM);

                ja_v[vI][0] = vI * DIM;
                ja_v[vI][1] = ja_v[vI][0] + 1;
                IJ2aI[ja_v[vI][0]][ja_v[vI][0]] = 0;
                IJ2aI[ja_v[vI][0]][ja_v[vI][1]] = 1;
                if constexpr (DIM == 3) {
                    ja_v[vI][2] = ja_v[vI][0] + 2;
                    IJ2aI[ja_v[vI][0]][ja_v[vI][2]] = 2;
                }

                int nnz = DIM;
                for (const auto& nbVI : vNeighbor[vI]) {
                    if (nbVI > vI) {
                        ja_v[vI][nnz] = nbVI * DIM;
                        ja_v[vI][nnz + 1] = ja_v[vI][nnz] + 1;
                        IJ2aI[ja_v[vI][0]][ja_v[vI][nnz]] = nnz;
                        IJ2aI[ja_v[vI][0]][ja_v[vI][nnz + 1]] = nnz + 1;
                        if constexpr (DIM == 3) {
                            ja_v[vI][nnz + 2] = ja_v[vI][nnz] + 2;
                            IJ2aI[ja_v[vI][0]][ja_v[vI][nnz + 2]] = nnz + 2;
                        }
                        nnz += DIM;
                    }
                }

                rowNNZ[ja_v[vI][0]] = nnz;
                if constexpr (DIM == 2) {
                    ja_v[vI].conservativeResize(nnz * DIM - 1);
                    ja_v[vI].tail(nnz - 1) = ja_v[vI].segment(1, nnz - 1);

                    IJ2aI[ja_v[vI][0] + 1] = IJ2aI[ja_v[vI][0]];
                    IJ2aI[ja_v[vI][0] + 1].erase(ja_v[vI][0]);

                    rowNNZ[ja_v[vI][0] + 1] = nnz - 1;
                }
                else {
                    ja_v[vI].conservativeResize(nnz * DIM - 3);
                    ja_v[vI].segment(nnz, nnz - 1) = ja_v[vI].segment(1, nnz - 1);
                    ja_v[vI].tail(nnz - 2) = ja_v[vI].segment(2, nnz - 2);

                    IJ2aI[ja_v[vI][0] + 1] = IJ2aI[ja_v[vI][0]];
                    IJ2aI[ja_v[vI][0] + 1].erase(ja_v[vI][0]);
                    IJ2aI[ja_v[vI][0] + 2] = IJ2aI[ja_v[vI][0] + 1];
                    IJ2aI[ja_v[vI][0] + 2].erase(ja_v[vI][0] + 1);

                    rowNNZ[ja_v[vI][0] + 1] = nnz - 1;
                    rowNNZ[ja_v[vI][0] + 2] = nnz - 2;
                }
            }
#ifdef USE_TBB
        );
#endif

        for (int rowI = 0; rowI < numRows; ++rowI) {
            ia[rowI + 1] = ia[rowI] + rowNNZ[rowI];
        }

        ja.resize(ia[numRows] - 1);
#ifdef USE_TBB
        tbb::parallel_for(0, (int)vNeighbor.size(), 1, [&](int vI)
#else
        for (int vI = 0; vI < vNeighbor.size(); ++vI)
#endif
            {
                int rowIStart = vI * DIM;

                ja.segment(ia[rowIStart] - 1, ja_v[vI].size()) = ja_v[vI];

                for (auto& indexI : IJ2aI[rowIStart]) {
                    indexI.second += ia[rowIStart] - 1;
                }
                for (auto& indexI : IJ2aI[rowIStart + 1]) {
                    indexI.second += ia[rowIStart + 1] - 2;
                }
                if constexpr (DIM == 3) {
                    for (auto& indexI : IJ2aI[rowIStart + 2]) {
                        indexI.second += ia[rowIStart + 2] - 3;
                    }
                }
            }
#ifdef USE_TBB
        );
#endif
        ja.array() += 1;
        a.resize(ja.size());

        //NOTE: fixed verts nnz entries are not eliminated
    }

    virtual void load(const char* filePath, Eigen::VectorXd& rhs)
    {
        FILE* in = fopen(filePath, "rb");
        assert(in);

        size_t vecSize;
        fread(&vecSize, sizeof(size_t), 1, in);
        std::cout << "ia size " << vecSize << std::endl;
        ia.resize(vecSize);
        fread(ia.data(), sizeof(ia[0]), vecSize, in);

        fread(&vecSize, sizeof(size_t), 1, in);
        std::cout << "ja size " << vecSize << std::endl;
        ja.resize(vecSize);
        fread(ja.data(), sizeof(ja[0]), vecSize, in);

        if (ia[0] == 0) {
            ia.array() += 1;
            ja.array() += 1;
        }

        fread(&vecSize, sizeof(size_t), 1, in);
        std::cout << "a size " << vecSize << std::endl;
        a.resize(vecSize);
        fread(a.data(), sizeof(a[0]), vecSize, in);

        fread(&vecSize, sizeof(size_t), 1, in);
        std::cout << "rhs size " << vecSize << std::endl;
        rhs.resize(vecSize);
        fread(rhs.data(), sizeof(rhs[0]), vecSize, in);

        numRows = vecSize;

        fclose(in);
        std::cout << "load done" << std::endl;
    }
    virtual void write(const char* filePath, const Eigen::VectorXd& rhs)
    {
        FILE* out = fopen(filePath, "wb");

        size_t vecSize = ia.size();
        fwrite(&vecSize, sizeof(vecSize), 1, out);
        fwrite(ia.data(), sizeof(ia[0]), ia.size(), out);

        vecSize = ja.size();
        fwrite(&vecSize, sizeof(vecSize), 1, out);
        fwrite(ja.data(), sizeof(ja[0]), ja.size(), out);

        vecSize = a.size();
        fwrite(&vecSize, sizeof(vecSize), 1, out);
        fwrite(a.data(), sizeof(a[0]), a.size(), out);

        vecSize = rhs.size();
        fwrite(&vecSize, sizeof(vecSize), 1, out);
        fwrite(rhs.data(), sizeof(rhs[0]), rhs.size(), out);

        fclose(out);
    }

    virtual void set_pattern(const Eigen::SparseMatrix<double>& mtr)
    {
        //NOTE: mtr must be SPD

        numRows = static_cast<int>(mtr.rows());

        ja.conservativeResize(mtr.nonZeros());
        memcpy(ja.data(), mtr.innerIndexPtr(),
            mtr.nonZeros() * sizeof(mtr.innerIndexPtr()[0]));

        ia.conservativeResize(numRows + 1);
        memcpy(ia.data(), mtr.outerIndexPtr(),
            (numRows + 1) * sizeof(mtr.outerIndexPtr()[0]));

        a.conservativeResize(mtr.nonZeros());
        memcpy(a.data(), mtr.valuePtr(),
            mtr.nonZeros() * sizeof(mtr.valuePtr()[0]));
    }

    virtual void analyze_pattern(void) = 0;

    virtual bool factorize(void) = 0;

    virtual void solve(Eigen::VectorXd& rhs,
        Eigen::VectorXd& result)
        = 0;

    virtual void multiply(const Eigen::VectorXd& x,
        Eigen::VectorXd& Ax)
    {
        assert(x.size() == numRows);
        assert(IJ2aI.size() == numRows);

        Ax.setZero(numRows);
        for (int rowI = 0; rowI < numRows; ++rowI) {
            for (const auto& colI : IJ2aI[rowI]) {
                Ax[rowI] += a[colI.second] * x[colI.first];
                if (rowI != colI.first) {
                    Ax[colI.first] += a[colI.second] * x[rowI];
                }
            }
        }
    }

public:
    virtual void outputFactorization(const std::string& filePath)
    {
        assert(0 && "please implement!");
    }
    virtual double coeffMtr(int rowI, int colI) const
    {
        if (rowI > colI) {
            // return only upper right part for symmetric matrix
            int temp = rowI;
            rowI = colI;
            colI = temp;
        }
        assert(rowI < IJ2aI.size());
        const auto finder = IJ2aI[rowI].find(colI);
        if (finder != IJ2aI[rowI].end()) {
            return a[finder->second];
        }
        else {
            return 0.0;
        }
    }
    virtual void getCoeffMtr(Eigen::SparseMatrix<double>& mtr) const
    {
        mtr.resize(numRows, numRows);
        mtr.setZero();
        mtr.reserve(a.size() * 2 - numRows);
        for (int rowI = 0; rowI < numRows; rowI++) {
            for (const auto& colIter : IJ2aI[rowI]) {
                mtr.insert(rowI, colIter.first) = a[colIter.second];
                if (rowI != colIter.first) {
                    mtr.insert(colIter.first, rowI) = a[colIter.second];
                }
            }
        }
    }
    virtual void getCoeffMtr_lower(Eigen::SparseMatrix<double>& mtr) const
    {
        assert(numRows > 0);

        mtr.conservativeResize(numRows, numRows);
        mtr.reserve(a.size());

        memcpy(mtr.innerIndexPtr(), ja.data(), ja.size() * sizeof(ja[0]));
        memcpy(mtr.outerIndexPtr(), ia.data(), ia.size() * sizeof(ia[0]));
        memcpy(mtr.valuePtr(), a.data(), a.size() * sizeof(a[0]));
    }
    virtual void getTriplets(const Eigen::VectorXi& nodeList,
        std::vector<Eigen::Triplet<double>>& triplet) const
    {
        std::map<int, int> rowIMapper;
        for (int i = 0; i < nodeList.size(); ++i) {
            int startI = i * DIM;
            int startRowI = nodeList[i] * DIM;

            rowIMapper[startRowI] = startI;
            rowIMapper[startRowI + 1] = startI + 1;
            if constexpr (DIM == 3) {
                rowIMapper[startRowI + 2] = startI + 2;
            }
        }

        triplet.resize(0);
        for (int rowI = 0; rowI < numRows; rowI++) {
            auto rowIFinder = rowIMapper.find(rowI);
            for (const auto& colIter : IJ2aI[rowI]) {
                auto colIFinder = rowIMapper.find(colIter.first);
                if (rowIFinder != rowIMapper.end() && colIFinder != rowIMapper.end()) {
                    triplet.emplace_back(rowIFinder->second, colIFinder->second, a[colIter.second]);
                    if (rowIFinder->second != colIFinder->second) {
                        triplet.emplace_back(colIFinder->second, rowIFinder->second, a[colIter.second]);
                    }
                }
            }
        }
    }
    virtual void setCoeff(int rowI, int colI, double val)
    {
        if (rowI <= colI) {
            assert(rowI < IJ2aI.size());
            const auto finder = IJ2aI[rowI].find(colI);
            assert(finder != IJ2aI[rowI].end());
            a[finder->second] = val;
        }
    }
    virtual void setCoeff(const LinSysSolver<vectorTypeI, vectorTypeS>* other,
        double multiplier)
    {
        assert(numRows == other->numRows);
        assert(ja.size() == other->a.size());

        a = multiplier * other->a;
    }
    virtual void setZero(void)
    {
        a.setZero();
    }
    virtual void setUnit_row(int rowI)
    {
        assert(numRows == IJ2aI.size());
        assert(rowI < numRows);
        for (const auto& colIter : IJ2aI[rowI]) {
            a[colIter.second] = (colIter.first == rowI);
        }
    }
    virtual void setUnit_row(int rowI, std::unordered_map<int, double>& rowVec)
    {
        assert(numRows == IJ2aI.size());
        assert(rowI < numRows);
        rowVec.clear();
        for (const auto& colIter : IJ2aI[rowI]) {
            rowVec[colIter.first] = a[colIter.second];
            a[colIter.second] = (colIter.first == rowI);
        }
    }
    virtual void setUnit_col(int colI, const std::set<int>& rowVIs)
    {
        assert(numRows == IJ2aI.size());
        assert(colI < numRows);
        for (const auto& rowVI : rowVIs) {
            for (int dimI = 0; dimI < DIM; ++dimI) {
                int rowI = rowVI * DIM + dimI;
                assert(rowI < numRows);
                if (rowI <= colI) {
                    const auto finder = IJ2aI[rowI].find(colI);
                    if (finder != IJ2aI[rowI].end()) {
                        a[finder->second] = (rowI == colI);
                    }
                }
            }
        }
    }
    virtual void setUnit_col_dim1(int colI, const std::set<int>& rowVIs)
    {
        assert(numRows == IJ2aI.size());
        assert(colI < numRows);
        for (const auto& rowI : rowVIs) {
            assert(rowI < numRows);
            if (rowI <= colI) {
                const auto finder = IJ2aI[rowI].find(colI);
                if (finder != IJ2aI[rowI].end()) {
                    a[finder->second] = (rowI == colI);
                }
            }
        }
    }

    virtual void addCoeff(int rowI, int colI, double val)
    {
        if (rowI <= colI) {
            assert(rowI < IJ2aI.size());
            const auto finder = IJ2aI[rowI].find(colI);
            assert(finder != IJ2aI[rowI].end());
            a[finder->second] += val;
        }
    }
    virtual void precondition_diag(const Eigen::VectorXd& input, Eigen::VectorXd& output)
    {
        assert(numRows == input.size());
        output.resize(numRows);
        for (int rowI = 0; rowI < numRows; ++rowI) {
            const auto finder = IJ2aI[rowI].find(rowI);
            assert(finder != IJ2aI[rowI].end());
            output[rowI] = input[rowI] / a[finder->second];
        }
    }
    virtual void getMaxDiag(double& maxDiag)
    {
        maxDiag = -std::numeric_limits<double>::infinity();
        for (int rowI = 0; rowI < numRows; ++rowI) {
            const auto finder = IJ2aI[rowI].find(rowI);
            assert(finder != IJ2aI[rowI].end());
            if (maxDiag < a[finder->second]) {
                maxDiag = a[finder->second];
            }
        }
    }
    virtual void addCoeff(const LinSysSolver<vectorTypeI, vectorTypeS>* other,
        double multiplier)
    {
        assert(numRows == other->numRows);
        if (a.size() == other->a.size()) {
            a += multiplier * other->a;
        }
        else {
            for (int rowI = 0; rowI < numRows; ++rowI) {
                for (const auto& colIter : other->IJ2aI[rowI]) {
                    const auto finder = IJ2aI[rowI].find(colIter.first);
                    if (finder != IJ2aI[rowI].end()) {
                        a[finder->second] += multiplier * other->a[colIter.second];
                    }
                }
            }
        }
    }

    virtual int getNumRows(void) const
    {
        return numRows;
    }
    virtual int getNumNonzeros(void) const
    {
        return a.size();
    }
    virtual const std::vector<std::map<int, int>>& getIJ2aI(void) const
    {
        return IJ2aI;
    }
    virtual Eigen::VectorXi& get_ia(void) { return ia; }
    virtual Eigen::VectorXi& get_ja(void) { return ja; }
    virtual Eigen::VectorXd& get_a(void) { return a; }
    virtual const Eigen::VectorXd& get_a(void) const { return a; }
};

} // namespace IPC

#endif /* LinSysSolver_hpp */


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under 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
//---------------------------------------------------------------------------///

#ifndef CRYPTO3_MAKE_UINT_T_HPP
#define CRYPTO3_MAKE_UINT_T_HPP

#include <tuple>

#include <boost/integer.hpp>

namespace boost {
    namespace crypto3 {
        namespace detail {
            template<std::size_t Size, typename Integer>
            static inline typename boost::uint_t<Size>::exact extract_uint_t(Integer v, std::size_t position) {
                return static_cast<typename boost::uint_t<Size>::exact>(v >>
                                                                        (((~position) & (sizeof(Integer) - 1)) << 3));
            }

            template<std::size_t Size, typename T>
            static inline typename boost::uint_t<Size>::exact make_uint_t(const std::initializer_list<T> &args) {
                typedef typename std::initializer_list<T>::value_type value_type;
                typename boost::uint_t<Size>::exact result = 0;

#pragma clang loop unroll(full)
                for (const value_type &itr : args) {
                    result = static_cast<typename boost::uint_t<Size>::exact>(
                        (result << std::numeric_limits<value_type>::digits) | itr);
                }

                return result;
            }

            template<std::size_t Size, typename... Args>
            static inline typename boost::uint_t<Size>::exact make_uint_t(Args... args) {
                return make_uint_t<Size, typename std::tuple_element<0, std::tuple<Args...>>::type>({args...});
            }
        }    // namespace detail
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_MAKE_UINT_T_HPP


// Copyright 2006-2009 Daniel James.
// Distributed under 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)

// clang-format off
#include "../helpers/prefix.hpp"
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include "../helpers/postfix.hpp"
// clang-format on

#include "../helpers/test.hpp"
#include <boost/limits.hpp>
#include "../helpers/random_values.hpp"

#if defined(BOOST_MSVC)
#pragma warning(push)
#pragma warning(disable : 4127) // conditional expression is constant
#endif

namespace load_factor_tests {

  test::seed_t initialize_seed(783656);

  template <class X> void set_load_factor_tests(X*)
  {
    X x;

    BOOST_TEST(x.max_load_factor() == 1.0);
    BOOST_TEST(x.load_factor() == 0);

    // A valid implementation could fail these tests, but I think they're
    // reasonable.
    x.max_load_factor(2.0);
    BOOST_TEST(x.max_load_factor() == 2.0);
    x.max_load_factor(0.5);
    BOOST_TEST(x.max_load_factor() == 0.5);
  }

  template <class X>
  void insert_test(X*, float mlf, test::random_generator generator)
  {
    X x;
    x.max_load_factor(mlf);
    float b = x.max_load_factor();

    test::random_values<X> values(1000, generator);

    for (BOOST_DEDUCED_TYPENAME test::random_values<X>::const_iterator
           it = values.begin(),
           end = values.end();
         it != end; ++it) {
      BOOST_DEDUCED_TYPENAME X::size_type old_size = x.size(),
                                          old_bucket_count = x.bucket_count();
      x.insert(*it);
      if (static_cast<double>(old_size + 1) <=
          b * static_cast<double>(old_bucket_count))
        BOOST_TEST(x.bucket_count() == old_bucket_count);
    }
  }

  template <class X>
  void load_factor_insert_tests(X* ptr, test::random_generator generator)
  {
    insert_test(ptr, 1.0f, generator);
    insert_test(ptr, 0.1f, generator);
    insert_test(ptr, 100.0f, generator);

    insert_test(ptr, (std::numeric_limits<float>::min)(), generator);

    if (std::numeric_limits<float>::has_infinity)
      insert_test(ptr, std::numeric_limits<float>::infinity(), generator);
  }

  boost::unordered_set<int>* int_set_ptr;
  boost::unordered_multiset<int>* int_multiset_ptr;
  boost::unordered_map<int, int>* int_map_ptr;
  boost::unordered_multimap<int, int>* int_multimap_ptr;

  using test::default_generator;
  using test::generate_collisions;
  using test::limited_range;

  UNORDERED_TEST(set_load_factor_tests,
    ((int_set_ptr)(int_multiset_ptr)(int_map_ptr)(int_multimap_ptr)))

  UNORDERED_TEST(load_factor_insert_tests,
    ((int_set_ptr)(int_multiset_ptr)(int_map_ptr)(int_multimap_ptr))(
      (default_generator)(generate_collisions)(limited_range)))
}

RUN_TESTS()

#if defined(BOOST_MSVC)
#pragma warning(pop)
#pragma warning(disable : 4127) // conditional expression is constant
#endif


#include <boost/numeric/interval.hpp>


/**
 * @file drusilla_select_test.cpp
 * @author Ryan Curtin
 *
 * Test for DrusillaSelect.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/methods/approx_kfn/drusilla_select.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace mlpack;
using namespace mlpack::neighbor;

BOOST_AUTO_TEST_SUITE(DrusillaSelectTest);

// If we have a dataset with an extreme outlier, then every point (except that
// one) should end up with that point as the furthest neighbor candidate.
BOOST_AUTO_TEST_CASE(DrusillaSelectExtremeOutlierTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 100);
  dataset.col(99) += 100; // Make last column very large.

  // Construct with some reasonable parameters.
  DrusillaSelect<> ds(dataset, 5, 5);

  // Query with every point except the extreme point.
  arma::mat distances;
  arma::Mat<size_t> neighbors;
  ds.Search(dataset.cols(0, 98), 1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 99);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 1);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 99);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 1);

  for (size_t i = 0; i < 99; ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors[i], 99);
  }
}

// If we use only one projection with the number of points equal to what is in
// the dataset, we should end up with the exact result.
BOOST_AUTO_TEST_CASE(DrusillaSelectExhaustiveExactTest)
{
  arma::mat dataset = arma::randu<arma::mat>(5, 100);

  // Construct with one projection and 100 points in that projection.
  DrusillaSelect<> ds(dataset, 100, 1);

  arma::mat distances, distancesTrue;
  arma::Mat<size_t> neighbors, neighborsTrue;

  ds.Search(dataset, 5, neighbors, distances);

  KFN kfn(dataset);
  kfn.Search(dataset, 5, neighborsTrue, distancesTrue);

  BOOST_REQUIRE_EQUAL(neighborsTrue.n_cols, neighbors.n_cols);
  BOOST_REQUIRE_EQUAL(neighborsTrue.n_rows, neighbors.n_rows);
  BOOST_REQUIRE_EQUAL(distancesTrue.n_cols, distances.n_cols);
  BOOST_REQUIRE_EQUAL(distancesTrue.n_rows, distances.n_rows);

  for (size_t i = 0; i < distances.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsTrue[i]);
    BOOST_REQUIRE_CLOSE(distances[i], distancesTrue[i], 1e-5);
  }
}

// Test that we can call Train() after calling the constructor.
BOOST_AUTO_TEST_CASE(RetrainTest)
{
  arma::mat firstDataset = arma::randu<arma::mat>(3, 10);
  arma::mat dataset = arma::randu<arma::mat>(3, 200);

  DrusillaSelect<> ds(firstDataset, 3, 3);
  ds.Train(std::move(dataset), 2, 2);

  arma::mat distances;
  arma::Mat<size_t> neighbors;
  ds.Search(dataset, 1, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 200);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 1);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 200);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 1);
}

// Test serialization.
BOOST_AUTO_TEST_CASE(SerializationTest)
{
  // Create a random dataset.
  arma::mat dataset = arma::randu<arma::mat>(3, 100);

  DrusillaSelect<> ds(dataset, 3, 3);

  arma::mat fakeDataset1 = arma::randu<arma::mat>(2, 15);
  arma::mat fakeDataset2 = arma::randu<arma::mat>(10, 18);
  DrusillaSelect<> dsXml(fakeDataset1, 5, 3);
  DrusillaSelect<> dsText(2, 2);
  DrusillaSelect<> dsBinary(5, 2);
  dsBinary.Train(fakeDataset2);

  // Now do the serialization.
  SerializeObjectAll(ds, dsXml, dsText, dsBinary);

  // Now do a search and make sure all the results are the same.
  arma::Mat<size_t> neighbors, neighborsXml, neighborsText, neighborsBinary;
  arma::mat distances, distancesXml, distancesText, distancesBinary;

  ds.Search(dataset, 3, neighbors, distances);
  dsXml.Search(dataset, 3, neighborsXml, distancesXml);
  dsText.Search(dataset, 3, neighborsText, distancesText);
  dsBinary.Search(dataset, 3, neighborsBinary, distancesBinary);

  BOOST_REQUIRE_EQUAL(neighbors.n_rows, neighborsXml.n_rows);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, neighborsXml.n_cols);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, neighborsText.n_rows);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, neighborsText.n_cols);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, neighborsBinary.n_rows);
  BOOST_REQUIRE_EQUAL(neighbors.n_cols, neighborsBinary.n_cols);

  BOOST_REQUIRE_EQUAL(distances.n_rows, distancesXml.n_rows);
  BOOST_REQUIRE_EQUAL(distances.n_cols, distancesXml.n_cols);
  BOOST_REQUIRE_EQUAL(distances.n_rows, distancesText.n_rows);
  BOOST_REQUIRE_EQUAL(distances.n_cols, distancesText.n_cols);
  BOOST_REQUIRE_EQUAL(distances.n_rows, distancesBinary.n_rows);
  BOOST_REQUIRE_EQUAL(distances.n_cols, distancesBinary.n_cols);

  for (size_t i = 0; i < neighbors.n_elem; ++i)
  {
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsXml[i]);
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsText[i]);
    BOOST_REQUIRE_EQUAL(neighbors[i], neighborsBinary[i]);

    BOOST_REQUIRE_CLOSE(distances[i], distancesXml[i], 1e-5);
    BOOST_REQUIRE_CLOSE(distances[i], distancesText[i], 1e-5);
    BOOST_REQUIRE_CLOSE(distances[i], distancesBinary[i], 1e-5);
  }
}

// Make sure we can create the object with a sparse matrix.
BOOST_AUTO_TEST_CASE(SparseTest)
{
  arma::sp_mat dataset;
  dataset.sprandu(50, 1000, 0.3);

  DrusillaSelect<arma::sp_mat> ds(dataset, 5, 10);

  // Run a search.
  arma::mat distances;
  arma::Mat<size_t> neighbors;
  ds.Search(dataset, 3, neighbors, distances);

  BOOST_REQUIRE_EQUAL(neighbors.n_cols, 1000);
  BOOST_REQUIRE_EQUAL(neighbors.n_rows, 3);
  BOOST_REQUIRE_EQUAL(distances.n_cols, 1000);
  BOOST_REQUIRE_EQUAL(distances.n_rows, 3);
}

BOOST_AUTO_TEST_SUITE_END();