///////////////////////////////////////////////////////////////////////////////
// weighted_mean.hpp
//
//  Copyright 2006 Eric Niebler, Olivier Gygi. 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 BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_MEAN_HPP_EAN_03_11_2005
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_MEAN_HPP_EAN_03_11_2005

#include <boost/mpl/assert.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/weights.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/sum.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_mean_impl
    //      lazy, by default
    template<typename Sample, typename Weight, typename Tag>
    struct weighted_mean_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, Weight>::result_type result_type;

        weighted_mean_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            typedef
                typename mpl::if_<
                    is_same<Tag, tag::sample>
                  , tag::weighted_sum
                  , tag::weighted_sum_of_variates<Sample, Tag>
                >::type
            weighted_sum_tag;

            extractor<weighted_sum_tag> const some_weighted_sum = {};

            return numeric::average(some_weighted_sum(args), sum_of_weights(args));
        }
    };

    ///////////////////////////////////////////////////////////////////////////////
    // immediate_weighted_mean_impl
    //      immediate
    template<typename Sample, typename Weight, typename Tag>
    struct immediate_weighted_mean_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, Weight>::result_type result_type;

        template<typename Args>
        immediate_weighted_mean_impl(Args const &args)
          : mean(
                numeric::average(
                    args[parameter::keyword<Tag>::get() | Sample()]
                      * numeric::one<Weight>::value
                  , numeric::one<Weight>::value
                )
            )
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            // Matthias:
            //  need to pass the argument pack since the weight might be an external
            //  accumulator set passed as a named parameter
            Weight w_sum = sum_of_weights(args);
            Weight w = args[weight];
            weighted_sample const &s = args[parameter::keyword<Tag>::get()] * w;
            this->mean = numeric::average(this->mean * (w_sum - w) + s, w_sum);
        }

        result_type result(dont_care) const
        {
            return this->mean;
        }

    private:
        result_type mean;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_mean
// tag::immediate_weighted_mean
//
namespace tag
{
    struct weighted_mean
      : depends_on<sum_of_weights, weighted_sum>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_mean_impl<mpl::_1, mpl::_2, tag::sample> impl;
    };
    struct immediate_weighted_mean
      : depends_on<sum_of_weights>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::immediate_weighted_mean_impl<mpl::_1, mpl::_2, tag::sample> impl;
    };
    template<typename VariateType, typename VariateTag>
    struct weighted_mean_of_variates
      : depends_on<sum_of_weights, weighted_sum_of_variates<VariateType, VariateTag> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_mean_impl<VariateType, mpl::_2, VariateTag> impl;
    };
    template<typename VariateType, typename VariateTag>
    struct immediate_weighted_mean_of_variates
      : depends_on<sum_of_weights>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::immediate_weighted_mean_impl<VariateType, mpl::_2, VariateTag> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_mean
// extract::weighted_mean_of_variates
//
namespace extract
{
    extractor<tag::mean> const weighted_mean = {};
    BOOST_ACCUMULATORS_DEFINE_EXTRACTOR(tag, weighted_mean_of_variates, (typename)(typename))

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_mean)
}

using extract::weighted_mean;
using extract::weighted_mean_of_variates;

// weighted_mean(lazy) -> weighted_mean
template<>
struct as_feature<tag::weighted_mean(lazy)>
{
    typedef tag::weighted_mean type;
};

// weighted_mean(immediate) -> immediate_weighted_mean
template<>
struct as_feature<tag::weighted_mean(immediate)>
{
    typedef tag::immediate_weighted_mean type;
};

// weighted_mean_of_variates<VariateType, VariateTag>(lazy) -> weighted_mean_of_variates<VariateType, VariateTag>
template<typename VariateType, typename VariateTag>
struct as_feature<tag::weighted_mean_of_variates<VariateType, VariateTag>(lazy)>
{
    typedef tag::weighted_mean_of_variates<VariateType, VariateTag> type;
};

// weighted_mean_of_variates<VariateType, VariateTag>(immediate) -> immediate_weighted_mean_of_variates<VariateType, VariateTag>
template<typename VariateType, typename VariateTag>
struct as_feature<tag::weighted_mean_of_variates<VariateType, VariateTag>(immediate)>
{
    typedef tag::immediate_weighted_mean_of_variates<VariateType, VariateTag> type;
};

}} // namespace boost::accumulators

#endif


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#include <iostream>
#include <fstream>
#include <string>
#include <iomanip>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <vw/Cartography/GeoReference.h>
#include <vw/Math/Functors.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/Statistics.h>
#include <vw/FileIO/DiskImageResource.h>
#include <vw/FileIO/DiskImageView.h>

using namespace vw;

/// \file lola_compare.cc Compares a DEM to a LOLA RDR file

// TODO: Move this class to a library file!
/// Class to compute a running standard deviation
/// - Code adapted from http://www.johndcook.com/standard_deviation.html
class RunningStatistics
{
public:
  RunningStatistics() : m_n(0), m_oldM(0), m_newM(0), m_oldS(0), m_newS(0)
  {
    m_min = std::numeric_limits<double>::max();
    m_max = std::numeric_limits<double>::min();
  }

  void Clear()
  {
    m_n = 0;
  }

  void Push(double x)
  {
    m_n++;

    if (x < m_min)
        m_min = x;
    if (x > m_max)
        m_max = x;

    // See Knuth TAOCP vol 2, 3rd edition, page 232
    if (m_n == 1)
    {
      m_oldM = m_newM = x;
      m_oldS = 0.0;
    }
    else
    {
      m_newM = m_oldM + (x - m_oldM)/m_n;
      m_newS = m_oldS + (x - m_oldM)*(x - m_newM);

      // set up for next iteration
      m_oldM = m_newM;
      m_oldS = m_newS;
    }
  }

  int NumDataValues() const
  {
    return m_n;
  }

  double Min() const
  {
    return m_min;
  }

  double Max() const
  {
    return m_max;
  }

  double Mean() const
  {
    return (m_n > 0) ? m_newM : 0.0;
  }

  double Variance() const
  {
    return ( (m_n > 1) ? m_newS/(m_n - 1) : 0.0 );
  }

  double StandardDeviation() const
  {
    return sqrt( Variance() );
  }

private:
  int    m_n;
  double m_oldM, m_newM, m_oldS, m_newS;
  double m_max, m_min;

};

/// Function to write the output statistics to an output stream
bool writeOutput(const std::vector<float > &diffVector,
                 const std::vector<double> &levels,
                 const std::vector<size_t> &hist,
                 const RunningStatistics   &statCalc,
                       std::ostream        &stream)
{

  stream << "Mean elevation difference    = " << statCalc.Mean()              << std::endl;
  stream << "Standard deviation           = " << statCalc.StandardDeviation() << std::endl;
  stream << "Num intersecting LOLA points = " << statCalc.NumDataValues()     << std::endl << std::endl;

  // Print out the percentile distribution
  stream << "Image distribution (5 percent intervals): " << std::endl;
  stream.setf(std::ios::fixed, std::ios::floatfield);
  stream.precision(2);
  stream.fill(' ');
  for (size_t i=0; i<hist.size(); ++i)
  {
    float percent   = 0.05f * i;
    int   index      = diffVector.size() * percent;
    float percentile = diffVector[index];

    stream << "Percentile " << std::right << std::setw(4) << percent << " = " << std::setw(5) << percentile << std::endl;
  }
  stream << "Percentile " << std::right << std::setw(4) << std::left << 1.0f << " = " <<  diffVector.back() << std::endl;

  stream << std::endl; // Put a space between the "charts"

  // Print out the histogram
  stream << "Image histogram (20 bins):" << std::endl;
  stream.fill(' ');
  double ratio = 100.0 / statCalc.NumDataValues();
  for (size_t i=0; i<hist.size(); ++i)
  {
    stream << std::right << std::setw(8) << levels[i] << " <-->" << std::setw(8) << levels[i+1] << " ="
           << std::setw(6) << hist[i] << " =" << std::setw(5) << hist[i]*ratio << "%" << std::endl;
  }

  return true;
}

int main( int argc, char *argv[] )
{
  //This is pulled straight from the ASU DEM!  It may need to be different for other cases.
  const double MEAN_MOON_RADIUS = 1737400;

  const int numBins = 20; // 5% intervals

  std::string demPath, lolaDataPath, outputPath="", mapPath="", csvPath="";
  int removeHistogramOutliers=0;
  bool useAbsolute, sub180;

  po::options_description general_options("Options");
  general_options.add_options()
    ("help,h",        "Display this help message")
    ("output-file,o", po::value<std::string>(&outputPath)->default_value(""),     "Specify an output text file to store the program output")
    ("csv-log,c",     po::value<std::string>(&csvPath)->default_value(""),        "Specify an csv file to record individual point errors to")
    ("map,m",         po::value<std::string>(&mapPath)->default_value(""),        "Write output diagnostic image to file.")
    ("absolute",      po::bool_switch       (&useAbsolute)->default_value(false), "Output the absolute difference as opposed to just the difference.")
    ("sub180",        po::bool_switch       (&sub180)->default_value(false),      "Force all output degrees in csv file to be in -180 to 180 range.")
    ("limit-hist",    po::value<int        >(&removeHistogramOutliers)->default_value(0), "Limits the histogram to +/- N standard deviations from the mean");

  po::options_description positional("");
  positional.add_options()
    ("input-dem",   po::value(&demPath     ), "Path to DEM file")
    ("lola-points", po::value(&lolaDataPath), "Path to LOLA RDR point file");

  po::positional_options_description positional_desc;
  positional_desc.add("input-dem",   1);
  positional_desc.add("lola-points", 1);

  std::string usage("[options] <input DEM path> <input LOLA path>\n");
  po::variables_map vm;
  try {
    po::options_description all_options;
    all_options.add(general_options).add(positional);

    po::store( po::command_line_parser( argc, argv ).options(all_options).positional(positional_desc).style( po::command_line_style::unix_style ).run(), vm );

    po::notify( vm );
  } catch (po::error const& e) {
    vw::vw_throw( vw::ArgumentErr() << "Error parsing input:\n"
                  << e.what() << "\n" << usage << general_options );
  }

  if ( !vm.count("input-dem") || !vm.count("lola-points") )
    vw_throw( vw::ArgumentErr() << "Requires <input DEM path> and <input LOLA path> input in order to proceed.\n\n"
              << usage << general_options );

  // Load the input DEM
  DiskImageView<PixelGray<float> > inputDem(demPath);
  cartography::GeoReference georef;
  if (!read_georeference(georef, demPath))
  {
    printf("Failed to read image georeference!\n");
    return false;
  }
  std::cout << "Image size = " << inputDem.rows() << ", " << inputDem.cols() << std::endl;

  // Loop through all lines in the LOLA data file
  std::ifstream lolaFile;
  lolaFile.open(lolaDataPath.c_str());
  std::string currentLine;
  std::getline(lolaFile, currentLine); // Skip header line
  if (lolaFile.fail())
  {
    printf("Failed to open LOLA data file %s\n", lolaDataPath.c_str());
    return false;
  }

  std::ofstream csvFile;
  if (csvPath.size() > 0) // Start CSV file and write header
  {
    csvFile.open(csvPath.c_str());
    csvFile << "# Latitude(Deg), Longitude(Deg), LOLA elevation(m), DEM elevation(m), difference(m)" << std::endl;
  }

  // Loop through all the lines in the file
  unsigned int numValidPixels = 0;
  RunningStatistics statCalc;
  double  demValue = 0;
  std::list<vw::Vector3> hitList;
  std::vector<float> diffVector;
  diffVector.reserve(5000); // Init this to an estimated size
  while (std::getline(lolaFile, currentLine))
  {
    // Get the location of the first few commas
    size_t timeComma    = currentLine.find(",");
    size_t lonComma     = currentLine.find(",", timeComma+1);
    size_t latComma     = currentLine.find(",", lonComma+1);
    size_t radiusComma  = currentLine.find(",", latComma+1);

    // Extract text containing the numbers we need
    std::string lonString    = currentLine.substr(timeComma+1, lonComma   -timeComma-1);
    std::string latString    = currentLine.substr(lonComma +1, latComma   -lonComma -1);
    std::string radiusString = currentLine.substr(latComma +1, radiusComma-latComma -1);

    // Convert to floating point
    double ptLonDeg   = atof(lonString.c_str()   );
    double ptLatDeg   = atof(latString.c_str()   );
    double ptRadiusKm = atof(radiusString.c_str());

    // Convert to common elevation datum
    double lolaElevation = (ptRadiusKm*1000.0) - MEAN_MOON_RADIUS;

    // Find the equivalent location in the image
    Vector2 gdcCoord(ptLonDeg, ptLatDeg);
    Vector2 gccCoord   = georef.lonlat_to_point(gdcCoord);
    Vector2 pixelCoord = georef.point_to_pixel(gccCoord);

    //std::cout << "gdcCoord = " << gdcCoord << std::endl;
    //std::cout << "pixelCoord = " << pixelCoord << std::endl;

    // Throw out pixel misses
    if ( (pixelCoord[0] < 0) ||
         (pixelCoord[0] >= inputDem.cols()) ||
         (pixelCoord[1] < 0) ||
         (pixelCoord[1] >= inputDem.rows())   )
      continue;

//    std::cout << "pixelCoord VALID = " << pixelCoord << std::endl;

    // Get DEM value and make sure it is valid
    demValue = inputDem(pixelCoord[0], pixelCoord[1])[0];
    if (demValue <= -32767) //TODO: Obtain the no-data value!
      continue; // Throw out flag values

//    std::cout << "demHeight     = " << demValue      << std::endl;
//    std::cout << "lolaElevation = " << lolaElevation << std::endl;

    // Check difference
    float diff = static_cast<float>(demValue - lolaElevation);
    if (useAbsolute)
      diff = fabs(diff);
    ++numValidPixels;

    // Update records
    statCalc.Push(diff);
    diffVector.push_back(diff); // Record difference values for computing histogram later
    hitList.push_back(vw::Vector3(pixelCoord[0], pixelCoord[1], fabs(diff)));

    // Write a file listing all the compared locations
    if (csvPath.size() > 0)
    {
      csvFile.precision(12);
      if (sub180)
      {
      if (ptLonDeg > 180)
        ptLonDeg -= 360;
      }
      csvFile << ptLatDeg << ", " << ptLonDeg << ", " << lolaElevation << ", " << demValue << ", " << diff << std::endl;
    }
  } // End of loop through lines

  if (csvPath.size() > 0)
    csvFile.close();

  printf("Found %d valid pixels\n", numValidPixels);

  double meanValue = statCalc.Mean();
  double stdDevVal = statCalc.StandardDeviation();
  double minVal    = statCalc.Min();
  double maxVal    = statCalc.Max();

  if (removeHistogramOutliers > 0) // Cut off range at +/- N std
  {
    printf("Diff min = %lf\n", minVal);
    printf("Diff max = %lf\n", maxVal);

    minVal = meanValue - removeHistogramOutliers*stdDevVal;
    if (minVal < statCalc.Min())
      minVal = statCalc.Min();
    maxVal = meanValue + removeHistogramOutliers*stdDevVal;
    if (maxVal > statCalc.Max())
      maxVal = statCalc.Max();

    printf("Restricting histogram to +/- %d standard deviations: %lf <--> %lf\n", removeHistogramOutliers, minVal, maxVal);
  }

  double range   = maxVal - minVal;
  double binSize = range / numBins;
  double factor  = 1.0 / binSize;

  // Set up levels structure
  int numLevels = numBins + 1;
  std::vector<double> levels(numLevels);
  for (int i=0; i<numLevels; ++i)
    levels[i] = minVal + i*binSize;

  // Next fill in histogram
  std::vector<size_t> hist;
  hist.assign(numBins, 0);
  for (size_t i = 0; i < diffVector.size(); ++i)
  {
    float diff = diffVector[i];
    int   bin  = (int)floor( factor * (diff - minVal)  );
    if ((bin >= 0) && (bin < numBins)) // If removeHistogramOutliers is set some values will not fit in a bin
      ++(hist[bin]);

  } // End diff loop

  // Compute CDF distribution
  // - LOLA data is small enough to do this by sorting the values vector
  std::sort(diffVector.begin(), diffVector.end());

  // Write output to console
  writeOutput(diffVector, levels, hist, statCalc, std::cout);

  if (!outputPath.empty())  // Write output to file
  {
    std::ofstream file(outputPath.c_str());
    writeOutput(diffVector, levels, hist, statCalc, file);
    file.close();
  }

  // Optionally write a diagnostic image
  if (mapPath.size() > 0)
  {
    double outputFraction = 0.1; // Currently output image is just a fraction of the input image

    //TODO: Use existing functions to do this!
    // Initialize a diagnostic image
    // - Convert the input image from float32 grayscale to uint8 RGB
    int plotWidth  = inputDem.cols() * outputFraction;
    int plotHeight = inputDem.rows() * outputFraction;
    ImageView<PixelRGB<unsigned char> > diffPlot(plotWidth, plotHeight);

    for (int r=0; r<plotHeight; ++r)
    {
      for (int c=0; c<plotWidth; ++c)
      {
        //TODO: Get scaling factors for input image!
        float         originalValue = inputDem(r*outputFraction, c*outputFraction);
        unsigned char grayValue     = 0;//(originalValue - minVal) * (range/255.0);
        diffPlot(r,c) = PixelRGB<unsigned char>(grayValue, grayValue, grayValue);
      }
    }

    // Loop through all valid pixel hits and draw a scaled error in red
    std::list<vw::Vector3>::const_iterator iter;
    for (iter=hitList.begin(); iter!=hitList.end(); ++iter)
    {
      double diff = iter->z();
      unsigned char scaledDiff = (diff - minVal) * (range/255.0);
      diffPlot(iter->x(), iter->y()) = PixelRGB<unsigned char>(scaledDiff, 0, 0);
    }

    vw_out() << "Writing error diagram: " << mapPath << "\n";
    write_image(mapPath, diffPlot);

  } // End of writing diagnostic image

  return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "ViewerCore.h"
#include "ViewerData.h"
#include "gl.h"
#include "../quat_to_mat.h"
#include "../snap_to_fixed_up.h"
#include "../look_at.h"
#include "../frustum.h"
#include "../ortho.h"
#include "../massmatrix.h"
#include "../barycenter.h"
#include "../PI.h"
#include <Eigen/Geometry>
#include <iostream>

IGL_INLINE void igl::opengl::ViewerCore::align_camera_center(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F)
{
	if (V.rows() == 0)
		return;

	get_scale_and_shift_to_fit_mesh(V, F, camera_base_zoom, camera_base_translation);
	// Rather than crash on empty mesh...
	if (V.size() > 0)
	{
		object_scale = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
	}
}

IGL_INLINE void igl::opengl::ViewerCore::get_scale_and_shift_to_fit_mesh(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F,
	float& zoom,
	Eigen::Vector3f& shift)
{
	if (V.rows() == 0)
		return;

	Eigen::MatrixXd BC;
	if (F.rows() <= 1)
	{
		BC = V;
	}
	else
	{
		igl::barycenter(V, F, BC);
	}
	return get_scale_and_shift_to_fit_mesh(BC, zoom, shift);
}

IGL_INLINE void igl::opengl::ViewerCore::align_camera_center(
	const Eigen::MatrixXd& V)
{
	if (V.rows() == 0)
		return;

	get_scale_and_shift_to_fit_mesh(V, camera_base_zoom, camera_base_translation);
	// Rather than crash on empty mesh...
	if (V.size() > 0)
	{
		object_scale = (V.colwise().maxCoeff() - V.colwise().minCoeff()).norm();
	}
}

IGL_INLINE void igl::opengl::ViewerCore::get_scale_and_shift_to_fit_mesh(
	const Eigen::MatrixXd& V,
	float& zoom,
	Eigen::Vector3f& shift)
{
	if (V.rows() == 0)
		return;

	auto min_point = V.colwise().minCoeff();
	auto max_point = V.colwise().maxCoeff();
	auto centroid = (0.5 * (min_point + max_point)).eval();
	shift.setConstant(0);
	shift.head(centroid.size()) = -centroid.cast<float>();
	zoom = 2.0 / (max_point - min_point).array().abs().maxCoeff();
}

IGL_INLINE void igl::opengl::ViewerCore::clear_framebuffers()
{
	// The glScissor call ensures we only clear this core's buffers,
	// (in case the user wants different background colors in each viewport.)
	glScissor(viewport(0), viewport(1), viewport(2), viewport(3));
	glEnable(GL_SCISSOR_TEST);
	glClearColor(background_color[0],
		background_color[1],
		background_color[2],
		background_color[3]);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glDisable(GL_SCISSOR_TEST);
}

IGL_INLINE void igl::opengl::ViewerCore::draw(
	Eigen::Matrix4f& worldMat,
	ViewerData& data,
	bool update_matrices)
{
	using namespace std;
	using namespace Eigen;

	if (depth_test)
		glEnable(GL_DEPTH_TEST);
	else
		glDisable(GL_DEPTH_TEST);

	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

	/* Bind and potentially refresh mesh/line/point data */
	if (data.dirty)
	{
		data.updateGL(data, data.invert_normals, data.meshgl);
		data.dirty = MeshGL::DIRTY_NONE;
	}
	data.meshgl.bind_mesh();

	// Initialize uniform
	glViewport(viewport(0), viewport(1), viewport(2), viewport(3));

	if (update_matrices)
	{
		view = Eigen::Matrix4f::Identity();
		proj = Eigen::Matrix4f::Identity();
		norm = Eigen::Matrix4f::Identity();

		float width = viewport(2);
		float height = viewport(3);

		// Set view
		look_at(camera_eye, camera_center, camera_up, view);
		view = view
			* (trackball_angle * Eigen::Scaling(camera_zoom * camera_base_zoom)
				* Eigen::Translation3f(camera_translation + camera_base_translation)).matrix() * worldMat * data.MakeTrans();

		norm = view.inverse().transpose();

		// Set projection
		if (orthographic)
		{
			float length = (camera_eye - camera_center).norm();
			float h = tan(camera_view_angle / 360.0 * igl::PI) * (length);
			ortho(-h * width / height, h * width / height, -h, h, camera_dnear, camera_dfar, proj);
		}
		else
		{
			float fH = tan(camera_view_angle / 360.0 * igl::PI) * camera_dnear;
			float fW = fH * (double)width / (double)height;
			frustum(-fW, fW, -fH, fH, camera_dnear, camera_dfar, proj);
		}
	}

	// Send transformations to the GPU
	GLint viewi = glGetUniformLocation(data.meshgl.shader_mesh, "view");
	GLint proji = glGetUniformLocation(data.meshgl.shader_mesh, "proj");
	GLint normi = glGetUniformLocation(data.meshgl.shader_mesh, "normal_matrix");
	glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
	glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
	glUniformMatrix4fv(normi, 1, GL_FALSE, norm.data());

	// Light parameters
	GLint specular_exponenti = glGetUniformLocation(data.meshgl.shader_mesh, "specular_exponent");
	GLint light_position_eyei = glGetUniformLocation(data.meshgl.shader_mesh, "light_position_eye");
	GLint lighting_factori = glGetUniformLocation(data.meshgl.shader_mesh, "lighting_factor");
	GLint fixed_colori = glGetUniformLocation(data.meshgl.shader_mesh, "fixed_color");
	GLint texture_factori = glGetUniformLocation(data.meshgl.shader_mesh, "texture_factor");

	glUniform1f(specular_exponenti, data.shininess);
	glUniform3fv(light_position_eyei, 1, light_position.data());
	glUniform1f(lighting_factori, lighting_factor); // enables lighting
	glUniform4f(fixed_colori, 0.0, 0.0, 0.0, 0.0);

	if (data.V.rows() > 0)
	{
		// Render fill
		if (is_set(data.show_faces))
		{
			// Texture
			glUniform1f(texture_factori, is_set(data.show_texture) ? 1.0f : 0.0f);
			data.meshgl.draw_mesh(true);
			glUniform1f(texture_factori, 0.0f);
		}

		// Render wireframe
		if (is_set(data.show_lines))
		{
			glLineWidth(data.line_width);
			glUniform4f(fixed_colori,
				data.line_color[0],
				data.line_color[1],
				data.line_color[2], 1.0f);
			data.meshgl.draw_mesh(false);
			glUniform4f(fixed_colori, 0.0f, 0.0f, 0.0f, 0.0f);
		}
	}

	if (is_set(data.show_overlay))
	{
		if (is_set(data.show_overlay_depth))
			glEnable(GL_DEPTH_TEST);
		else
			glDisable(GL_DEPTH_TEST);

		if (data.lines.rows() > 0)
		{
			data.meshgl.bind_overlay_lines();
			viewi = glGetUniformLocation(data.meshgl.shader_overlay_lines, "view");
			proji = glGetUniformLocation(data.meshgl.shader_overlay_lines, "proj");

			glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
			glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
			// This must be enabled, otherwise glLineWidth has no effect
			glEnable(GL_LINE_SMOOTH);
			glLineWidth(data.line_width);

			data.meshgl.draw_overlay_lines();
		}

		if (data.points.rows() > 0)
		{
			data.meshgl.bind_overlay_points();
			viewi = glGetUniformLocation(data.meshgl.shader_overlay_points, "view");
			proji = glGetUniformLocation(data.meshgl.shader_overlay_points, "proj");

			glUniformMatrix4fv(viewi, 1, GL_FALSE, view.data());
			glUniformMatrix4fv(proji, 1, GL_FALSE, proj.data());
			glPointSize(data.point_size);

			data.meshgl.draw_overlay_points();
		}

		glEnable(GL_DEPTH_TEST);
	}

}

IGL_INLINE void igl::opengl::ViewerCore::UpdateUniforms(Eigen::Matrix4f& worldMat, ViewerData& data, bool update_matrices)
{

}

IGL_INLINE void igl::opengl::ViewerCore::draw_buffer(Eigen::Matrix4f& worldMat, ViewerData& data,
	bool update_matrices,
	Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>& R,
	Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>& G,
	Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>& B,
	Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>& A)
{
	assert(R.rows() == G.rows() && G.rows() == B.rows() && B.rows() == A.rows());
	assert(R.cols() == G.cols() && G.cols() == B.cols() && B.cols() == A.cols());

	unsigned width = R.rows();
	unsigned height = R.cols();

	// https://learnopengl.com/Advanced-OpenGL/Anti-Aliasing
	unsigned int framebuffer;
	glGenFramebuffers(1, &framebuffer);
	glBindFramebuffer(GL_FRAMEBUFFER, framebuffer);
	// create a multisampled color attachment texture
	unsigned int textureColorBufferMultiSampled;
	glGenTextures(1, &textureColorBufferMultiSampled);
	glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, textureColorBufferMultiSampled);
	glTexImage2DMultisample(GL_TEXTURE_2D_MULTISAMPLE, 4, GL_RGBA, width, height, GL_TRUE);
	glBindTexture(GL_TEXTURE_2D_MULTISAMPLE, 0);
	glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D_MULTISAMPLE, textureColorBufferMultiSampled, 0);
	// create a (also multisampled) renderbuffer object for depth and stencil attachments
	unsigned int rbo;
	glGenRenderbuffers(1, &rbo);
	glBindRenderbuffer(GL_RENDERBUFFER, rbo);
	glRenderbufferStorageMultisample(GL_RENDERBUFFER, 4, GL_DEPTH24_STENCIL8, width, height);
	glBindRenderbuffer(GL_RENDERBUFFER, 0);
	glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_STENCIL_ATTACHMENT, GL_RENDERBUFFER, rbo);
	assert(glCheckFramebufferStatus(GL_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE);
	glBindFramebuffer(GL_FRAMEBUFFER, 0);

	// configure second post-processing framebuffer
	unsigned int intermediateFBO;
	glGenFramebuffers(1, &intermediateFBO);
	glBindFramebuffer(GL_FRAMEBUFFER, intermediateFBO);
	// create a color attachment texture
	unsigned int screenTexture;
	glGenTextures(1, &screenTexture);
	glBindTexture(GL_TEXTURE_2D, screenTexture);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, screenTexture, 0);	// we only need a color buffer
	assert(glCheckFramebufferStatus(GL_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE);
	glBindFramebuffer(GL_FRAMEBUFFER, 0);

	glBindFramebuffer(GL_FRAMEBUFFER, framebuffer);

	// Clear the buffer
	glClearColor(background_color(0), background_color(1), background_color(2), 0.f);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	// Save old viewport
	Eigen::Vector4f viewport_ori = viewport;
	viewport << 0, 0, width, height;
	// Draw
	draw(worldMat, data, update_matrices);
	// Restore viewport
	viewport = viewport_ori;

	glBindFramebuffer(GL_READ_FRAMEBUFFER, framebuffer);
	glBindFramebuffer(GL_DRAW_FRAMEBUFFER, intermediateFBO);
	glBlitFramebuffer(0, 0, width, height, 0, 0, width, height, GL_COLOR_BUFFER_BIT, GL_NEAREST);

	glBindFramebuffer(GL_FRAMEBUFFER, intermediateFBO);
	// Copy back in the given Eigen matrices
	GLubyte* pixels = (GLubyte*)calloc(width * height * 4, sizeof(GLubyte));
	glReadPixels(0, 0, width, height, GL_RGBA, GL_UNSIGNED_BYTE, pixels);

	// Clean up
	glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
	glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
	glBindFramebuffer(GL_FRAMEBUFFER, 0);
	glDeleteTextures(1, &screenTexture);
	glDeleteTextures(1, &textureColorBufferMultiSampled);
	glDeleteFramebuffers(1, &framebuffer);
	glDeleteFramebuffers(1, &intermediateFBO);
	glDeleteRenderbuffers(1, &rbo);

	int count = 0;
	for (unsigned j = 0; j < height; ++j)
	{
		for (unsigned i = 0; i < width; ++i)
		{
			R(i, j) = pixels[count * 4 + 0];
			G(i, j) = pixels[count * 4 + 1];
			B(i, j) = pixels[count * 4 + 2];
			A(i, j) = pixels[count * 4 + 3];
			++count;
		}
	}
	// Clean up
	free(pixels);
}

IGL_INLINE void igl::opengl::ViewerCore::set_rotation_type(
	const igl::opengl::ViewerCore::RotationType& value)
{
	using namespace Eigen;
	using namespace std;
	const RotationType old_rotation_type = rotation_type;
	rotation_type = value;
	if (rotation_type == ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP &&
		old_rotation_type != ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP)
	{
		snap_to_fixed_up(Quaternionf(trackball_angle), trackball_angle);
	}
}

IGL_INLINE void igl::opengl::ViewerCore::set(unsigned int& property_mask, bool value) const
{
	if (!value)
		unset(property_mask);
	else
		property_mask |= id;
}

IGL_INLINE void igl::opengl::ViewerCore::unset(unsigned int& property_mask) const
{
	property_mask &= ~id;
}

IGL_INLINE void igl::opengl::ViewerCore::toggle(unsigned int& property_mask) const
{
	property_mask ^= id;
}

IGL_INLINE bool igl::opengl::ViewerCore::is_set(unsigned int property_mask) const
{
	return (property_mask & id);
}

IGL_INLINE igl::opengl::ViewerCore::ViewerCore()
{
	// Default colors
	background_color << 0.3f, 0.3f, 0.5f, 1.0f;

	// Default lights settings
	light_position << 0.0f, 0.3f, 0.0f;
	lighting_factor = 1.0f; //on

	// Default trackball
	trackball_angle = Eigen::Quaternionf::Identity();
	set_rotation_type(ViewerCore::ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP);

	// Camera parameters
	camera_base_zoom = 1.0f;
	camera_zoom = 1.0f;
	orthographic = false;
	camera_view_angle = 45.0;
	camera_dnear = 1.0;
	camera_dfar = 100.0;
	camera_base_translation << 0, 0, 0;
	camera_translation << 0, 0, 0;
	camera_eye << 0, 0, 5;
	camera_center << 0, 0, 0;
	camera_up << 0, 1, 0;

	depth_test = true;

	is_animating = false;
	animation_max_fps = 30.;

	viewport.setZero();
}

IGL_INLINE void igl::opengl::ViewerCore::init()
{
}

IGL_INLINE void igl::opengl::ViewerCore::shut()
{
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// 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)
//
// SHAPELIB example

// Shapelib is a well-known and often used library to read (and write) shapefiles by Frank Warmerdam

// To build and run this example:
// 1) download shapelib from http://shapelib.maptools.org/
// 2) extract and put the source "shpopen.cpp" in project or makefile
// 3) download a shapefile, for example world countries from http://aprsworld.net/gisdata/world
// Alternativelly, install Shapelib using OSGeo4W installer from http://trac.osgeo.org/osgeo4w/
// that provides Windows binary packages

#include "shapefil.h"

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

using namespace boost::geometry;

template <typename T, typename F>
void read_shapefile(const std::string& filename, std::vector<T>& polygons, F functor)
{
    try
    {
        SHPHandle handle = SHPOpen(filename.c_str(), "rb");
        if (handle <= 0)
        {
            throw std::string("File " + filename + " not found");
        }

        int nShapeType, nEntities;
        double adfMinBound[4], adfMaxBound[4];
        SHPGetInfo(handle, &nEntities, &nShapeType, adfMinBound, adfMaxBound );

        for (int i = 0; i < nEntities; i++)
        {
            SHPObject* psShape = SHPReadObject(handle, i );

            // Read only polygons, and only those without holes
            if (psShape->nSHPType == SHPT_POLYGON && psShape->nParts == 1)
            {
                T polygon;
                functor(psShape, polygon);
                polygons.push_back(polygon);
            }
            SHPDestroyObject( psShape );
        }
        SHPClose(handle);
    }
    catch(const std::string& s)
    {
        throw s;
    }
    catch(...)
    {
        throw std::string("Other exception");
    }
}

template <typename T>
void convert(SHPObject* psShape, T& polygon)
{
    double* x = psShape->padfX;
    double* y = psShape->padfY;
    for (int v = 0; v < psShape->nVertices; v++)
    {
        typename point_type<T>::type point;
        assign_values(point, x[v], y[v]);
        append(polygon, point);
    }
}

int main()
{
    std::string filename = "c:/data/spatial/shape/world_free/world.shp";

    typedef model::polygon<model::d2::point_xy<double> > polygon_2d;
    std::vector<polygon_2d> polygons;

    try
    {
        read_shapefile(filename, polygons, convert<polygon_2d>);
    }
    catch(const std::string& s)
    {
        std::cout << s << std::endl;
        return 1;
    }

    // Do something with the polygons, for example simplify them
    for (std::vector<polygon_2d>::iterator it = polygons.begin(); it != polygons.end(); it++)
    {
        polygon_2d p;
        simplify(*it, p, 0.01);
        std::cout << it->outer().size() << "," << p.outer().size() << std::endl;
        *it = p;
    }
    std::cout << "Simplified " << polygons.size() << std::endl;

    double sum = 0;
    for (std::vector<polygon_2d>::const_iterator it = polygons.begin(); it != polygons.end(); it++)
    {
        sum += area(*it);
    }
    std::cout << "Total area of " << polygons.size() << " polygons, total: " << sum << std::endl;

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::inverse_pdf::inverse_pdf.hp               //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  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_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_INVERSE_PDF_INVERSE_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_INVERSE_PDF_INVERSE_PDF_HPP_ER_2009
#include <boost/statistics/detail/distribution_common/meta/inherit/policy.hpp>
#include <boost/statistics/detail/distribution_common/meta/inherit/value.hpp>
#include <boost/statistics/detail/distribution_toolkit/meta/is_pseudo_scalar_distribution.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    // A pseudo distribution resulting from mapping the pdf of a distribution
    // to its inverse
    template<typename D>
    class inverse_pdf : public
        distribution::meta::inherit_policy<
            D,
            distribution::meta::inherit_value<D>
        >
    {

    public:
        typedef typename distribution::meta::value<D>::type value_type;

        inverse_pdf(){}
        explicit inverse_pdf(const D& d):d_(d){}
        inverse_pdf(const inverse_pdf& that):d_(that.d_){}
        inverse_pdf&
        operator=(const inverse_pdf& that){
            if(&that!=this){
                d_ = that.d_;
            }
            return *this;
        }

        const D&
        distribution()const{ return d_; }

    private:
        D d_;
    };

    template<typename D>
    inverse_pdf<D>
    make_inverse_pdf(const D& d){ return inverse_pdf<D>(d); }

}// distribution
}// toolkit

}// detail
}// statistics
}// boost

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/list/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/detail/minimal/list.hpp>

#include <boost/hana/integral.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto is = [](auto x) {
    return [=](auto y) { return equal(x, y); };
};

// take_until requires the predicate to return an Integral boolean, so we
// need the comparison to be purely compile-time.
template <int i>
constexpr auto x = int_<i>;

template <typename mcd>
void test() {
    BOOST_HANA_CONSTEXPR_LAMBDA auto list = detail::minimal::list<mcd>;
    constexpr auto z = x<999>;

    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list()) == list());

    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(x<1>)) == list(x<1>));
    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(z)) == list());

    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(x<1>, x<2>)) == list(x<1>, x<2>));
    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(x<1>, z)) == list(x<1>));
    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(z, x<2>)) == list());

    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(x<1>, x<2>, x<3>)) == list(x<1>, x<2>, x<3>));
    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(x<1>, x<2>, z)) == list(x<1>, x<2>));
    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(x<1>, z, x<3>)) == list(x<1>));
    BOOST_HANA_CONSTANT_ASSERT(take_until(is(z), list(z, x<2>, x<3>)) == list());
}

int main() {
    test<List::mcd<void>>();
    (void)is;
}


#ifndef TYPES_
#define TYPES_

#include <armadillo>

typedef arma::mat Matrix;
typedef arma::vec Vector;
typedef arma::rowvec RowVector;
typedef arma::umat UMatrix;
typedef arma::uvec UVector;

namespace arma {

Vector sigmoid(const Vector&);
RowVector sigmoid(const RowVector&);
Matrix sigmoid(const Matrix&);

}

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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
//==============================================================================
#include <boost/simd/bitwise/include/functions/twopower.hpp>
#include <boost/simd/sdk/simd/native.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

NT2_TEST_CASE_TPL ( twopower_ui,  BOOST_SIMD_SIMD_INTEGRAL_TYPES)
{
  using boost::simd::twopower;
  using boost::simd::tag::twopower_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<twopower_(vT)>::type r_t;

  // specific values tests
  for(uint32_t i=0; i < sizeof(T) ; i++)
  {
    NT2_TEST_EQUAL(twopower(boost::simd::splat<vT>(i)), boost::simd::splat<r_t>(T(1) << i));

  }
} // end of test for unsigned_int_


// Copyright 2015-2018 Hans Dembinski
//
// 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 BOOST_HISTOGRAM_ACCUMULATORS_WEIGHTED_SUM_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_WEIGHTED_SUM_HPP

#include <boost/histogram/fwd.hpp>
#include <type_traits>

namespace boost {
namespace histogram {
namespace accumulators {

/// Holds sum of weights and its variance estimate
template <typename RealType>
class weighted_sum {
public:
  weighted_sum() = default;
  explicit weighted_sum(const RealType& value) noexcept
      : sum_of_weights_(value), sum_of_weights_squared_(value) {}
  weighted_sum(const RealType& value, const RealType& variance) noexcept
      : sum_of_weights_(value), sum_of_weights_squared_(variance) {}

  /// Increment by one.
  weighted_sum& operator++() { return operator+=(1); }

  /// Increment by value.
  template <typename T>
  weighted_sum& operator+=(const T& value) {
    sum_of_weights_ += value;
    sum_of_weights_squared_ += value * value;
    return *this;
  }

  /// Added another weighted sum.
  template <typename T>
  weighted_sum& operator+=(const weighted_sum<T>& rhs) {
    sum_of_weights_ += static_cast<RealType>(rhs.sum_of_weights_);
    sum_of_weights_squared_ += static_cast<RealType>(rhs.sum_of_weights_squared_);
    return *this;
  }

  /// Scale by value.
  weighted_sum& operator*=(const RealType& x) {
    sum_of_weights_ *= x;
    sum_of_weights_squared_ *= x * x;
    return *this;
  }

  bool operator==(const RealType& rhs) const noexcept {
    return sum_of_weights_ == rhs && sum_of_weights_squared_ == rhs;
  }

  template <typename T>
  bool operator==(const weighted_sum<T>& rhs) const noexcept {
    return sum_of_weights_ == rhs.sum_of_weights_ &&
           sum_of_weights_squared_ == rhs.sum_of_weights_squared_;
  }

  template <typename T>
  bool operator!=(const T& rhs) const noexcept {
    return !operator==(rhs);
  }

  /// Return value of the sum.
  const RealType& value() const noexcept { return sum_of_weights_; }

  /// Return estimated variance of the sum.
  const RealType& variance() const noexcept { return sum_of_weights_squared_; }

  // lossy conversion must be explicit
  template <class T>
  explicit operator T() const {
    return static_cast<T>(sum_of_weights_);
  }

  template <class Archive>
  void serialize(Archive&, unsigned /* version */);

private:
  RealType sum_of_weights_ = RealType();
  RealType sum_of_weights_squared_ = RealType();
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED
namespace std {
template <class T, class U>
struct common_type<boost::histogram::accumulators::weighted_sum<T>,
                   boost::histogram::accumulators::weighted_sum<U>> {
  using type = boost::histogram::accumulators::weighted_sum<common_type_t<T, U>>;
};

template <class T, class U>
struct common_type<boost::histogram::accumulators::weighted_sum<T>, U> {
  using type = boost::histogram::accumulators::weighted_sum<common_type_t<T, U>>;
};

template <class T, class U>
struct common_type<T, boost::histogram::accumulators::weighted_sum<U>> {
  using type = boost::histogram::accumulators::weighted_sum<common_type_t<T, U>>;
};
} // namespace std
#endif

#endif


#include <stan/math/prim.hpp>
#include <test/unit/math/prim/fun/binary_scalar_tester.hpp>
#include <boost/math/special_functions/owens_t.hpp>
#include <gtest/gtest.h>
#include <cmath>
#include <limits>

TEST(MathFunctions, owens_t) {
  double a = 1.0;
  double b = 2.0;
  EXPECT_FLOAT_EQ(stan::math::owens_t(a, b), boost::math::owens_t(a, b));
}

TEST(MathFunctions, owens_t_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_TRUE(std::isnan(stan::math::owens_t(1.0, nan)));
  EXPECT_TRUE(std::isnan(stan::math::owens_t(nan, 2.0)));
  EXPECT_TRUE(std::isnan(stan::math::owens_t(nan, nan)));
}

TEST(MathFunctions, owens_t_vec) {
  auto f = [](const auto& x1, const auto& x2) {
    using stan::math::owens_t;
    return owens_t(x1, x2);
  };

  Eigen::VectorXd in1(3);
  in1 << 1.8, 3.24, 1.8;
  Eigen::VectorXd in2(3);
  in2 << 7.3, 0.7, 2.8;
  stan::test::binary_scalar_tester(f, in1, in2);
}


//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <boost/test/unit_test.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>

BOOST_AUTO_TEST_SUITE(test_strides)

using test_types = std::tuple<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

using extents       = boost::numeric::ublas::extents<>;
using first_order   = boost::numeric::ublas::layout::first_order;
using last_order    = boost::numeric::ublas::layout::last_order;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_strides_ctor, value, test_types)
{
  namespace ublas = boost::numeric::ublas;
  constexpr auto layout = value{};

  auto s1   = ublas::to_strides(extents    {1},layout);
  auto s5   = ublas::to_strides(extents    {5},layout);
  auto s11  = ublas::to_strides(extents  {1,1},layout);
  auto s12  = ublas::to_strides(extents  {1,2},layout);
  auto s21  = ublas::to_strides(extents  {2,1},layout);
  auto s23  = ublas::to_strides(extents  {2,3},layout);
  auto s231 = ublas::to_strides(extents{2,3,1},layout);
  auto s123 = ublas::to_strides(extents{1,2,3},layout);
  auto s423 = ublas::to_strides(extents{4,2,3},layout);

  BOOST_CHECK  (!  s1.empty());
  BOOST_CHECK  (!  s5.empty());
  BOOST_CHECK  (! s11.empty());
  BOOST_CHECK  (! s12.empty());
  BOOST_CHECK  (! s21.empty());
  BOOST_CHECK  (! s23.empty());
  BOOST_CHECK  (!s231.empty());
  BOOST_CHECK  (!s123.empty());
  BOOST_CHECK  (!s423.empty());

  BOOST_CHECK_EQUAL (   s1.size(), 1);
  BOOST_CHECK_EQUAL (   s5.size(), 1);
  BOOST_CHECK_EQUAL (  s11.size(), 2);
  BOOST_CHECK_EQUAL (  s12.size(), 2);
  BOOST_CHECK_EQUAL (  s21.size(), 2);
  BOOST_CHECK_EQUAL (  s23.size(), 2);
  BOOST_CHECK_EQUAL ( s231.size(), 3);
  BOOST_CHECK_EQUAL ( s123.size(), 3);
  BOOST_CHECK_EQUAL ( s423.size(), 3);
}

BOOST_AUTO_TEST_CASE( test_strides_ctor_access_first_order)
{
  namespace ublas = boost::numeric::ublas;
  constexpr auto layout = first_order{};

  auto s1   = ublas::to_strides(extents    {1},layout);
  auto s5   = ublas::to_strides(extents    {5},layout);
  auto s11  = ublas::to_strides(extents  {1,1},layout);
  auto s12  = ublas::to_strides(extents  {1,2},layout);
  auto s21  = ublas::to_strides(extents  {2,1},layout);
  auto s23  = ublas::to_strides(extents  {2,3},layout);
  auto s231 = ublas::to_strides(extents{2,3,1},layout);
  auto s123 = ublas::to_strides(extents{1,2,3},layout);
  auto s423 = ublas::to_strides(extents{4,2,3},layout);

  BOOST_REQUIRE_EQUAL ( s11 .size(),2);
  BOOST_REQUIRE_EQUAL ( s12 .size(),2);
  BOOST_REQUIRE_EQUAL ( s21 .size(),2);
  BOOST_REQUIRE_EQUAL ( s23 .size(),2);
  BOOST_REQUIRE_EQUAL ( s231.size(),3);
  BOOST_REQUIRE_EQUAL ( s123.size(),3);
  BOOST_REQUIRE_EQUAL ( s423.size(),3);

  BOOST_CHECK_EQUAL ( s11[0], 1);
  BOOST_CHECK_EQUAL ( s11[1], 1);

  BOOST_CHECK_EQUAL ( s12[0], 1);
  BOOST_CHECK_EQUAL ( s12[1], 1);

  BOOST_CHECK_EQUAL ( s21[0], 1);
  BOOST_CHECK_EQUAL ( s21[1], 1);

  BOOST_CHECK_EQUAL ( s23[0], 1);
  BOOST_CHECK_EQUAL ( s23[1], 2);

  BOOST_CHECK_EQUAL ( s231[0], 1);
  BOOST_CHECK_EQUAL ( s231[1], 2);
  BOOST_CHECK_EQUAL ( s231[2], 6);

  BOOST_CHECK_EQUAL ( s123[0], 1);
  BOOST_CHECK_EQUAL ( s123[1], 1);
  BOOST_CHECK_EQUAL ( s123[2], 2);

  BOOST_CHECK_EQUAL ( s423[0], 1);
  BOOST_CHECK_EQUAL ( s423[1], 4);
  BOOST_CHECK_EQUAL ( s423[2], 8);
}

BOOST_AUTO_TEST_CASE( test_strides_ctor_access_last_order)
{
  namespace ublas = boost::numeric::ublas;
  constexpr auto layout = last_order{};

  auto s1   = ublas::to_strides(extents    {1},layout);
  auto s5   = ublas::to_strides(extents    {5},layout);
  auto s11  = ublas::to_strides(extents  {1,1},layout);
  auto s12  = ublas::to_strides(extents  {1,2},layout);
  auto s21  = ublas::to_strides(extents  {2,1},layout);
  auto s23  = ublas::to_strides(extents  {2,3},layout);
  auto s231 = ublas::to_strides(extents{2,3,1},layout);
  auto s123 = ublas::to_strides(extents{1,2,3},layout);
  auto s423 = ublas::to_strides(extents{4,2,3},layout);

  BOOST_REQUIRE_EQUAL ( s11 .size(),2);
  BOOST_REQUIRE_EQUAL ( s12 .size(),2);
  BOOST_REQUIRE_EQUAL ( s21 .size(),2);
  BOOST_REQUIRE_EQUAL ( s23 .size(),2);
  BOOST_REQUIRE_EQUAL ( s231.size(),3);
  BOOST_REQUIRE_EQUAL ( s123.size(),3);
  BOOST_REQUIRE_EQUAL ( s423.size(),3);

  BOOST_CHECK_EQUAL  ( s11[0], 1);
  BOOST_CHECK_EQUAL  ( s11[1], 1);

  BOOST_CHECK_EQUAL   ( s12[0], 1);
  BOOST_CHECK_EQUAL   ( s12[1], 1);

  BOOST_CHECK_EQUAL   ( s21[0], 1);
  BOOST_CHECK_EQUAL   ( s21[1], 1);

  BOOST_CHECK_EQUAL   ( s23[0], 3);
  BOOST_CHECK_EQUAL   ( s23[1], 1);

  BOOST_CHECK_EQUAL   ( s231[0], 3);
  BOOST_CHECK_EQUAL   ( s231[1], 1);
  BOOST_CHECK_EQUAL   ( s231[2], 1);

  BOOST_CHECK_EQUAL   ( s123[0], 6);
  BOOST_CHECK_EQUAL   ( s123[1], 3);
  BOOST_CHECK_EQUAL   ( s123[2], 1);

  BOOST_CHECK_EQUAL   ( s423[0], 6);
  BOOST_CHECK_EQUAL   ( s423[1], 3);
  BOOST_CHECK_EQUAL   ( s423[2], 1);
}

BOOST_AUTO_TEST_SUITE_END()


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

#ifndef NETKET_SR_HPP
#define NETKET_SR_HPP

#include <complex>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <nonstd/optional.hpp>

#include "Utils/exceptions.hpp"
#include "Utils/messages.hpp"
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "common_types.hpp"
#include "matrix_replacement.hpp"

namespace netket {

/**
 * Stochastic reconfiguration solver.
 */
class SR {
 public:
  using OkRef = Eigen::Ref<const RowMatrixXcd>;
  using GradRef = Eigen::Ref<const VectorXcd>;
  using OutputRef = Eigen::Ref<VectorXcd>;

  enum LSQSolver { LLT = 0, LDLT = 1, ColPivHouseholder = 2, BDCSVD = 3 };

  static nonstd::optional<LSQSolver> SolverFromString(const std::string& name);
  static const char* SolverAsString(LSQSolver solver);

  explicit SR(LSQSolver solver, double diagshift = 0.01,
              bool use_iterative = false, bool is_holomorphic = true,
              nonstd::optional<double> svd_threshold = nonstd::nullopt)
      : solver_(solver),
        sr_diag_shift_(diagshift),
        use_iterative_(use_iterative),
        is_holomorphic_(is_holomorphic),
        svd_threshold_(svd_threshold) {
    if (svd_threshold.has_value()) {
      if (use_iterative || solver != BDCSVD) {
        throw InvalidInputError{
            "svd_threshold option only available for BDCSVD solver"};
      }
    }
  }

  explicit SR(double diagshift = 0.01, bool use_iterative = false,
              bool use_cholesky = true, bool is_holomorphic = true)
      __attribute__((deprecated))
      : SR(use_cholesky ? LLT : ColPivHouseholder, diagshift, use_iterative,
           is_holomorphic) {}

  /**
   * Solves the SR flow equation for the parameter update ẋ.
   *
   * The SR update is computed by solving the linear euqation
   *    Sẋ = f
   * where S is the covariance matrix of the partial derivatives
   * O_i(v_j) = ∂/∂x_i log Ψ(v_j) and f is a generalized force (the loss
   * gradient).
   *
   * @param Oks The matrix 𝕆 of centered log-derivatives,
   *    𝕆_ij = O_i(v_j) - ⟨O_i⟩.
   * @param grad The loss gradient f.
   * @param deltaP Output parameter for the update ẋ.
   */
  void ComputeUpdate(OkRef Oks, GradRef grad, OutputRef deltaP);

  void SetParameters(LSQSolver solver, double diagshift = 0.01,
                     bool use_iterative = false, bool is_holomorphic = true);
  void SetParameters(double diagshift = 0.01, bool use_iterative = false,
                     bool use_cholesky = true, bool is_holomorphic = true)
      __attribute__((deprecated));

  /**
   * Returns a string describing the current parameters of the SR class.
   */
  std::string LongDesc(Index depth = 0) const;
  std::string ShortDesc() const;

  /**
   * Becca and Sorella (2017), pp. 143-144.
   */
  void SetScaleInvariantRegularization(bool enabled) {
    if (use_iterative_ && enabled) {
      // TODO: implement
      throw std::runtime_error{
          "Scale-invariant regularization is not implemented "
          "for iterative solvers at the moment."};
    }
    if (enabled) {
      InfoMessage() << "Using scale-invariant preconditioning." << std::endl;
    }
    scale_invariant_pc_ = enabled;
  }
  bool ScaleInvariantRegularizationEnabled() const {
    return scale_invariant_pc_;
  }

  /**
   * Returns the rank of the S matrix computed during the last call to
   * `ComputeUpdate` or `nullopt`, in case storing the rank is not enabled
   * and before the first call to `ComputeUpdate`.
   *
   * Storing the rank is enabled and disabled by `SetStoreRank` below.
   */
  nonstd::optional<Index> LastRank() { return last_rank_; }
  bool StoreRankEnabled() const { return store_rank_; }
  void SetStoreRank(bool enabled);

  /**
   * Returns the S matrix computed during the last call to
   * `ComputeUpdate` or `nullopt`, in case storing the S matrix is not enabled
   * and before the first call to `ComputeUpdate`.
   *
   * Storing the S matrix is enabled and disabled by `SetStoreFullSMatrix`
   * below.
   */
  const nonstd::optional<MatrixXcd>& LastSMatrix() const { return last_S_; }
  bool StoreFullSMatrixEnabled() const { return store_full_S_matrix_; }
  void SetStoreFullSMatrix(bool enabled);

  /**
   * Returns the is_holomorphic parameter
   * @return
   */
  bool IsHolomorphic() { return is_holomorphic_; };
  void SetIsHolomorphic(bool is_holomorphic) {is_holomorphic_ = is_holomorphic;};

 private:
  LSQSolver solver_;
  double sr_diag_shift_;
  bool use_iterative_;
  bool is_holomorphic_;
  nonstd::optional<double> svd_threshold_;

  bool scale_invariant_pc_ = false;
  VectorXd diag_S_;

  Eigen::MatrixXd Sreal_;
  Eigen::MatrixXcd Scomplex_;

  bool store_rank_ = false;
  bool store_full_S_matrix_ = false;

  nonstd::optional<Index> last_rank_;
  nonstd::optional<MatrixXcd> last_S_;

  template <class Mat>
  void BuildSMatrix(Eigen::Ref<const Mat> S_local, Mat& S_out, double nsamp) {
    static_assert(std::is_same<Mat, MatrixXd>::value ||
                      std::is_same<Mat, MatrixXcd>::value,
                  "S_out must be of type MatrixXd or MatrixXcd");

    S_out = S_local;
    SumOnNodes(S_out);
    S_out /= nsamp;
  }

  template <class Mat, class Vec>
  void ApplyPreconditioning(Mat& S, Vec& grad) {
    if (scale_invariant_pc_) {
      // Even if S is complex, its diagonal elements should be real since it
      // is Hermitian.
      diag_S_ = S.diagonal().real().cwiseSqrt();

      static const double CUTOFF = 1e-10;
      for (Index i = 0; i < diag_S_.rows(); i++) {
        if (diag_S_(i) <= CUTOFF) {
          diag_S_(i) = 1.0;
          S.col(i).setZero();
          S.row(i).setZero();
          S(i, i) = 1.0;
        }
      }

      S.array() /= (diag_S_ * diag_S_.transpose()).array();
      grad.array() /= diag_S_.array();
    }
    // Apply diagonal shift
    S.diagonal().array() += sr_diag_shift_;
  }

  void RevertPreconditioning(OutputRef solution) {
    if (scale_invariant_pc_) {
      solution.array() /= diag_S_.array();
    }
  }

  template <class Vec>
  void SolveIterative(OkRef Oks, Eigen::Ref<const Vec> grad, OutputRef deltaP,
                      double nsamp) {
    // Use SrMatrixReal iff Vec is a real vector, else SrMatrixComplex
    using IsReal = std::is_same<typename Vec::Scalar, double>;
    using SrMatrixType = typename std::conditional<IsReal::value, SrMatrixReal,
                                                   SrMatrixComplex>::type;
    // Make sure this code does not compile by accident if grad is a different
    // type
    static_assert(std::is_same<typename Vec::Scalar, double>::value ||
                      std::is_same<typename Vec::Scalar, Complex>::value,
                  "grad must be a real or complex vector");

    SrMatrixType S;
    S.attachMatrix(Oks);
    S.setShift(sr_diag_shift_);
    S.setScale(1. / nsamp);

    using SolverType =
        Eigen::ConjugateGradient<SrMatrixType, Eigen::Lower | Eigen::Upper,
                                 Eigen::IdentityPreconditioner>;
    SolverType it_solver(S);
    it_solver.setTolerance(1.0e-3);
    deltaP = it_solver.solve(grad);
  }

  template <class Mat, class Vec, class Out>
  void SolveLeastSquares(Mat& A, Eigen::Ref<const Vec> b, Out&& deltaP) {
    if (store_full_S_matrix_) {
      last_S_.emplace(A);
    }

    if (solver_ == LLT) {
      Eigen::LLT<Mat> llt(A);
      deltaP = llt.solve(b);
    } else if (solver_ == LDLT) {
      Eigen::LDLT<Mat> ldlt(A);
      deltaP = ldlt.solve(b);
    } else if (solver_ == ColPivHouseholder) {
      Eigen::ColPivHouseholderQR<Mat> qr(A);
      deltaP = qr.solve(A);
      if (store_rank_) {
        last_rank_ = qr.rank();
      }
    } else if (solver_ == BDCSVD) {
      const constexpr auto options = Eigen::ComputeThinU | Eigen::ComputeThinV;
      Eigen::BDCSVD<Mat> bdcsvd(A.rows(), A.cols(), options);
      if (svd_threshold_.has_value()) {
        bdcsvd.setThreshold(*svd_threshold_);
      }
      bdcsvd.compute(A);
      deltaP = bdcsvd.solve(b);
      if (store_rank_) {
        last_rank_ = bdcsvd.rank();
      }
    } else {
      throw std::runtime_error{
          "Unknown LSQSolver enum value in SR. This should never happen."};
    }
  }

  static void CheckSolverCompatibility(bool use_iterative, LSQSolver solver,
                                       bool rank_enabled) {
    if (!rank_enabled) {
      return;
    }
    if (use_iterative) {
      throw std::logic_error{
          "SR cannot store matrix rank with interactive solver."};
    }
    if (solver == LLT || solver == LDLT) {
      std::stringstream str;
      str << "SR cannot store matrix rank: Solver " << SolverAsString(solver)
          << " is not rank-revealing.";
      throw std::logic_error{str.str()};
    }
  }
};

}  // namespace netket

#endif


/**
 * @file searches.cc
 *
 * Implement the BGL dfs and bfs wrappers.
 */

/*
 * David Gleich
 * 19 April 2006
 */

/*
 * 18 April 2007
 * Added src/dst pairs to all algorithms to stop searches early.
 * Corrected small documentation bugs.
 *
 * 9 July 2007
 * Switched to simple_csr_matrix graph type
 *
 * 29 August 2007
 * Switched to make_iterator_property_map in astar_search* functions
 * to fix compile bugs on g++-4.1
 */

#include "include/matlab_bgl.h"

#include <yasmic/simple_csr_matrix_as_graph.hpp>
#include <yasmic/iterator_utility.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/astar_search.hpp>
#include <utility>

#include "visitor_macros.hpp"
#include "stop_visitors.hpp"

struct stop_bfs {}; // stop bfs exception

/**
 * Wrap a boost graph library call to bfs.
 *
 * the ja and ia arrays specify the connectivity of the underlying graph,
 * ia is a length (nverts+1) array with the indices in ja that start the
 * nonzeros in each row.  ja is a length (ia(nverts)) array with the
 * columns of the connectivity.
 *
 * if d, dt, or pred is NULL, then that parameter is not computed.
 *   (currently not implemented)
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param src the source vertex for the search
 * @param dst a target vertex unless src=dst
 * @param d the distance array
 * @param dt the discover time array
 * @param pred the predecessor array
 * @return an error code if possible
 */
int breadth_first_search(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex src, mbglIndex dst, /* problem data */
    int* d, int* dt, mbglIndex* pred /* output data: distance, discover time, predecessor */
    )
{
    using namespace yasmic;
    using namespace boost;
    using namespace std;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    if (!d || !dt || !pred)
    {
        return (-1);
    }

    // set the predecessor to itself
    for (mbglIndex i = 0; i < nverts; i++)
    {
        pred[i] = i;
    }

    int time = 0;

    if (dst == nverts)
    {
        // this case means we don't stop the search
        boost::breadth_first_search(g, src,
                    boost::visitor(make_bfs_visitor(
                        make_pair(record_distances(d, on_tree_edge()),
                        make_pair(stamp_times(dt, time, on_discover_vertex()),
                                  record_predecessors(pred, on_tree_edge()))))));
    }
    else
    {
        // this case means we do stop the search
        try {
            boost::breadth_first_search(g, src,
                        boost::visitor(make_bfs_visitor(
                            make_pair(record_distances(d, on_tree_edge()),
                            make_pair(stamp_times(dt, time, on_discover_vertex()),
                            make_pair(record_predecessors(pred, on_tree_edge()),
                                stop_search_on_vertex_target(dst, stop_bfs(), on_discover_vertex())))))));
        } catch (stop_bfs) {}
    }

    return (0);
}

template <class Graph>
struct c_bfs_visitor
{
    bfs_visitor_funcs_t *vis;

    VISITOR_VERTEX_FUNC(initialize_vertex, stop_bfs)
    VISITOR_VERTEX_FUNC(examine_vertex, stop_bfs)
    VISITOR_VERTEX_FUNC(discover_vertex, stop_bfs)
    VISITOR_VERTEX_FUNC(finish_vertex, stop_bfs)

    VISITOR_EDGE_FUNC(examine_edge, stop_bfs)
    VISITOR_EDGE_FUNC(tree_edge, stop_bfs)
    VISITOR_EDGE_FUNC(non_tree_edge, stop_bfs)
    VISITOR_EDGE_FUNC(gray_target, stop_bfs)
    VISITOR_EDGE_FUNC(black_target, stop_bfs)
};

int breadth_first_search_visitor(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex src, /* problem data */
    bfs_visitor_funcs_t vis /* visitor */
    )
{
    /*
     * History
     *
     * 2007-04-17
     * Added try-catch for stop_bfs exception
     */
    using namespace yasmic;
    using namespace boost;
    using namespace std;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    c_bfs_visitor<crs_graph> visitor;
    visitor.vis = &vis;

    try {
        boost::breadth_first_search(g, src,
            boost::visitor(visitor));
    }
    catch (stop_bfs)
    {
    }

    return (0);
}

struct stop_dfs {}; // stop dfs exception

/**
 * Wrap a boost graph library call to dfs.
 *
 * the ja and ia arrays specify the connectivity of the underlying graph,
 * ia is a length (nverts+1) array with the indices in ja that start the
 * nonzeros in each row.  ja is a length (ia(nverts)) array with the
 * columns of the connectivity.
 *
 * if dt, ft, or pred is NULL, then that parameter is not computed.
 *   (currently not implemented)
 *
 * @param nverts the number of vertices in the graph
 * @param ja the connectivity for each vertex
 * @param ia the row connectivity points into ja
 * @param src the source vertex for the flow
 * @param full if full is non-zero, then compute the full dfs over all
 *    vertices, not just the connected component.
 * @param dst the target vertex if n < nverts and src != dst which
 *    stops the dfs if it is reached
 * @param d
 * @param dt the discover time array
 * @param ft the finish time array
 * @param pred the predecessor array
 * @return an error code if possible
 */
int depth_first_search(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex src, int full, mbglIndex dst, /* problem data */
    int* d, int* dt, int *ft, mbglIndex *pred /* output data: discover time, finish time, predecessor */
    )
{
    using namespace yasmic;
    using namespace boost;
    using namespace std;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    if (!d || !dt || !ft || !pred)
    {
        return (-1);
    }

    // set the predecessor to itself
    for (mbglIndex i = 0; i < nverts; i++)
    {
        pred[i] = i;
    }

    int time=0;

    if (!full)
    {
        std::vector<default_color_type> color_vec(num_vertices(g));
        default_color_type c = white_color; // avoid warning about un-init

        //
        // call visit because they don't want the full dfs
        //

        if (dst == nverts) {
            // they don't want to stop early
            boost::depth_first_visit(g, src,
                        make_dfs_visitor(
                            make_pair(record_distances(d, on_tree_edge()),
                            make_pair(stamp_times(dt, time, on_discover_vertex()),
                            make_pair(stamp_times(ft, time, on_finish_vertex()),
                                      record_predecessors(pred, on_tree_edge()))))),
                make_iterator_property_map(color_vec.begin(),
                    get(vertex_index,g)));
        } else {
            // they want to try and stop early
            try {
                boost::depth_first_visit(g, src,
                            make_dfs_visitor(
                                make_pair(record_distances(d, on_tree_edge()),
                                make_pair(stamp_times(dt, time, on_discover_vertex()),
                                make_pair(stamp_times(ft, time, on_finish_vertex()),
                                make_pair(record_predecessors(pred, on_tree_edge()),
                                    stop_search_on_vertex_target(dst, stop_dfs(), on_discover_vertex())))))),
                    make_iterator_property_map(color_vec.begin(),
                        get(vertex_index,g)));
            } catch (stop_dfs) {}
        }
    }
    else
    {
        // call search because they want the full dfs
        boost::depth_first_search(g,
                    boost::visitor(make_dfs_visitor(
                        make_pair(record_distances(d, on_tree_edge()),
                        make_pair(stamp_times(dt, time, on_discover_vertex()),
                                  record_predecessors(pred, on_tree_edge()))))));
    }
    return (0);
}

template <class Graph>
struct c_dfs_visitor
{
    dfs_visitor_funcs_t *vis;

    VISITOR_VERTEX_FUNC(initialize_vertex, stop_dfs)
    VISITOR_VERTEX_FUNC(start_vertex, stop_dfs)
    VISITOR_VERTEX_FUNC(discover_vertex, stop_dfs)
    VISITOR_VERTEX_FUNC(finish_vertex, stop_dfs)

    VISITOR_EDGE_FUNC(examine_edge, stop_dfs)
    VISITOR_EDGE_FUNC(tree_edge, stop_dfs)
    VISITOR_EDGE_FUNC(back_edge, stop_dfs)
    VISITOR_EDGE_FUNC(forward_or_cross_edge, stop_dfs)
};

int depth_first_search_visitor(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, /* connectivity params */
    mbglIndex src, int full, /* problem data */
    dfs_visitor_funcs_t vis)
{
    using namespace yasmic;
    using namespace boost;
    using namespace std;

    typedef simple_csr_matrix<mbglIndex,double> crs_graph;
    crs_graph g(nverts, nverts, ia[nverts], ia, ja, NULL);

    c_dfs_visitor<crs_graph> visitor;
    visitor.vis = &vis;

    try
    {
        if (!full)
        {
            std::vector<default_color_type> color_vec(num_vertices(g));
            default_color_type c = white_color; // avoid warning about un-init

            // call visit because they don't want the full dfs
            boost::depth_first_visit(g, src,
                visitor,
                make_iterator_property_map(color_vec.begin(),
                    get(vertex_index,g)));
        }
        else
        {
            // call search because they want the full dfs
            boost::depth_first_search(g,
                        boost::visitor(visitor)
                        );
        }
    }
    catch (stop_dfs)
    {
    }

    return (0);
}

struct stop_astar {}; // stop astar exception

template <class Graph>
struct c_astar_visitor
{
    astar_visitor_funcs_t *vis;

    VISITOR_VERTEX_FUNC(initialize_vertex, stop_astar)
    VISITOR_VERTEX_FUNC(examine_vertex, stop_astar)
    VISITOR_VERTEX_FUNC(discover_vertex, stop_astar)
    VISITOR_VERTEX_FUNC(finish_vertex, stop_astar)

    VISITOR_EDGE_FUNC(examine_edge, stop_astar)
    VISITOR_EDGE_FUNC(edge_relaxed, stop_astar)
    VISITOR_EDGE_FUNC(edge_not_relaxed, stop_astar)
    VISITOR_EDGE_FUNC(black_target, stop_astar)
};

template <class Graph, class CostType>
class astar_heuristic_data : public std::unary_function<
    typename boost::graph_traits<Graph>::vertex_descriptor, CostType>
{
private:
    CostType *_data;

public:
    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    astar_heuristic_data(CostType *data) : _data(data) {}
    CostType operator()(Vertex u) { return _data[u]; }
};

template <class Graph, class CostType>
class astar_heuristic_func : public std::unary_function<
    typename boost::graph_traits<Graph>::vertex_descriptor, CostType>
{
private:
    void *_pdata;
    CostType (*_func)(void *pdata, mbglIndex u);

public:
    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
    astar_heuristic_func(CostType (*hfunc)(void* pdata, mbglIndex u), void* pdata) : _func(hfunc), _pdata(pdata) {}
    CostType operator()(Vertex u) { return _func(_pdata, u); }
};

int astar_search(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src, mbglIndex dst, /* start, end vertex */
    double *d, mbglIndex *pred, double *f, /* output */
    double *h /* heuristic function value for all vertices */, double dinf)
{
    using namespace yasmic;
    using namespace boost;

    // create the graph g
    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    astar_heuristic_data<crs_weighted_graph,double> hdata(h);

    if (dst == nverts)
    {
        astar_search(g, src, hdata,
            distance_inf(dinf).
            predecessor_map(make_iterator_property_map(pred, get(vertex_index,g))).
            rank_map(make_iterator_property_map(f, get(vertex_index,g))).
            distance_map(make_iterator_property_map(d, get(vertex_index,g))));
    } else {
        try {
            astar_search(g, src, hdata,
                distance_inf(dinf).
                predecessor_map(make_iterator_property_map(pred, get(vertex_index,g))).
                rank_map(make_iterator_property_map(f, get(vertex_index,g))).
                distance_map(make_iterator_property_map(d, get(vertex_index,g))).
                visitor(make_astar_visitor(
                    stop_search_on_vertex_target(dst, stop_astar(), on_discover_vertex()))));
        } catch (stop_astar) {}
    }

    return (0);
}

int astar_search_hfunc(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src, mbglIndex dst, /* start vertex */
    double *d, mbglIndex *pred, double *f,
    double (*hfunc)(void* pdata, mbglIndex u) /* heuristic function */, void* pdata, double dinf)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    astar_heuristic_func<crs_weighted_graph,double> h(hfunc, pdata);

    if (dst == nverts)
    {
        astar_search(g, src, h,
            distance_inf(dinf).
            predecessor_map(make_iterator_property_map(pred, get(vertex_index,g))).
            rank_map(make_iterator_property_map(f, get(vertex_index,g))).
            distance_map(make_iterator_property_map(d, get(vertex_index,g))));
    } else {
        try {
            astar_search(g, src, h,
                distance_inf(dinf).
                predecessor_map(make_iterator_property_map(pred, get(vertex_index,g))).
                rank_map(make_iterator_property_map(f, get(vertex_index,g))).
                distance_map(make_iterator_property_map(d, get(vertex_index,g))).
                visitor(make_astar_visitor(
                    stop_search_on_vertex_target(dst, stop_astar(), on_discover_vertex()))));
        } catch (stop_astar) {}
    }

    return (0);
}

int astar_search_hfunc_visitor(
    mbglIndex nverts, mbglIndex *ja, mbglIndex *ia, double *weight, /* connectivity params */
    mbglIndex src, /* start vertex */
    double *d, mbglIndex *pred, double *f,
    double (*hfunc)(void* pdata, mbglIndex u) /* heuristic function */, void* pdata, double dinf,
    astar_visitor_funcs_t vis)
{
    using namespace yasmic;
    using namespace boost;

    typedef simple_csr_matrix<mbglIndex,double> crs_weighted_graph;
    crs_weighted_graph g(nverts, nverts, ia[nverts], ia, ja, weight);

    astar_heuristic_func<crs_weighted_graph,double> h(hfunc, pdata);

    c_astar_visitor<crs_weighted_graph> visitor_impl;
    visitor_impl.vis = &vis;

    try
    {
        astar_search(g, src, h,
            distance_inf(dinf).
            predecessor_map(make_iterator_property_map(pred, get(vertex_index,g))).
            rank_map(make_iterator_property_map(f, get(vertex_index,g))).
            distance_map(make_iterator_property_map(d, get(vertex_index,g))).
            visitor(visitor_impl));
    }
    catch (stop_astar)
    {
    }

    return (0);
}


#pragma once

#include <ilqr/ilqr_taylor_expansions.hh>
#include <ilqr/ilqr_tree.hh>

#include <Eigen/Dense>

#include <vector>

namespace ilqr
{

// Constructs a normal iLQR chain structure using the iLQRTree class.
std::vector<ilqr::TreeNodePtr> construct_chain(
        const std::vector<Eigen::VectorXd>& xstars,
        const std::vector<Eigen::VectorXd>& ustars,
        const ilqr::DynamicsFunc &dyn,
        const ilqr::CostFunc &cost,
        ilqr::iLQRTree& ilqr_tree);

// Extracts the forward pass states, controls, and computes the costs
// from a chain structure (vector of tree nodes).
void get_forward_pass_info(const std::vector<ilqr::TreeNodePtr> &chain,
                           const ilqr::CostFunc &cost,
                           std::vector<Eigen::VectorXd> &states,
                           std::vector<Eigen::VectorXd> &controls,
                           std::vector<double> &costs);

// Construct an iLQRTree that splits with b-number of dynamics function branches at the first
// time step. The probabilities and dynamics function std::vectors are both size b.
// The time steps in the tree is the size of the std::vectors xstars and ustars.
void construct_hindsight_split_tree(const std::vector<Eigen::VectorXd>& xstars,
        const std::vector<Eigen::VectorXd>& ustars,
        const std::vector<double> &probabilities,
        const std::vector<ilqr::DynamicsFunc> &dynamics_funcs,
        const ilqr::CostFunc &cost,
        ilqr::iLQRTree& ilqr_tree);

// Construct an iLQRTree that splits with b-number of cost function branches at the first
// time step. The probabilities and cost function std::vectors are both size b.
// The time steps in the tree is the size of the std::vectors xstars and ustars.
void construct_hindsight_split_tree(const std::vector<Eigen::VectorXd>& xstars,
        const std::vector<Eigen::VectorXd>& ustars,
        const std::vector<double> &probabilities,
        const ilqr::DynamicsFunc &dynamics_func,
        const std::vector<ilqr::CostFunc> &cost_funcs,
        ilqr::iLQRTree& ilqr_tree);

} // namespace ilqr


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014-2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_linear_linear
#endif

#include <boost/test/included/unit_test.hpp>

#include "test_distance_common.hpp"

typedef bg::model::point<double,2,bg::cs::cartesian>  point_type;
typedef bg::model::segment<point_type>                segment_type;
typedef bg::model::linestring<point_type>             linestring_type;
typedef bg::model::multi_linestring<linestring_type>  multi_linestring_type;

namespace services = bg::strategy::distance::services;
typedef bg::default_distance_result<point_type>::type return_type;

typedef bg::strategy::distance::projected_point<> point_segment_strategy;

//===========================================================================

template <typename Strategy>
void test_distance_segment_segment(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "segment/segment distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<segment_type, segment_type> tester;

    tester::apply("segment(0 0,10 0)",
                  "segment(4 2,4 0.5)",
                  return_type(0.5), return_type(0.25), strategy);

    tester::apply("segment(0 0,10 0)",
                  "segment(4 2,4 -0.5)",
                  return_type(0), return_type(0), strategy);

    tester::apply("segment(0 0,10 0)",
                  "segment(4 2,0 0)",
                  return_type(0), return_type(0), strategy);

    tester::apply("segment(0 0,10 0)",
                  "segment(-2 3,1 2)",
                  return_type(2), return_type(4), strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_segment_linestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "segment/linestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<segment_type, linestring_type> tester;

    tester::apply("segment(-1 -1,-2 -2)",
                  "linestring(2 1,1 2,4 0)",
                  sqrt(12.5), 12.5, strategy);

    tester::apply("segment(1 1,2 2)",
                  "linestring(2 1,1 2,4 0)",
                  0, 0, strategy);

    tester::apply("segment(1 1,2 2)",
                  "linestring(3 3,3 3)",
                  sqrt(2.0), 2, strategy);

    tester::apply("segment(1 1,2 2)",
                  "linestring(3 3)",
                  sqrt(2.0), 2, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_linestring_linestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "linestring/linestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            linestring_type, linestring_type
        > tester;

    // It is not obvious that linestrings with only one point are valid
    tester::apply("linestring(1 1)", "linestring(2 1)",
                  1, 1, strategy);

    tester::apply("linestring(1 1,3 1)", "linestring(2 1)",
                  0, 0, strategy);

    tester::apply("linestring(1 1)", "linestring(0 0,-2 0,2 -2,2 0)",
                  sqrt(2.0), 2, strategy);

    tester::apply("linestring(1 1,1 1)", "linestring(2 1,2 1)",
                  1, 1, strategy);

    tester::apply("linestring(1 1,1 1,1 1)", "linestring(2 1,2 1,2 1,2 1)",
                  1, 1, strategy);

    tester::apply("linestring(1 1,3 1)", "linestring(2 1,2 1)",
                  0, 0, strategy);

    tester::apply("linestring(1 1,1 1)", "linestring(0 0,-2 0,2 -2,2 0)",
                  sqrt(2.0), 2, strategy);

    tester::apply("linestring(1 1,3 1)", "linestring(2 1, 4 1)",
                  0, 0, strategy);

    tester::apply("linestring(1 1,2 2,3 3)", "linestring(2 1,1 2,4 0)",
                  0, 0, strategy);

    tester::apply("linestring(1 1,2 2,3 3)", "linestring(1 0,2 -1,4 0)",
                  1, 1, strategy);

    tester::apply("linestring(1 1,2 2,3 3)",
                  "linestring(1 -10,2 0,2.1 -10,4 0)",
                  sqrt(2.0), 2, strategy);

    tester::apply("linestring(1 1,2 2,3 3)",
                  "linestring(1 -10,2 1.9,2.1 -10,4 0)",
                  sqrt(0.005), 0.005, strategy);

    tester::apply("linestring(1 1,1 2)", "linestring(0 0,-2 0,2 -2,2 0)",
                  sqrt(2.0), 2, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_segment_multilinestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "segment/multilinestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            segment_type, multi_linestring_type
        > tester;

    tester::apply("segment(-1 -1,-2 -2)",
                  "multilinestring((2 1,1 2),(4 0,4 10))",
                  sqrt(12.5), 12.5, strategy);

    tester::apply("segment(1 1,2 2)",
                  "multilinestring((2 1,1 2),(4 0,4 10))",
                  0, 0, strategy);

    tester::apply("segment(1 1,2 2)",
                  "multilinestring((2.5 0,4 0,5 0),(3 3,3 3))",
                   sqrt(2.0), 2, strategy);

    tester::apply("segment(1 1,2 2)",
                  "multilinestring((2.5 0),(3 3,3 3))",
                   sqrt(2.0), 2, strategy);

    tester::apply("segment(1 1,2 2)",
                  "multilinestring((2.5 0,4 0,5 0),(3 3))",
                   sqrt(2.0), 2, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_linestring_multilinestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "linestring/multilinestring distance tests" << std::endl;
#endif

    typedef test_distance_of_geometries
        <
            linestring_type, multi_linestring_type
        > tester;

    tester::apply("linestring(1 1,2 2,3 3)",
                  "multilinestring((2 1,1 2,4 0),(1 -10,2 1.9,2.1 -10,4 0))",
                  0, 0, strategy);

    tester::apply("linestring(1 1,2 2,3 3)",
                  "multilinestring((1 -10,2 0,2.1 -10,4 0),(1 -10,2 1.9,2.1 -10,4 0))",
                  sqrt(0.005), 0.005, strategy);

    tester::apply("linestring(1 1,2 2)",
                  "multilinestring((2.5 0,4 0,5 0),(3 3,3 3))",
                   sqrt(2.0), 2, strategy);

    tester::apply("linestring(2 2)",
                  "multilinestring((2.5 0,4 0,5 0),(3 3,3 3))",
                   sqrt(2.0), 2, strategy);

    tester::apply("linestring(1 1,2 2)",
                  "multilinestring((2.5 0,4 0,5 0),(3 3))",
                   sqrt(2.0), 2, strategy);

    tester::apply("linestring(1 1,2 2,3 3,4 4,5 5,6 6,7 7,8 8,9 9)",
                  "multilinestring((2.5 0,4 0,5 0),(10 10,10 10))",
                   sqrt(2.0), 2, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multilinestring_multilinestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multilinestring/multilinestring distance tests" << std::endl;
#endif

    typedef test_distance_of_geometries
        <
            multi_linestring_type, multi_linestring_type
        > tester;

    tester::apply("multilinestring((0 0,0 1,1 1),(10 0,11 1,12 2))",
                  "multilinestring((0.5 0.5,0.75 0.75),(11 0,11 7))",
                  0, 0, strategy);

    tester::apply("multilinestring((0 0,0 1,1 1),(10 0,11 1,12 2))",
                  "multilinestring((0.5 0.5,0.75 0.75),(11 0,11 0.9))",
                  sqrt(0.005), 0.005, strategy);

    tester::apply("multilinestring((0 0,0 1,1 1),(10 0,11 1,12 2))",
                  "multilinestring((0.5 0.5,0.75 0.75),(11.1 0,11.1 0.9))",
                  sqrt(0.02), 0.02, strategy);

    tester::apply("multilinestring((0 0),(1 1))",
                  "multilinestring((2 2),(3 3))",
                  sqrt(2.0), 2, strategy);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_more_empty_input_linear_linear(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "testing on empty inputs... " << std::flush;
#endif
    bg::model::linestring<Point> line_empty;
    bg::model::multi_linestring<bg::model::linestring<Point> > multiline_empty;

    bg::model::linestring<Point> line =
        from_wkt<bg::model::linestring<Point> >("linestring(0 0,1 1)");

    // 1st geometry is empty
    test_empty_input(line_empty, line, strategy);
    test_empty_input(multiline_empty, line, strategy);

    // 2nd geometry is empty
    test_empty_input(line, line_empty, strategy);
    test_empty_input(line, multiline_empty, strategy);

    // both geometries are empty
    test_empty_input(line_empty, line_empty, strategy);
    test_empty_input(line_empty, multiline_empty, strategy);
    test_empty_input(multiline_empty, multiline_empty, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << "done!" << std::endl;
#endif
}

//===========================================================================

BOOST_AUTO_TEST_CASE( test_all_segment_segment )
{
    test_distance_segment_segment(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_segment_linestring )
{
    test_distance_segment_linestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_linestring_linestring )
{
    test_distance_linestring_linestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_segment_multilinestring )
{
    test_distance_segment_multilinestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_linestring_multilinestring )
{
    test_distance_linestring_multilinestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_multilinestring_multilinestring )
{
    test_distance_multilinestring_multilinestring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_empty_input_linear_linear )
{
    test_more_empty_input_linear_linear<point_type>(point_segment_strategy());
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_UTIL_ALGORITHM_HPP
#define BOOST_GEOMETRY_UTIL_ALGORITHM_HPP

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/util/type_traits_std.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

// Other implementations can be found in the history of this file.
// The discussion and benchmarks can be found here:
// https://github.com/boostorg/geometry/pull/827

// O(logN) version 2

template <std::size_t N>
struct for_each_index_impl2
{
    static const std::size_t N1 = N / 2;
    static const std::size_t N2 = N - N1;

    template <std::size_t Offset, typename UnaryFunction>
    constexpr static inline void apply(UnaryFunction& function)
    {
        for_each_index_impl2<N1>::template apply<Offset>(function);
        for_each_index_impl2<N2>::template apply<Offset + N1>(function);
    }
};

template <>
struct for_each_index_impl2<3>
{
    template <std::size_t Offset, typename UnaryFunction>
    constexpr static inline void apply(UnaryFunction& function)
    {
        function(util::index_constant<Offset>());
        function(util::index_constant<Offset + 1>());
        function(util::index_constant<Offset + 2>());
    }
};

template <>
struct for_each_index_impl2<2>
{
    template <std::size_t Offset, typename UnaryFunction>
    constexpr static inline void apply(UnaryFunction& function)
    {
        function(util::index_constant<Offset>());
        function(util::index_constant<Offset + 1>());
    }
};

template <>
struct for_each_index_impl2<1>
{
    template <std::size_t Offset, typename UnaryFunction>
    constexpr static inline void apply(UnaryFunction& function)
    {
        function(util::index_constant<Offset>());
    }
};

template <>
struct for_each_index_impl2<0>
{
    template <std::size_t Offset, typename UnaryFunction>
    constexpr static inline void apply(UnaryFunction& )
    {}
};

// Interface

template <std::size_t N, typename UnaryFunction>
constexpr inline UnaryFunction for_each_index(UnaryFunction function)
{
    for_each_index_impl2
        <
            N
        >::template apply<0>(function);
    return function;
}

template <typename Geometry, typename UnaryFunction>
constexpr inline UnaryFunction for_each_dimension(UnaryFunction function)
{
    for_each_index_impl2
        <
            geometry::dimension<Geometry>::value
        >::template apply<0>(function);
    return function;
}

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

// O(logN) version 2

template <std::size_t N>
struct all_indexes_of_impl2
{
    static const std::size_t N1 = N / 2;
    static const std::size_t N2 = N - N1;

    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return all_indexes_of_impl2<N1>::template apply<Offset>(predicate)
            && all_indexes_of_impl2<N2>::template apply<Offset + N1>(predicate);
    }
};

template <>
struct all_indexes_of_impl2<3>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return predicate(util::index_constant<Offset>())
            && predicate(util::index_constant<Offset + 1>())
            && predicate(util::index_constant<Offset + 2>());
    }
};

template <>
struct all_indexes_of_impl2<2>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return predicate(util::index_constant<Offset>())
            && predicate(util::index_constant<Offset + 1>());
    }
};

template <>
struct all_indexes_of_impl2<1>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return predicate(util::index_constant<Offset>());
    }
};

template <>
struct all_indexes_of_impl2<0>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& )
    {
        return true;
    }
};

// Interface

template <std::size_t N, typename UnaryPredicate>
constexpr inline bool all_indexes_of(UnaryPredicate predicate)
{
    return all_indexes_of_impl2<N>::template apply<0>(predicate);
}

template <typename Geometry, typename UnaryPredicate>
constexpr inline bool all_dimensions_of(UnaryPredicate predicate)
{
    return all_indexes_of_impl2
        <
            geometry::dimension<Geometry>::value
        >::template apply<0>(predicate);
}

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

// O(logN) version 2

template <std::size_t N>
struct any_index_of_impl2
{
    static const std::size_t N1 = N / 2;
    static const std::size_t N2 = N - N1;

    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return any_index_of_impl2<N1>::template apply<Offset>(predicate)
            || any_index_of_impl2<N2>::template apply<Offset + N1>(predicate);
    }
};

template <>
struct any_index_of_impl2<3>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return predicate(util::index_constant<Offset>())
            || predicate(util::index_constant<Offset + 1>())
            || predicate(util::index_constant<Offset + 2>());
    }
};

template <>
struct any_index_of_impl2<2>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return predicate(util::index_constant<Offset>())
            || predicate(util::index_constant<Offset + 1>());
    }
};

template <>
struct any_index_of_impl2<1>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& predicate)
    {
        return predicate(util::index_constant<Offset>());
    }
};

template <>
struct any_index_of_impl2<0>
{
    template <std::size_t Offset, typename UnaryPredicate>
    constexpr static inline bool apply(UnaryPredicate& )
    {
        return false;
    }
};

// Interface

template <std::size_t N, typename UnaryPredicate>
constexpr inline bool any_index_of(UnaryPredicate predicate)
{
    return any_index_of_impl2<N>::template apply<0>(predicate);
}

template <typename Geometry, typename UnaryPredicate>
constexpr inline bool any_dimension_of(UnaryPredicate predicate)
{
    return any_index_of_impl2
        <
            geometry::dimension<Geometry>::value
        >::template apply<0>(predicate);
}

template <std::size_t N, typename UnaryPredicate>
constexpr inline bool none_index_of(UnaryPredicate predicate)
{
    return ! any_index_of_impl2<N>::template apply<0>(predicate);
}

template <typename Geometry, typename UnaryPredicate>
constexpr inline bool none_dimension_of(UnaryPredicate predicate)
{
    return ! any_index_of_impl2
        <
            geometry::dimension<Geometry>::value
        >::template apply<0>(predicate);
}

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

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_UTIL_ALGORITHM_HPP