// Copyright (c) 2021 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#include <gtest/gtest.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <fstream>
#include <random>

#include "pyinterp/detail/math/descriptive_statistics.hpp"
#include "pyinterp/detail/math/streaming_histogram.hpp"

namespace math = pyinterp::detail::math;

constexpr auto POINTS = 1000;

auto quantile(const std::vector<double>& x, double q) {
  const auto ix = (x.size() - 1) * q;
  const auto lo = floor(ix);
  const auto hi = ceil(ix);

  return (x[lo] + x[hi]) * 0.5;
}

TEST(math_streaming_histogram, push) {
  auto instance = math::StreamingHistogram<double>(3, false);

  instance(10);
  const auto& bins = instance.bins();
  ASSERT_EQ(bins.size(), 1);
  EXPECT_EQ(bins[0].value, 10);
  EXPECT_EQ(bins[0].weight, 1);

  instance(13);
  ASSERT_EQ(bins.size(), 2);
  EXPECT_EQ(bins[0].value, 10);
  EXPECT_EQ(bins[0].weight, 1);
  EXPECT_EQ(bins[1].value, 13);
  EXPECT_EQ(bins[1].weight, 1);

  instance(3);
  ASSERT_EQ(bins.size(), 3);
  EXPECT_EQ(bins[0].value, 3);
  EXPECT_EQ(bins[0].weight, 1);
  EXPECT_EQ(bins[1].value, 10);
  EXPECT_EQ(bins[1].weight, 1);
  EXPECT_EQ(bins[2].value, 13);
  EXPECT_EQ(bins[2].weight, 1);

  instance(13);
  ASSERT_EQ(bins.size(), 3);
  EXPECT_EQ(bins[0].value, 3);
  EXPECT_EQ(bins[0].weight, 1);
  EXPECT_EQ(bins[1].value, 10);
  EXPECT_EQ(bins[1].weight, 1);
  EXPECT_EQ(bins[2].value, 13);
  EXPECT_EQ(bins[2].weight, 2);

  instance(3);
  ASSERT_EQ(bins.size(), 3);
  EXPECT_EQ(bins[0].value, 3);
  EXPECT_EQ(bins[0].weight, 2);
  EXPECT_EQ(bins[1].value, 10);
  EXPECT_EQ(bins[1].weight, 1);
  EXPECT_EQ(bins[2].value, 13);
  EXPECT_EQ(bins[2].weight, 2);

  instance(10);
  ASSERT_EQ(bins.size(), 3);
  EXPECT_EQ(bins[0].value, 3);
  EXPECT_EQ(bins[0].weight, 2);
  EXPECT_EQ(bins[1].value, 10);
  EXPECT_EQ(bins[1].weight, 2);
  EXPECT_EQ(bins[2].value, 13);
  EXPECT_EQ(bins[2].weight, 2);

  instance(11);
  ASSERT_EQ(bins.size(), 3);
  EXPECT_EQ(bins[0].value, 3);
  EXPECT_EQ(bins[0].weight, 2);
  EXPECT_NEAR(bins[1].value, 10 + 1.0 / 3.0, 1e-9);
  EXPECT_EQ(bins[1].weight, 3);
  EXPECT_EQ(bins[2].value, 13);
  EXPECT_EQ(bins[2].weight, 2);
}

TEST(math_streaming_histogram, sum_of_weights) {
  auto instance = math::StreamingHistogram<double>(3, false);
  EXPECT_EQ(instance.count(), 0);
  EXPECT_EQ(instance.size(), 0);
  EXPECT_EQ(instance.sum_of_weights(), 0);

  instance(0, 4);
  EXPECT_EQ(instance.count(), 1);
  EXPECT_EQ(instance.size(), 1);
  EXPECT_EQ(instance.sum_of_weights(), 4);

  instance(1, 3);
  EXPECT_EQ(instance.count(), 2);
  EXPECT_EQ(instance.size(), 2);
  EXPECT_EQ(instance.sum_of_weights(), 7);

  instance(2, 5);
  EXPECT_EQ(instance.count(), 3);
  EXPECT_EQ(instance.size(), 3);
  EXPECT_EQ(instance.sum_of_weights(), 12);
}

TEST(math_streaming_histogram, bounds) {
  auto rd = std::random_device();
  auto gen = std::mt19937(rd());
  auto normal = std::normal_distribution<>();
  auto instance = math::StreamingHistogram<double>();
  auto min = std::numeric_limits<double>::max();
  auto max = std::numeric_limits<double>::min();

  for (auto ix = 0; ix < POINTS; ++ix) {
    auto value = normal(gen);
    instance(value);
    min = std::min(min, value);
    max = std::max(max, value);
  }
  EXPECT_NEAR(min, instance.min(), 1e-6);
  EXPECT_NEAR(max, instance.max(), 1e-6);
}

TEST(math_streaming_histogram, quantile) {
  auto instance = math::StreamingHistogram<double>(3, false);
  instance(1, 4);
  instance(5, 3);
  instance(10, 5);

  auto expected = instance.quantile(0.5);
  EXPECT_NEAR(expected, 5.625, 1e-9);
}

TEST(math_streaming_histogram, quantile_not_enough_elements) {
  auto instance = math::StreamingHistogram<double>(10, false);
  for (const auto& item : std::vector<double>({31, 56, 40, 39, 82, 17})) {
    instance(item);
  }

  auto expected = instance.quantile(0.5);
  EXPECT_NEAR(expected, 39.5, 1e-9);
}

TEST(math_streaming_histogram, quantile_on_left) {
  auto instance = math::StreamingHistogram<double>(6, false);
  for (const auto& item : std::vector<double>(
           {3.075, 1.3, 1.35, 1.225, 1.375, 1.4, 2.05, 7.6325, 5.875, 3.495})) {
    instance(item);
  }

  auto expected = instance.quantile(0.01);
  auto exact = 1.23175;
  EXPECT_NEAR(expected, exact, exact * 0.01);

  expected = instance.quantile(0.05);
  exact = 1.25875;
  EXPECT_NEAR(expected, exact, exact * 0.05);

  expected = instance.quantile(0.25);
  exact = 1.35625;
  EXPECT_NEAR(expected, exact, exact * 0.05);
}

TEST(math_streaming_histogram, quantile_on_right) {
  auto instance = math::StreamingHistogram<double>(6, false);
  for (const auto& item :
       std::vector<double>({3.075, 2.05, 25.1325, 5.875, 3.495, 50., 50.05,
                            50.2, 50.1, 50.025})) {
    instance(item);
  }

  auto expected = instance.quantile(0.99);
  auto exact = 50.191;
  EXPECT_NEAR(expected, exact, exact * 0.01);

  expected = instance.quantile(0.85);
  exact = 50.0825;
  EXPECT_NEAR(expected, exact, exact * 0.01);
}

TEST(math_streaming_histogram, stats) {
  auto rd = std::random_device();
  auto gen = std::mt19937(rd());
  auto normal = std::normal_distribution<>();
  auto acc = math::DescriptiveStatistics<double>();
  auto instance = math::StreamingHistogram<double>(40, false);
  auto values = std::vector<double>();

  for (auto ix = 0; ix < POINTS; ++ix) {
    auto value = normal(gen);
    instance(value);
    acc(value);
    values.push_back(value);
  }

  std::sort(values.begin(), values.end());

  auto expected = instance.quantile(0.5);
  auto exact = quantile(values, 0.5);
  ASSERT_NEAR(std::abs(expected - exact), 0, 0.2);

  expected = instance.quantile(0.8);
  exact = quantile(values, 0.8);
  ASSERT_NEAR(std::abs(expected - exact), 0, 0.2);

  EXPECT_EQ(acc.count(), instance.count());
  EXPECT_EQ(acc.min(), instance.min());
  EXPECT_EQ(acc.max(), instance.max());
  EXPECT_EQ(acc.sum_of_weights(), instance.sum_of_weights());
  acc.clear();
  for (const auto& item : instance.bins()) {
    acc(item.value, item.weight);
  }
  EXPECT_NEAR(acc.mean(), instance.mean(), 1e-6);
  EXPECT_NEAR(acc.variance(), instance.variance(), 1e-6);
  EXPECT_NEAR(acc.skewness(), instance.skewness(), 1e-6);
  EXPECT_NEAR(acc.kurtosis(), instance.kurtosis(), 1e-6);
}

TEST(math_streaming_histogram, merge) {
  auto rd = std::random_device();
  auto gen = std::mt19937(rd());
  auto normal = std::normal_distribution<>();
  auto acc = math::DescriptiveStatistics<double>();
  auto instance1 = math::StreamingHistogram<double>(40, false);
  auto instance2 = math::StreamingHistogram<double>(40, false);
  auto values = std::vector<double>();

  for (auto ix = 0; ix < POINTS / 2; ++ix) {
    auto value = normal(gen);
    instance1(value);
    acc(value);
    values.push_back(value);
  }

  for (auto ix = 0; ix < POINTS / 2; ++ix) {
    auto value = normal(gen);
    instance2(value);
    acc(value);
    values.push_back(value);
  }

  std::sort(values.begin(), values.end());
  instance1 += instance2;

  auto expected = instance1.quantile(0.5);
  auto exact = quantile(values, 0.5);
  ASSERT_NEAR(std::abs(expected - exact), 0, 0.2);

  expected = instance1.quantile(0.8);
  exact = quantile(values, 0.8);
  ASSERT_NEAR(std::abs(expected - exact), 0, 0.2);

  EXPECT_EQ(acc.count(), instance1.count());
  EXPECT_NEAR(acc.min(), instance1.min(), 1e-6);
  EXPECT_NEAR(acc.max(), instance1.max(), 1e-6);
  EXPECT_EQ(acc.sum_of_weights(), instance1.sum_of_weights());
  acc.clear();
  for (const auto& item : instance1.bins()) {
    acc(item.value, item.weight);
  }
  EXPECT_NEAR(acc.mean(), instance1.mean(), 1e-6);
  EXPECT_NEAR(acc.variance(), instance1.variance(), 1e-6);
}

TEST(math_streaming_histogram, quantile_out_of_bounds) {
  auto instance = math::StreamingHistogram<double>(6, false);
  EXPECT_TRUE(std::isnan(instance.quantile(-0.2)));

  for (const auto& item : std::vector<double>({1, 2, 3, 4, 5, 6, 6.1, 6.2})) {
    instance(item);
  }

  EXPECT_THROW(instance.quantile(-0.2), std::invalid_argument);
  EXPECT_THROW(instance.quantile(10), std::invalid_argument);
}

TEST(math_streaming_histogram, serialization) {
  auto instance = math::StreamingHistogram<double>(6, false);

  auto dump = static_cast<std::string>(instance);
  auto instance2 = math::StreamingHistogram<double>(dump);
  ASSERT_EQ(instance.count(), instance2.count());
  ASSERT_EQ(instance.sum_of_weights(), instance2.sum_of_weights());
  ASSERT_EQ(instance.bins().size(), instance2.bins().size());

  for (const auto& item :
       std::vector<double>({1, 2, 3, 4, 5, 6, 7, 8, 9, 10})) {
    instance(item);
  }

  dump = static_cast<std::string>(instance);
  instance2 = math::StreamingHistogram<double>(dump);
  ASSERT_EQ(instance.count(), instance2.count());
  ASSERT_EQ(instance.sum_of_weights(), instance2.sum_of_weights());
  ASSERT_EQ(instance.size(), instance2.size());
  for (size_t ix = 0; ix < instance.size(); ++ix) {
    ASSERT_EQ(instance.bins()[ix].value, instance2.bins()[ix].value);
  }

  ASSERT_THROW(math::StreamingHistogram<double>("AZERTYUIOP"),
               std::invalid_argument);
}

TEST(math_streaming_histogram, weighted) {
  static double x[20] = {0.00402322, 0.19509434, 0.6425439,  0.66463742,
                         0.76523411, 0.91985221, 0.82729929, 0.21502902,
                         0.48254104, 0.97854649, 0.61394511, 0.00583773,
                         0.06630172, 0.57173946, 0.5881294,  0.30185368,
                         0.18126563, 0.84524097, 0.13754961, 0.17343529};
  static double w[20] = {0.45463566, 0.46341234, 0.2072285,  0.02272363,
                         0.76796619, 0.01987153, 0.43634701, 0.1369698,
                         0.65012667, 0.18825124, 0.96310554, 0.31995482,
                         0.28808939, 0.69961506, 0.97369255, 0.98436659,
                         0.05230501, 0.8073624,  0.40509977, 0.6325752};
  auto acc = math::DescriptiveStatistics<double>();
  auto instance = math::StreamingHistogram<double>(20, false);

  for (auto ix = 0; ix < 20; ++ix) {
    acc(x[ix], w[ix]);
    instance(x[ix], w[ix]);
  }

  EXPECT_EQ(instance.count(), acc.count());
  EXPECT_EQ(instance.sum_of_weights(), acc.sum_of_weights());
  EXPECT_DOUBLE_EQ(instance.mean(), acc.mean());
  EXPECT_NEAR(instance.variance(), acc.variance(), 1e-12);
  EXPECT_NEAR(instance.quantile(0.5), 0.5716742560345885, 1e-12);

  instance = math::StreamingHistogram<double>(10, false);

  for (auto ix = 0; ix < 20; ++ix) {
    instance(x[ix], w[ix]);
  }

  EXPECT_EQ(instance.count(), acc.count());
  EXPECT_TRUE(instance.count() > instance.size());
  EXPECT_NEAR(instance.sum_of_weights(), acc.sum_of_weights(), 1e-6);
  EXPECT_NEAR(instance.mean(), acc.mean(), 1e-6);
  EXPECT_NEAR(instance.variance(), acc.variance(), 1e-3);
  EXPECT_NEAR(instance.quantile(0.5), 0.5716742560345885, 1e-1);
}


#ifndef RASTERIZER_HPP
#define RASTERIZER_HPP

#include <algorithm>
#include <boost/geometry/geometries/geometries.hpp>
#include <limits>

namespace rasterize {

namespace bg = boost::geometry;

/*  rasterize a polygon implementation  */

template <typename point_t, typename float_t = double, typename int_t = int64_t>
struct Edge {
  typedef bg::model::segment<point_t> segment_t;
  float_t x, xNorm, yNorm;
  float_t minX, maxX;
  float_t slope;
  int_t yMax, yMin; // min-max of edge
  float_t dX, dY;
  segment_t line;
  Edge(const point_t &a, const point_t &b) : line{a, b} {
    auto &pMin = (bg::get<1>(a) < bg::get<1>(b) ? a : b);
    auto &pMax = (bg::get<1>(a) < bg::get<1>(b) ? b : a);

    yMax = bg::get<1>(pMax);
    yMin = bg::get<1>(pMin);

    x = bg::get<0>(pMin);                          // start x
    minX = std::min(bg::get<0>(a), bg::get<0>(b)); // start x
    maxX = std::max(bg::get<0>(a), bg::get<0>(b)); // start x

    dX = (bg::get<0>(pMax) - bg::get<0>(pMin));
    dY = (bg::get<1>(pMax) - bg::get<1>(pMin));
    slope = dY != 0 ? slope = dX / dY : 0;

    auto yOff = yMin - std::floor(yMin);
    auto xOff = x - std::floor(x);

    yNorm = std::round(yMin) + 0.5;
    xNorm = x + (yNorm - yMin) * slope;
  }

  friend std::ostream &operator<<(std::ostream &stream, const Edge &edge) {
    return std::cout << "Edge bucket: " << edge.yMin << "=>" << edge.yMax
                     << "dx=" << edge.dX << "; dy=" << edge.dY
                     << "; x=" << edge.x << ";" << std::endl;
  }
};

template <typename point_t, typename float_t = double> struct Rasterizer {
  typedef bg::model::polygon<point_t> polygon_t;
  typedef Edge<point_t, float_t> edge_t;
  struct {
    bool operator()(const edge_t &a, const edge_t &b) const {
      return a.yMin < b.yMin;
    }
  } yMinCompare;

  enum { STEP_INTERSECT, STEP_RASTERIZE };
  std::list<edge_t> ET; // edge table
  std::list<edge_t> AL; // active list

  int scanline;

  std::pair<bool, int> custom_scanline;

  void set_scanline(int _scanline) {
    custom_scanline = std::make_pair(true, _scanline);
  }

  void clear_scanline() { custom_scanline = std::make_pair(false, 0); }

  void clear() {
    clear_scanline();
    ET.clear();
    AL.clear();
  }

  void init(const polygon_t &p) {
    clear();
    if (p.outer().size() < 3) // don't render invalid polygons
      return;
    // build edge table containing all edges n=>0, 0=>1, ...n-1=>n
    edge_t e = {p.outer().back(), p.outer()[0]};
    if (e.dY != 0)
      ET.push_back(e);
    for (size_t i = 0; i < p.outer().size() - 1; i++) {
      e = {p.outer()[i], p.outer()[i + 1]};
      if (e.dY != 0)
        ET.push_back(e);
    }
    // sort according to yMin
    ET.sort(yMinCompare);

    //	std::cout << bg::wkt(p) << std::endl;
    if (ET.empty())
      return;

    if (!custom_scanline.first) {
      scanline = ET.front().yMin;
    } else {
      auto _scanline = custom_scanline.second;
      if (ET.front().yMin != _scanline)
        //	    std::cout << "Starting at different scanlines: " <<
        // ET.front().yMin << " vs." << scanline << std::endl;
        scanline = ET.front().yMin;
      //	    std::cout << "silent step should now do " << scanline << "
      // until " << _scanline << std::endl;
      while (scanline < _scanline) {
        //	      std::cout << "silent step:" << scanline << " towards " <<
        //_scanline << std::endl;
        step_intersect([](int x, int y) {});
      }
    }
  }

  bool done() { return (ET.empty() && AL.empty()); }
  template <typename func> void step_intersect(func putpixel) {
#ifdef DEBUG_LOG
    std::cout << "scanline(" << scanline << "):" << std::endl;
#endif
    // all edges that start here are moved from ET to AL
    for (auto it = ET.begin(); it != ET.end();) {
      if (std::floor(it->yMin - 1) <= scanline) {
        AL.push_back(*it);
#ifdef DEBUG_LOG
        std::cout << "erase (" << it->yMin << ") ";
#endif
        it = ET.erase(it);
      } else
        it++;
    }
#ifdef DEBUG_LOG
    std::cout << "active(" << AL.size() << ") ";
#endif
    // all edges that end here, are removed from AL
    for (auto it = AL.begin(); it != AL.end();) {
      if (std::ceil(it->yMax + 1) < scanline) {
        it = AL.erase(it);
      } else
        it++;
    }

    // theoretic: for multipolygons, this could become empty. However, for
    // polygons it should always have at least two active edges
    if (AL.empty()) {
      scanline++;
      return;
    }

    // sort according to X coordinate
    AL.sort([](const edge_t &a, const edge_t &b) {
      // it is an iterator pointing to Edge
      // sort by x and slope
      return (a.x < b.x || (a.x == b.x && a.slope < b.slope));
    });
#ifdef DEBUG_LOG
    std::cout << "-> (" << AL.size() << "):" << std::endl;
    for (auto &a : AL) {
      std::cout << "{(" << a.line.first.get<0>() << "," << a.line.first.get<1>()
                << "),"
                << "(" << a.line.second.get<0>() << ","
                << a.line.second.get<1>() << ")},";
    }
#endif

    // prepare scanline filling
    auto minX = std::floor(AL.front().minX - 1);
    auto maxX = std::ceil(AL.back().maxX);
    auto line = typename edge_t::segment_t(point_t(minX, scanline + 0.5),
                                           point_t(maxX, scanline + 0.5));

    std::vector<point_t> output;
    for (auto &e : AL) {
      bg::intersection(line, e.line, output);
#ifdef DEBUG_LOG
      std::cout << "[";
      for (auto i = output.begin(); i != output.end(); ++i)
        std::cout << "(" << bg::get<0>(*i) << ',' << bg::get<1>(*i) << "),";
      std::cout << "], ";
#endif
    }
#ifdef DEBUG_LOG
    std::cout << " | sum: " << output.size() << std::endl;
#endif
    // assert(output.size() % 2 == 0);
    std::sort(output.begin(), output.end(),
              ([](const point_t &a, const point_t &b) {
                // sort by x
                return bg::get<0>(a) < bg::get<0>(b);
              }));
#ifdef BORDERS_ONLY
    // Just the borders
    auto index = 0;
    for (auto &i : output) {
      //	float_t pixelX = bg::get<0>(i) - std::floor(bg::get<0>(i));

      int x = index % 2 == 0 ? std::round(bg::get<0>(i))
                             : std::floor(bg::get<0>(i) - 0.5);

      putpixel(x, scanline);
      index++;
    }
#else
    if (output.size() > 0) {

      for (auto i = 1; i < output.size(); i += 2) {
        auto xstart = std::round(bg::get<0>(output[i - 1]));
        auto xend = std::floor(bg::get<0>(output[i]) - 0.5);
#ifdef DEBUG_LOG
        std::cout << "xstart: " << xstart << " xend: " << xend << std::endl;
#endif
        for (auto x = xstart; x <= xend; x++)
          putpixel(x, scanline);
      }
    }
#endif

    scanline++;
    return;
  }
  template <typename func> void step_rasterize(func putpixel) {
//#define DEBUG_LOG
#ifdef DEBUG_LOG
    std::cout << "scanline(" << scanline << "):" << std::endl;
#endif
    // all edges that start here are moved from ET to AL
    for (auto it = ET.begin(); it != ET.end();) {
      if (std::floor(it->yNorm) <= scanline) {
        AL.push_back(*it);
#ifdef DEBUG_LOG
        std::cout << "erase (" << it->yMin << ") ";
#endif
        it = ET.erase(it);
      } else
        it++;
    }
#ifdef DEBUG_LOG
    std::cout << "active(" << AL.size() << ") ";
#endif
    // all edges that end here, are removed from AL
    for (auto it = AL.begin(); it != AL.end();) {
      if (std::ceil(it->yMax - 1) < scanline) {
        it = AL.erase(it);
      } else
        it++;
    }

    // theoretic: for multipolygons, this could become empty. However, for
    // polygons it should always have at least two active edges
    if (AL.empty()) {
      scanline++;
      return;
    }

    // sort according to X coordinate
    AL.sort([](const edge_t &a, const edge_t &b) {
      // it is an iterator pointing to Edge
      // sort by x and slope
      return ((a.xNorm < b.xNorm) || (a.xNorm == b.xNorm && a.slope < b.slope));
    });
#ifdef DEBUG_LOG
    std::cout << "-> (" << AL.size() << "):" << std::endl;
    for (auto &a : AL) {
      std::cout << "{(" << a.line.first.get<0>() << "," << a.line.first.get<1>()
                << "),"
                << "(" << a.line.second.get<0>() << ","
                << a.line.second.get<1>() << ")},";
    }
#endif

    // prepare scanline filling
    auto minX = std::floor(AL.front().minX - 2);
    auto maxX = std::ceil(AL.back().maxX);
    typename edge_t::segment_t line(point_t(minX, scanline + 0.5),
                                    point_t(maxX, scanline + 0.5));
    auto it = AL.begin();
    bool inside = false;
#ifdef DEBUG_LOG
    std::cout << "[" << std::endl;
#endif
    for (float_t x = minX; x <= maxX; x++) {
      auto go = true;
      while (go) {
        go = false;
        if (!inside) {
          if (x + 0.5 >= it->xNorm && it != AL.end()) {
            inside = !inside;
            it++;
            go = true;
          }
        } else {
          if ((x + 0.499999) > it->xNorm) {
#ifdef DEBUG_LOG
            std::cout.precision(std::numeric_limits<float_t>::max_digits10);
            std::cout << std::endl
                      << "(" << x + 0.5 << " > " << it->xNorm << ")"
                      << std::endl;
            std::cout.precision(2);
#endif
            inside = !inside;
            it++;
            go = true;
          }
        }
        if (it == AL.end())
          break;
      }
#ifdef DEBUG_LOG
      std::cout << "n:" << it->xNorm << "~" << it->x << " x:" << x
                << " in:" << inside << ", ";
#endif

      if (inside)
        putpixel(x, scanline);
    }
#ifdef DEBUG_LOG
    std::cout << "]" << std::endl;
#endif

    // increment all the X based on slope
    for (auto &e : AL) {
      if (e.dX != 0)
        e.xNorm += e.slope;
    }

    scanline++;
    return;
  }

  void rasterize(const polygon_t &p,
                 std::function<void(float_t, float_t)> putpixel,
                 int step_id = STEP_RASTERIZE, int steps = -1) {
    init(p);

    switch (step_id) {
    case STEP_INTERSECT:
      while (!done()) {
        step_intersect(putpixel);
      }
      break;
    case STEP_RASTERIZE:
      while (!done()) {
        step_rasterize(putpixel);
      }
      break;
    }
    // Process ET=>AL=>(void)
  }
};

} // namespace rasterize

#endif


#ifndef _RBF_HPP
#define _RBF_HPP

#include <armadillo>
#include <vector>
#include <math.h>

using namespace arma;
using namespace std;

class rbf {
    double sigma;
    vector<double> w;
    vector<double> g;
public:
    rbf(vector<double> &x, vector<double> &y,
                           double lambda);

    double gaussian(double xi, double xj);

    double get_output(double x);

    vector<double> test(const vector<double> &x);
};

#endif


// Example of using Array<T,N>::iterator

#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    // Create a 4x4 array and fill it with some numbers

    Array<int,2> A(4,4);
    A = tensor::i * 10 + tensor::j;
    cout << "A = " << A << endl;

    // Use an iterator to list the array elements

    Array<int,2>::iterator iter = A.begin(), end = A.end();

    while (iter != end)
    {
        cout << iter.position() << '\t' << (*iter) << endl;
        ++iter;
    }

    return 0;
}


/*
 * matrices.cpp
 *
 *  Created on: Jan 12, 2012
 *      Author: eba
 */
#include "mhfpython.h"
#include <eigen3/Eigen/Dense>
#include <GaussianHypothesis.h>
#include <boost/python/stl_iterator.hpp>
#include <list>
#include <numpy/noprefix.h>
#include <string>

using namespace Eigen;

typedef GaussianHypothesis<dim>::MeanMatrix MeanMatrix;
typedef GaussianHypothesis<dim>::CovMatrix CovMatrix;

typedef Matrix<double,1,1> ScalarMatrix;

template<int rows, int cols>
void matrix_assign(Matrix<double, rows, cols>& m, boost::python::object o) {
    // Turn a Python sequence into an STL input range
    boost::python::stl_input_iterator<double > begin(o), end;
    std::list<double> l; l.assign(begin,end);
    std::list<double>::iterator it = l.begin();
    for(int r = 0; r<rows; r++)
    	for(int c=0; c<cols; c++)
    		m(r,c) = *(it++);
}

template<int rows, int cols>
//boost::python::numeric::array *
PyObject *
pyArray(Matrix<double, rows, cols> & mat) {
	npy_intp N[] = {rows,cols};
	//return new array(handle<>(PyArray_SimpleNewFromData(2,N, PyArray_DOUBLE, mat.data())));
	//return static_cast<array>(handle<>(PyArray_SimpleNewFromData(2,N, PyArray_DOUBLE, mat.data())));
	return PyArray_SimpleNewFromData(2,N, PyArray_DOUBLE, mat.data()); // EIGEN_DEFAULT_TO_ROW_MAJOR is needed!!
}

template<int rows, int cols>
Matrix<double, rows,cols> * createMatrix(object o) {
	Matrix<double, rows, cols> * result = new Matrix<double, rows, cols>();
	for(int r = 0; r<rows; r++)
		for(int c = 0; c<cols; c++)
			(*result)(r,c) = extract<double>(o.attr("__getitem__")(r).attr("__getitem__")(c));
	return result;

}

template<int rows, int cols>
class export_proxy {
public:
	static bool exported;
};

template<int rows, int cols >
bool export_proxy<rows,cols>::exported = false;

template<int rows, int cols>
void export_matrix() {

	if(export_proxy<rows,cols>::exported) return;
	typedef Matrix<double, rows, cols> Mat;
	string name = "Matrix" + lexical_cast<string>(rows) + lexical_cast<string>(cols);

    double &(Mat::*MemberAccess) (typename Mat::Index) = &Mat::operator();
    class_<Mat>(name.c_str())
    	    .def("__init__", make_constructor(createMatrix<rows,cols>) )
			.def("__call__", MemberAccess, return_value_policy<copy_non_const_reference>())
    		.def(self_ns::str(self))
	;
    def("assign",matrix_assign<rows,cols>);
    def("pyArray", pyArray<rows,cols>, with_custodian_and_ward_postcall<0,1>());
    export_proxy<rows,cols>::exported=true;
}

template<int rows, int cols>
Matrix<double, rows, cols> * construct_matrix() {
	return new Matrix<double, rows, cols>();
}

template<int rows, int cols>
void export_matrix(string name) {
	def(name.c_str(), make_constructor(createMatrix<rows,cols>));
	def(name.c_str(), construct_matrix<rows, cols>, return_value_policy<manage_new_object >());
	export_matrix<rows, cols>();
}

void export_matrices() {

	import_array();
	array::set_module_and_type("numpy", "ndarray");

	export_matrix<1,1>("ScalarMatrix");
	export_matrix<dim,1>("MeanMatrix");
	export_matrix<dim,dim>("CovMatrix");
	export_matrix<inputdim,1>();
	export_matrix<procnoisedim,1>();
	export_matrix<procnoisedim,procnoisedim>();
	export_matrix<measdim,1>();
	export_matrix<measdim,measdim>();
	export_matrix<1,1>();
	export_matrix<2,1>();

}


//=======================================================================
// Copyright 2002 Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

/*
  Adapted from the GIRTH program of the Stanford GraphBase.

  Sample output:

  This program explores the girth and diameter of Ramanujan graphs.
  The bipartite graphs have q^3-q vertices, and the non-bipartite
  graphs have half that number. Each vertex has degree p+1.
  Both p and q should be odd prime numbers;
    or you can try p = 2 with q = 17 or 43.

  Choose a branching factor, p: 2
  Ok, now choose the cube root of graph size, q: 17
  Starting at any given vertex, there are
  3 vertices at distance 1,
  6 vertices at distance 2,
  12 vertices at distance 3,
  24 vertices at distance 4,
  46 vertices at distance 5,
  90 vertices at distance 6,
  169 vertices at distance 7,
  290 vertices at distance 8,
  497 vertices at distance 9,
  634 vertices at distance 10,
  521 vertices at distance 11,
  138 vertices at distance 12,
  13 vertices at distance 13,
  3 vertices at distance 14,
  1 vertices at distance 15.
  So the diameter is 15, and the girth is 9.

 */

#include <boost/config.hpp>
#include <vector>
#include <list>
#include <iostream>
#include <boost/limits.hpp>
#include <boost/graph/stanford_graph.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/graph_utility.hpp>

typedef boost::graph_traits<Graph*> Traits;
typedef Traits::vertex_descriptor vertex_descriptor;
typedef Traits::edge_descriptor edge_descriptor;
typedef Traits::vertex_iterator vertex_iterator;

std::vector<std::size_t> distance_list;

typedef boost::v_property<long> dist_t;
boost::property_map<Graph*, dist_t>::type d_map;

typedef boost::u_property<vertex_descriptor> pred_t;
boost::property_map<Graph*, pred_t>::type p_map;

typedef boost::w_property<long> color_t;
boost::property_map<Graph*, color_t>::type c_map;

class diameter_and_girth_visitor : public boost::bfs_visitor<>
{
public:
  diameter_and_girth_visitor(std::size_t& k_, std::size_t& girth_)
    : k(k_), girth(girth_) { }

  void tree_edge(edge_descriptor e, Graph* g) {
    vertex_descriptor u = source(e, g), v = target(e, g);
    k = d_map[u] + 1;
    d_map[v] = k;
    ++distance_list[k];
    p_map[v] = u;
  }
  void non_tree_edge(edge_descriptor e, Graph* g) {
    vertex_descriptor u = source(e, g), v = target(e, g);
    k = d_map[u] + 1;
    if (d_map[v] + k < girth && v != p_map[u])
      girth = d_map[v]+ k;
  }
private:
  std::size_t& k;
  std::size_t& girth;
};

int
main()
{
  std::cout <<
    "This program explores the girth and diameter of Ramanujan graphs."
            << std::endl;
  std::cout <<
    "The bipartite graphs have q^3-q vertices, and the non-bipartite"
            << std::endl;
  std::cout <<
    "graphs have half that number. Each vertex has degree p+1."
            << std::endl;
  std::cout << "Both p and q should be odd prime numbers;" << std::endl;
  std::cout << "  or you can try p = 2 with q = 17 or 43." << std::endl;

  while (1) {

    std::cout << std::endl
              << "Choose a branching factor, p: ";
    long p = 0, q = 0;
    std::cin >> p;
    if (p == 0)
      break;
    std::cout << "Ok, now choose the cube root of graph size, q: ";
    std::cin >> q;
    if (q == 0)
      break;

    Graph* g;
    g = raman(p, q, 0L, 0L);
    if (g == 0) {
      std::cerr << " Sorry, I couldn't make that graph (error code "
        << panic_code << ")" << std::endl;
      continue;
    }
    distance_list.clear();
    distance_list.resize(boost::num_vertices(g), 0);

    // obtain property maps
    d_map = get(dist_t(), g);
    p_map = get(pred_t(), g);
    c_map = get(color_t(), g);

    vertex_iterator i, end;
    for (boost::tie(i, end) = boost::vertices(g); i != end; ++i)
      d_map[*i] = 0;

    std::size_t k = 0;
    std::size_t girth = (std::numeric_limits<std::size_t>::max)();
    diameter_and_girth_visitor vis(k, girth);

    vertex_descriptor s = *boost::vertices(g).first;

    boost::breadth_first_search(g, s, visitor(vis).color_map(c_map));

    std::cout << "Starting at any given vertex, there are" << std::endl;

    for (long d = 1; distance_list[d] != 0; ++d)
      std::cout << distance_list[d] << " vertices at distance " << d
                << (distance_list[d+1] != 0 ? "," : ".") << std::endl;

    std::cout << "So the diameter is " << k - 1
              << ", and the girth is " << girth
              << "." << std::endl;
  } // end while

  return 0;
}


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * 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 <iostream>
#include <cmath>

#include <string>
#include <sstream>
#include <random>

#include <boost/math/constants/constants.hpp>

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/mathtools/sphcoords.h>
#include <tomographer/tools/eigenutil.h>
#include <tomographer/mathtools/check_derivatives.h>

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

// for checking debug results
// see http://en.wikipedia.org/wiki/N-sphere

inline double known_vol_sph(int);

inline double known_surf_sph(int sphdim)
{
  if (sphdim == 0) {
    return 2;
  }
  return 2 * M_PI * known_vol_sph(sphdim - 1);
}
inline double known_vol_sph(int cartdim)
{
  if (cartdim == 0) {
    return 1;
  }
  return known_surf_sph(cartdim - 1) / cartdim;
}

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

static const double pi = boost::math::constants::pi<double>();

// utility to check for cart_to_sph_jacobian
template<int CART_DIM = 3, int SPH_DIM = CART_DIM - 1>
struct TestSphJacFixture
{
  TestSphJacFixture() { }
  ~TestSphJacFixture() { }

  double calc_montecarlo_vol(std::mt19937::result_type seed, std::size_t npoints)
  {
    // random number generator with reproducable results
    std::mt19937 rng(seed);
    // generate uniformly distributed numbers in [0.0, 1.0[
    std::uniform_real_distribution<double> dist(0.0, 1.0);

    // do very naive monte carlo integration to obtain volume of a sphere
    const int ds = SPH_DIM;
    const double R = 1.0; // radius of ball

    double vol = 0;

    Eigen::VectorXd rtheta(CART_DIM);

    for (std::size_t k = 0; k < npoints; ++k) {
      // get a random point in theta-space; weigh with jacobian to estimate volume of n-ball
      rtheta = Tomographer::Tools::denseRandom<Eigen::VectorXd>(rng, dist, CART_DIM);
      // so translate them to the correct ranges.
      rtheta(0) *= R; // rtheta(0) in [0, R]
      rtheta.segment(1,ds-1) = rtheta.segment(1,ds-1) * pi; // theta_i in [0, pi] for 1 <= i < ds
      rtheta(ds) = rtheta(ds) * 2 * pi; // theta_{ds} in [0, 2*pi]

      vol += Tomographer::MathTools::SphCoords::cart_to_sph_jacobian(rtheta);
    }

    // average all volume elements
    vol /= npoints;

    // multiply by volume of parameter space
    vol *=  R * 2*pi *  Eigen::VectorXd::Constant(ds-1, pi).array().prod();

    return vol;
  }

  double calc_montecarlo_surf(std::mt19937::result_type seed, std::size_t npoints)
  {
    // random number generator with reproducable results
    std::mt19937 rng(seed);
    // generate uniformly distributed numbers in [0.0, 1.0[
    std::uniform_real_distribution<double> dist(0.0, 1.0);

    // do very naive monte carlo integration to obtain volume of a sphere
    const int ds = SPH_DIM;
    const double pi = boost::math::constants::pi<double>();

    Eigen::VectorXd theta((Eigen::VectorXd::Index)ds);
    double surf = 0;

    for (std::size_t k = 0; k < npoints; ++k) {
      // get a random point in theta-space; add weighted with Jacobian to estimate surface of n-sphere
      theta = Tomographer::Tools::denseRandom<Eigen::VectorXd>(rng, dist, SPH_DIM);
      // so translate them to the correct ranges.
      theta.block(0,0,ds-1,1) = theta.block(0,0,ds-1,1) * pi; // theta_i in [0, pi] for 0 <= i < ds-1
      theta(ds-1) = theta(ds-1) * 2 * pi; // theta_{ds-1} in [0, 2*pi]

      surf += Tomographer::MathTools::SphCoords::surf_sph_jacobian(theta);
    }

    // average all volume elements
    surf /= npoints;

    // multiply by volume of parameter space: 2*pi * pi^(ds-1)
    surf *=  2*pi *  Eigen::VectorXd::Constant(ds-1, pi).array().prod();

    return surf;
  }
};

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_sph_cart)

BOOST_AUTO_TEST_CASE(test_cart_to_sph_3)
{
  Eigen::Vector3d cart;
  cart << 1.0, 2.0, 3.0; // a random point in 3d space

  Eigen::Vector3d rtheta;
  Tomographer::MathTools::SphCoords::cart_to_sph(rtheta, cart); // cart -> rtheta

  BOOST_CHECK_CLOSE(rtheta(0), cart.norm(), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::cos(rtheta(1)), cart(0), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::cos(rtheta(2)), cart(1), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::sin(rtheta(2)), cart(2), tol_percent);

  Eigen::Vector3d backtocart = Eigen::Vector3d::Zero();
  Tomographer::MathTools::SphCoords::sph_to_cart(backtocart, rtheta); // back to -> cart
  BOOST_CHECK_CLOSE(backtocart(0), cart(0), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(1), cart(1), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(2), cart(2), tol_percent);
  BOOST_CHECK_CLOSE(backtocart.norm(), rtheta(0), tol_percent);

  // test sphsurf
  Eigen::Vector3d cartonsphsurf = Eigen::Vector3d::Zero();
  Tomographer::MathTools::SphCoords::sphsurf_to_cart(cartonsphsurf, rtheta.segment(1,2));
  double orignorm = cart.norm();
  BOOST_CHECK_CLOSE(cartonsphsurf(0)*orignorm, cart(0), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(1)*orignorm, cart(1), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(2)*orignorm, cart(2), tol_percent);
}

BOOST_AUTO_TEST_CASE(test_cart_to_sph_7)
{
  Eigen::VectorXd cart(7);
  cart << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0;

  Eigen::VectorXd rtheta(7);
  Tomographer::MathTools::SphCoords::cart_to_sph(rtheta, cart); // cart -> rtheta

  BOOST_CHECK_CLOSE(rtheta(0), cart.norm(), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::cos(rtheta(1)), cart(0), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::cos(rtheta(2)), cart(1), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::sin(rtheta(2))*std::cos(rtheta(3)), cart(2), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::sin(rtheta(2))*std::sin(rtheta(3))*std::cos(rtheta(4)), cart(3), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::sin(rtheta(2))*std::sin(rtheta(3))*std::sin(rtheta(4))*std::cos(rtheta(5)), cart(4), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::sin(rtheta(2))*std::sin(rtheta(3))*std::sin(rtheta(4))*std::sin(rtheta(5))*std::cos(rtheta(6)), cart(5), tol_percent);
  BOOST_CHECK_CLOSE(rtheta(0)*std::sin(rtheta(1))*std::sin(rtheta(2))*std::sin(rtheta(3))*std::sin(rtheta(4))*std::sin(rtheta(5))*std::sin(rtheta(6)), cart(6), tol_percent);

  Eigen::VectorXd backtocart = Eigen::VectorXd::Zero(7);
  Tomographer::MathTools::SphCoords::sph_to_cart(backtocart, rtheta); // back to -> cart
  BOOST_CHECK_CLOSE(backtocart(0), cart(0), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(1), cart(1), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(2), cart(2), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(3), cart(3), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(4), cart(4), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(5), cart(5), tol_percent);
  BOOST_CHECK_CLOSE(backtocart(6), cart(6), tol_percent);

  // test sphsurf
  Eigen::VectorXd cartonsphsurf = Eigen::VectorXd::Zero(7);
  Tomographer::MathTools::SphCoords::sphsurf_to_cart(cartonsphsurf, rtheta.segment(1,6));
  double orignorm = cart.norm();
  BOOST_CHECK_CLOSE(cartonsphsurf(0)*orignorm, cart(0), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(1)*orignorm, cart(1), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(2)*orignorm, cart(2), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(3)*orignorm, cart(3), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(4)*orignorm, cart(4), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(5)*orignorm, cart(5), tol_percent);
  BOOST_CHECK_CLOSE(cartonsphsurf(6)*orignorm, cart(6), tol_percent);
}

BOOST_AUTO_TEST_SUITE_END()

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

BOOST_AUTO_TEST_SUITE(test_sph_jacobians)

static const int NPOINTS = 1000000;

BOOST_FIXTURE_TEST_CASE(test_sph_jacobians_3, TestSphJacFixture<3>)
{
  const int CART_DIM = 3;

  // first time with seed=0
  double vol = calc_montecarlo_vol(0, NPOINTS);
  // another time with a different seed
  double vol2 = calc_montecarlo_vol(4689392, NPOINTS);

  const double ok_vol = known_vol_sph(CART_DIM);
  BOOST_CHECK_CLOSE(vol, ok_vol, 1.0/*one percent*/);
  BOOST_CHECK_CLOSE(vol2, ok_vol, 1.0/*one percent*/);

  // ... and do the same for the surface of a sphere
  double surf = calc_montecarlo_surf(0, NPOINTS);

  const double ok_surf = known_surf_sph(CART_DIM - 1);
  BOOST_CHECK_CLOSE(surf, ok_surf, 1.0/*one percent*/);
}

BOOST_FIXTURE_TEST_CASE(test_sph_jacobians_5, TestSphJacFixture<5>)
{
  const int CART_DIM = 5;

  // first time with seed=0
  double vol = calc_montecarlo_vol(0, NPOINTS);
  // another time with a different seed
  double vol2 = calc_montecarlo_vol(4689392, NPOINTS);

  const double ok_vol = known_vol_sph(CART_DIM);
  BOOST_CHECK_CLOSE(vol, ok_vol, 1.0/*one percent*/);
  BOOST_CHECK_CLOSE(vol2, ok_vol, 1.0/*one percent*/);

  // ... and do the same for the surface of a sphere
  double surf = calc_montecarlo_surf(0, NPOINTS);

  const double ok_surf = known_surf_sph(CART_DIM - 1);
  BOOST_CHECK_CLOSE(surf, ok_surf, 1.0/*one percent*/);
}

BOOST_AUTO_TEST_SUITE_END()

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

struct sphsurf_to_cart_fn {
  template<typename Der1, typename Der2>
  void operator()(Eigen::MatrixBase<Der2>& cart, const Eigen::MatrixBase<Der1>& theta) {
    Tomographer::MathTools::SphCoords::sphsurf_to_cart(cart, theta);
  }
};

template<int N, int DS = N-1>
struct sphsurf_to_diffcart_fn {
  template<typename Der1, typename Der2>
  void operator()(Eigen::MatrixBase<Der2>& dxdthetalinear, const Eigen::MatrixBase<Der1>& theta) {
    Eigen::Array<double, N, DS> dxdtheta;
    //std::cout << "start fn eval\n";
    Tomographer::MathTools::SphCoords::sphsurf_diffjac(dxdtheta, theta);
    //std::cout << "mid fn eval, dxdtheta's shape is (rows="<<dxdtheta.rows()<<",cols="<<dxdtheta.cols()<<")\n";
    for (int i = 0; i < DS; ++i) {
      dxdthetalinear.block(N*i, 0, N, 1) = dxdtheta.block(0, i, N, 1);
    }
    //std::cout << "end fn eval\n";
  }
};

template<int DEF_N_>
struct test_diffjac_fixture {
  enum {
    DEF_N = DEF_N_,
    DEF_DS = DEF_N_-1
  };

  Eigen::Matrix<double, DEF_DS, 1> theta;

  test_diffjac_fixture()
  {
    // some interesting theta point
    for (int k = 0; k < DEF_DS; ++k) {
      theta(k) = k;
    }
  }
};

BOOST_AUTO_TEST_SUITE(test_diffjacs)

const double tol_der = 1e-6;

BOOST_FIXTURE_TEST_CASE(test_diffjac, test_diffjac_fixture<11>)
{
  Eigen::Array<double, DEF_N, DEF_DS> dxdtheta;
  Tomographer::MathTools::SphCoords::sphsurf_diffjac(dxdtheta, theta);

  std::stringstream msgstream;
  bool ok = Tomographer::MathTools::check_derivatives(
      dxdtheta, // derivatives
      theta, // point
      sphsurf_to_cart_fn(), // fn
      DEF_N, // valdims
      tol_der,
      tol_der,
      msgstream
      );
  std::string msg = msgstream.str();
  if (msg.size()) {
    BOOST_MESSAGE(msg.c_str());
  }
  BOOST_CHECK(ok);
}

BOOST_FIXTURE_TEST_CASE(test_diffjac2, test_diffjac_fixture<8>)
{
  // now, check second derivatives
  Eigen::Array<double, DEF_N, DEF_DS*DEF_DS> ddxddtheta;
  Tomographer::MathTools::SphCoords::sphsurf_diffjac2(ddxddtheta, theta);

  Eigen::Array<double, DEF_N*DEF_DS, DEF_DS> ddxddtheta_reshaped;
  for (int k = 0; k < DEF_N; ++k) {
    for (int i = 0; i < DEF_DS; ++i) {
      for (int j = 0; j < DEF_DS; ++j) {
        ddxddtheta_reshaped(DEF_N*i + k, j) = ddxddtheta(k, i+DEF_DS*j);
      }
    }
  }

  std::stringstream msgstream;
  bool ok = Tomographer::MathTools::check_derivatives(
      ddxddtheta_reshaped, // derivatives of the derivatives :)
      theta, // point
      sphsurf_to_diffcart_fn<DEF_N,DEF_DS>(), //fn
      DEF_N*DEF_DS, // valdims
      tol_der,
      tol_der,
      msgstream
      );
  std::string msg = msgstream.str();
  if (msg.size()) {
    BOOST_MESSAGE(msg.c_str());
  }
  BOOST_CHECK(ok);
}

BOOST_AUTO_TEST_SUITE_END()


#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "NeuralNetwork.h"

int main(void)
{
  NeuralNetwork nn;

  float loss;
  Matrix<float, Dynamic, Dynamic> out;

#if 1
  Matrix<float, 1, 2> input[4];
  Matrix<float, 1, 1> expect[4];
  Matrix<float, Dynamic, Dynamic> W1(2,2);
  Matrix<float, Dynamic, Dynamic> W2(2,1);

  input[0] << 0, 0; expect[0] << 0;
  input[1] << 0, 1; expect[1] << 1;
  input[2] << 1, 0; expect[2] << 1;
  input[3] << 1, 1; expect[3] << 0;

  W1 << -0.0032973, -0.02393722, -0.02107663, 0.01280219 ;
  W2 << -0.01024167, -0.01514217;

  nn.createNewLayer(2,2, false, &W1);
  nn.createNewLayer(2,1, true,  &W2);

  loss = 100.f;
  nn.printnet();
  // while (loss > 0.001f)
  for(int j=0; j<10; j++)
    {
      loss = 0.f;
      for(int i=0; i<4; i++)
        {
          // std::cout << "==== " << i+1 << " ===="  << std::endl;
          out = nn.forward( input[i] );
          loss += nn.backward(0.1f, expect[i]);
          nn.printnet();
          // std::cout << "   out = " << out << std::endl;
        }
      loss /= 4;
      std::cout << "loss = " << loss << std::endl;
      nn.printnet();
    }

  for(int i=0; i<4; i++)
    {
      std::cout << "Expect = " << expect[i] << " Input  = " << input[i] << std::endl;
      out = nn.forward( input[i] );
      std::cout << "Output = " << out << std::endl;
    }

#else
  Matrix<float, 1, 2> input;
  Matrix<float, 1, 2> expect;

  input << 3, 3;
  expect << 0, 1;

  nn.createNewLayer(2, 5);
  nn.createNewLayer(5, 3);
  nn.createNewLayer(3, 2, true);

  std::cout << "tmp input = " << input << std::endl;
  std::cout << "expect = " << expect << std::endl;
  std::cout << "input * expect = " << input.array() * expect.array() << std::endl;

  std::cout << "input = " << input << std::endl;
  input = input.array() + 1;
  std::cout << "input = " << input << std::endl;

  out = nn.forward( input );
  std::cout << "out.rows = " << out.rows() << std::endl;
  std::cout << "out.cols = " << out.cols() << std::endl;
  std::cout << "out = " << out << std::endl;

  loss = 100.f;
  while (loss > 0.001f)
    {
      out = nn.forward( input );
      loss = nn.backward(0.1f, expect);
      std::cout << "out = " << out << std::endl;
      std::cout << "loss = " << loss << std::endl;
    }
#endif

  return 0;
}


#include <SDL2/SDL.h>
#include <vector>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <cstdio>
#include <sys/types.h>
#include <unistd.h>
#include <iostream>

double cos_rule_angle(double A, double B, double C) {
  if (fabs((A + B) - C) < 0.000001) {
    return M_PI;
  }
  if (!(A + B >= C)) {
    fprintf(stderr, "Assertion Error: A + B >= C\n");
    exit(1);
  }
  return acos((A * A + B * B - C * C) / (2.0 * A * B));
}

bool concave(const arma::vec &l, const arma::vec &c, const arma::vec &r, double lrad, double crad, double rrad) {
  double dlcx = c(0) - l(0);
  double dlcy = c(1) - l(1);
  double drcx = c(0) - r(0);
  double drcy = c(1) - r(1);
  double lc = sqrt(dlcx * dlcx + dlcy * dlcy);
  double rc = sqrt(drcx * drcx + drcy * drcy);
  double theta1 = cos_rule_angle(lc, crad, lrad);
  double theta2 = cos_rule_angle(rc, crad, rrad);
  return ((theta1 + theta2) - M_PI) > -0.1;
}

bool within(const arma::vec &l, const arma::vec &c, const arma::vec &r, double lrad, double crad, double rrad) {
  if (crad == 0.0) {
    return true;
  }
  return concave(l, c, r, lrad, crad, rrad);
}

std::vector<arma::vec> get_filled_boundary(const std::vector<arma::vec> &pts) {
  arma::vec X(pts.size());
  arma::vec Y(pts.size());
  for (int i = 0; i < pts.size(); i++) {
    X(i) = pts[i](0) * cos(pts[i](1));
    Y(i) = pts[i](0) * sin(pts[i](1));
  }
  int min_x = 0;
  int min_y = 0;
  int max_x = 0;
  int max_y = 0;
  int tx;
  int ty;
  for (int i = 0; i < pts.size(); i++) {
    tx = (int)floor(X(i));
    if (tx < min_x) {
      min_x = tx;
    }
    tx = (int)ceil(X(i));
    if (tx > max_x) {
      max_x = tx;
    }
    ty = (int)floor(Y(i));
    if (ty < min_y) {
      min_y = ty;
    }
    ty = (int)ceil(Y(i));
    if (ty > max_y) {
      max_y = ty;
    }
  }
  max_x++;
  max_y++;
  // create vmap
  arma::mat vmap = arma::zeros<arma::mat>(max_y - min_y, max_x - min_x);
  std::vector<arma::vec> colored;
  std::vector<arma::vec> infection_queue = {{ 0.0, 0.0 }};
  vmap(-min_y, -min_x) = 1.0;
  int index = 0;
  while (infection_queue.size() > index) {
    double x = infection_queue[index](0);
    double y = infection_queue[index](1);

    double theta = atan2(y, x);
    if (theta < 0.0) {
      theta += 2.0 * M_PI;
    }
    double radius = sqrt(x * x + y * y);
    // binary search
    int left = 0;
    int right = pts.size() - 1;
    if (pts[right](1) <= theta || theta < pts[left](1)) {
      left = right;
      right = 0;
    } else {
      int mid = (left + right) / 2;
      while (left + 1 != right) {
        if (theta >= pts[mid](1)) {
          left = mid;
        } else {
          right = mid;
        }
        mid = (left + right) / 2;
      }
    }

    // bfs
    arma::vec l({X(left), Y(left)});
    arma::vec c({x, y});
    arma::vec r({X(right), Y(right)});
    int dx = x - min_x;
    int dy = y - min_y;
    if (within(l, c, r, pts[left](0), radius, pts[right](0))) {
      colored.push_back(c);
      if (y + 1 < max_y && vmap(dy + 1, dx) == 0.0) {
        infection_queue.push_back({x, y + 1});
        vmap(dy + 1, dx) = 1.0;
      }
      if (y - 1 >= min_y && vmap(dy - 1, dx) == 0.0) {
        infection_queue.push_back({x, y - 1});
        vmap(dy - 1, dx) = 1.0;
      }
      if (x + 1 < max_x && vmap(dy, dx + 1) == 0.0) {
        infection_queue.push_back({x + 1, y});
        vmap(dy, dx + 1) = 1.0;
      }
      if (x - 1 >= min_x && vmap(dy, dx - 1) == 0.0) {
        infection_queue.push_back({x - 1, y});
        vmap(dy, dx - 1) = 1.0;
      }
    }
    //infection_queue.erase(infection_queue.begin());
    index++;
  }
  return colored;
}

std::vector<arma::vec> lidar_read(void) {
  std::vector<arma::vec> pts;
  double theta = 0.0;
  while (theta < 360.0) {
    theta += (double)rand() / (double)RAND_MAX + 2.0;
    if (theta > 360.0) {
      break;
    }
    double radius = (double)rand() / (double)RAND_MAX * 100.0 + 100.0;
    pts.push_back(arma::vec({radius, theta * M_PI / 180.0}));
  }
  return pts;
}

int main(int argc, char *argv[]) {
  arma::vec center = { 300.0, 300.0 };

  srand(getpid());
  SDL_Init(SDL_INIT_VIDEO);
  SDL_Window *window = SDL_CreateWindow("raycast", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, 640, 640, SDL_WINDOW_SHOWN);
  SDL_Renderer *renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_ACCELERATED | SDL_RENDERER_PRESENTVSYNC);
  SDL_Surface *screen = SDL_CreateRGBSurface(0, 640, 640, 32, 0, 0, 0, 0);
  SDL_Texture *texture = SDL_CreateTextureFromSurface(renderer, screen);

  bool done = false;
  uint32_t color_black = SDL_MapRGB(screen->format, 0, 0, 0);
  uint32_t color_red = SDL_MapRGB(screen->format, 255, 0, 0);
  uint32_t color_green = SDL_MapRGB(screen->format, 0, 255, 0);
  uint32_t color_blue = SDL_MapRGB(screen->format, 0, 0, 255);
  while (true) {
    SDL_Event e;
    while (SDL_PollEvent(&e)) {
      if (e.type == SDL_QUIT) {
        done = true;
        break;
      }
    }
    if (done) {
      break;
    }

    std::vector<arma::vec> data = lidar_read();
    printf("getting raycast area...\n");
    std::vector<arma::vec> raycast_area = get_filled_boundary(data);

    SDL_FillRect(screen, NULL, color_black);
    for (arma::vec &pt : raycast_area) {
      int x = (int)(pt(0) + center(0));
      int y = (int)(-pt(1) + center(1));
      ((uint32_t *)screen->pixels)[y * 640 + x] = color_red;
    }
    for (arma::vec &pt : data) {
      int x = (int)(pt(0) * cos(pt(1)) + center(0));
      int y = (int)(-pt(0) * sin(pt(1)) + center(1));
      ((uint32_t *)screen->pixels)[y * 640 + x] = color_green;
    }
    ((uint32_t *)screen->pixels)[300 * 640 + 299] = color_blue;
    ((uint32_t *)screen->pixels)[300 * 640 + 300] = color_blue;
    ((uint32_t *)screen->pixels)[300 * 640 + 301] = color_blue;
    ((uint32_t *)screen->pixels)[299 * 640 + 300] = color_blue;
    ((uint32_t *)screen->pixels)[301 * 640 + 300] = color_blue;

    SDL_UpdateTexture(texture, NULL, screen->pixels, screen->pitch);
    SDL_RenderClear(renderer);
    SDL_RenderCopy(renderer, texture, NULL, NULL);
    SDL_RenderPresent(renderer);
    SDL_Delay(25);
  }
  SDL_DestroyTexture(texture);
  SDL_FreeSurface(screen);
  SDL_DestroyRenderer(renderer);
  SDL_DestroyWindow(window);
  SDL_Quit();
  return 0;
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef GRADIENT_FUNCTOR_INC_HPP
#define GRADIENT_FUNCTOR_INC_HPP

#include "gradient_functor.hpp"

#include <boost/bind.hpp>
#include <boost/gil/extension/numeric/kernel.hpp>
#include <boost/gil/extension/numeric/convolve.hpp>
#include <boost/gil/extension/io_new/tiff_read.hpp>

namespace internal {

#if !defined(M_PI)
    const double M_PI = 4.0 * atan(1.0);
#endif

    template<typename Kernel1D>
    void initKernelGaussian1D(Kernel1D& kernel, double m_sigma)
    {
	// Gaussian smoothing
        typedef	typename Kernel1D::value_type vt;
        const vt z = 1.0 / (std::sqrt(2 * M_PI) * m_sigma);
        const vt m_sigmasquared = m_sigma * m_sigma;
        vt x = -1.0 * kernel.center();
        typename Kernel1D::iterator i;
        vt sum = 0.;
        for (i=kernel.begin(); i!=kernel.end(); ++i, ++x)
        {
            *i = z * (std::exp(-0.5*(x*x/m_sigmasquared)));
            sum += *i;
        }
        for (i=kernel.begin(); i!=kernel.end(); ++i) *i /= sum;
    }

    template<typename Kernel1D>
    void initKernelGaussianDeriv1D(Kernel1D& kernel, double m_sigma)
    {
	// Gaussian derivative smoothing
        typedef	typename Kernel1D::value_type vt;
        const vt z = 1.0 / (std::sqrt(2 * M_PI) * m_sigma);
        const vt m_sigmasquared = m_sigma * m_sigma;
        vt x = -1.0 * kernel.center();
        typename Kernel1D::iterator i;
        vt sum = 0.;
        for (i=kernel.begin(); i!=kernel.end(); ++i, ++x)
        {
            *i = - (x/m_sigmasquared) * z * (std::exp(-0.5*(x*x/m_sigmasquared)));
            sum += *i * x;
        }
        for (i=kernel.begin(); i!=kernel.end(); ++i) *i /= sum;
    }

}

template<typename Image, typename View>
typename gradient_functor::result_type gradient_functor::operator()(Image& g, const View& v) const
{
    using namespace boost::gil;

    typedef typename Image::view_t g_view_t;
    typedef typename get_pixel_type<g_view_t>::type g_pixel_t;
    typedef typename kth_element_type<g_pixel_t,0>::type element_0_t;
    typedef typename kth_element_type<g_pixel_t,1>::type element_1_t;
    typedef pixel<element_0_t, gray_layout_t > g0_pixel_t;
    typedef pixel<element_1_t, gray_layout_t > g1_pixel_t;

    g.recreate(v.dimensions());

    unsigned int half_size = (unsigned int) (3* m_sigma) ;
    const size_t kws = 2 * half_size + 1;
    kernel_1d<float> ksmooth(kws, kws / 2);
    kernel_1d<float> kderiv(kws, kws / 2);
    internal::initKernelGaussian1D(ksmooth, m_sigma);
    internal::initKernelGaussianDeriv1D(kderiv, m_sigma);

    convolve_cols<g0_pixel_t> (v, ksmooth, kth_channel_view<0> (view(g)), convolve_option_extend_constant);
    convolve_rows<g0_pixel_t> (kth_channel_view<0> (view(g)), kderiv, kth_channel_view<0> (view(g)), convolve_option_extend_constant);

    convolve_rows<g1_pixel_t> (v, ksmooth, kth_channel_view<1> (view(g)), convolve_option_extend_constant);
    convolve_cols<g1_pixel_t> (kth_channel_view<1> (view(g)), kderiv, kth_channel_view<1> (view(g)), convolve_option_extend_constant);
}

#endif // GRADIENT_FUNCTOR_INC_HPP


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <string>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include "DatasetAR.h"
#include "DatasetARX.h"
#include "GaussSeq.h"
#include "ARSeq.h"
#include "ARPoly.h"
#include "utils.h"
using utils::my_float;
using boost::multiprecision::cpp_bin_float_50;
using boost::random::uniform_real_distribution;

int main() {
    boost::random::mt19937 gen {};
    uniform_real_distribution<my_float> u(-1, 1);

    // prepare
    std::map<int, my_float> coeff {
        {3, 0.2},
        {20, 0.5},
    };
    std::map<int, my_float> coeff_2 {
        {3, 0.75},
        {20, -0.9},
    };
    std::vector<my_float> v_seed(20);

    // Stationary AR process
    for (auto type = 0; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            // Seed differently
            for (auto i = 0; i < 20; i++) {
                v_seed[i] = u(gen);
            }
            ARSeq targ_seq(coeff, u(gen));
            targ_seq.seed_prev_vals(v_seed);

            DatasetAR dat("./ARStat/ARStat-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), targ_seq, type);
            dat.write_csv();
        }
    }

    // Non-stationary AR process
    for (auto type = 0; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            // Seed differently
            for (auto i = 0; i < 20; i++) {
                v_seed[i] = u(gen);
            }
            ARSeq targ_seq(coeff_2, u(gen));
            targ_seq.seed_prev_vals(v_seed);

            DatasetAR dat("./ARNStat/ARNStat-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), targ_seq, type);
            dat.write_csv();
        }
    }

    // Non-stationary ARX process
    for (auto type = 1; type < 5; type++) {
        for (auto samp = 0; samp < utils::N_SAMPLE; samp++) {
            DatasetARX dat("./ARXNStat/ARXNStat-" + std::to_string(type) +
                "/Sample-" + std::to_string(samp), type);
            dat.write_csv();
        }
    }
    return 0;
}


#ifndef OCV_CBIR_BOVW_HPP
#define OCV_CBIR_BOVW_HPP

#include "../arma/type_traits.hpp"

#include <opencv2/core.hpp>

#include <armadillo>

/*!
 *  \addtogroup ocv
 *  @{
 */
namespace ocv{

/*!
 *  \addtogroup cbir
 *  @{
 */
namespace cbir{

/**
 *Convert the features of images into histogram
 *@tparam T type of the code_book and features
 *@tparam Hist type of histogram
 *@code
 *arma::Mat<float> code_book;
 *code_book.load("ukbench.h5", arma::hdf5_binary);
 *bovw<float> bv(std::move(code_book));
 *arma::Mat<float> features;
 *features.load("uk3000", arma::raw_ascii);
 *auto const hist = bv.describe(features);
 *@endcode
 */
template<typename T, typename Hist = arma::SpMat<arma::uword>>
class bovw
{
public:
    bovw()
    {
        static_assert(std::is_arithmetic<T>::value,
                      "T should be arithmetic type");

        static_assert(armd::is_arma_matrix<Hist>::value,
                      "Hist should be arma::SpMat, arma::Mat,"
                      "arma::Col or arma::Row");
    }

    /**
     * Convert the features of images into histogram
     * @param features features of the image
     * @param code_book code book of the data sets
     * @return histogram of bovw
     */
    Hist describe(arma::Mat<T> const &features,
                  arma::Mat<T> const &code_book) const
    {
        arma::Mat<T> dist(features.n_cols, code_book.n_cols);
        for(arma::uword i = 0; i != features.n_cols; ++i){
            dist.row(i) = euclidean_dist(features.col(i),
                                         code_book);
        }
        //dist.print("dist");

        Hist hist = create_hist(dist.n_cols,
                                armd::is_two_dim<Hist>::type());
        for(arma::uword i = 0; i != dist.n_rows; ++i){
            arma::uword min_idx;
            dist.row(i).min(min_idx);
            ++hist(min_idx);
        }
        //hist.print("\nhist");

        return hist;
    }

private:
    Hist create_hist(arma::uword size, std::true_type) const
    {
        return arma::zeros<Hist>(size,1);
    }

    Hist create_hist(arma::uword size, std::false_type) const
    {
        return arma::zeros<Hist>(size);
    }

    template<typename U>
    arma::Mat<T> euclidean_dist(U const &x,
                                arma::Mat<T> const &y) const
    {
        return arma::sqrt(arma::sum
                          (arma::square(y.each_col() - x)));
    }
};

} /*! @} End of Doxygen Groups*/

} /*! @} End of Doxygen Groups*/

#endif // OCV_CBIR_BOVW_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SQRT1PM1_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SQRT1PM1_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing sqrt1pm1 capabilities

    Returns \f$\sqrt{1+x}-1\f$ and the
    result is accurate even for x  with small modulus

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = sqrt1pm1(x);
    @endcode

    is similar to:

    @code
    T r = dec(sqrt(Oneplus(x));
    @endcode

    @see lol1p, expm1.

  **/
  Value sqrt1pm1(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/sqrt1pm1.hpp>
#include <boost/simd/function/simd/sqrt1pm1.hpp>

#endif


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace boost;

int
main(int, char *[])
{
  typedef adjacency_list_traits<listS, listS,
    directedS>::vertex_descriptor vertex_descriptor;
  typedef adjacency_list < listS, listS, directedS,
    property<vertex_index_t, int,
    property<vertex_name_t, char,
    property<vertex_distance_t, int,
    property<vertex_predecessor_t, vertex_descriptor> > > >,
    property<edge_weight_t, int> > graph_t;
  typedef std::pair<int, int> Edge;

  const int num_nodes = 5;
  enum nodes { A, B, C, D, E };
  Edge edge_array[] = { Edge(A, C), Edge(B, B), Edge(B, D), Edge(B, E),
    Edge(C, B), Edge(C, D), Edge(D, E), Edge(E, A), Edge(E, B)
  };
  int weights[] = { 1, 2, 1, 2, 7, 3, 1, 1, 1 };
  int num_arcs = sizeof(edge_array) / sizeof(Edge);
  graph_traits<graph_t>::vertex_iterator i, iend;

  graph_t g(edge_array, edge_array + num_arcs, weights, num_nodes);
  property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);

  // Manually intialize the vertex index and name maps
  property_map<graph_t, vertex_index_t>::type indexmap = get(vertex_index, g);
  property_map<graph_t, vertex_name_t>::type name = get(vertex_name, g);
  int c = 0;
  for (boost::tie(i, iend) = vertices(g); i != iend; ++i, ++c) {
    indexmap[*i] = c;
    name[*i] = 'A' + c;
  }

  vertex_descriptor s = vertex(A, g);

  property_map<graph_t, vertex_distance_t>::type
    d = get(vertex_distance, g);
  property_map<graph_t, vertex_predecessor_t>::type
    p = get(vertex_predecessor, g);
  dijkstra_shortest_paths(g, s, predecessor_map(p).distance_map(d));

  std::cout << "distances and parents:" << std::endl;
  graph_traits < graph_t >::vertex_iterator vi, vend;
  for (boost::tie(vi, vend) = vertices(g); vi != vend; ++vi) {
    std::cout << "distance(" << name[*vi] << ") = " << d[*vi] << ", ";
    std::cout << "parent(" << name[*vi] << ") = " << name[p[*vi]] << std::
      endl;
  }
  std::cout << std::endl;

  std::ofstream dot_file("figs/dijkstra-eg.dot");
  dot_file << "digraph D {\n"
    << "  rankdir=LR\n"
    << "  size=\"4,3\"\n"
    << "  ratio=\"fill\"\n"
    << "  edge[style=\"bold\"]\n" << "  node[shape=\"circle\"]\n";

  graph_traits < graph_t >::edge_iterator ei, ei_end;
  for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
    graph_traits < graph_t >::edge_descriptor e = *ei;
    graph_traits < graph_t >::vertex_descriptor
      u = source(e, g), v = target(e, g);
    dot_file << name[u] << " -> " << name[v]
      << "[label=\"" << get(weightmap, e) << "\"";
    if (p[v] == u)
      dot_file << ", color=\"black\"";
    else
      dot_file << ", color=\"grey\"";
    dot_file << "]";
  }
  dot_file << "}";
  return EXIT_SUCCESS;
}


#pragma once

#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <eigen_conversions/eigen_msg.h>

#include <ndt_map/pointcloud_utils.h>
#include <ndt_rviz/ndt_rviz.h>

#include <sstream>

#include <visualization_msgs/MarkerArray.h>

Eigen::Affine3d getAsAffine(const Eigen::Vector3d &transl, const Eigen::Vector3d &euler) {
  Eigen::Affine3d T;
  {
    Eigen::Matrix3d m;
    m = Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ());
    Eigen::Translation3d v(transl);
    T = v*m;
  }
  return T;
}

std::string affine3dToString(const Eigen::Affine3d &T) {
  std::ostringstream stream;
  stream << std::setprecision(std::numeric_limits<double>::digits10);
  Eigen::Vector3d rot = T.rotation().eulerAngles(0,1,2);

  stream << T.translation().transpose() << " " << rot.transpose();
  return stream.str();
}

class NDTCalibScan {
public:
  pcl::PointCloud<pcl::PointXYZ> cloud; // cloud in sensor coords
  Eigen::Affine3d pose;  // estimated pose in global frame (preferably from a GT system)...
  Eigen::Affine3d estSensorPose; // estimated sensor pose in global frame (typically from a SLAM / registration system)
  double stamp;

  NDTCalibScan() {

  }

  NDTCalibScan(const Eigen::Affine3d &_pose, const Eigen::Affine3d &_estSensorPose, double _stamp) : pose(_pose), estSensorPose(_estSensorPose), stamp(_stamp) {

  }

  NDTCalibScan(const pcl::PointCloud<pcl::PointXYZ> &_cloud, const Eigen::Affine3d &_pose, double _stamp) : cloud(_cloud), pose(_pose), stamp(_stamp)
  {

  }

  NDTCalibScan(const pcl::PointCloud<pcl::PointXYZ> &_cloud, const Eigen::Affine3d &_pose, const Eigen::Affine3d &_estSensorPose, double _stamp)  : cloud(_cloud), pose(_pose), estSensorPose(_estSensorPose), stamp(_stamp)
  {

  }

  void appendPointCloudGlobal(const Eigen::Affine3d &Ts, pcl::PointCloud<pcl::PointXYZ> &p) const {

    pcl::PointCloud<pcl::PointXYZ> c =cloud;
    Eigen::Affine3d T = pose * Ts;
    perception_oru::transformPointCloudInPlace(T, c);
    p += c;
  }

  // Simply the euclidean offset bettwen the estimated sensor pose and the given gt poses
  double scoreEstSensorPose(const Eigen::Affine3d &Ts) const {
    Eigen::Affine3d T = pose * Ts;
    return (T * estSensorPose.inverse()).translation().norm();
  }

    // Return Ts from the global estimate sensor pose and the vehicle pose (both in the same global reference frame).
  Eigen::Affine3d getTs() const {
    return (pose.inverse() * estSensorPose);
  }

  Eigen::Affine3d getSensorPoseFromPose(const Eigen::Affine3d &Ts) const  {
    return pose*Ts;
  }
};

Eigen::VectorXd getTranslationEulerAnglesVector(const Eigen::Affine3d &T) {
  Eigen::VectorXd ret(6);
  ret[0] = T.translation()[0];
  ret[1] = T.translation()[1];
  ret[2] = T.translation()[2];
  Eigen::Vector3d rot = T.rotation().eulerAngles(0,1,2);
  ret[3] = rot[0];
  ret[4] = rot[1];
  ret[5] = rot[2];

  return ret;
}

class NDTCalibScanPair {

public:
  NDTCalibScan first;
  NDTCalibScan second;

  NDTCalibScanPair() {

  }
  NDTCalibScanPair(const pcl::PointCloud<pcl::PointXYZ> &p1, const Eigen::Affine3d &pose1,
                   const pcl::PointCloud<pcl::PointXYZ> &p2, const Eigen::Affine3d &pose2) :
    first(NDTCalibScan(p1, pose1, -1.)), second(NDTCalibScan(p2, pose2, -1.))
  {

  }

  Eigen::Affine3d getRelativePose() const {
    return this->second.pose * this->first.pose.inverse();
  }

  Eigen::Affine3d getRelativeEstSensorPose() const {
    return this->second.estSensorPose * this->first.estSensorPose.inverse();
  }

  // Compute the score for a relative sensor pose offset - that is the pose and the est sensor pose don't have to be in the same coordinate frame.
  double scoreEstSensorPoseRel(const Eigen::Affine3d &Ts) const {
    Eigen::Affine3d rel = getRelativePose();
    Eigen::Affine3d rel_est = getRelativeEstSensorPose();
    return (Ts * rel_est * Ts.inverse() * rel.inverse()).translation().norm();
  }

  // Compute the predicted relative estimated sensor pose.
  Eigen::Affine3d getPredictedRelativeEstSensorPose(const Eigen::Affine3d &Ts) const {
    Eigen::Affine3d rel = getRelativePose();
    return (Ts.inverse() * rel * Ts);
  }

  // Compute difference between the relative estimated and the predicted sensor pose
  Eigen::Affine3d getDifference(const Eigen::Affine3d &Ts) const {
    Eigen::Affine3d T = getPredictedRelativeEstSensorPose(Ts);
    Eigen::Affine3d rel_est = getRelativeEstSensorPose();

    return rel_est.inverse()*T;
  }

  // Get the difference in x,y,z and euler angles
  Eigen::VectorXd getDifferenceVector(const Eigen::Affine3d &Ts)  const {
    return getTranslationEulerAnglesVector(getDifference(Ts));
  }

  ///Get point cloud in global coords
  void appendPointCloudGlobal(const Eigen::Affine3d &Ts, pcl::PointCloud<pcl::PointXYZ> &p) const {

    this->first.appendPointCloudGlobal(Ts, p);
    this->second.appendPointCloudGlobal(Ts, p);
  }

  /**
   * Compute ICP score for sensor offset
   */
  double scoreICP(const Eigen::Affine3d &Ts) const {
    pcl::PointCloud<pcl::PointXYZ> c1=this->first.cloud;
    Eigen::Affine3d p1 = this->first.pose * Ts;
    perception_oru::transformPointCloudInPlace(p1, c1);

    pcl::PointCloud<pcl::PointXYZ> c2=this->second.cloud;
    Eigen::Affine3d p2 = this->second.pose * Ts;
    perception_oru::transformPointCloudInPlace(p2, c2);

    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    typename pcl::KdTree<pcl::PointXYZ>::PointCloudPtr mp (new pcl::PointCloud<pcl::PointXYZ>);
    if(c1.size()==0){
      fprintf(stderr,"Check -> num points = %d\n",(int)c1.size());
    }
    //fprintf(stderr,"1: '%d' -- ",c1.size());
    (*mp) = c1;
    //fprintf(stderr,"2: '%d' == '%d'\n",c1.size(), mp->size());
    kdtree.setInputCloud (mp);
    int K = 1;
    std::vector<int> pointIdxNKNSearch(K);
    std::vector<float> pointNKNSquaredDistance(K);
    double e=0, error_th=0.1*0.1;

    for(unsigned int i=0;i<c2.size();i++){
      if ( kdtree.nearestKSearch (c2[i], K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 ){
        if(pointNKNSquaredDistance[0] > error_th){
          e+=error_th;
        }
        else{
          e+= pointNKNSquaredDistance[0];
        }
        //fprintf(f,"%f\n",pointNKNSquaredDistance[0]);
      }
    }//FOR

    return e;
  }
};

// Load the evaluation files that are generated by the fuser, <timestamp> x y x qx qy qz qw.
std::vector<Eigen::Affine3d> loadAffineFromEvalFile(const std::string &fileName) {
  std::vector<Eigen::Affine3d> ret;
  std::string line;
  std::ifstream myfile (fileName.c_str());
  if (myfile.is_open())
  {
    while ( getline (myfile,line) )
    {
      double time, x, y, z, qx, qy, qz, qw;
      std::istringstream ss(line);
      ss >> time >> x >> y >> z >> qx >> qy >> qz >> qw;
      ret.push_back(Eigen::Translation3d(x,y,z)*Eigen::Quaterniond(qw, qx, qy, qz));
    }
    myfile.close();
  }
  else {
    std::cout << "Unable to open file : " << fileName << std::endl;
  }

  return ret;
}

// Load timestamps from the evaluation files that are generated by the fuser...
std::vector<double> loadTimeStampFromEvalFile(const std::string &fileName) {
  std::vector<double> ret;
  std::string line;
  std::ifstream myfile (fileName.c_str());
  if (myfile.is_open())
  {
    while ( getline (myfile,line) )
    {
      double time, x, y, z, qx, qy, qz, qw;
      std::istringstream ss(line);
      ss >> time >> x >> y >> z >> qx >> qy >> qz >> qw;
      ret.push_back(time);
   }
   myfile.close();
 }
  else std::cout << "Unable to open file : " << fileName << std::endl;;

  return ret;
}

void loadNDTCalibScanPairs(const std::string &gt_file, const std::string &est_sensorpose_file, const std::string &base_name_pcd, std::vector<NDTCalibScanPair> &scans) {
  scans.resize(0);

  // Load the data...
  std::vector<double> stamps = loadTimeStampFromEvalFile(gt_file);
  std::vector<Eigen::Affine3d> Tgt = loadAffineFromEvalFile(gt_file);
  std::vector<Eigen::Affine3d> Test_sensorpose = loadAffineFromEvalFile(est_sensorpose_file);

  if (Tgt.size() != stamps.size() || Tgt.size() != Test_sensorpose.size()) {
    std::cerr << "Warning: the length of the provided files to not match(!)" << std::endl;
  }

  // Need to find a pair of scans which is useful in the optimization scheme. Pair of scans with limited translation but with some rotation...
  size_t j = 0;
  for (size_t i = 1; i < Tgt.size(); i++) {
    Eigen::Affine3d Tmotion = Tgt[j].inverse()*Tgt[i];

    Eigen::Affine3d Ttest = Tgt[j]*Tmotion;
    // while (Tmotion.translation().norm() > 3) {
    //   j++;
    //   Tmotion = Tgt[i] * Tgt[j].inverse();
    // }
    if (Tmotion.translation().norm() > 3) {
      j = i; // reset -> make this better, could be that many potential pairs are left out..., add a while loop instead.
      continue;
    }
    if (Tmotion.rotation().eulerAngles(0,1,2).norm() < 5.0*M_PI/180.0) {
      continue;
    }

    // Good pair found...
    NDTCalibScanPair pair;
    pair.first = NDTCalibScan(Tgt[j], Test_sensorpose[j], stamps[j]);
    pair.second = NDTCalibScan(Tgt[i], Test_sensorpose[i], stamps[i]);
    scans.push_back(pair);
    j = i; // reset
  }

}

// // Helper functions for ROS
// geometry_msgs::PoseStamped getGtPoseFromCalibScan(const NDTCalibScan &scan) {
//   geometry_msgs::PoseStamped p;
//   tf::poseEigenToMsg (scan.pose, p.pose);
//   p.header.stamp = ros::Time(scan.stamp);
//   p.header.frame_id = std::string("/world");

//   return p;
// }

// geometry_msgs::Pose getEstSensorPoseFromCalibScan(const NDTCalibScan &scan) {
//   geometry_msgs::PoseStamped p;
//   tf::poseEigenToMsg (scan.estSensorPose, p.pose);
//   p.header.stamp = ros::Time(scan.stamp);
//   p.header.frame_id = std::string("/world");

//   return p;
// }

namespace ndt_visualisation {

// Visualization markers.
void appendMarkerArray(visualization_msgs::MarkerArray &array, const visualization_msgs::MarkerArray &add) {
  for (size_t i = 0; i < add.markers.size(); i++) {
    array.markers.push_back(add.markers[i]);
  }
}

visualization_msgs::Marker getMarkerArrowAffine3d(const Eigen::Affine3d &T, int id, int color, const std::string &ns) {
  visualization_msgs::Marker m;
  assignDefault(m);
  assignColor(m, color);
  m.ns = ns;
  m.type = visualization_msgs::Marker::ARROW;
  m.action = visualization_msgs::Marker::ADD;
  m.scale.y = 0.1; m.scale.z = 0.1;
  m.id = id;
  tf::poseEigenToMsg (T, m.pose);
  return m;
}

visualization_msgs::Marker getMarkerCylinder(const Eigen::Affine3d &T,
                                                  int id, int color,
                                                  double length, double radius,
                                                  const std::string &ns) {
  visualization_msgs::Marker m;
  assignDefault(m);
  assignColor(m, color);
  m.ns = ns;
  m.type = visualization_msgs::Marker::CYLINDER;
  m.action = visualization_msgs::Marker::ADD;
  m.id = id;

  m.scale.x = radius; m.scale.y = radius; m.scale.z = length;
  tf::poseEigenToMsg(T, m.pose);
  return m;
}

/// Draw an x,y,z coordsystem given an affine3d.
visualization_msgs::MarkerArray getMarkerFrameAffine3d(const Eigen::Affine3d &T, const std::string &ns, double length, double radius) {

  visualization_msgs::MarkerArray m;
  // X
  {
    Eigen::Affine3d T_x =
      Eigen::Translation3d(length / 2.0, 0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY());
    T_x = T * T_x;
    m.markers.push_back(getMarkerCylinder(T_x, 0, 0, length, radius, ns));
  }
  // Y
  {
    Eigen::Affine3d T_y =
      Eigen::Translation3d(0, length / 2.0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX());
    T_y = T * T_y;
    m.markers.push_back(getMarkerCylinder(T_y, 1, 1, length, radius, ns));
  }
  // Z
  {
    Eigen::Affine3d T_z = Eigen::Translation3d(0, 0, length / 2.0) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
    T_z = T * T_z;
    m.markers.push_back(getMarkerCylinder(T_z, 2, 2, length, radius, ns));
  }
  return m;
}

visualization_msgs::Marker getMarkerPoseFromNDTCalibScan(const NDTCalibScan &scan, int id, int color) {
  return getMarkerArrowAffine3d(scan.pose, id, color, "ndt_calib_pose");
}

visualization_msgs::Marker getMarkerEstSensorPoseFromNDTCalibScan(const NDTCalibScan &scan, int id, int color) {
  return getMarkerArrowAffine3d(scan.estSensorPose, id, color, "ndt_calib_est_sensor_pose");
}

visualization_msgs::Marker getCorrMarkerPoseToEstSensorPose(const NDTCalibScan &scan, int id, int color) {
 visualization_msgs::Marker m;
 assignDefault(m);
 assignColor(m, color);
 m.type = visualization_msgs::Marker::LINE_STRIP;
 m.action = visualization_msgs::Marker::ADD;
 m.id = id;
 m.ns = "ndt_calib_corr_pose_est_sensor_pose";
 m.scale.x = 0.05;

 geometry_msgs::Point p;
 tf::pointEigenToMsg (scan.pose.translation(), p);
 m.points.push_back(p);
 tf::pointEigenToMsg (scan.estSensorPose.translation(), p);
 m.points.push_back(p);
 return m;
}

visualization_msgs::MarkerArray getMarkerArrayFromNDTCalibScanPairs(const std::vector<NDTCalibScanPair> &pairs) {
  visualization_msgs::MarkerArray m;

  for (size_t i = 0; i < pairs.size(); i++) {
    m.markers.push_back(getMarkerPoseFromNDTCalibScan(pairs[i].first, 2*i, 0));
    m.markers.push_back(getMarkerPoseFromNDTCalibScan(pairs[i].second, 2*i+1, 1));
    m.markers.push_back(getMarkerEstSensorPoseFromNDTCalibScan(pairs[i].first, 2*i, 2));
    m.markers.push_back(getMarkerEstSensorPoseFromNDTCalibScan(pairs[i].second, 2*i+1, 2));
    m.markers.push_back(getCorrMarkerPoseToEstSensorPose(pairs[i].first, i, 0));
  }
  return m;
}

visualization_msgs::MarkerArray getMarkerArrayRelFromNDTCalibScanPair(const NDTCalibScanPair &pair, const Eigen::Affine3d &Ts) {
  visualization_msgs::MarkerArray m;

  // Draw the coordinate system of the relative frames.
  appendMarkerArray(m, getMarkerFrameAffine3d(pair.getRelativePose(),
                                               std::string("ndt_calib_rel_pose"), 1.0, 0.1));
  appendMarkerArray(m, getMarkerFrameAffine3d(pair.getRelativeEstSensorPose(),
                                              std::string("ndt_calib_rel_est_sensor_pose"), 1.0, 0.1));
  appendMarkerArray(m, getMarkerFrameAffine3d(pair.getPredictedRelativeEstSensorPose(Ts),
                                              std::string("ndt_calib_pred_rel_est_sensor_pose"), 1.0, 0.1));

  appendMarkerArray(m, getMarkerFrameAffine3d(pair.first.getSensorPoseFromPose(Ts), std::string("ndt_calib_sensor_pose_from_first_pose"), 1., 0.1));
  appendMarkerArray(m, getMarkerFrameAffine3d(pair.first.getSensorPoseFromPose(Ts), std::string("ndt_calib_sensor_pose_from_second_pose"), 1., 0.1));

  return m;
}

} // namespace ndt_visualisation


/*BISHOPS - Bishops
#math #big-numbers #ad-hoc-1

Yesterday was Sam's birthday. The most interesting gift was definitely the chessboard. Sam quickly learned the rules of chess and defeated his father, all his friends, his little sister, and now no one wants to play with him any more.

So he decided to play with another birthday gift – a Book of Math Problems for Young Mathematicians. He opened the book somewhere in the middle and read the following problem: "How many knights can be placed on a chessboard without threatening each other?" After a while he realized that this was trivial and moved on to the next problem: "How many bishops can be placed on a chessboard without threatening each other?". Sam is in trouble here. He is not able to solve this problem and needs your help.

Sam's chessboard has size N x N. A bishop can move to any distance in any of the four diagonal directions. A bishop threatens another bishop if it can move to the other bishop's position. Your task is to compute the maximum number of bishops that can be placed on a chessboard in such a way that no two bishops threaten each other.
Input

The input file consists of several lines. The line number i contains a single positive integer N representing the size of the i-th chessboard. [1 <= N <= 10^100]
Output

The output file should contain the same number of lines as the input file. The i-th line should contain one number – the maximum number of bishops that can be placed on i-th chessboard without threatening each other.
Example

Input:
2
3

Output:
2
4

*/

#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>

int main()
{
    using namespace boost::multiprecision;

    cpp_int num;

    while (std::cin >> num)
    {
        std::cout << (num <= 1 ? num : 2*num - 2) << std::endl;
    }

    return 0;
}


#ifndef BG_TYPES_HPP
#define BG_TYPES_HPP

#include <boost/geometry.hpp>

using PointGeo = boost::geometry::model::d2::point_xy<
    double, boost::geometry::cs::geographic<boost::geometry::degree>>;
using BoxGeo = boost::geometry::model::box<PointGeo>;
using LinestringGeo = boost::geometry::model::linestring<PointGeo>;
using RingGeo = boost::geometry::model::ring<PointGeo>;
using PolygonGeo = boost::geometry::model::polygon<PointGeo>;
using MultipolygonGeo = boost::geometry::model::multi_polygon<PolygonGeo>;

using Point2D = boost::geometry::model::d2::point_xy<double>;
using Box2D = boost::geometry::model::box<Point2D>;
using Linestring2D = boost::geometry::model::linestring<Point2D>;
using Ring2D = boost::geometry::model::ring<Point2D>;
using Polygon2D = boost::geometry::model::polygon<Point2D>;
using Multipolygon2D = boost::geometry::model::multi_polygon<Polygon2D>;

#endif  // BG_TYPES_HPP


//==================================================================================================
/*!
  @file
    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ERF_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ERF_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-euler
   This function object computes  the error function:
   \f$\displaystyle \frac{2}{\sqrt\pi}\int_0^{x} e^{-t^2}\mbox{d}t\f$

    @par Header <boost/simd/function/erf.hpp>

    @par Decorators

      - std_ calls @c std::erf

    @see erfc,  erfcx

    @par Example:

      @snippet erf.cpp erf

    @par Possible output:

      @snippet erf.txt erf
  **/
  IEEEValue erf(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/erf.hpp>
#include <boost/simd/function/simd/erf.hpp>

#endif


#ifndef TEST_CASE_H_
#define TEST_CASE_H_

#include <Eigen/Dense>

class Case1 {
public:
  Case1(const Eigen::Ref<const Eigen::MatrixXd> &matM,
        const Eigen::Ref<const Eigen::MatrixXd> &matD,
        const Eigen::Ref<const Eigen::MatrixXd> &matK);

private:
  const int ndim_;
  Eigen::MatrixXd matM_, matD_, matK_;
  Eigen::MatrixXd matC_, matG_;

public:
  Eigen::VectorXcd alphas;
  Eigen::VectorXd betas;
};

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Jianwei Cui <thucjw@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

template <int DataLayout>
static void test_fft_2D_golden() {
  Tensor<float, 2, DataLayout, long> input(2, 3);
  input(0, 0) = 1;
  input(0, 1) = 2;
  input(0, 2) = 3;
  input(1, 0) = 4;
  input(1, 1) = 5;
  input(1, 2) = 6;

  array<int, 2> fft;
  fft[0] = 0;
  fft[1] = 1;

  Tensor<std::complex<float>, 2, DataLayout, long> output = input.template fft<Eigen::BothParts, Eigen::FFT_FORWARD>(fft);

  std::complex<float> output_golden[6]; // in ColMajor order
  output_golden[0] = std::complex<float>(21, 0);
  output_golden[1] = std::complex<float>(-9, 0);
  output_golden[2] = std::complex<float>(-3, 1.73205);
  output_golden[3] = std::complex<float>( 0, 0);
  output_golden[4] = std::complex<float>(-3, -1.73205);
  output_golden[5] = std::complex<float>(0 ,0);

  std::complex<float> c_offset = std::complex<float>(1.0, 1.0);

  if (DataLayout == ColMajor) {
    VERIFY_IS_APPROX(output(0) + c_offset, output_golden[0] + c_offset);
    VERIFY_IS_APPROX(output(1) + c_offset, output_golden[1] + c_offset);
    VERIFY_IS_APPROX(output(2) + c_offset, output_golden[2] + c_offset);
    VERIFY_IS_APPROX(output(3) + c_offset, output_golden[3] + c_offset);
    VERIFY_IS_APPROX(output(4) + c_offset, output_golden[4] + c_offset);
    VERIFY_IS_APPROX(output(5) + c_offset, output_golden[5] + c_offset);
  }
  else {
    VERIFY_IS_APPROX(output(0)+ c_offset, output_golden[0]+ c_offset);
    VERIFY_IS_APPROX(output(1)+ c_offset, output_golden[2]+ c_offset);
    VERIFY_IS_APPROX(output(2)+ c_offset, output_golden[4]+ c_offset);
    VERIFY_IS_APPROX(output(3)+ c_offset, output_golden[1]+ c_offset);
    VERIFY_IS_APPROX(output(4)+ c_offset, output_golden[3]+ c_offset);
    VERIFY_IS_APPROX(output(5)+ c_offset, output_golden[5]+ c_offset);
  }
}

static void test_fft_complex_input_golden() {
  Tensor<std::complex<float>, 1, ColMajor, long> input(5);
  input(0) = std::complex<float>(1, 1);
  input(1) = std::complex<float>(2, 2);
  input(2) = std::complex<float>(3, 3);
  input(3) = std::complex<float>(4, 4);
  input(4) = std::complex<float>(5, 5);

  array<int, 1> fft;
  fft[0] = 0;

  Tensor<std::complex<float>, 1, ColMajor, long> forward_output_both_parts = input.fft<BothParts, FFT_FORWARD>(fft);
  Tensor<std::complex<float>, 1, ColMajor, long> reverse_output_both_parts = input.fft<BothParts, FFT_REVERSE>(fft);

  Tensor<float, 1, ColMajor, long> forward_output_real_part = input.fft<RealPart, FFT_FORWARD>(fft);
  Tensor<float, 1, ColMajor, long> reverse_output_real_part = input.fft<RealPart, FFT_REVERSE>(fft);

  Tensor<float, 1, ColMajor, long> forward_output_imag_part = input.fft<ImagPart, FFT_FORWARD>(fft);
  Tensor<float, 1, ColMajor, long> reverse_output_imag_part = input.fft<ImagPart, FFT_REVERSE>(fft);

  VERIFY_IS_EQUAL(forward_output_both_parts.dimension(0), input.dimension(0));
  VERIFY_IS_EQUAL(reverse_output_both_parts.dimension(0), input.dimension(0));

  VERIFY_IS_EQUAL(forward_output_real_part.dimension(0), input.dimension(0));
  VERIFY_IS_EQUAL(reverse_output_real_part.dimension(0), input.dimension(0));

  VERIFY_IS_EQUAL(forward_output_imag_part.dimension(0), input.dimension(0));
  VERIFY_IS_EQUAL(reverse_output_imag_part.dimension(0), input.dimension(0));

  std::complex<float> forward_golden_result[5];
  std::complex<float> reverse_golden_result[5];

  forward_golden_result[0] = std::complex<float>(15.000000000000000,+15.000000000000000);
  forward_golden_result[1] = std::complex<float>(-5.940954801177935, +0.940954801177934);
  forward_golden_result[2] = std::complex<float>(-3.312299240582266, -1.687700759417735);
  forward_golden_result[3] = std::complex<float>(-1.687700759417735, -3.312299240582266);
  forward_golden_result[4] = std::complex<float>( 0.940954801177934, -5.940954801177935);

  reverse_golden_result[0] = std::complex<float>( 3.000000000000000, + 3.000000000000000);
  reverse_golden_result[1] = std::complex<float>( 0.188190960235587, - 1.188190960235587);
  reverse_golden_result[2] = std::complex<float>(-0.337540151883547, - 0.662459848116453);
  reverse_golden_result[3] = std::complex<float>(-0.662459848116453, - 0.337540151883547);
  reverse_golden_result[4] = std::complex<float>(-1.188190960235587, + 0.188190960235587);

  for(int i = 0; i < 5; ++i) {
    VERIFY_IS_APPROX(forward_output_both_parts(i), forward_golden_result[i]);
    VERIFY_IS_APPROX(forward_output_real_part(i), forward_golden_result[i].real());
    VERIFY_IS_APPROX(forward_output_imag_part(i), forward_golden_result[i].imag());
  }

  for(int i = 0; i < 5; ++i) {
    VERIFY_IS_APPROX(reverse_output_both_parts(i), reverse_golden_result[i]);
    VERIFY_IS_APPROX(reverse_output_real_part(i), reverse_golden_result[i].real());
    VERIFY_IS_APPROX(reverse_output_imag_part(i), reverse_golden_result[i].imag());
  }
}

static void test_fft_real_input_golden() {
  Tensor<float, 1, ColMajor, long> input(5);
  input(0) = 1.0;
  input(1) = 2.0;
  input(2) = 3.0;
  input(3) = 4.0;
  input(4) = 5.0;

  array<int, 1> fft;
  fft[0] = 0;

  Tensor<std::complex<float>, 1, ColMajor, long> forward_output_both_parts = input.fft<BothParts, FFT_FORWARD>(fft);
  Tensor<std::complex<float>, 1, ColMajor, long> reverse_output_both_parts = input.fft<BothParts, FFT_REVERSE>(fft);

  Tensor<float, 1, ColMajor, long> forward_output_real_part = input.fft<RealPart, FFT_FORWARD>(fft);
  Tensor<float, 1, ColMajor, long> reverse_output_real_part = input.fft<RealPart, FFT_REVERSE>(fft);

  Tensor<float, 1, ColMajor, long> forward_output_imag_part = input.fft<ImagPart, FFT_FORWARD>(fft);
  Tensor<float, 1, ColMajor, long> reverse_output_imag_part = input.fft<ImagPart, FFT_REVERSE>(fft);

  VERIFY_IS_EQUAL(forward_output_both_parts.dimension(0), input.dimension(0));
  VERIFY_IS_EQUAL(reverse_output_both_parts.dimension(0), input.dimension(0));

  VERIFY_IS_EQUAL(forward_output_real_part.dimension(0), input.dimension(0));
  VERIFY_IS_EQUAL(reverse_output_real_part.dimension(0), input.dimension(0));

  VERIFY_IS_EQUAL(forward_output_imag_part.dimension(0), input.dimension(0));
  VERIFY_IS_EQUAL(reverse_output_imag_part.dimension(0), input.dimension(0));

  std::complex<float> forward_golden_result[5];
  std::complex<float> reverse_golden_result[5];

  forward_golden_result[0] = std::complex<float>(  15, 0);
  forward_golden_result[1] = std::complex<float>(-2.5, +3.44095480117793);
  forward_golden_result[2] = std::complex<float>(-2.5, +0.81229924058227);
  forward_golden_result[3] = std::complex<float>(-2.5, -0.81229924058227);
  forward_golden_result[4] = std::complex<float>(-2.5, -3.44095480117793);

  reverse_golden_result[0] = std::complex<float>( 3.0, 0);
  reverse_golden_result[1] = std::complex<float>(-0.5, -0.688190960235587);
  reverse_golden_result[2] = std::complex<float>(-0.5, -0.162459848116453);
  reverse_golden_result[3] = std::complex<float>(-0.5, +0.162459848116453);
  reverse_golden_result[4] = std::complex<float>(-0.5, +0.688190960235587);

  std::complex<float> c_offset(1.0, 1.0);
  float r_offset = 1.0;

  for(int i = 0; i < 5; ++i) {
    VERIFY_IS_APPROX(forward_output_both_parts(i) + c_offset, forward_golden_result[i] + c_offset);
    VERIFY_IS_APPROX(forward_output_real_part(i)  + r_offset, forward_golden_result[i].real() + r_offset);
    VERIFY_IS_APPROX(forward_output_imag_part(i)  + r_offset, forward_golden_result[i].imag() + r_offset);
  }

  for(int i = 0; i < 5; ++i) {
    VERIFY_IS_APPROX(reverse_output_both_parts(i) + c_offset, reverse_golden_result[i] + c_offset);
    VERIFY_IS_APPROX(reverse_output_real_part(i)  + r_offset, reverse_golden_result[i].real() + r_offset);
    VERIFY_IS_APPROX(reverse_output_imag_part(i)  + r_offset, reverse_golden_result[i].imag() + r_offset);
  }
}

template <int DataLayout, typename RealScalar, bool isComplexInput, int FFTResultType, int FFTDirection, int TensorRank>
static void test_fft_real_input_energy() {

  Eigen::DSizes<long, TensorRank> dimensions;
  int total_size = 1;
  for (int i = 0; i < TensorRank; ++i) {
    dimensions[i] = rand() % 20 + 1;
    total_size *= dimensions[i];
  }
  const DSizes<long, TensorRank> arr = dimensions;

  typedef typename internal::conditional<isComplexInput == true, std::complex<RealScalar>, RealScalar>::type InputScalar;

  Tensor<InputScalar, TensorRank, DataLayout, long> input;
  input.resize(arr);
  input.setRandom();

  array<int, TensorRank> fft;
  for (int i = 0; i < TensorRank; ++i) {
    fft[i] = i;
  }

  typedef typename internal::conditional<FFTResultType == Eigen::BothParts, std::complex<RealScalar>, RealScalar>::type OutputScalar;
  Tensor<OutputScalar, TensorRank, DataLayout> output;
  output = input.template fft<FFTResultType, FFTDirection>(fft);

  for (int i = 0; i < TensorRank; ++i) {
    VERIFY_IS_EQUAL(output.dimension(i), input.dimension(i));
  }

  float energy_original = 0.0;
  float energy_after_fft = 0.0;

  for (int i = 0; i < total_size; ++i) {
    energy_original += pow(std::abs(input(i)), 2);
  }

  for (int i = 0; i < total_size; ++i) {
    energy_after_fft += pow(std::abs(output(i)), 2);
  }

  if(FFTDirection == FFT_FORWARD) {
    VERIFY_IS_APPROX(energy_original, energy_after_fft / total_size);
  }
  else {
    VERIFY_IS_APPROX(energy_original, energy_after_fft * total_size);
  }
}

void test_cxx11_tensor_fft() {
    test_fft_complex_input_golden();
    test_fft_real_input_golden();

    test_fft_2D_golden<ColMajor>();
    test_fft_2D_golden<RowMajor>();

    test_fft_real_input_energy<ColMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 1>();
    test_fft_real_input_energy<ColMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 1>();
    test_fft_real_input_energy<ColMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 1>();
    test_fft_real_input_energy<ColMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 1>();

    test_fft_real_input_energy<ColMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 2>();
    test_fft_real_input_energy<ColMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 2>();
    test_fft_real_input_energy<ColMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 2>();
    test_fft_real_input_energy<ColMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 2>();

    test_fft_real_input_energy<ColMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 3>();
    test_fft_real_input_energy<ColMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 3>();
    test_fft_real_input_energy<ColMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 3>();
    test_fft_real_input_energy<ColMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 3>();

    test_fft_real_input_energy<ColMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 4>();
    test_fft_real_input_energy<ColMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 4>();
    test_fft_real_input_energy<ColMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 4>();
    test_fft_real_input_energy<ColMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 4>();

    test_fft_real_input_energy<RowMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 1>();
    test_fft_real_input_energy<RowMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 1>();
    test_fft_real_input_energy<RowMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 1>();
    test_fft_real_input_energy<RowMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 1>();

    test_fft_real_input_energy<RowMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 2>();
    test_fft_real_input_energy<RowMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 2>();
    test_fft_real_input_energy<RowMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 2>();
    test_fft_real_input_energy<RowMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 2>();

    test_fft_real_input_energy<RowMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 3>();
    test_fft_real_input_energy<RowMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 3>();
    test_fft_real_input_energy<RowMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 3>();
    test_fft_real_input_energy<RowMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 3>();

    test_fft_real_input_energy<RowMajor, float,  true,  Eigen::BothParts, FFT_FORWARD, 4>();
    test_fft_real_input_energy<RowMajor, double, true,  Eigen::BothParts, FFT_FORWARD, 4>();
    test_fft_real_input_energy<RowMajor, float,  false,  Eigen::BothParts, FFT_FORWARD, 4>();
    test_fft_real_input_energy<RowMajor, double, false,  Eigen::BothParts, FFT_FORWARD, 4>();
}


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

#include "required_defines.hpp"

#include "performance_measure.hpp"

#include <boost/math/special_functions/beta.hpp>
#include <boost/array.hpp>

#define T double
#  include "../test/beta_small_data.ipp"
#  include "../test/beta_med_data.ipp"
#  include "../test/beta_exp_data.ipp"

template <std::size_t N>
double beta_evaluate2(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += boost::math::beta(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(beta_test, "beta")
{
   double result = beta_evaluate2(beta_small_data);
   result += beta_evaluate2(beta_med_data);
   result += beta_evaluate2(beta_exp_data);

   consume_result(result);
   set_call_count(
      (sizeof(beta_small_data)
      + sizeof(beta_med_data)
      + sizeof(beta_exp_data)) / sizeof(beta_exp_data[0]));
}

#ifdef TEST_DCDFLIB
#include <dcdflib.h>

template <std::size_t N>
double beta_evaluate2_dcd(const boost::array<boost::array<T, 3>, N>& data)
{
   double result = 0;
   for(unsigned i = 0; i < N; ++i)
      result += ::beta(data[i][0], data[i][1]);
   return result;
}

BOOST_MATH_PERFORMANCE_TEST(beta_test_dcd, "beta-dcd")
{
   double result = beta_evaluate2_dcd(beta_small_data);
   result += beta_evaluate2_dcd(beta_med_data);
   result += beta_evaluate2_dcd(beta_exp_data);

   consume_result(result);
   set_call_count(
      (sizeof(beta_small_data)
      + sizeof(beta_med_data)
      + sizeof(beta_exp_data)) / sizeof(beta_exp_data[0]));
}

#endif