//==============================================================================
//         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/arithmetic/include/functions/correct_fma.hpp>
#include <boost/simd/sdk/simd/native.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/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/eps.hpp>
#include <boost/simd/include/functions/simd/oneplus.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

NT2_TEST_CASE_TPL ( correct_fma_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::correct_fma;
  using boost::simd::tag::correct_fma_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(correct_fma(boost::simd::Inf<vT>(), boost::simd::Inf<vT>(), boost::simd::Inf<vT>()), boost::simd::Inf<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::Minf<vT>(), boost::simd::Minf<vT>(), boost::simd::Minf<vT>()), boost::simd::Nan<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::Nan<vT>(), boost::simd::Nan<vT>(), boost::simd::Nan<vT>()), boost::simd::Nan<vT>());
#endif
  NT2_TEST_EQUAL(correct_fma(boost::simd::Mone<vT>(), boost::simd::Mone<vT>(), boost::simd::Mone<vT>()), boost::simd::Zero<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::One<vT>(), boost::simd::One<vT>(), boost::simd::One<vT>()), boost::simd::Two<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::One<vT>()+boost::simd::Eps<vT>(), boost::simd::One<vT>()-boost::simd::Eps<vT>(),boost::simd::Mone<vT>()), -boost::simd::Eps<vT>()*boost::simd::Eps<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::Zero<vT>(), boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Zero<vT>());
#ifndef  BOOST_SIMD_DONT_CARE_CORRECT_FMA_OVERFLOW
  NT2_TEST_EQUAL(correct_fma(boost::simd::Valmax<vT>(), boost::simd::Two<vT>(), -boost::simd::Valmax<vT>()), boost::simd::Valmax<vT>());
#endif
} // end of test for floating_

NT2_TEST_CASE_TPL ( correct_fma_si,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::correct_fma;
  using boost::simd::tag::correct_fma_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
  NT2_TEST_EQUAL(correct_fma(boost::simd::Mone<vT>(), boost::simd::Mone<vT>(), boost::simd::Mone<vT>()), boost::simd::Zero<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::One<vT>(), boost::simd::One<vT>(), boost::simd::One<vT>()), boost::simd::Two<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::Zero<vT>(), boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Zero<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::Valmax<vT>(), boost::simd::Two<vT>(),  boost::simd::oneplus(boost::simd::Valmin<T>())), boost::simd::Valmax<vT>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( correct_fma_ui,  BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{
  using boost::simd::correct_fma;
  using boost::simd::tag::correct_fma_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
  NT2_TEST_EQUAL(correct_fma(boost::simd::One<vT>(), boost::simd::One<vT>(), boost::simd::One<vT>()), boost::simd::Two<vT>());
  NT2_TEST_EQUAL(correct_fma(boost::simd::Zero<vT>(), boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Zero<vT>());
} // end of test for floating_


/**
 * @file mesh_loader.cc
 * @author Daeyun Shin <daeyun@dshin.org>
 * @version 0.1
 * @date 2015-01-02
 * @copyright librender is free software released under the BSD 2-Clause
 * license.
 */
#include "mesh_loader.h"

#include <stdexcept>
#include <armadillo>
#include <boost/format.hpp>
#include "config.h"
#include "graphics.h"
#include "shape.h"

namespace librender {

/**
 * @brief Import shape from a Wavefront .obj file. Vertex normals are estimated
 *        if not provided.
 * @param[in] render_params
 * @param[out] mesh
 */
void LoadObj(const RenderParams& render_params, Shape& mesh) {
  std::vector<tinyobj::shape_t> shapes;
  std::vector<tinyobj::material_t> materials;

  std::string err =
      tinyobj::LoadObj(shapes, materials, render_params.in_filename.c_str());

  if (!err.empty()) throw std::runtime_error(err);
  if (shapes.empty())
    throw std::runtime_error(std::string("Shape not found in ") +
                             render_params.in_filename);

  const int kVertexDims = 3;
  const int kTextureDims = 2;

  for (tinyobj::shape_t shape : shapes) {
    if (shape.mesh.positions.empty())
      throw std::runtime_error(std::string("Vertex not found in ") +
                               render_params.in_filename);
    if (shape.mesh.indices.empty())
      throw std::runtime_error(std::string("Face not found in ") +
                               render_params.in_filename);

    int kNumVertex = shape.mesh.positions.size() / kVertexDims;
    int kNumFace = shape.mesh.indices.size() / kVertexDims;
    arma::fmat shape_v(&shape.mesh.positions[0], kVertexDims, kNumVertex);
    arma::umat shape_f(&shape.mesh.indices[0], kVertexDims, kNumFace);
    arma::fmat shape_n;
    arma::fmat shape_uv;

    if (shape.mesh.normals.empty()) {
      ComputeNormals(shape_v, shape_f, shape_n);
    } else {
      shape_n = arma::fmat(&shape.mesh.normals[0], kVertexDims, kNumVertex);
    }

    if (!shape.mesh.texcoords.empty()) {
      shape_uv = arma::fmat(&shape.mesh.texcoords[0], kTextureDims, kNumVertex);
      shape_uv.swap_rows(0, 1);
    }

    mesh.v = join_cols(mesh.v, shape_v);
    mesh.ind = join_cols(mesh.ind, shape_f);
    mesh.vn = join_cols(mesh.vn, shape_n);
    mesh.uv = join_cols(mesh.uv, shape_uv);
  }

  mesh.vn = arma::normalise(mesh.vn);

  if (render_params.will_normalize) {
    NormalizeCoords(mesh.v);
  }

  switch (render_params.up_axis) {
    case X:
      mesh.v = arma::join_vert(mesh.v.rows(1, 2), mesh.v.row(0));
      mesh.vn = arma::join_vert(mesh.vn.rows(1, 2), mesh.vn.row(0));
      break;
    case Y:
      mesh.v = arma::join_vert(mesh.v.row(2), mesh.v.rows(0, 1));
      mesh.vn = arma::join_vert(mesh.vn.row(2), mesh.vn.rows(0, 1));
      break;
    case Z:
      // Nothing to do
      break;
  }

  // RGBA color per vertex
  mesh.vc.reshape(4, mesh.v.n_cols);
  mesh.vc.each_col() = render_params.color;

  mesh.type = ShapeType::kTriangles;
}

/**
 * @brief Compute the face and vertex normals from a triangle mesh of indexed
 *        vertices.
 * @param v[in] 3 by n vertices
 * @param f[in] 3 by m faces
 * @param vn[out] 3 by n vertex normals
 */
void ComputeNormals(const arma::fmat& v, const arma::umat& f, arma::fmat& vn) {
  arma::fmat a = v.cols(f.row(1)) - v.cols(f.row(0));
  arma::fmat b = v.cols(f.row(2)) - v.cols(f.row(0));

  // Cross product of each pair of columns
  arma::fmat fn;
  CrossCol(a, b, fn);

  // Accumulate vertex normal
  vn.zeros(v.n_rows, v.n_cols);
  arma::umat indices = reshape(f.t(), 1, f.n_cols * 3);
  vn.cols(indices) += arma::repmat(fn, 1, 3);

  arma::fmat num_adj_f;
  num_adj_f.zeros(1, v.n_cols);
  num_adj_f(indices) += 1;

  // Divide by the number of adjacent faces
  vn /= arma::repmat(num_adj_f, 3, 1);
}

/**
 * @brief Compute a column-wise cross product of \a a and \a b.
 * @param[in] a,b 3 by m matrices
 * @param[out] out 3 by m matrix
 */
void CrossCol(const arma::fmat& a, const arma::fmat& b, arma::fmat& out) {
  out = join_cols(join_cols(a.row(1) % b.row(2) - a.row(2) % b.row(1),
                            a.row(2) % b.row(0) - a.row(0) % b.row(2)),
                  a.row(0) % b.row(1) - a.row(1) % b.row(0));
}

/**
 * @brief Re-scale the data points so that the maximum dimension range is 1 and
 *        the center of the object is at (0, 0).
 * @param v 3 by n matrix of vertices.
 */
void NormalizeCoords(arma::fmat& v) {
  arma::fvec vmax = arma::max(v, 1);
  arma::fvec vmin = arma::min(v, 1);

  arma::fvec range = vmax - vmin;

  v.each_col() -= vmin + range / 2;
  v /= arma::max(range);
}
}


/*! \file polygon_overlap_area.hpp
  \brief Calculates the overlap between two polygons
  \author Elad Steinberg
 */

#include "PolyIntersect.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

//! \brief Alias for random number generator
typedef boost::mt19937_64 base_generator_type;

using std::vector;

//! \brief Overlap of two polygons
class PolygonOverlap
{
private:
  base_generator_type gen;
public:
//! \brief Class constructor
  PolygonOverlap(void);

  /*!
  \brief Calcualted the area of a convex polygon
  \param polygon The vertices of the polygon, should be ordered as a convex hull
  \return The area
  */
  double PolyArea(vector<Vector2D> const& polygon);
  /*!
  \brief Calculates the area overlaped between two convex polygons
  \param ch1 The convex hull of the first polygon
  \param ch2 The convex hull of the second polygon
  \param R0 A fraction of the effective radius of the first polygon
  \param R1 A fraction of the effective radius of the second polygon
  \return The overlaped area
  */
  double polygon_overlap_area(vector<Vector2D> const& ch1,
			      vector<Vector2D> const& ch2,double R0,double R1);
};


#include "gtest/gtest.h"

#include "ale.h"

#include <stdexcept>
#include <cmath>

#include <gsl/gsl_interp.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

TEST(PositionInterpTest, LinearInterp) {
  vector<double> times = { -3, -2, -1,  0,  1,  2};
  vector<vector<double>> data = {{ -3, -2, -1,  0,  1,  2},
                                 {  9,  4,  1,  0,  1,  4},
                                 {-27, -8, -1,  0,  1,  8}};

  vector<double> coordinate = ale::getPosition(data, times, -1.5, ale::linear);

  ASSERT_EQ(3, coordinate.size());
  EXPECT_DOUBLE_EQ(-1.5, coordinate[0]);
  EXPECT_DOUBLE_EQ(2.5,  coordinate[1]);
  EXPECT_DOUBLE_EQ(-4.5, coordinate[2]);
}

TEST(PositionInterpTest, FourCoordinates) {
  vector<double> times = { -3, -2, -1,  0,  1,  2};
  vector<vector<double>> data = {{ -3, -2, -1,  0,  1,  2},
                                 {  9,  4,  1,  0,  1,  4},
                                 {-27, -8, -1,  0,  1,  8},
                                 { 25,  0, -5, 25,  3,  6}};

  EXPECT_THROW(ale::getPosition(data, times, 0.0, ale::linear),
               invalid_argument);
}

TEST(LinearInterpTest, ExampleInterpolation) {
  vector<double> times = {0,  1,  2, 3};
  vector<double> data = {0, 2, 1, 0};

  EXPECT_DOUBLE_EQ(0.0, ale::interpolate(data, times, 0.0, ale::linear, 0));
  EXPECT_DOUBLE_EQ(1.0, ale::interpolate(data, times, 0.5, ale::linear, 0));
  EXPECT_DOUBLE_EQ(2.0, ale::interpolate(data, times, 1.0, ale::linear, 0));
  EXPECT_DOUBLE_EQ(1.5, ale::interpolate(data, times, 1.5, ale::linear, 0));
  EXPECT_DOUBLE_EQ(1.0, ale::interpolate(data, times, 2.0, ale::linear, 0));
  EXPECT_DOUBLE_EQ(0.5, ale::interpolate(data, times, 2.5, ale::linear, 0));
  EXPECT_DOUBLE_EQ(0.0, ale::interpolate(data, times, 3.0, ale::linear, 0));
}

TEST(LinearInterpTest, NoPoints) {
  vector<double> times = {};
  vector<double> data = {};

  EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::linear, 0),
               invalid_argument);
}

TEST(LinearInterpTest, DifferentCounts) {
  vector<double> times = { -3, -2, -1,  0,  2};
  vector<double> data = { -3, -2, 1,  2};

  EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::linear, 0),
               invalid_argument);
}

TEST(LinearInterpTest, Extrapolate) {
  vector<double> times = {0,  1,  2, 3};
  vector<double> data = {0, 2, 1, 0};

  EXPECT_THROW(ale::interpolate(data, times, -1.0, ale::linear, 0),
               invalid_argument);
  EXPECT_THROW(ale::interpolate(data, times, 4.0, ale::linear, 0),
               invalid_argument);
}

TEST(SplineInterpTest, ExampleInterpolation) {
  // From http://www.maths.nuigalway.ie/~niall/teaching/Archive/1617/MA378/2-2-CubicSplines.pdf
  vector<double> times = {0,  1,  2, 3};
  vector<double> data = {0, 2, 1, 0};
  // Spline functions is:
  //        2.8x - 0.8x^3,                 x in [0, 1]
  // S(x) = x^3 - 5.4x^2 + 8.2x - 1.8,     x in [1, 2]
  //        -0.2x^3 + 1.8x^2 - 6.2x + 7.8, x in [2, 3]

  // The spline interpolation is only ~1e-10 so we have to define a tolerance
  double tolerance = 1e-10;
  EXPECT_NEAR(0.0, ale::interpolate(data, times, 0.0, ale::spline, 0), tolerance);
  EXPECT_NEAR(2.8 * 0.5 - 0.8 * 0.125,
              ale::interpolate(data, times, 0.5, ale::spline, 0), tolerance);
  EXPECT_NEAR(2.0, ale::interpolate(data, times, 1.0, ale::spline, 0), tolerance);
  EXPECT_NEAR(3.375 - 5.4 * 2.25 + 8.2 * 1.5 - 1.8,
              ale::interpolate(data, times, 1.5, ale::spline, 0), tolerance);
  EXPECT_NEAR(1.0, ale::interpolate(data, times, 2.0, ale::spline, 0), tolerance);
  EXPECT_NEAR(-0.2 * 15.625 + 1.8 * 6.25 - 6.2 * 2.5 + 7.8,
              ale::interpolate(data, times, 2.5, ale::spline, 0), tolerance);
  EXPECT_NEAR(0.0, ale::interpolate(data, times, 3.0, ale::spline, 0), tolerance);
}

TEST(SplineInterpTest, NoPoints) {
  vector<double> times = {};
  vector<double> data = {};

  EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::spline, 0),
               invalid_argument);
}

TEST(SplineInterpTest, DifferentCounts) {
  vector<double> times = { -3, -2, -1,  0,  2};
  vector<double> data = { -3, -2, 1,  2};

  EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::spline, 0),
               invalid_argument);
}

TEST(SplineInterpTest, Extrapolate) {
  vector<double> times = {0,  1,  2, 3};
  vector<double> data = {0, 2, 1, 0};

  EXPECT_THROW(ale::interpolate(data, times, -1.0, ale::spline, 0),
               invalid_argument);
  EXPECT_THROW(ale::interpolate(data, times, 4.0, ale::spline, 0),
               invalid_argument);
}

TEST(PolynomialTest, Evaluate) {
  vector<double> coeffs = {1.0, 2.0, 3.0}; // 1 + 2x + 3x^2
  EXPECT_EQ(2.0, ale::evaluatePolynomial(coeffs, -1, 0));
}

TEST(PolynomialTest, Derivatives) {
  vector<double> coeffs = {1.0, 2.0, 3.0}; // 1 + 2x + 3x^2
  EXPECT_EQ(-4.0, ale::evaluatePolynomial(coeffs, -1, 1));
  EXPECT_EQ(6.0, ale::evaluatePolynomial(coeffs, -1, 2));
}

TEST(PolynomialTest, EmptyCoeffs) {
  vector<double> coeffs = {};
  EXPECT_THROW(ale::evaluatePolynomial(coeffs, -1, 1), invalid_argument);
}

TEST(PolynomialTest, BadDerivative) {
  vector<double> coeffs = {1.0, 2.0, 3.0};
  EXPECT_THROW(ale::evaluatePolynomial(coeffs, -1, -1), invalid_argument);
}

TEST(PoisitionCoeffTest, SecondOrderPolynomial) {
  double time = 2.0;
  vector<vector<double>> coeffs = {{1.0, 2.0, 3.0},
                                   {1.0, 3.0, 2.0},
                                   {3.0, 2.0, 1.0}};

  vector<double> coordinate = ale::getPosition(coeffs, time);

  ASSERT_EQ(3, coordinate.size());
  EXPECT_DOUBLE_EQ(17.0, coordinate[0]);
  EXPECT_DOUBLE_EQ(15.0, coordinate[1]);
  EXPECT_DOUBLE_EQ(11.0, coordinate[2]);
}

TEST(PoisitionCoeffTest, DifferentPolynomialDegrees) {
  double time = 2.0;
  vector<vector<double>> coeffs = {{1.0},
                                   {1.0, 2.0},
                                   {1.0, 2.0, 3.0}};

  vector<double> coordinate = ale::getPosition(coeffs, time);

  ASSERT_EQ(3, coordinate.size());
  EXPECT_DOUBLE_EQ(1.0,  coordinate[0]);
  EXPECT_DOUBLE_EQ(5.0,  coordinate[1]);
  EXPECT_DOUBLE_EQ(17.0, coordinate[2]);
}

TEST(PoisitionCoeffTest, NegativeInputs) {
  double time = -2.0;
  vector<vector<double>> coeffs = {{-1.0, -2.0, -3.0},
                                   {1.0, -2.0, 3.0},
                                   {-1.0, 2.0, -3.0}};

  vector<double> coordinate = ale::getPosition(coeffs, time);

  ASSERT_EQ(3, coordinate.size());
  EXPECT_DOUBLE_EQ(-9.0,  coordinate[0]);
  EXPECT_DOUBLE_EQ(17.0,  coordinate[1]);
  EXPECT_DOUBLE_EQ(-17.0, coordinate[2]);
}

TEST(PoisitionCoeffTest, InvalidInput) {
  double valid_time = 0.0;
  vector<vector<double>> invalid_coeffs_sizes = {{3.0, 2.0, 1.0},
                                                 {1.0, 2.0, 3.0}};

  EXPECT_THROW(ale::getPosition(invalid_coeffs_sizes, valid_time), invalid_argument);
}

TEST(VelocityCoeffTest, SecondOrderPolynomial) {
  double time = 2.0;
  vector<vector<double>> coeffs = {{1.0, 2.0, 3.0},
                                   {1.0, 3.0, 2.0},
                                   {3.0, 2.0, 1.0}};

  vector<double> coordinate = ale::getVelocity(coeffs, time);

  ASSERT_EQ(3, coordinate.size());
  EXPECT_DOUBLE_EQ(14.0, coordinate[0]);
  EXPECT_DOUBLE_EQ(11.0, coordinate[1]);
  EXPECT_DOUBLE_EQ(6.0, coordinate[2]);
}

TEST(VelocityCoeffTest, InvalidInput) {
  double valid_time = 0.0;
  vector<vector<double>> invalid_coeffs_sizes = {{3.0, 2.0, 1.0},
                                                 {1.0, 2.0, 3.0}};

  EXPECT_THROW(ale::getVelocity(invalid_coeffs_sizes, valid_time), invalid_argument);
}

TEST(RotationCoeffTest, ZeroOrderPolynomial) {
  double time = 1.0;
  vector<vector<double>> coeffs = {{90},
                                                            {0},
                                                            {0}};

  vector<double> coordinate = ale::getRotation(coeffs, time);

  ASSERT_EQ(4, coordinate.size());
  EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[0]);
  EXPECT_DOUBLE_EQ(0, coordinate[1]);
  EXPECT_DOUBLE_EQ(0, coordinate[2]);
  EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[3]);
}

TEST(RotationCoeffTest, InvalidInput) {
  double time = 1.0;
  vector<vector<double>> coeffs = {{90},
                                                            {0}};

  EXPECT_THROW(ale::getRotation(coeffs, time), invalid_argument);
}

TEST(AngularVelocityCoeffTest, DefaultAngularExample) {
  vector<vector<double>> coeffs = {{0, 90}, {0, 90}, {0, 90}};
  double time = 1.0;

  vector<double> av = ale::getAngularVelocity(coeffs, time);

  ASSERT_EQ(3, av.size());
  EXPECT_DOUBLE_EQ(90, av[0]);
  EXPECT_DOUBLE_EQ(90, av[1]);
  EXPECT_DOUBLE_EQ(90, av[2]);
}

TEST(AngularVelocityCoeffTest, InvalidInput) {
  vector<vector<double>> coeffs = {{0, 90}, {0, 90}};
  double time = 2.0;

  EXPECT_THROW(ale::getAngularVelocity(coeffs, time), invalid_argument);
}

TEST(RotationInterpTest, ExampleGetRotation) {
  // simple test, only checks if API hit correctly and output is normalized
  vector<double> times = {0,  1,  2, 3};
  vector<vector<double>> rots({{1,1,1,1}, {0,0,0,0}, {1,1,1,1}, {0,0,0,0}});
  vector<double> r = ale::getRotation(rots, times, 2, ale::linear);
  Eigen::Quaterniond quat(r[0], r[1], r[2], r[3]);

  EXPECT_DOUBLE_EQ(1, quat.norm());
}

TEST(RotationInterpTest, GetRotationDifferentCounts) {
  // incorrect params
  vector<double> times = {0, 1, 2};
  vector<vector<double>> rots({{1,1,1,1}, {0,0,0,0}, {1,1,1,1}, {0,0,0,0}});
  EXPECT_THROW(ale::getRotation(rots, times, 2, ale::linear), invalid_argument);
}

TEST(PyInterfaceTest, LoadInvalidLabel) {
  std::string label = "Not a Real Label";
  EXPECT_THROW(ale::load(label), invalid_argument);
}

TEST(PyInterfaceTest, LoadValidLabel) {
  std::string label = "../pytests/data/EN1072174528M/EN1072174528M_spiceinit.lbl";
  ale::load(label, "", "isis");
}

TEST(AngularVelocityInterpTest, ExampleGetRotation) {
  vector<double> times = {0,  1};
  vector<vector<double>> rots({{0,0}, {1,0}, {0,1}, {0,0}});
  vector<double> av = ale::getAngularVelocity(rots, times, 0.5, ale::linear);

  EXPECT_DOUBLE_EQ(0, av[0]);
  EXPECT_DOUBLE_EQ(0, av[1]);
  EXPECT_DOUBLE_EQ(2 * sqrt(2), av[2]);
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// @project        Open Space Toolkit ▸ Mathematics
/// @file           OpenSpaceToolkit/Mathematics/Objects/Vector.hpp
/// @author         Lucas Brémond <lucas@loftorbital.com>
/// @license        Apache License 2.0

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#ifndef __OpenSpaceToolkit_Mathematics_Objects_Vector__
#define __OpenSpaceToolkit_Mathematics_Objects_Vector__

#include <OpenSpaceToolkit/Core/Types/String.hpp>

#define EIGEN_MATRIXBASE_PLUGIN "OpenSpaceToolkit/Mathematics/Objects/Eigen.hpp"

// Disable Eigen warnings

#pragma GCC diagnostic push // Save diagnostic state

#pragma GCC diagnostic ignored "-Wshadow"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wint-in-bool-context"
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"

#include <Eigen/Core>
#include <Eigen/Geometry>

#pragma GCC diagnostic pop // Turn the warnings back on

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

namespace ostk
{
namespace math
{
namespace obj
{

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

using Vector2i = Eigen::Vector2i ;
using Vector3i = Eigen::Vector3i ;
using Vector4i = Eigen::Vector4i ;

using VectorXi = Eigen::VectorXi ;

using Vector2d = Eigen::Vector2d ;
using Vector3d = Eigen::Vector3d ;
using Vector4d = Eigen::Vector4d ;

using VectorXd = Eigen::VectorXd ;

using RowVectorXd = Eigen::RowVectorXd ;

using Matrix2i = Eigen::Matrix2i ;
using Matrix3i = Eigen::Matrix3i ;
using Matrix4i = Eigen::Matrix4i ;

using MatrixXi = Eigen::MatrixXi ;

using Matrix2d = Eigen::Matrix2d ;
using Matrix3d = Eigen::Matrix3d ;
using Matrix4d = Eigen::Matrix4d ;

using MatrixXd = Eigen::MatrixXd ;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

}
}
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#endif

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Sebastian Schlenkrich

*/

#ifndef quantlib_templatetdstochvolmodel_hpp
#define quantlib_templatetdstochvolmodel_hpp

#include <complex>
#include <ql/shared_ptr.hpp>
#include <boost/function.hpp>
#include <ql/errors.hpp>
#include <ql/experimental/templatemodels/auxilliaries/auxilliariesT.hpp>
#include <ql/experimental/templatemodels/auxilliaries/gausslobattoT.hpp>
#include <ql/experimental/templatemodels/auxilliaries/complexT.hpp>
#include <ql/experimental/templatemodels/auxilliaries/solver1dT.hpp>
#include <ql/experimental/templatemodels/stochasticprocessT.hpp>
#include <ql/experimental/templatemodels/stochvol/hestonmodelT.hpp>

#define _MIN_( a, b ) ( (a) < (b) ? (a) : (b) )
#define _MAX_( a, b ) ( (a) > (b) ? (a) : (b) )

namespace QuantLib {

    // general stochastic volatility model interface with time-dependent parameters and parameter averaging pricing formula
    //
    //    dS(t) = lambda(t) [ b(t) S(t) + (1-b(t)) L ] sqrt[z(t)] dW(t)
    //    dz(t) = theta [ m - z(t) ] dt + eta(t) sqrt[z(t)] dZ(t)
    //    dW(t) dZ(t) = rho dt
    //
    template <class DateType, class PassiveType, class ActiveType>
    class TimeDependentStochVolModelT : public StochasticProcessT<DateType,PassiveType,ActiveType> {
    public:
        // abstract inspectors
        virtual ActiveType  lambda( const DateType t) = 0;
        virtual ActiveType  b(      const DateType t) = 0;
        virtual ActiveType  L()                       = 0;
        virtual ActiveType  theta()                   = 0;
        virtual ActiveType  m()                       = 0;
        virtual ActiveType  eta(    const DateType t) = 0;
        virtual ActiveType  z0()                      = 0;
        virtual ActiveType  rho()                     = 0;
        virtual ActiveType  S0()                      = 0;

        // from base class
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VecD;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VecP;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VecA;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::MatA;
        using typename StochasticProcessT<DateType, PassiveType, ActiveType>::VolEvolv;
        using StochasticProcessT<DateType, PassiveType, ActiveType>::volEvolv;

        // helper functions for vol averaging, Piterbarg, 10.2.4
        inline static ActiveType A_CIR ( ActiveType c1, ActiveType c2, ActiveType z0, ActiveType theta, ActiveType eta, DateType dt ) {
            ActiveType gamma = sqrt((theta*theta + 2.0 * eta*eta * c2));
            ActiveType t1 = theta*z0/eta/eta * (theta + gamma)*dt;
            ActiveType t2 = 1.0 + (theta + gamma + c1 * eta*eta) * (exp(gamma * dt) - 1.0) / 2.0 / gamma;
            QL_REQUIRE(t2>0,"A_CIR: require positive log()-argument");
            return t1 - 2.0*theta*z0/eta/eta*log(t2);
        }

        inline static ActiveType B_CIR ( ActiveType c1, ActiveType c2, ActiveType z0, ActiveType theta, ActiveType eta, DateType dt ) {
            ActiveType gamma = sqrt((theta*theta + 2.0 * eta*eta * c2));
            ActiveType emGdt = exp(-gamma * dt);
            ActiveType numer = (2.0*c2 - theta*c1)*(1.0 - emGdt) + gamma*c1*(1.0 + emGdt);
            ActiveType denum = (theta + gamma + c1*eta*eta) * (1.0 - emGdt) + 2.0*gamma*emGdt;
            return numer / denum;
        }

        // abstract averaging formula definitions
        inline virtual ActiveType  averageLambda( const DateType T) = 0;
        inline virtual ActiveType  averageB     ( const DateType T) = 0;
        inline virtual ActiveType  averageEta   ( const DateType T) = 0;

        // undiscounted expectation of vanilla payoff
        inline ActiveType vanillaOption(const PassiveType forwardPrice,
                                        const PassiveType strikePrice,
                                        const DateType    term,
                                        const int         callOrPut,
                                        const PassiveType accuracy,
                                        const size_t      maxEvaluations) {
                StochVolModelT<DateType,PassiveType,ActiveType> model(averageLambda(term), averageB(term),L(),theta(),m(),averageEta(term),z0(),rho());
                return model.vanillaOption( forwardPrice, strikePrice, term, callOrPut, accuracy, maxEvaluations );
        }

        // conditional moments of vol process used for z-integration, Piterbarg, 8.3.3.
        // E[ z(T) | z(t) ]
        inline virtual ActiveType expectationZ( DateType t, ActiveType zt, DateType dT ) {
            return z0() + (zt - z0())*exp(-theta()*dT);
        }
        // Var[ z(T) | z(t) ]
        inline virtual ActiveType varianceZ( DateType t, ActiveType zt, DateType dT ) {
            ActiveType expmThDT = exp(-theta()*dT);
            ActiveType onemETDT = 1 - expmThDT;
            ActiveType eta2oThe = eta(t+dT/2.0)*eta(t+dT/2.0)/theta();  // approx eta(t)=eta for s \in [t, t+dT]
            return zt*eta2oThe*expmThDT*onemETDT + z0()*eta2oThe/2.0*onemETDT*onemETDT;
        }

        // stochastic process interface
        // dimension of X
        inline virtual size_t size() { return 2; }
        // stochastic factors of x and z (maybe distinguish if trivially eta=0)
        inline virtual size_t factors() { return 2; }
        // initial values for simulation
        inline virtual VecP initialValues() {
            VecP X(2);
            X[0] = S0();
            X[1] = z0();
            return X;
        }
        // a[t,X(t)]
        inline virtual VecA drift( const DateType t, const VecA& X) {
            VecA a(2);
            // S-variable drift-less
            a[0] = 0.0;
            // z-variable theta [ m - z(t)^+ ]  (full truncation)
            a[1] = theta()*(m() - ((X[1]>0)?(X[1]):(0.0)));
            return a;
        }
        // b[t,X(t)]
        inline virtual MatA diffusion( const DateType t, const VecA& X) {
            MatA B(2);
            B[0].resize(2);
            B[1].resize(2);
            ActiveType sqrtz = ( (X[1]>0) ? (sqrt(X[1])) : (0.0) );   // full truncation
            // S-variable lambda(t) [ b(t) S(t) + (1-b(t)) L ] sqrt[z(t)] dW(t)
            B[0][0] = lambda(t) * (b(t) * X[0] + (1.0-b(t)) * L()) * sqrtz;
            B[0][1] = 0.0;
            // z-variable
            B[1][0] = rho()*eta(t)*sqrtz;
            B[1][1] = sqrt(1-rho()*rho())*eta(t)*sqrtz;
            // finished
            return B;
        }

        // integrate X1 = X0 + drift()*dt + diffusion()*dW*sqrt(dt)
        inline virtual void evolve( const DateType t0, const VecA& X0, const DateType dt, const VecD& dW, VecA& X1 ) {
            // ensure X1 has size of X0
            VecA a = drift(t0, X0);
            MatA b = diffusion(t0, X0);
            // S-variable
            X1[0] = X0[0] + a[0]*dt + b[0][0]*dW[0]*sqrt(dt);
            // z-variable
            if (volEvolv()==VolEvolv::FullTruncation) {
                X1[1] = X0[1] + a[1]*dt + (b[1][0]*dW[0]+b[1][1]*dW[1])*sqrt(dt);
                return;
            }
            if (volEvolv()==VolEvolv::LogNormalApproximation) {
                ActiveType e = expectationZ(t0, X0[1], dt);
                ActiveType v = varianceZ(t0, X0[1], dt);
                ActiveType dZ = rho()*dW[0] + sqrt(1-rho()*rho())*dW[1];
                ActiveType si = sqrt(log(1.0 + v/e/e));
                ActiveType mu = log(e) - si*si/2.0;
                X1[1] = exp(mu + si*dZ);
            }
            return;
        }

        // embeded classes

        class PieceWiseConstant  {
        protected:
            // check for dimensions
            bool isConsistent_;
            // time grid
            std::vector<DateType>     times_;
            // time-dependent model parameters
            std::vector<ActiveType>   lambda_;
            std::vector<ActiveType>   b_;
            std::vector<ActiveType>   eta_;
            // time-homogeneous model parameters
            ActiveType                L_;
            ActiveType                theta_;
            ActiveType                m_;
            ActiveType                z0_;
            ActiveType                rho_;
            ActiveType                S0_;
            // helper
            inline size_t idx( const DateType t ) { return TemplateAuxilliaries::idx(times_,t); }
        public:
            // constructor
            PieceWiseConstant( const std::vector<DateType>&    times,
                               const std::vector<ActiveType>&  lambda,
                               const std::vector<ActiveType>&  b,
                               const std::vector<ActiveType>&  eta,
                               const ActiveType                L,
                               const ActiveType                theta,
                               const ActiveType                m,
                               const ActiveType                z0,
                               const ActiveType                rho,
                               const ActiveType                S0 )
                : times_(times), lambda_(lambda), b_(b), eta_(eta), L_(L), theta_(theta), m_(m), z0_(z0), rho_(rho), S0_(S0) {
                isConsistent_ = true;
                QL_REQUIRE(times_.size()>0,"TemplateTimeDependentStochVolModel::PieceWiseConstant: non-empty times required");
                for (size_t k=0; k<times_.size()-1; ++k) QL_REQUIRE(times_[k]<times_[k+1],"TemplateTimeDependentStochVolModel::PieceWiseConstant: ascending time-grid required");
                QL_REQUIRE(lambda_.size()==times_.size(),"TemplateTimeDependentStochVolModel::PieceWiseConstant: lambda dimension mismatch");
                QL_REQUIRE(b_.size()     ==times_.size(),"TemplateTimeDependentStochVolModel::PieceWiseConstant: b dimension mismatch");
                QL_REQUIRE(eta_.size()   ==times_.size(),"TemplateTimeDependentStochVolModel::PieceWiseConstant: eta dimension mismatch");
            }
            // inspectors
            virtual ActiveType  lambda( const DateType t)  { return lambda_[idx(t)]; }
            virtual ActiveType  b(      const DateType t)  { return b_[idx(t)];      }
            virtual ActiveType  eta(    const DateType t)  { return eta_[idx(t)];    }
            virtual ActiveType  L()                        { return L_;              }
            virtual ActiveType  theta()                    { return theta_;          }
            virtual ActiveType  m()                        { return m_;              }
            virtual ActiveType  z0()                       { return z0_;             }
            virtual ActiveType  rho()                      { return rho_;            }
            virtual ActiveType  S0()                       { return S0_;            }

        };

        // averaging assuming piecewise constant parameters on ( T_k-1, T_k ]
        class MidPointIntegration {
        protected:
            // reference to model
            TimeDependentStochVolModelT* model_;
            // time grid for integration
            std::vector<DateType>               times_;
            inline std::vector<DateType> getTimes( const DateType T ) {
                std::vector<DateType> times;
                times.push_back(0.0);
                for (size_t k=0; k<times_.size(); ++k) {
                    if (times_[k]>0.0 && times_[k]<T) times.push_back(times_[k]);
                    if (times_[k]>=T) break;
                }
                times.push_back(T);
                return times;
            }

            class AverageLambdaObjective {
            protected:
                ActiveType z0_;
                ActiveType theta_;
                ActiveType eta_;
                DateType   dt_;
                ActiveType target_;
            public:
                AverageLambdaObjective ( const ActiveType z0,
                                         const ActiveType theta,
                                         const ActiveType eta,
                                         const DateType   dt,
                                         const ActiveType target )
                                         : z0_(z0), theta_(theta), eta_(eta), dt_(dt), target_(target) {}
                ActiveType operator() ( const ActiveType avLambda2c ) {
                    ActiveType A = A_CIR(0, -avLambda2c, z0_, theta_, eta_, dt_);
                    ActiveType B = B_CIR(0, -avLambda2c, z0_, theta_, eta_, dt_);
                    ActiveType res = A - B*z0_ - target_;
                    return res;
                }
            };

            // Prop. 9.1.2
            class RiccatiODE {
                TimeDependentStochVolModelT* model_;
                ActiveType v_;
                ActiveType u_;
                ActiveType averageB_;
            public:
                RiccatiODE (TimeDependentStochVolModelT* model, ActiveType v, ActiveType u, ActiveType averageB)
                    : model_(model), v_(v), u_(u), averageB_(averageB) {}
                void operator() (const DateType t, const std::vector<ActiveType>& y, std::vector<ActiveType>& fy) {
                    fy[0] = - model_->theta() * model_->z0() * y[1];
                    fy[1] = (model_->theta() - model_->rho()*model_->eta(t)*averageB_*u_*model_->lambda(t))*y[1]
                            - model_->eta(t)*model_->eta(t)/2.0*y[1]*y[1] - v_*model_->lambda(t)*model_->lambda(t);
                }
            };

        public:
            // constructor
            MidPointIntegration ( TimeDependentStochVolModelT* model,
                                  std::vector<DateType>        times )
                                  : model_(model), times_(times) { }

            virtual ~MidPointIntegration() = default;

            // averaging formula implementations
            virtual ActiveType  averageEta   ( const DateType T) {
                std::vector<DateType> times(getTimes(T));
                std::vector<ActiveType> f(times.size());
                std::vector<ActiveType> w(times.size()-1);
                f[f.size()-1] = 0.0;
                for (size_t k=f.size()-1; k>0; --k) {
                    ActiveType lambda = model_->lambda((times[k-1]+times[k])/2.0);
                    ActiveType tmp    = exp(-model_->theta()*(times[k]-times[k-1]));
                    f[k-1] = lambda*lambda/model_->theta()*(1.0 - tmp) + tmp*f[k];
                }
                ActiveType sum = 0.0;
                for (size_t k=0; k<w.size(); ++k) {
                    ActiveType lambda  = model_->lambda((times[k]+times[k+1])/2.0);
                    ActiveType lambda2 = lambda*lambda;
                    ActiveType theta   = model_->theta();
                    ActiveType theta2  = theta*theta;
                    w[k] = (f[k+1]*f[k+1] - f[k]*f[k])/2.0/theta +
                           (f[k+1] - f[k])*lambda2/theta2 +
                           (times[k+1]-times[k])*lambda2*lambda2/theta2;
                    w[k] *= 0.5;
                    sum  += w[k];
                }
                ActiveType eta2=0.0;
                for (size_t k=0; k<w.size(); ++k) {
                    ActiveType eta = model_->eta((times[k]+times[k+1])/2.0);
                    eta2 += w[k] * eta * eta;
                }
                eta2 = eta2 / sum;
                return sqrt(eta2);
            }

            virtual ActiveType  averageB( const DateType T) {
                std::vector<DateType> times(getTimes(T));
                std::vector<ActiveType> w(times.size()-1);
                ActiveType theta = model_->theta();
                ActiveType z0 = model_->z0();
                ActiveType sumLambda2dT = 0.0;
                ActiveType S1=0.0, S2=0.0, S3=0.0;
                ActiveType sum = 0.0;
                for (size_t k=0; k<w.size(); ++k) {
                    // v1
                    ActiveType lambda    = model_->lambda((times[k]+times[k+1])/2.0);
                    ActiveType lambda2   = lambda*lambda;
                    ActiveType eta       = model_->eta((times[k]+times[k+1])/2.0);
                    ActiveType lambda2dT = lambda2*(times[k+1]-times[k]);
                    ActiveType v1        = z0*z0*(times[k+1]-times[k])*(lambda2dT/2.0 + sumLambda2dT);
                    sumLambda2dT        += lambda2dT;
                    // v3, v4, v5
                    ActiveType expmThdT  = exp(-theta*(times[k+1]-times[k]));
                    ActiveType v3        = z0/theta*(S2+S3)*(1.0-expmThdT);
                    ActiveType v4        = (times[k+1]-times[k]) - (1.0-expmThdT)/theta -
                                           (1.0-expmThdT)*(1.0-expmThdT)/2.0/theta;
                    ActiveType laEtaTh   = lambda*eta/theta;
                    v4                  *= z0*laEtaTh*laEtaTh/2.0;
                    ActiveType theta2    = theta*theta;
                    ActiveType v5        = z0*lambda2/theta2/2.0*S1*(1.0-expmThdT)*(1.0-expmThdT);
                    // updating S1, S2, S3
                    S3                   = expmThdT * ( S3 + S1*lambda2/theta*(1.0-expmThdT) );
                    S2                   = expmThdT*S2 + laEtaTh*laEtaTh/2.0*(1.0-expmThdT)*(1.0-expmThdT);
                    S1                   = expmThdT*expmThdT*S1 + eta*eta/theta/2.0*(1.0 - expmThdT*expmThdT);
                    // gathering things together...
                    w[k]                 = lambda2 * ( v1 + v3 + v4 + v5 );
                    sum                 += w[k];
                }
                ActiveType b=0;
                for (size_t k=0; k<w.size(); ++k) {
                    b += w[k] * model_->b((times[k]+times[k+1])/2.0);
                }
                b = b / sum;
                return b;
            }

            virtual ActiveType  averageLambda     ( const DateType T) {
                ActiveType b   = averageB(T);
                ActiveType eta = averageEta(T);  // maybe better use time-dep eta
                std::vector<DateType> times(getTimes(T));
                // c = h''(zeta) / h'(zeta)
                ActiveType zeta = 0.0;
                for (size_t k=0; k<times.size()-1; ++k) {
                    ActiveType lambda    = model_->lambda((times[k]+times[k+1])/2.0);
                    zeta += lambda*lambda*(times[k+1]-times[k]);
                }
                ActiveType avLambda2 = zeta / (times[times.size()-1] - times[0]);
                zeta *= model_->z0();
                ActiveType c = -(b*b/4.0 + 1.0/zeta)/2.0;
                // Psi_{z lambda^2}
                ActiveType A = 0.0, B = 0.0;
                //std::vector<ActiveType> y1(2,0.0), y0(2,0.0);
                //RiccatiODE ode(model_,c,0.0,b);
                for (size_t k=times.size()-1; k>0; --k) {
                    DateType  t = (times[k]+times[k-1])/2.0;
                    DateType dt = (times[k]-times[k-1]);
                    ActiveType lambda = model_->lambda(t);
                    A = A + A_CIR(B, -c*lambda*lambda, model_->z0(), model_->theta(), eta, dt);
                    B = B_CIR(B, -c*lambda*lambda, model_->z0(), model_->theta(), eta, dt);
                    // Riccati ODE via Runge Kutta Method
                    //y1 = y0;
                    //TemplateAuxilliaries::rungeKuttaStep<DateType,ActiveType>(y1,times[k],ode,times[k-1]-times[k],y0);
                }
                ActiveType target = A - B*model_->z0();
                //ActiveType target = y0[0] + y0[1]*model_->z0();
                AverageLambdaObjective f(model_->z0(), model_->theta(), eta, T, target);
                ActiveType avLambda2c = avLambda2 * c;
                avLambda2c = TemplateAuxilliaries::solve1d<ActiveType>(f, 1.0e-8, avLambda2c, avLambda2c, 10);
                ActiveType avLambda = sqrt(avLambda2c / c);
                return avLambda;
            };
        };
    };

    // piecewise constant parameters and numerical integration
    template <class DateType, class PassiveType, class ActiveType>
    class PWCAnalytical : public TimeDependentStochVolModelT<DateType,PassiveType,ActiveType> {
        // from base class
        using typename TimeDependentStochVolModelT<DateType, PassiveType, ActiveType>::PieceWiseConstant;
        using typename TimeDependentStochVolModelT<DateType, PassiveType, ActiveType>::MidPointIntegration;
        using typename TimeDependentStochVolModelT<DateType, PassiveType, ActiveType>::VolEvolv;

    private:
        ext::shared_ptr<PieceWiseConstant>    pwc_;
        ext::shared_ptr<MidPointIntegration>  mp_;
        VolEvolv volEvolv_;
    public:
        PWCAnalytical(const std::vector<DateType>&    times,
                        const std::vector<ActiveType>&  lambda,
                        const std::vector<ActiveType>&  b,
                        const std::vector<ActiveType>&  eta,
                        const ActiveType                L,
                        const ActiveType                theta,
                        const ActiveType                m,
                        const ActiveType                z0,
                        const ActiveType                rho,
                        const ActiveType                S0,
                        const VolEvolv                  volEvolv = VolEvolv::FullTruncation )
            :    pwc_(new PieceWiseConstant(times,lambda,b,eta,L,theta,m,z0,rho,S0)),
                    mp_(new MidPointIntegration(this,times)), volEvolv_(volEvolv) {}
        // inspectors
        virtual ActiveType  lambda( const DateType t)  { return pwc_->lambda(t);    }
        virtual ActiveType  b(      const DateType t)  { return pwc_->b(t) ;        }
        virtual ActiveType  eta(    const DateType t)  { return pwc_->eta(t);       }
        virtual ActiveType  L()                        { return pwc_->L();          }
        virtual ActiveType  theta()                    { return pwc_->theta();      }
        virtual ActiveType  m()                        { return pwc_->m();          }
        virtual ActiveType  z0()                       { return pwc_->z0();         }
        virtual ActiveType  rho()                      { return pwc_->rho();        }
        virtual ActiveType  S0()                       { return pwc_->S0();         }
        virtual VolEvolv    volEvolv()                 { return volEvolv_;          }
        // averaging formula implementations
        virtual ActiveType  averageLambda( const DateType T) { return mp_->averageLambda( T ); }
        virtual ActiveType  averageB     ( const DateType T) { return mp_->averageB( T );      }
        virtual ActiveType  averageEta   ( const DateType T) { return mp_->averageEta( T );    }
    };

}

#undef _MIN_
#undef _MAX_

#endif  /* ifndef quantlib_templatetdstochvolmodel_hpp */


#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>
#include <bitset>

namespace mp = boost::multiprecision;
using namespace std;

int main(void) {
    string s;
    cin >> s;

    int a = 0;
    int b = 0;
    for(int i = 0 ; i < s.length() ; ++i) {
        if(s[i] == '0') {
            if(i & 0x1) { ++a; }
            else { ++b; }
        } else {
            if(i & 0x1) { ++b; }
            else { ++a; }
        }
    }
    auto ans = min(a, b);
    cout << ans << endl;

    return 0;
}


#include <fstream>
#include <bits/stdc++.h>
#include <iostream>
#include <string>
#include <vector>
#include <sstream>
#include <algorithm>
#include <math.h>
#include <cstring>
#include <queue>
#include <stack>
#include <map>
#include <Eigen/Dense>
using namespace std;
#define INF 0x3f3f3f3f
typedef Eigen::Vector3i beacon;
typedef Eigen::Matrix3i Perm;
typedef tuple<int,int,int> triplet;

vector<beacon> generatePerm(beacon input){

    auto roll = [](beacon in){
        beacon res = {in(0),in(2),-in(3)};
        return res;
    };
    auto turn = [](beacon in){
        beacon res = {-in(1),in(0),in(2)};
        return res;
    };
    vector<beacon> res;
    for(int c = 0; c < 2; c++){
        for(int s = 0; s < 3; s++){
            input = roll(input);
            res.push_back(input);
            for(int i = 0; i < 3; i++){
                input = turn(input);
                res.push_back(input);
            }
        }
        input = roll(turn(roll(input)));
    }
    return res;
}

bool read_input(vector<vector<beacon>> &input){
    string filename = "inputPartOne.txt";
    ifstream input_file(filename);
    int i = 0;
    string temp1;
    getline(input_file,temp1);
    vector<beacon> row;
    for(string line;getline(input_file, line);){
        int n = line.size();
        char arr[n+1];
        strcpy(arr,line.c_str());
        if(arr[0] == '-' && arr[1] == '-' && arr[2] == '-'){
            i++;
            continue;
        }
        if(n == 0){
            input.push_back(row);
            row.clear();
            continue;
        }
        stringstream ss(line);
        std::string s;

        getline(ss,s,',');
        int x = stoi(s);
        getline(ss,s,',');
        int y = stoi(s);
        getline(ss,s,',');
        int z = stoi(s);
        beacon t = {x,y,z};
        row.push_back(t);
    }
    return false;
}

triplet convBeacon(beacon in){
    return make_tuple(in(0),in(1),in(2));
}
beacon convTriplet(triplet in){
    int x,y,z;
    std::tie(x,y,z) = in;
    beacon res;
    res << x,y,z;
    return res;
}
beacon calc(beacon a, beacon b){
    beacon res;
    res = a + b ;

    return res;
}
void solve(vector<vector<beacon>> input){
    vector<Perm> rot;
    beacon p;
    vector<beacon> flips;
    Perm one, two, three, four, five , six;
    beacon a,b,c,d,e,f,g,h;
    a << 1,1,1;
    b << 1,1,-1; c << 1,-1,1; d << -1,1,1; e << 1,-1,-1; f << -1,-1,1;
    g << -1,1,-1; h << -1,-1,-1;
    flips.push_back(a);flips.push_back(b);flips.push_back(c);flips.push_back(d);
    flips.push_back(e);flips.push_back(f);flips.push_back(g);flips.push_back(h);
    one << 1, 0, 0 , 0 , 1 , 0, 0 , 0 , 1;
    two << 1,0,0,0,0,1,0,1,0;
    three << 0,1,0,1,0,0,0,0,1;
    four << 0,1,0,0,0,1,1,0,0;
    five << 0,0,1,0,1,0,1,0,0;
    six << 0,0,1,1,0,0,0,1,0;
    //cout << one << endl << two << endl << three << endl << four << endl << five << endl << six << endl;
    rot.push_back(one);rot.push_back(two);rot.push_back(three);rot.push_back(four);
    rot.push_back(five);rot.push_back(six);
    vector<Perm> perms;
    for(Perm A: rot){
        for(beacon B: flips){

            Perm res = B.asDiagonal() * A;
            perms.push_back(res);
        }
    }
    int n = input.size();
    vector<beacon> pos(n);
    vector<int> rotation(n, -1);
    vector<bool> vis(n, false);
    pos[0] = {0,0,0};
    rotation[0] = 0;
    vis[0] = true;
    for(int scanner1 = 0; scanner1 < n; scanner1++){
        if(!vis[scanner1])
            continue;
        for(int scanner2 = scanner1+1; scanner2 < n; scanner2++){
            bool foundWinnter = false;
                for(int rot2 = 0; rot2 < 48; rot2++){
                    map<triplet, int> diffCounter;
                    for(int beacon1 = 0; beacon1 < input[scanner1].size(); beacon1++){
                        for(int beacon2 = 0; beacon2 < input[scanner2].size(); beacon2++){
                            beacon b1 = input[scanner1][beacon1];
                            beacon b2 = perms[rot2] * input[scanner2][beacon2];
                            beacon diff = (b1 - b2);
                            triplet diffForMap = convBeacon(diff);
                            diffCounter[diffForMap]++;
                            if(diffCounter[diffForMap] >= 12){
                                foundWinnter = true;
                                beacon relDist = pos[scanner1] - diff;
                                beacon relDist2 = pos[scanner1] + diff;
                                beacon relDist3 = diff - pos[scanner1] ;
                                cout << " found match: " << scanner1 << " " << scanner2 << endl;
                                cout << pos[scanner1].transpose() << " pos Scanner 1 "<< endl;
                                cout << diff.transpose() << " diff" << endl;
                                cout << (relDist).transpose() << " relDist"  << endl;
                                cout << (relDist2).transpose() << " relDist2"  << endl;
                                cout << (relDist3).transpose() << " relDist3"  << endl;
                                cout << endl;
                                //cout << perms[rot2]<< endl;
                                rotation[scanner2] = rot2;
                                pos[scanner2] = diff;

                            }
                        }
                        if(foundWinnter)
                            break;
                    }
                    if(foundWinnter)
                    break;
                }
            if(!foundWinnter)
                continue;
            vector<beacon> curr = input[scanner2];
            int rot = rotation[scanner2];
            beacon posMe = pos[scanner2];
            for(int i = 0; i < curr.size(); i++){
                curr[i] = perms[rot] * curr[i];
                curr[i] += posMe;
                cout << " " << curr[i].transpose() << endl;
            }
            vis[scanner2] = true;
        }
    }

}

int main(){
    vector<vector<beacon>> input;
    read_input(input);
    solve(input);

}


#include "camera_array/RandomArrayManager.h"
#include "argus_utils/random/RandomUtils.hpp"
#include <boost/foreach.hpp>

namespace argus
{

RandomArrayManager::RandomArrayManager( const ros::NodeHandle& nh,
                                        const ros::NodeHandle& ph )
: CameraArrayManager( nh, ph )
{

	boost::random::random_device rng;
	generator.seed( rng );

	cameraNames.reserve( cameraRegistry.size() );
	BOOST_FOREACH( const CameraRegistry::value_type& item, cameraRegistry )
	{
		cameraNames.push_back( item.first );
	}

  double updateRate;
  ph.param<double>( "update_rate", updateRate, 1.0 );
  timer = std::make_shared<ros::Timer>
    ( nodeHandle.createTimer( ros::Duration( 1.0/updateRate ),
			      &RandomArrayManager::TimerCallback,
			      this ) );

}

void RandomArrayManager::TimerCallback( const ros::TimerEvent& event )
{
	std::vector<unsigned int> cameraInds;
	BitmapSampling( cameraNames.size(), maxNumActive, cameraInds, generator );

	CameraSet active;
	for( unsigned int i = 0; i < cameraInds.size(); i++ )
	{
		active.insert( cameraNames[ cameraInds[i] ] );
		ROS_INFO_STREAM( "Activating " << cameraNames[ cameraInds[i] ] );
	}
	SetActiveCameras( active );
}

}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ARITHMETIC_FUNCTION_SCALAR_TWO_PROD_HPP_INCLUDED
#define NT2_TOOLBOX_ARITHMETIC_FUNCTION_SCALAR_TWO_PROD_HPP_INCLUDED
#include <nt2/sdk/meta/adapted_traits.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/include/functions/is_invalid.hpp>
#include <nt2/include/functions/two_split.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is fundamental_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::two_prod_, tag::cpu_,
                          (A0)(A1),
                          (real_<A0>)(real_<A1>)
                         )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::two_prod_(tag::real_,tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)>
    {
      typedef typename std::tr1::result_of<meta::floating(A0, A1)>::type rtype;
      typedef typename boost::fusion::tuple<rtype,rtype>              type;
    };

    NT2_FUNCTOR_CALL(2)
    {
      typename NT2_RETURN_TYPE(2)::type res;
      eval(a0,a1, boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
    private:
    template<class A0,class A1,class R0,class R1> inline void
    eval(A0 const& a, A1 const& b,R0& r0, R1& r1)const
    {
      r0  = a*b;
      if (is_invalid(r0))
	{
	  r1 = Zero<R1>();
	  return;
	}
      A0 a1, a2, b1, b2;
      boost::fusion::tie(a1, a2) = two_split(a);
      boost::fusion::tie(b1, b2) = two_split(b);
      r1 = a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2);
    }
  };
} }

#endif
// modified by jt the 26/12/2010


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "Feature.h"

#include <Eigen/Dense>
#include <Core/Utility/Console.h>
#include <Core/Geometry/PointCloud.h>
#include <Core/Geometry/KDTreeFlann.h>

namespace three {

namespace {

Eigen::Vector4d ComputePairFeatures(const Eigen::Vector3d &p1,
		const Eigen::Vector3d &n1, const Eigen::Vector3d &p2,
		const Eigen::Vector3d &n2)
{
	Eigen::Vector4d result;
	Eigen::Vector3d dp2p1 = p2 - p1;
	result(3) = dp2p1.norm();
	if (result(3) == 0.0) {
		return Eigen::Vector4d::Zero();
	}
	auto n1_copy = n1;
	auto n2_copy = n2;
	double angle1 = n1_copy.dot(dp2p1) / result(3);
	double angle2 = n2_copy.dot(dp2p1) / result(3);
	if (acos(fabs(angle1)) > acos(fabs(angle2))) {
		n1_copy = n2;
		n2_copy = n1;
		dp2p1 *= -1.0;
		result(2) = -angle2;
	} else {
		result(2) = angle1;
	}
	auto v = dp2p1.cross(n1_copy);
	double v_norm = v.norm();
	if (v_norm == 0.0) {
		return Eigen::Vector4d::Zero();
	}
	v /= v_norm;
	auto w = n1_copy.cross(v);
	result(1) = v.dot(n2_copy);
	result(0) = atan2(w.dot(n2_copy), n1_copy.dot(n2_copy));
	return result;
}

std::shared_ptr<Feature> ComputeSPFHFeature(const PointCloud &input,
		const KDTreeFlann &kdtree, const KDTreeSearchParam &search_param)
{
	auto feature = std::make_shared<Feature>();
	feature->Resize(33, (int)input.points_.size());
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)input.points_.size(); i++) {
		const auto &point = input.points_[i];
		const auto &normal = input.normals_[i];
		std::vector<int> indices;
		std::vector<double> distance2;
		if (kdtree.Search(point, search_param, indices, distance2) > 1) {
			// only compute SPFH feature when a point has neighbors
			double hist_incr = 100.0 / (double)(indices.size() - 1);
			for (size_t k = 1; k < indices.size(); k++) {
				// skip the point itself, compute histogram
				auto pf = ComputePairFeatures(point, normal,
						input.points_[indices[k]], input.normals_[indices[k]]);
				int h_index = (int)(floor(11 * (pf(0) + M_PI) / (2.0 * M_PI)));
				if (h_index < 0) h_index = 0;
				if (h_index >= 11) h_index = 10;
				feature->data_(h_index, i) += hist_incr;
				h_index = (int)(floor(11 * (pf(1) + 1.0) * 0.5));
				if (h_index < 0) h_index = 0;
				if (h_index >= 11) h_index = 10;
				feature->data_(h_index + 11, i) += hist_incr;
				h_index = (int)(floor(11 * (pf(2) + 1.0) * 0.5));
				if (h_index < 0) h_index = 0;
				if (h_index >= 11) h_index = 10;
				feature->data_(h_index + 22, i) += hist_incr;
			}
		}
	}
	return feature;
}

class ProjectedPoints
{
public:
	int id;		// global point ID
	double u;	// u coordinate on the projected patch
	double v;	// v coordinate on the projected patch
	double depth;
};

std::vector<ProjectedPoints> PlanarParameterizationForOnePoint(
		size_t point_id, const PointCloud &cloud, const KDTreeFlann &kdtree,
		const KDTreeSearchParamHybrid &search_param,
		const PlanarParameterizationOption &option)
{
	const int patch_half_size = option.half_patch_size_;
	const int patch_size = patch_half_size * 2 + 1;
	const double patch_half_diag_length = sqrt((2.0 * pow(patch_half_size, 2.0)));

	std::vector<int> indices;
	std::vector<double> distance2;
	Eigen::Matrix3d tangential_axis;
	Eigen::Vector3d vt, vt_n, vt_t1, vt_t2, vt_adj, vt_proj;
	Eigen::Vector2d vt_val = Eigen::Vector2d::Zero();
	Eigen::Vector2i vt_val_descrete = Eigen::Vector2i::Zero();
	Eigen::MatrixXd patch = Eigen::MatrixXd::Zero(patch_size, patch_size);
	double depth = -1.0;
	int adj_id = -1;
	vt = cloud.points_[point_id];

	// giving enough search radius to give enough space in the boundry pixels
	KDTreeSearchParamHybrid search_param_buffer = search_param;
	search_param_buffer.radius_ = search_param.radius_ * 2.0;
	int number_of_searched_neighbors =
			kdtree.Search(vt, search_param_buffer, indices, distance2);
	std::vector<ProjectedPoints> output;
	output.resize(number_of_searched_neighbors);

	if (number_of_searched_neighbors >= 1) {
		tangential_axis = ComputeTangentialAxis(cloud, indices);
		vt_n = tangential_axis.col(0);
		vt_t1 = tangential_axis.col(1);
		vt_t2 = tangential_axis.col(2);
		// project points onto tangential PlanarParameterization
		for (int j = 0; j < number_of_searched_neighbors; j++){
			adj_id = indices[j];
			vt_adj = cloud.points_[adj_id];
			depth = (vt_adj - vt).dot(vt_n);
			vt_proj = vt_adj - depth * vt_n;
			vt_val(0) = (vt_proj - vt).dot(vt_t1);
			vt_val(1) = (vt_proj - vt).dot(vt_t2);
			vt_val = vt_val / search_param.radius_ * patch_half_diag_length;
			output[j].u = vt_val(0);
			output[j].v = vt_val(1);
			output[j].depth = depth;
			output[j].id = adj_id;
		}
	}
	return std::move(output);
}

class ProcessedPatch
{
public:
	// kernel x kernel size, local depth map
	Eigen::MatrixXd depth_map;
	// kernel x kernel size, point cloud index
	std::vector<Eigen::MatrixXi> index_map;
	// kernel x kernel size, point cloud weight to adjacent index
	std::vector<Eigen::MatrixXd> weight_map;
};

std::shared_ptr<ProcessedPatch> PlanarParameterizationForOnePatch(
		const std::vector<ProjectedPoints> &projected_points,
		const PlanarParameterizationOption &option)
{
	const int number_of_neighbors = option.number_of_neighbors_;
	const int patch_half_size = option.half_patch_size_;
	const int patch_size = patch_half_size * 2 + 1;
	const double sigma_pow2 = option.sigma_ * option.sigma_;

	auto output = std::make_shared<ProcessedPatch>();
	output->index_map.resize(number_of_neighbors);
	output->weight_map.resize(number_of_neighbors);
	for (int i = 0; i < number_of_neighbors; i++){
		output->index_map[i].resize(patch_size, patch_size);
		output->weight_map[i].resize(patch_size, patch_size);
		output->index_map[i].setConstant(-1);
		output->weight_map[i].setZero();
	}
	output->depth_map.resize(patch_size, patch_size);

	const int number_of_points = int(projected_points.size());
	Eigen::MatrixXd data = Eigen::MatrixXd::Zero(2, number_of_points);
	int cnt = 0;
	for (int i = 0; i < (int)projected_points.size(); i++){
		data(0,i) = projected_points[i].u;
		data(1,i) = projected_points[i].v;
	}
	KDTreeFlann kdtree;
	kdtree.SetMatrixData(data);
	std::vector<int> indices;
	std::vector<double> distance2;
	for (int u = 0; u < (int)patch_size; u++){
		for (int v = 0; v < (int)patch_size; v++){
			Eigen::Vector2d query;
			query(0) = u - patch_half_size;
			query(1) = v - patch_half_size;
			// todo: should we use SearchKNN here? maybe fill in blank
			int number_of_searched_neighbors = kdtree.SearchKNN(
					query, number_of_neighbors, indices, distance2);
			double sum_weight = 0.0, sum_weighted_val = 0.0;
			for (int i = 0; i < number_of_searched_neighbors; i++) {
				int i_adj = indices[i];
				double dist_adj = distance2[i];
				double weight_i_th_adj = exp(-dist_adj/sigma_pow2);
				double depth_adj = projected_points[i_adj].depth;
				sum_weight += weight_i_th_adj;
				sum_weighted_val += weight_i_th_adj * depth_adj;
				output->index_map[i](v, u) = projected_points[i_adj].id;
				output->weight_map[i](v, u) = weight_i_th_adj;
			}
			// normalize weight_map
			if (sum_weight != 0.0){
				for (int i = 0; i < number_of_searched_neighbors; i++) {
					output->weight_map[i](v, u) /= sum_weight;
				}
			}
			if (number_of_searched_neighbors >= 1){
				if (option.depth_densify_method_ ==
					depth_densify_gaussian_kernel && sum_weight != 0.0)
					output->depth_map(v, u) = sum_weighted_val / sum_weight;
				else if (option.depth_densify_method_ ==
						depth_densify_nearest_neighbor)
					output->depth_map(v, u) =
					projected_points[indices[0]].depth;
			}
		}
	}
	return output;
}

}	// unnamed namespace

std::shared_ptr<Feature> ComputeFPFHFeature(const PointCloud &input,
		const KDTreeSearchParam &search_param/* = KDTreeSearchParamKNN()*/)
{
	auto feature = std::make_shared<Feature>();
	feature->Resize(33, (int)input.points_.size());
	if (input.HasNormals() == false) {
		PrintDebug("[ComputeFPFHFeature] Failed because input point cloud has no normal.\n");
		return feature;
	}
	KDTreeFlann kdtree(input);
	auto spfh = ComputeSPFHFeature(input, kdtree, search_param);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < (int)input.points_.size(); i++) {
		const auto &point = input.points_[i];
		std::vector<int> indices;
		std::vector<double> distance2;
		if (kdtree.Search(point, search_param, indices, distance2) > 1) {
			double sum[3] = {0.0, 0.0, 0.0};
			for (size_t k = 1; k < indices.size(); k++) {
				// skip the point itself
				double dist = distance2[k];
				if (dist == 0.0)
					continue;
				for (int j = 0; j < 33; j++) {
					double val = spfh->data_(j, indices[k]) / dist;
					sum[j / 11] += val;
					feature->data_(j, i) += val;
				}
			}
			for (int j = 0; j < 3; j++)
				if (sum[j] != 0.0) sum[j] = 100.0 / sum[j];
			for (int j = 0; j < 33; j++) {
				feature->data_(j, i) *= sum[j / 11];
				// The commented line is the fpfh function in the paper.
				// But according to PCL implementation, it is skipped.
				// Our initial test shows that the full fpfh function in the
				// paper seems to be better than PCL implementation. Further
				// test required.
				feature->data_(j, i) += spfh->data_(j, i);
			}
		}
	}
	return feature;
}

std::shared_ptr<PlanarParameterizationOutput> PlanarParameterization(
		const PointCloud &cloud,
		const KDTreeSearchParamHybrid &search_param,
		const PlanarParameterizationOption &option)
{
	// parameters
	const int patch_size = option.half_patch_size_ * 2 + 1;
	const int patch_size_pow2 = patch_size * patch_size;
	const int number_of_points = (int)cloud.points_.size();

	auto output = std::make_shared<PlanarParameterizationOutput>();
	output->depth_.Resize(patch_size_pow2, number_of_points);
	for (int i = 0; i < option.number_of_neighbors_; i++){
		Feature data_weight;
		data_weight.Resize(patch_size_pow2, number_of_points);
		output->weight_.push_back(data_weight);
		Eigen::MatrixXi data_index;
		data_index.resize(patch_size_pow2, number_of_points);
		output->index_.push_back(data_index);
	}
	KDTreeFlann kdtree;
	kdtree.SetGeometry(cloud);

#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
	for (int i = 0; i < number_of_points; i++) {
		auto projected_points = PlanarParameterizationForOnePoint(
				i, cloud, kdtree, search_param, option);
		auto information_patch = PlanarParameterizationForOnePatch(
				projected_points, option);
		for (int j = 0; j < option.number_of_neighbors_; j++){
			for (int k = 0; k < patch_size_pow2; k++){
				output->weight_[j].data_(k, i) =
						information_patch->weight_map[j](k);
				output->index_[j](k, i) =
						information_patch->index_map[j](k);
			}
		}
		for (size_t k = 0; k < patch_size_pow2; k++){
			output->depth_.data_(k, i) = information_patch->depth_map(k);
		}
	}
	return output;
}

}	// namespace three


#include <NumCpp.hpp>

#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/filesystem.hpp>
#undef BOOST_NO_CXX11_SCOPED_ENUMS

#include <catch2/catch.hpp>

#include <iostream>

namespace {
template <typename T> void mark_unread(const T var) { (void)var; }
} // namespace

int main([[maybe_unused]] int argc, [[maybe_unused]] const char **argv) {
  // Containers
  [[maybe_unused]] nc::NdArray<int> a0 = {{1, 2}, {3, 4}};
  mark_unread(a0);
  [[maybe_unused]] nc::NdArray<int> a1 = {{1, 2}, {3, 4}, {5, 6}};
  a1.reshape(2, 3);
  [[maybe_unused]] auto a2 = a1.astype<double>();
  mark_unread(a2);

  // // Initializers
  auto a3 = nc::linspace<int>(1, 10, 5);
  auto a4 = nc::arange<int>(3, 7);
  auto a5 = nc::eye<int>(4);
  auto a6 = nc::zeros<int>(3, 4);
  auto a7 = nc::NdArray<int>(3, 4) = 0;
  auto a8 = nc::ones<int>(3, 4);
  auto a9 = nc::NdArray<int>(3, 4) = 1;
  auto a10 = nc::nans(3, 4);
  auto a11 = nc::NdArray<double>(3, 4) = nc::constants::nan;
  auto a12 = nc::empty<int>(3, 4);
  auto a13 = nc::NdArray<int>(3, 4);
  mark_unread(a3);
  mark_unread(a4);
  mark_unread(a5);
  mark_unread(a6);
  mark_unread(a7);
  mark_unread(a8);
  mark_unread(a9);
  mark_unread(a10);
  mark_unread(a11);
  mark_unread(a12);
  mark_unread(a13);

  // Slicing/Broadcasting
  auto a14 = nc::random::randInt<int>({10, 10}, 0, 100);
  auto value = a14(2, 3);
  auto slice = a14({2, 5}, {2, 5});
  auto rowSlice = a14(a14.rSlice(), 7);
  auto values = a14[a14 > 50];
  a14.putMask(a14 > 50, 666);
  mark_unread(value);
  mark_unread(slice);
  mark_unread(rowSlice);
  mark_unread(values);

  // random
  nc::random::seed(666);
  auto a15 = nc::random::randN<double>({3, 4});
  auto a16 = nc::random::randInt<int>({3, 4}, 0, 10);
  auto a17 = nc::random::rand<double>({3, 4});
  auto a18 = nc::random::choice(a17, 3);
  mark_unread(a15);
  mark_unread(a16);
  mark_unread(a17);
  mark_unread(a18);

  // Concatenation
  auto a = nc::random::randInt<int>({3, 4}, 0, 10);
  auto b = nc::random::randInt<int>({3, 4}, 0, 10);
  auto c = nc::random::randInt<int>({3, 4}, 0, 10);
  auto a19 = nc::stack({a, b, c}, nc::Axis::ROW);
  auto a20 = nc::vstack({a, b, c});
  auto a21 = nc::hstack({a, b, c});
  auto a22 = nc::append(a, b, nc::Axis::COL);
  mark_unread(a19);
  mark_unread(a20);
  mark_unread(a21);
  mark_unread(a22);

  // Diagonal, Traingular, and Flip
  auto d = nc::random::randInt<int>({5, 5}, 0, 10);
  auto a23 = nc::diagonal(d);
  auto a24 = nc::triu(a);
  auto a25 = nc::tril(a);
  auto a26 = nc::flip(d, nc::Axis::ROW);
  auto a27 = nc::flipud(d);
  auto a28 = nc::fliplr(d);
  mark_unread(a23);
  mark_unread(a24);
  mark_unread(a25);
  mark_unread(a26);
  mark_unread(a27);
  mark_unread(a28);

  // iteration
  for (auto it = a.cbegin(); it < a.cend(); ++it) {
    std::cout << *it << " ";
  }
  std::cout << std::endl;
  for (const auto &arrayValue : a) {
    std::cout << arrayValue << " ";
  }
  std::cout << std::endl;

  // Logical
  auto a29 = nc::where(a > 5, a, b);
  auto a30 = nc::any(a);
  auto a31 = nc::all(a);
  auto a32 = nc::logical_and(a, b);
  auto a33 = nc::logical_or(a, b);
  auto a34 = nc::isclose(a, b);
  auto a35 = nc::allclose(a, b);
  mark_unread(a29);
  mark_unread(a30);
  mark_unread(a31);
  mark_unread(a32);
  mark_unread(a33);
  mark_unread(a34);
  mark_unread(a35);

  // Comparisons
  auto a36 = nc::equal(a, b);
  auto a37 = a == b;
  auto a38 = nc::not_equal(a, b);
  auto a39 = a != b;
  auto [rows, cols] = nc::nonzero(a);
  mark_unread(a36);
  mark_unread(a37);
  mark_unread(a38);
  mark_unread(a39);
  mark_unread(rows);
  mark_unread(cols);

  // Minimum, Maximum, Sorting
  auto value1 = nc::min(a);
  auto value2 = nc::max(a);
  auto value3 = nc::argmin(a);
  auto value4 = nc::argmax(a);
  auto a41 = nc::sort(a, nc::Axis::ROW);
  auto a42 = nc::argsort(a, nc::Axis::COL);
  auto a43 = nc::unique(a);
  auto a44 = nc::setdiff1d(a, b);
  auto a45 = nc::diff(a);
  mark_unread(value1);
  mark_unread(value2);
  mark_unread(value3);
  mark_unread(value4);
  mark_unread(a41);
  mark_unread(a42);
  mark_unread(a43);
  mark_unread(a44);
  mark_unread(a45);

  // Reducers
  auto value5 = nc::sum<int>(a);
  auto a46 = nc::sum<int>(a, nc::Axis::ROW);
  auto value6 = nc::prod<int>(a);
  auto a47 = nc::prod<int>(a, nc::Axis::ROW);
  auto value7 = nc::mean(a);
  auto a48 = nc::mean(a, nc::Axis::ROW);
  auto value8 = nc::count_nonzero(a);
  auto a49 = nc::count_nonzero(a, nc::Axis::ROW);
  mark_unread(value5);
  mark_unread(value6);
  mark_unread(value7);
  mark_unread(value8);
  mark_unread(a46);
  mark_unread(a47);
  mark_unread(a48);
  mark_unread(a49);

  // I/O
  a.print();
  std::cout << a << std::endl;
  // auto tempDir = boost::filesystem::temp_directory_path();
  // auto tempTxt = (tempDir / "temp.txt").string();
  // a.tofile(tempTxt, "\n");
  // auto a50 = nc::fromfile<int>(tempTxt, "\n");
  // auto tempBin = (tempDir / "temp.bin").string();
  // nc::dump(a, tempBin);
  // auto a51 = nc::load<int>(tempBin);

  // Mathematical Functions

  // Basic Functions
  auto a52 = nc::abs(a);
  auto a53 = nc::sign(a);
  auto a54 = nc::remainder(a, b);
  auto a55 = nc::clip(a, 3, 8);
  auto xp = nc::linspace<double>(0.0, 2.0 * nc::constants::pi, 100);
  auto fp = nc::sin(xp);
  auto x = nc::linspace<double>(0.0, 2.0 * nc::constants::pi, 1000);
  auto f = nc::interp(x, xp, fp);
  mark_unread(a52);
  mark_unread(a53);
  mark_unread(a54);
  mark_unread(a55);
  mark_unread(f);

  // Exponential Functions
  auto a56 = nc::exp(a);
  auto a57 = nc::expm1(a);
  auto a58 = nc::log(a);
  auto a59 = nc::log1p(a);
  mark_unread(a56);
  mark_unread(a57);
  mark_unread(a58);
  mark_unread(a59);

  // Power Functions
  auto a60 = nc::power<int>(a, 4);
  auto a61 = nc::sqrt(a);
  auto a62 = nc::square(a);
  auto a63 = nc::cbrt(a);
  mark_unread(a60);
  mark_unread(a61);
  mark_unread(a62);
  mark_unread(a63);

  // Trigonometric Functions
  auto a64 = nc::sin(a);
  auto a65 = nc::cos(a);
  auto a66 = nc::tan(a);
  mark_unread(a64);
  mark_unread(a65);
  mark_unread(a66);

  // Hyperbolic Functions
  auto a67 = nc::sinh(a);
  auto a68 = nc::cosh(a);
  auto a69 = nc::tanh(a);
  mark_unread(a67);
  mark_unread(a68);
  mark_unread(a69);

  // Classification Functions
  auto a70 = nc::isnan(a.astype<double>());
  mark_unread(a70);

  // Linear Algebra
  auto a71 = nc::norm<int>(a);
  auto a72 = nc::dot<int>(a, b.transpose());
  auto a73 = nc::random::randInt<int>({3, 3}, 0, 10);
  auto a74 = nc::random::randInt<int>({4, 3}, 0, 10);
  auto a75 = nc::random::randInt<int>({1, 4}, 0, 10);
  auto value9 = nc::linalg::det(a73);
  mark_unread(a71);
  mark_unread(a72);
  mark_unread(a73);
  mark_unread(a74);
  mark_unread(a75);
  mark_unread(value9);

  auto a76 = nc::linalg::inv(a73);
  auto a77 = nc::linalg::lstsq(a74, a75);
  auto a78 = nc::linalg::matrix_power<int>(a73, 3);
  auto a79 = nc::linalg::multi_dot<int>({a, b.transpose(), c});
  nc::NdArray<double> u;
  nc::NdArray<double> s;
  nc::NdArray<double> vt;
  nc::linalg::svd(a.astype<double>(), u, s, vt);
  mark_unread(a76);
  mark_unread(a77);
  mark_unread(a78);
  mark_unread(a79);
  mark_unread(u);
  mark_unread(s);
  mark_unread(vt);

  std::cout << "Hello World! yay\n";
  return 0;
}


#include <iostream>
#include <armadillo>

#include "printing.h"
#include "CycleTimer.h"

void saxpy_arma_copy(float scale,
                     arma::Col<float> X,
                     arma::Col<float> Y,
                     arma::Col<float>& result)
{

  result = (scale * X) + Y;

  return;

}

void saxpy_arma_reference(float scale,
                          arma::Col<float>& X,
                          arma::Col<float>& Y,
                          arma::Col<float>& result)
{

  result = (scale * X) + Y;

  return;

}

int main()
{

  const size_t N = 50 * 1000 * 1000; // 50 M element vectors
  const size_t TOTAL_BYTES = 4 * N * sizeof(float);
  const size_t TOTAL_FLOPS = 2 * N;
  const size_t num_run_times = 5;

  printf("Number of repeats: %d\n", num_run_times);

  size_t i;

  float scale = 2.f;

  arma::Col<float> arrayX(N);
  arma::Col<float> arrayY(N);
  arma::Col<float> result(N);

  // initialize array values
  for (i = 0; i < N; i++) {
    arrayX(i) = i;
    arrayY(i) = i;
    result(i) = 0.f;
  }

  //
  // Run the serial Armadillo implementation, call by value.
  //
  double min_arma_serial_cbv = 1e30;
  for (i = 0; i < num_run_times; ++i) {
    double startTime = CycleTimer::currentSeconds();
    saxpy_arma_copy(scale, arrayX, arrayY, result);
    double endTime = CycleTimer::currentSeconds();
    min_arma_serial_cbv = std::min(min_arma_serial_cbv, endTime - startTime);
  }

  printf("[saxpy arma serial cbv]:\t\t[%.3f] ms\t[%.3f] GB/s\t[%.3f] GFLOPS\n",
         min_arma_serial_cbv * 1000,
         toBW(TOTAL_BYTES, min_arma_serial_cbv),
         toGFLOPS(TOTAL_FLOPS, min_arma_serial_cbv));

  // Clear out the buffer
  for (i = 0; i < N; i++)
    result(i) = 0.f;

  //
  // Run the serial Armadillo implementation, call by reference.
  //
  double min_arma_serial_cbr = 1e30;
  for (i = 0; i < num_run_times; ++i) {
    double startTime = CycleTimer::currentSeconds();
    saxpy_arma_reference(scale, arrayX, arrayY, result);
    double endTime = CycleTimer::currentSeconds();
    min_arma_serial_cbr = std::min(min_arma_serial_cbr, endTime - startTime);
  }

  printf("[saxpy arma serial cbr]:\t\t[%.3f] ms\t[%.3f] GB/s\t[%.3f] GFLOPS\n",
         min_arma_serial_cbr * 1000,
         toBW(TOTAL_BYTES, min_arma_serial_cbr),
         toGFLOPS(TOTAL_FLOPS, min_arma_serial_cbr));

  // Clear out the buffer
  for (i = 0; i < N; i++)
    result(i) = 0.f;

  return 0;

}


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

#include <cstdio>
#include <boost/thread/thread.hpp>
#include "defs.h"
#include "options.h"
#include "util.h"
#include "output.h"
#include "prng/splittable_mrg.h"
#include "prng/utils.h"

inline static vertex_t generate_vertex(mrg_state* pstate, const vertex_t nvertices)
{
  /* Generate a pseudorandom number in the range [0, vertices) without modulo bias. */
  vertex_t limit = VT_MAX % nvertices;
  vertex_t v;

  do {
    v = mrg_get_uint_orig(pstate);
#ifndef VERTEX_TYPE_32
    v = (v << 32) + mrg_get_uint_orig(pstate);
#endif
  } while (UNLIKELY(v < limit));
  return v % nvertices;
}

static void generate(thread_buffer* buffer, const mrg_state& state, const vertex_t nvertices,
                     const edge_t start, const edge_t end, const bool allow_self_loops, const bool symmetric)
{
  for (edge_t ei = start; ei < end; ++ei)
  {
    mrg_state new_state = state;
    mrg_skip(&new_state, 0, ei, 0);
    struct edge_struct* edge = buffer->edge_struct();
    edge->src = generate_vertex(&new_state, nvertices);
    do {
      edge->dst = generate_vertex(&new_state, nvertices);
    } while (UNLIKELY(edge->src == edge->dst && !allow_self_loops));
#ifdef WEIGHT
    edge->weight = (value_t)mrg_get_double_orig(&new_state);
#endif
    if (symmetric) {
      struct edge_struct* reverse_edge = buffer->edge_struct();
      reverse_edge->src = edge->dst;
      reverse_edge->dst = edge->src;
#ifdef WEIGHT
      reverse_edge->weight = edge->weight;
#endif
    }
  }
  buffer->flush();
}

static void generate_bipartite(thread_buffer* buffer, const mrg_state& state, const vertex_t nleft, const vertex_t nvertices,
                               const edge_t start, const edge_t end, const bool symmetric)
{
  for (edge_t ei = start; ei < end; ++ei)
  {
    mrg_state new_state = state;
    mrg_skip(&new_state, 0, ei, 0);
    struct edge_struct* edge = buffer->edge_struct();
    edge->src = generate_vertex(&new_state, nvertices);
    if (edge->src < nleft) {
      edge->dst = nleft + generate_vertex(&new_state, nvertices - nleft);
    } else {
      edge->dst = generate_vertex(&new_state, nleft);
    }
#ifdef WEIGHT
    edge->weight = (value_t)mrg_get_double_orig(&new_state);
#endif
    if (symmetric) {
      struct edge_struct* reverse_edge = buffer->edge_struct();
      reverse_edge->src = edge->dst;
      reverse_edge->dst = edge->src;
#ifdef WEIGHT
      reverse_edge->weight = edge->weight;
#endif
    }
  }
  buffer->flush();
}

int main(int argc, char** argv)
{
  struct options options;
  if (process_options(argc, argv, false, &options) != 0)
    return 0;

  uint_fast32_t seed[5];
  make_mrg_seed(options.rng.userseed1, options.rng.userseed2, seed);
  mrg_state state;
  mrg_seed(&state, seed);
  //mrg_skip(&new_state, 50, 7, 0); // Do an initial skip?

  edge_t total_edges = options.erdos_renyi.edges;
  if (options.global.symmetric) {
    total_edges *= 2;
  }

  printf("Generator type: Erdos-Renyi\n");
  printf("Vertices: %" PRIvt "\n", options.erdos_renyi.vertices);
  printf("Edges: %" PRIet "\n", total_edges);
  printf("Self-loops:%s allowed\n", (options.erdos_renyi.self_loops ? "" : " not"));

  double start = get_time();

  // io thread
  size_t buffer_size = calculate_buffer_size(options.global.buffer_size);
  buffer_queue flushq;
  buffer_manager manager(&flushq, options.global.buffers_per_thread, buffer_size);
  io_thread_func io_func(options.global.graphname.c_str(), total_edges, &flushq, &manager, buffer_size);
  boost::thread io_thread(boost::ref(io_func));

  // worker threads
  int nthreads = options.global.nthreads;
  edge_t edges_per_thread = options.erdos_renyi.edges / nthreads;
  threadid_t* workers[nthreads];
  boost::thread* worker_threads[nthreads];
  for (int i = 0; i < nthreads; i++) {
    workers[i] = new threadid_t(i);
    thread_buffer* buffer = manager.register_thread(*workers[i]);
    // last thread gets the remainder (if any)
    edge_t start = i * edges_per_thread;
    edge_t end = (i == nthreads-1) ? (options.erdos_renyi.edges) : ((i+1) * edges_per_thread);
    if (options.erdos_renyi.bipartite) {
      worker_threads[i] = new boost::thread(
        generate_bipartite, buffer,
        state, options.erdos_renyi.bipartite, options.erdos_renyi.vertices, start, end, options.global.symmetric
      );
    } else {
      worker_threads[i] = new boost::thread(
        generate, buffer,
        state, options.erdos_renyi.vertices, start, end, options.erdos_renyi.self_loops, options.global.symmetric
      );
    }
  }

  // Wait until work completes
  for (int i = 0; i < nthreads; i++) {
    worker_threads[i]->join();
  }
  io_func.stop();
  io_thread.join();

  // cleanup
  for (int i = 0; i < nthreads; i++) {
    manager.unregister_thread(*workers[i]);
    delete worker_threads[i];
    delete workers[i];
  }

  double elapsed = get_time() - start;
  printf("Generation time: %fs\n", elapsed);

  make_ini_file(options.global.graphname.c_str(), options.erdos_renyi.vertices, total_edges);

  return 0;
}


#ifndef CPU_LR_HOG_DETECTOR_H
#define CPU_LR_HOG_DETECTOR_H
#include <Eigen/Dense>
#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/ximgproc/segmentation.hpp>
#include <iostream>
#include <random>
#include <chrono>
#include <string>
#include "../utils/c_utils.hpp"
#include "../libs/nms/nms.hpp"
#include "../dpp/dpp.hpp"
#include "../likelihood/logistic_regression.hpp"
#include "../likelihood/CPU_logistic_regression.hpp"
#include "../libs/piotr_fhog/fhog.hpp"

struct Args {
	bool make_gray;
    bool resize_src;
    int hog_width;
    int hog_height;
    double gr_threshold;
    double hit_threshold;
    int n_orients;
    int bin_size;
    double overlap_threshold;
    double p_accept;
    double lambda;
    double epsilon;
    double tolerance;
    int n_iterations;
} ;

using namespace cv;
using namespace std;
using namespace Eigen;
using namespace cv::ximgproc::segmentation;
class CPU_LR_HOGDetector
{
public:
	void init(double group_threshold, double hit_threshold);
	vector<Rect> detect(Mat &frame);
	double train();
	VectorXd getFeatures(Mat &frame);
    vector<double> getWeights();
	void generateFeatures(Mat &frame, double label);
	void dataClean();
	void draw();
	void saveToCSV(string name, bool append = true);
	void loadModel(VectorXd weights,VectorXd featureMean, VectorXd featureStd, VectorXd featureMax, VectorXd featureMin, double bias);
	void loadFeatures(MatrixXd features, VectorXd labels);
	VectorXd predictTest(MatrixXd features,bool data_processing);
	MatrixXd feature_values;
	vector<double> weights;

protected:
	Args args;
	VectorXd genHog(Mat &frame);
	VectorXd genRawPixels(Mat &frame);
	HOGDescriptor hog;
	CPU_LogisticRegression logistic_regression;
	int num_frame=0;
	double max_value=1.0;
	int group_threshold;
	double hit_threshold;
	int n_descriptors, n_data;
	vector<Rect> detections;
	VectorXd labels;
	Mat frame;
	C_utils tools;
	mt19937 generator;
	bool initialized;

};

#endif


#include <math.h>
#include <limits>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>

#include <boost/lexical_cast.hpp>

#include <opencv2/plot.hpp>

#include <miniking_ros/AcousticBeam.h>
#include <collision_avoidance/ObstacleInfo.h>

class SonarObstacleDetector
{
  ros::NodeHandle nh_;
  ros::NodeHandle nh_private_;

  ros::Subscriber acousticbeam_sub_;
  ros::Publisher  pub_obstacle_info_;
  ros::Publisher  intensities_pub_;
  ros::Publisher  peakdetector_pub_;

  // Operational parameters
  double min_range_;
  double max_range_;
  double min_obstacle_size_;

public:
  SonarObstacleDetector() : nh_private_("~")
  {
    // Params
    nh_private_.param("min_range", min_range_, 2.0);
    nh_private_.param("min_obstacle_size", min_obstacle_size_, 0.06);

    // Topics
    acousticbeam_sub_   = nh_.subscribe("/sonar", 1, &SonarObstacleDetector::acousticbeamCb, this);
    pub_obstacle_info_  = nh_private_.advertise<collision_avoidance::ObstacleInfo>("obstacle_info", 2, true);
    intensities_pub_    = nh_private_.advertise<sensor_msgs::Image>("intensities", 2, true);
    peakdetector_pub_   = nh_private_.advertise<sensor_msgs::Image>("peak_detector", 2, true);
  }

  void acousticbeamCb(const miniking_ros::AcousticBeamConstPtr beam)
  {
    // Copy and convert
    std::vector<double> intensities;
    std::vector<unsigned char> input_intensities(beam->intensities);
    for (size_t i=0; i<input_intensities.size(); i++)
    {
      int tmp = (int)input_intensities[i];
      intensities.push_back(boost::lexical_cast<double>(tmp));
    }

    // Remove intensities in the minimum range
    max_range_ = beam->range_max;
    float distance_btw_bins = max_range_/beam->bins;
    int N = round(min_range_/distance_btw_bins);
    intensities.erase(intensities.begin(), intensities.begin() + N);

    // Create the ranges vector
    std::vector<double> ranges;
    for (double r=min_range_; r<=max_range_; r=r+distance_btw_bins)
      ranges.push_back(r);

    // Low pass filter
    std::vector<double> filtered_intensities = lowPassFilter(intensities);

    // Publish debugging images
    if (intensities_pub_.getNumSubscribers() > 0)
    {
      cv::Mat original = drawGraph(ranges, intensities, cv::Scalar(50, 50, 255));
      cv::Mat filtered = drawGraph(ranges, filtered_intensities, cv::Scalar(50, 255, 50));

      // Combine 2 images
      cv::Mat combined;
      double alpha = 0.5;
      double beta = ( 1.0 - alpha );
      cv::addWeighted( original, alpha, filtered, beta, 0.0, combined);

      // Publish
      cv_bridge::CvImage ros_image;
      ros_image.image = combined.clone();
      ros_image.header.stamp = ros::Time::now();
      ros_image.encoding = "bgr8";
      intensities_pub_.publish(ros_image.toImageMsg());
    }

    if (peakdetector_pub_.getNumSubscribers() > 0)
    {
      cv::Mat combined = drawGraph(ranges, filtered_intensities, cv::Scalar(50, 255, 50));

      // Calc peaks
      std::vector<double> x_peak, y_peak;
      peakDetector(ranges, filtered_intensities, x_peak, y_peak);
      for (size_t i=0; i<x_peak.size(); i++)
      {
        // Convert point to image coordinates
        double u = (x_peak[i] - min_range_) * combined.cols / (max_range_ - min_range_); //combined.cols * x_peak[i] / (max_range_-min_range_);
        double v = combined.rows * (100.0-y_peak[i]) / 100;
        cv::Point2d p(u,v);
        cv::circle(combined, p, 5, cv::Scalar(50, 255, 50));
      }

      // Publish
      cv_bridge::CvImage ros_image;
      ros_image.image = combined.clone();
      ros_image.header.stamp = ros::Time::now();
      ros_image.encoding = "bgr8";
      peakdetector_pub_.publish(ros_image.toImageMsg());
    }

  }

  std::vector<double> lowPassFilter(const std::vector<double>& input,
                                    const int& order = 10)
  {

    // Get the maximum
    double max_in = *std::max_element(input.begin(), input.end());

    std::vector<double> output;
    for (size_t n=0; n<input.size(); n++)
    {
      size_t idx = n;
      double sum = 0.0;
      for (size_t k=0; k<order; k++)
      {
        if (idx < 0) break;
        sum += 0.01 * input[idx];
        idx--;
      }
      output.push_back(sum);
    }

    // Preserve scale
    double max_out = *std::max_element(output.begin(), output.end());
    double gain = max_in / max_out;
    for (size_t i=0; i<output.size(); i++)
      output[i] = output[i] * gain;

    return output;
  }

  void peakDetector(const std::vector<double>& x,
                    const std::vector<double>& y,
                    std::vector<double>& x_peak,
                    std::vector<double>& y_peak,
                    const double& delta = 0.1)
  {
    x_peak.clear();
    y_peak.clear();

    double mn = std::numeric_limits<double>::max();
    double mx = std::numeric_limits<double>::min();
    double mxpos = std::numeric_limits<double>::quiet_NaN();
    double mnpos = std::numeric_limits<double>::quiet_NaN();
    bool lookformax = true;

    for (size_t i=0; i<y.size(); i++)
    {
      if (y[i] > mx)
      {
        mx = y[i];
        mxpos = x[i];
      }
      if (y[i] < mn)
      {
        mn = y[i];
        mnpos = x[i];
      }

      if (lookformax)
      {
        if (y[i] < mx-delta)
        {
          x_peak.push_back(mxpos);
          y_peak.push_back(mx);
          mn = y[i];
          mnpos = x[i];
          lookformax = false;
        }
      }
      else
      {
        if (y[i] > mn+delta)
        {
          mx = y[i];
          mxpos = x[i];
          lookformax = true;
        }
      }
    }
  }

  cv::Mat drawGraph(const std::vector<double>& x,
                    const std::vector<double>& y,
                    const cv::Scalar& line_color,
                    const int& line_width = 2)
  {
    // Convert to cv::Mat
    cv::Mat x_data( x.size(), 1, CV_64F );
    cv::Mat y_data( y.size(), 1, CV_64F );
    for (size_t i=0; i<x.size(); i++)
      x_data.at<double>(i,0) = x[i];
    for (size_t i=0; i<y.size(); i++)
      y_data.at<double>(i,0) = y[i];

    // Draw
    cv::Mat tmp, plot_result;
    cv::Ptr<cv::plot::Plot2d> plot = cv::plot::createPlot2d(x_data, y_data);
    plot->setMaxX(max_range_);
    plot->setMinX(min_range_);
    plot->setMaxY(100);
    plot->setMinY(0);
    plot->setPlotTextColor(cv::Scalar(50, 50, 50));
    plot->setPlotBackgroundColor(cv::Scalar(50, 50, 50));
    plot->setPlotLineColor(line_color);
    plot->setPlotLineWidth(line_width);
    plot->render(tmp);
    cv::flip(tmp, plot_result, 0);
    return plot_result;
  }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "sonar_obstacle_detector");
  SonarObstacleDetector node;
  ros::spin();
  return 0;
}


#include "math/util/modular-x86.h"
#include "math/util/prime.h"

#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

BOOST_AUTO_TEST_SUITE( modular_x86 )

typedef boost::mpl::list<
        uint32_t
#ifdef __LP64__
        ,uint64_t
#endif
    > UIntTypes;

BOOST_AUTO_TEST_CASE_TEMPLATE( mul_add_test, U, UIntTypes ) {
  U a = math::ModOp<U>::add(2,4,5);
  BOOST_CHECK_EQUAL( a, 1 );

  U p = math::Prime<U>::A;

  U x = math::ModOp<U>::add(p-2,p-3,p);
  BOOST_CHECK_EQUAL( x, p-5 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( sub_mod_test, U, UIntTypes ) {
  U a = math::ModOp<U>::sub(2,3,5);
  BOOST_CHECK_EQUAL( a, 4 );

  U p = math::Prime<U>::A;

  U x = math::ModOp<U>::sub(p-2,p-3,p);
  BOOST_CHECK_EQUAL( x, 1 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( mul_mod_test, U, UIntTypes ) {
  U a = math::ModOp<U>::mul(2,3,5);
  BOOST_CHECK_EQUAL( a, 1 );

  U p = math::Prime<U>::A;

  U x = math::ModOp<U>::mul(p-2,p-3,p);
  BOOST_CHECK_EQUAL( x, 6 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( mul_add_mod_test, U, UIntTypes ) {
  U a = math::ModOp<U>::mul_add(2,3,4,5);
  BOOST_CHECK_EQUAL( a, 0 );

  U p = math::Prime<U>::A;

  U x = math::ModOp<U>::mul_add(p-2,p-3,p-6,p);
  BOOST_CHECK_EQUAL( x, 0 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( sub_mul_mod_test, U, UIntTypes ) {
  U a = math::ModOp<U>::sub_mul(2,3,4,5);
  BOOST_CHECK_EQUAL( a, 1 );

  U p = math::Prime<U>::A;

  U x = math::ModOp<U>::sub_mul(p-2,p-4,(p-1)/2,p);
  BOOST_CHECK_EQUAL( x, p-1 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( pow_mod_test, U, UIntTypes ) {
  U a = math::ModOp<U>::pow(2,3,5);
  BOOST_CHECK_EQUAL( a, 3 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( sqrt_mod_test, U, UIntTypes) {
   U a = math::ModOp<U>::sqrt(1,5);
   BOOST_CHECK_EQUAL( a, 1 );

   U p = math::Prime<U>::A;
   U x = math::ModOp<U>::sqrt(p-1,p);
   U y = math::ModOp<U>::mul(x,x,p);
   BOOST_CHECK_EQUAL( y, p-1 );
}

BOOST_AUTO_TEST_CASE_TEMPLATE( multi_mod_test, U, UIntTypes ) {
  U primes[] = //{ math::Prime<U>::A, math::Prime<U>::B };
  { 3, 5, 7, 11 };
  const size_t N = sizeof(primes)/sizeof(*primes);

  U mixMuls[N];
  math::MultiMod<U>::mixedMults(mixMuls, primes, N);
  BOOST_CHECK_EQUAL(mixMuls[0], 1);
  BOOST_CHECK_EQUAL(mixMuls[1], 2); // 3*2 mod 5 == 1
  BOOST_CHECK_EQUAL(mixMuls[2], 1); // (3*5) * 1 mod 7 == 1
  BOOST_CHECK_EQUAL(mixMuls[3], 2); // (3*5*7) * 2 mod 11 == 1

  math::MultiMod<U> crt(primes, N);

  U rads[N];
  U crts[N] = {1, 1, 1, 1};
  crt.crtToMixedRadix(rads, crts);
  BOOST_CHECK_EQUAL(rads[0], 1);
  BOOST_CHECK_EQUAL(rads[1], 0);
  BOOST_CHECK_EQUAL(rads[2], 0);
  BOOST_CHECK_EQUAL(rads[3], 0);

  BOOST_CHECK_EQUAL(false, crt.mixedRadixIsNegative(rads));

  char buf[20];
  int len = crt.mixedRadixToString(buf,sizeof(buf),rads,10);
  BOOST_CHECK_EQUAL(len, 1);
  BOOST_CHECK_EQUAL(buf[0], '1');
  BOOST_CHECK_EQUAL(buf[1], '\0');
}

BOOST_AUTO_TEST_SUITE_END()

bool init_unit_test() {
  return true;
}


#ifndef RICPAD_DIFFERENTIATE
#define RICPAD_DIFFERENTIATE

#include <Eigen/Dense>

namespace solver {

template <typename T, int N>
T differentiate(
        // A function which takes an Eigen Matrix
        const std::function<T(Eigen::Matrix<T,N,1>&)>& f,
        // The variables contained in an Eigen matrix
        const Eigen::Matrix<T, N, 1>& x,
        // With respect to which variable we differentiate
        const int k,
        // Step size
        const T& h
      )
{
    Eigen::Matrix<T, N, 1> xp(x), xm(x);

    xp(k) += h;
    xm(k) -= h;

    T ans = f(xp) - f(xm);
    ans /= (h*T(2));

    return ans;
};

}; // namespace differentiate
#endif


#include <mex.h>
#include <Eigen/Core>
#include "splineGeneration.h"
#include "drakeUtil.h"
#include <iostream>

using namespace std;
using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
  string usage = "[coefs, objval] = twoWaypointCubicSplinemex(ts, xs, xd0, xdf)";
  if (nrhs != 4)
    mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplinemex:WrongNumberOfInputs", usage.c_str());
  if (nlhs > 2)
    mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplinemex:WrongNumberOfOutputs", usage.c_str());

  const std::vector<double> segment_times = matlabToStdVector(prhs[0]);
  MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[1]);
  auto xd0 = matlabToEigen<Dynamic, 1>(prhs[2]);
  auto xdf = matlabToEigen<Dynamic, 1>(prhs[3]);

  mwSize ndof = static_cast<mwSize>(xs.rows());
  mwSize num_segments = 3;
  mwSize num_coeffs_per_segment = 4;
  mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
  plhs[0] = mxCreateNumericArray(num_segments, dims, mxDOUBLE_CLASS, mxREAL);
  double objective_value = 0.0;
  for (mwSize dof = 0; dof < ndof; dof++) {
    PiecewisePolynomial<double> spline = twoWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof], xs(dof, 3), xdf[dof], xs(dof, 1), xs(dof, 2));

    PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
    acceleration_squared *= acceleration_squared;
    PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
    objective_value += acceleration_squared_integral.value(spline.getEndTime()) - acceleration_squared_integral.value(spline.getStartTime());

    for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) {
      for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) {
        mwSize sub[] = {dof, segment_index, num_coeffs_per_segment - coefficient_index - 1}; // Matlab's reverse coefficient indexing...
        *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(segment_index).getCoefficients()[coefficient_index];
      }
    }
  }

  if (nlhs > 1) {
    plhs[1] = mxCreateDoubleScalar(objective_value);
  }
}