// Copyright (C) 2017 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (sweeney.chris.m@gmail.com)

#include "theia/sfm/visibility_pyramid.h"

#include <Eigen/Core>
#include <glog/logging.h>
#include <vector>

#include "theia/math/util.h"

namespace theia {

// The inputs are the view/image width and height, as well as the number of
// desired levels in the image pyramid.
VisibilityPyramid::VisibilityPyramid(const int width,
                                     const int height,
                                     const int num_pyramid_levels)
    : width_(width),
      height_(height),
      num_pyramid_levels_(num_pyramid_levels),
      max_cells_in_dimension_(1 << num_pyramid_levels) {
  CHECK_GT(width_, 0);
  CHECK_GT(height_, 0);
  CHECK_GT(num_pyramid_levels_, 0);

  pyramid_.resize(num_pyramid_levels);
  // Create the occupancy pyramid such that the coarsest level is a 2x2 grid.
  for (int i = 0; i < pyramid_.size(); i++) {
    const int num_grid_cells_per_dimension = 1 << (1 + i);
    pyramid_[i].setZero(num_grid_cells_per_dimension,
                        num_grid_cells_per_dimension);
  }
}

// Add a point to the visibility pyramid.
void VisibilityPyramid::AddPoint(const Eigen::Vector2d& point) {
  // Determine the grid cell of the point in the highest-resolution level of the
  // pyramid.
  int grid_cell_x = theia::Clamp(
      static_cast<int>(max_cells_in_dimension_ * point.x() / width_),
      0,
      max_cells_in_dimension_ - 1);
  int grid_cell_y = theia::Clamp(
      static_cast<int>(max_cells_in_dimension_ * point.y() / height_),
      0,
      max_cells_in_dimension_ - 1);

  // Go through the pyramid from fine to coarse and add the observation to the
  // occupancy grid.
  for (int i = pyramid_.size() - 1; i >= 0; --i) {
    pyramid_[i](grid_cell_x, grid_cell_y) += 1;

    // The next coarsest level of the pyramid will have half the number of grid
    // cells so we can use a simple bitshift to get the next pyramid level's
    // grid cells.
    grid_cell_x = grid_cell_x >> 1;
    grid_cell_y = grid_cell_y >> 1;
  }
}

// The score is accumulated by counting the number of grid cells in each level
// of the pyramid. The score for each level is weighted by the number of grid
// cells in that level of the pyramid. This scheme favors good spatial
// distribution at high resolutions.
int VisibilityPyramid::ComputeScore() const {
  int score = 0;
  for (int i = 0; i < pyramid_.size(); i++) {
    score += pyramid_[i].count() * pyramid_[i].size();
  }
  return score;
}

}  // namespace theia


/*
 // Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
 // component of the Texas A&M University System.

 // All rights reserved.

 // The information and source code contained herein is the exclusive
 // property of TEES and may not be disclosed, examined or reproduced
 // in whole or in part without explicit written authorization from TEES.
*/

#include <stapl/runtime.hpp>
#include <stapl/utility/tuple.hpp>
#include <stapl/views/mapping_functions/linearization.hpp>
#include <stapl/containers/type_traits/default_traversal.hpp>

#include <test/algorithms/test_utils.h>
#include "../../test_report.hpp"

#include <algorithm>
#include <sstream>
#include <boost/iterator/counting_iterator.hpp>

#include <boost/fusion/include/copy.hpp>
#include <boost/fusion/include/transform.hpp>

#include <boost/preprocessor/seq/for_each.hpp>

using namespace stapl;

template<int N, typename T, typename Traversal>
struct linear_equality
{
  T m_size;
  T m_index;

  linear_equality(T const& size, T const& index)
    : m_size(size), m_index(index)
  { }

  bool operator()(bool b, size_t const& x)
  {
    get<N>(m_index) = x;

    return b && std::accumulate(
                  boost::make_counting_iterator<size_t>(0),
                  boost::make_counting_iterator<size_t>(get<N>(m_size)), b,
                  linear_equality<N-1, T, Traversal>(m_size, m_index));
  }
};

template<typename T, typename Traversal>
struct linear_equality<0, T, Traversal>
{
  T m_size;
  T m_index;

  linear_equality(T const& size, T const& index)
    : m_size(size), m_index(index)
  { }

  bool operator()(bool b, size_t const& x)
  {
    get<0>(m_index) = x;
    nd_linearize<T, Traversal> lin(m_size);
    nd_reverse_linearize<T, Traversal> r_lin(m_size);

    return b && r_lin(lin(m_index)) == m_index;
  }
};

struct random_size
{
  typedef size_t result_type;

  size_t operator()(size_t const& x) const
  {
    return rand() % x;
  }
};

// FIXME - use general identity...
//
template<typename T>
class my_identity
{
private:
  T const& m_t;

public:
  typedef T result_type;

  my_identity(T const& t)
   : m_t(t)
  { }

  template<typename Q>
  T operator()(Q const&) const
  {
    return m_t;
  }
};

#define TEST_LINEARIZATION_EQUALITY(r, n, dim)                                 \
{                                                                              \
  typedef default_traversal<dim>::type              traversal_type;            \
  typedef indexed_domain<size_t, dim, traversal_type>::size_type index_type;   \
  typedef result_of::reverse<traversal_type>::type  alternate_traversal_type;  \
  typedef index_type                                size_type;                 \
                                                                               \
  typedef stapl::result_of::transform<                                         \
            size_type,                                                         \
            my_identity<size_t>                                                \
          >::type transf_t;                                                    \
  transf_t size = tuple_ops::transform(size_type(), my_identity<size_t>(n));   \
                                                                               \
  boost::counting_iterator<size_t> b =                                         \
    boost::make_counting_iterator<size_t>(0);                                  \
  boost::counting_iterator<size_t> e =                                         \
    boost::make_counting_iterator<size_t>(n);                                  \
                                                                               \
  linear_equality<dim-1, transf_t, traversal_type> dole(size, index_type());   \
  bool passed = std::accumulate(b, e, true, dole);                             \
                                                                               \
  stapl_bool res(passed);                                                      \
  bool p = res.reduce();                                                       \
  std::stringstream str;                                                       \
  str << "Testing " << dim                                                     \
      <<  "d linearize (default traversal & homogeneous size)";                \
  STAPL_TEST_REPORT(p, str.str());                                             \
                                                                               \
  linear_equality<dim-1,                                                       \
                  transf_t,                                                    \
                  alternate_traversal_type> aole(size, index_type());          \
  passed = std::accumulate(b, e, true, aole);                                  \
                                                                               \
  res = stapl_bool(passed);                                                    \
  p = res.reduce();                                                            \
  str.str(std::string());                                                      \
  str << "Testing " << dim                                                     \
      <<  "d linearize (alt. traversal & homogeneous size)";                   \
  STAPL_TEST_REPORT(p, str.str());                                             \
/*                                                                             \
  sizev = boost::fusion::transform(sizev, random_size());                      \
  size_type hetero_size;                                                       \
  boost::fusion::copy(sizev, hetero_size);                                     \
  std::cout << sizev << std::endl;                                             \
                                                                               \
  linear_equality<dim-1, size_type, alternate_traversal_type>                  \
    aele(hetero_size, index_type());                                           \
  passed = std::accumulate(b, e, true, aele);                                  \
                                                                               \
  res = stapl_bool(passed);                                                    \
  p = res.reduce();                                                            \
  str.str(std::string());                                                      \
  str << "Testing " << dim                                                     \
      <<  "d linearize (default traversal & heterogeneous size)";              \
  STAPL_TEST_REPORT(p, str.str());                                          \*/\
}

stapl::exit_code stapl_main(int argc, char* argv[])
{
  if (argc < 2) {
    std::cout << "usage: " << argv[0] << " n" << std::endl;
    exit(1);
  }

  const size_t n = atoi(argv[1]);

  #define DIMENSIONS (2)(3)(4)(5)
  BOOST_PP_SEQ_FOR_EACH(TEST_LINEARIZATION_EQUALITY, n, DIMENSIONS)
  #undef DIMENSIONS

  return EXIT_SUCCESS;
}


#include <NTL/GF2.h>

#include <NTL/new.h>

NTL_START_IMPL

GF2 power(GF2 a, long e)
{
   if (e == 0) {
      return to_GF2(1);
   }

   if (e < 0 && IsZero(a))
      Error("GF2: division by zero");

   return a;
}

ostream& operator<<(ostream& s, GF2 a)
{
   if (a == 0)
      s << "0";
   else
      s << "1";

   return s;
}

istream& operator>>(istream& s, ref_GF2 x)
{
   static ZZ a;

   s >> a;
   conv(x, a);
   return s;
}

NTL_END_IMPL


#include <gtest/gtest.h>

#include <boost/shared_ptr.hpp>

#include <NFmiAreaFactory.h>
#include <NFmiArea.h>

#include "areaUtil.h"

TEST(areaUtilTests, IsDataInMapFindsAreaInside) {
	auto scand = AreaUtil::GetScandicArea();
	boost::shared_ptr<NFmiArea> zoomedDataArea = NFmiAreaFactory::Create("stereographic, 20, 90, 60:7, 53, 17, 60");

	ASSERT_TRUE(AreaUtil::IsDataInMap(zoomedDataArea.get(), scand.get()));

}

TEST(areaUtilTests, IsDataInMapTellsDataIsNotInArea) {
	boost::shared_ptr<NFmiArea> zoomedArea = NFmiAreaFactory::Create("stereographic, 20, 90, 60:7, 53, 17, 60");
	boost::shared_ptr<NFmiArea> offDataArea = NFmiAreaFactory::Create("stereographic, -40, 10, 60:7, 23, 5, 60");

	ASSERT_FALSE(AreaUtil::IsDataInMap(offDataArea.get(), zoomedArea.get()));

}

TEST(areaUtilTests, ScandicExtentIdentity) {
	NFmiRect r{ 0,1,1,0 };

	auto ext = AreaUtil::RectIntoExtentScandic(
		AreaUtil::areaToWorldScandic(	r	)	);
	//TODO move to fixture
	ASSERT_EQ(0,ext[0]);
	ASSERT_EQ(89,ext[1]);
	ASSERT_EQ(0,ext[2]);
	ASSERT_EQ(99,ext[3]);
}


// 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.
#pragma once
#include <pybind11/numpy.h>

#include <Eigen/Core>

#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/geometry/box.hpp"
#include "pyinterp/detail/math.hpp"
#include "pyinterp/detail/thread.hpp"
#include "pyinterp/geodetic/algorithm.hpp"
#include "pyinterp/geodetic/point.hpp"
#include "pyinterp/geodetic/polygon.hpp"
#include "pyinterp/geodetic/system.hpp"

namespace pyinterp::geodetic {

// Defines a box made of two describing points.
class Box : public boost::geometry::model::box<Point> {
 public:
  /// @brief Default constructor
  Box() : boost::geometry::model::box<Point>() {}

  /// @brief Constructor taking the minimum corner point and the maximum corner
  /// point.
  /// @param min_corner the minimum corner point
  /// @param max_corner the maximum corner point
  Box(const Point& min_corner, const Point& max_corner)
      : boost::geometry::model::box<Point>(min_corner, max_corner) {}

  /// @brief Returns the box covering the whole earth.
  [[nodiscard]] static auto whole_earth() -> Box {
    return {{-180, -90}, {180, 90}};
  }

  // Returns the box, or the two boxes on either side of the dateline if the
  // defined box wraps around the globe (i.e. the longitude of the min corner
  // is greater than the longitude of the max corner.)
  [[nodiscard]] auto split() const -> std::list<Box> {
    // box wraps around the globe ?
    if (this->min_corner().lon() > this->max_corner().lon()) {
      return {Box({this->min_corner().lon(), this->min_corner().lat()},
                  {180, this->max_corner().lat()}),
              Box({-180, this->min_corner().lat()},
                  {this->max_corner().lon(), this->max_corner().lat()})};
    }
    return {*this};
  }

  /// @brief Returns the center of the box.
  [[nodiscard]] inline auto centroid() const -> Point {
    return boost::geometry::return_centroid<Point, Box>(*this);
  }

  /// @brief Returns the delta of the box in latitude and longitude.
  [[nodiscard]] inline auto delta(bool round) const
      -> std::tuple<double, double> {
    auto x = this->max_corner().lon() - this->min_corner().lon();
    auto y = this->max_corner().lat() - this->min_corner().lat();
    if (round) {
      x = Box::max_decimal_power(x);
      y = Box::max_decimal_power(y);
    }
    return std::make_tuple(x, y);
  }

  /// @brief Returns a point inside the box, making an effort to round to
  /// minimal precision.
  [[nodiscard]] inline auto round() const -> Point {
    const auto xy = delta(true);
    const auto x = std::get<0>(xy);
    const auto y = std::get<1>(xy);
    return {std::ceil(this->min_corner().lon() / x) * x,
            std::ceil(this->min_corner().lat() / y) * y};
  }

  /// @brief Test if the given point is inside or on border of this instance
  ///
  /// @param pt Point to test
  //  @return True if the given point is inside or on border of this Box
  [[nodiscard]] auto covered_by(const Point& point) const -> bool {
    return boost::geometry::covered_by(point, *this);
  }

  /// @brief Test if the coordinates of the points provided are located inside
  /// or at the edge of this box.
  ///
  /// @param lon Longitudes coordinates in degrees to check
  /// @param lat Latitude coordinates in degrees to check
  /// @return Returns a vector containing a flag equal to 1 if the coordinate is
  /// located in the box or at the edge otherwise 0.
  [[nodiscard]] auto covered_by(const Eigen::Ref<const Eigen::VectorXd>& lon,
                                const Eigen::Ref<const Eigen::VectorXd>& lat,
                                const size_t num_threads) const
      -> pybind11::array_t<int8_t> {
    return geodetic::covered_by<Point, Box>(*this, lon, lat, num_threads);
  }

  /// Converts a Box into a string with the same meaning as that of this
  /// instance.
  [[nodiscard]] auto to_string() const -> std::string {
    std::stringstream ss;
    ss << boost::geometry::dsv(*this);
    return ss.str();
  }

  /// Calculate the area
  [[nodiscard]] auto area(const std::optional<System>& wgs) const -> double {
    return static_cast<Polygon>(*this).area(wgs);
  }

  /// Calculate the distance between two boxes
  [[nodiscard]] auto distance(const Box& other) const -> double {
    return static_cast<Polygon>(*this).distance(static_cast<Polygon>(other));
  }

  /// Calculate the distance between this instance and a point
  [[nodiscard]] auto distance(const Point& other) const -> double {
    return static_cast<Polygon>(*this).distance(other);
  }

  /// Get a tuple that fully encodes the state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    return pybind11::make_tuple(this->min_corner().getstate(),
                                this->max_corner().getstate());
  }

  /// Create a new instance from a registered state of an instance of this
  /// object.
  static auto setstate(const pybind11::tuple& state) -> Box {
    if (state.size() != 2) {
      throw std::runtime_error("invalid state");
    }
    return Box(Point::setstate(state[0].cast<pybind11::tuple>()),
               Point::setstate(state[1].cast<pybind11::tuple>()));
  }

  /// Converts this instance into a polygon
  explicit operator Polygon() const {
    Polygon result;
    boost::geometry::convert(*this, result);
    return result;
  }

 private:
  // Returns the maximum power of 10 from a number (x > 0)
  static auto max_decimal_power(const double x) -> double {
    auto m = static_cast<int32_t>(std::floor(std::log10(x)));
    return detail::math::power10(m);
  }
};

}  // namespace pyinterp::geodetic

// BOOST specialization to accept pyinterp::geodectic::Box as a geometry
// entity
namespace boost::geometry::traits {

namespace pg = pyinterp::geodetic;

/// Box tag
template <>
struct tag<pg::Box> {
  using type = box_tag;
};

/// Type of a point
template <>
struct point_type<pg::Box> {
  using type = pg::Point;
};

template <std::size_t Dimension>
struct indexed_access<pg::Box, min_corner, Dimension> {
  /// get corner of box
  static inline auto get(pg::Box const& box) -> double {
    return geometry::get<Dimension>(box.min_corner());
  }

  /// set corner of box
  static inline void set(pg::Box& box,  // NOLINT
                         double const& value) {
    geometry::set<Dimension>(box.min_corner(), value);
  }
};

template <std::size_t Dimension>
struct indexed_access<pg::Box, max_corner, Dimension> {
  /// get corner of box
  static inline auto get(pg::Box const& box) -> double {
    return geometry::get<Dimension>(box.max_corner());
  }

  /// set corner of box
  static inline void set(pg::Box& box,  // NOLINT
                         double const& value) {
    geometry::set<Dimension>(box.max_corner(), value);
  }
};

}  // namespace boost::geometry::traits


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/function/interleave_first.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

STF_CASE_TPL( "unary interleave_first based shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 4
 {
    bs::pack<T,4> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>() };
    bs::pack<T,4> b{ T(13), bs::Valmin<T>(), T(37), bs::Valmax<T>() };
    bs::pack<T,4> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 0, 0, 1, 1>(a))  , (bs::interleave_first(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 0, 0, 1, 1>(a,b)), (bs::interleave_first(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 0,-1, 1>(a))  , (bs::interleave_first(z,a)) );

    STF_ALL_EQUAL( (bs::shuffle< 4, 4, 5, 5>(a,b)), (bs::interleave_first(b,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 4,-1, 5>(a,b)), (bs::interleave_first(z,b)) );

  }

  // cardinal 8
  {
    bs::pack<T,8> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>(), T(37), bs::Valmax<T>(), T(55), T(99) };
    bs::pack<T,8> b{ T(13), bs::Valmax<T>(), T(42), bs::Valmax<T>(), T(76), bs::Valmin<T>(), T(42), T(69) };
    bs::pack<T,8> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 0,0, 1,1, 2,2, 3,3>(a))  , (bs::interleave_first(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 0,0, 1,1, 2,2, 3,3>(a,b)), (bs::interleave_first(a,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1,0,-1,1,-1,2,-1,3>(a))  , (bs::interleave_first(z,a)) );

    STF_ALL_EQUAL( (bs::shuffle< 8,8, 9,9,10,10,11,11>(a,b)), (bs::interleave_first(b,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1,8,-1,9,-1,10,-1,11>(a,b)), (bs::interleave_first(z,b)) );
  }
}

STF_CASE_TPL( "binary interleave_first based shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 4
 {
    bs::pack<T,4> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>() };
    bs::pack<T,4> b{ T(13), bs::Valmin<T>(), T(37), bs::Valmax<T>() };
    bs::pack<T,4> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 0, 4, 1, 5>(a,b)), (bs::interleave_first(a,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 4,-1, 5>(a,b)), (bs::interleave_first(z,b)) );
    STF_ALL_EQUAL( (bs::shuffle< 0,-1, 1,-1>(a,b)), (bs::interleave_first(a,z)) );

    STF_ALL_EQUAL( (bs::shuffle< 4, 0, 5, 1>(a,b)), (bs::interleave_first(b,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 0,-1, 1>(a,b)), (bs::interleave_first(z,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 4,-1, 5,-1>(a,b)), (bs::interleave_first(b,z)) );
  }

  // cardinal 8
  {
    bs::pack<T,8> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>(), T(37), bs::Valmax<T>(), T(55), T(99) };
    bs::pack<T,8> b{ T(13), bs::Valmax<T>(), T(42), bs::Valmax<T>(), T(76), bs::Valmin<T>(), T(42), T(69) };
    bs::pack<T,8> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 0, 8, 1, 9, 2,10, 3,11>(a,b)), (bs::interleave_first(a,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 8,-1, 9,-1,10,-1,11>(a,b)), (bs::interleave_first(z,b)) );
    STF_ALL_EQUAL( (bs::shuffle< 0,-1, 1,-1, 2,-1, 3,-1>(a,b)), (bs::interleave_first(a,z)) );

    STF_ALL_EQUAL( (bs::shuffle< 8, 0, 9, 1,10, 2,11, 3>(a,b)), (bs::interleave_first(b,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 0,-1, 1,-1, 2,-1, 3>(a,b)), (bs::interleave_first(z,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 8,-1, 9,-1,10,-1,11,-1>(a,b)), (bs::interleave_first(b,z)) );
  }
}


//
// MIT License
//
// Copyright (c) Deif Lou
//
// 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 <opencv2/imgproc.hpp>
#include <itkImage.h>
#include <itkImportImageFilter.h>
#include <itkN4BiasFieldCorrectionImageFilter.h>
#include <itkBSplineControlPointImageFilter.h>
#include <Eigen/Dense>

#include "filter.h"
#include "filterwidget.h"
#include <imgproc/lut.h>
#include <imgproc/types.h>
#include <imgproc/colorconversion.h>
#include <misc/util.h>

#define MAX_IMAGE_SIZE 128

Filter::Filter() :
    mGridSize(3),
    mOutputMode(CorrectedImageMode1)
{
}

Filter::~Filter()
{
}

ImageFilter *Filter::clone()
{
    Filter * f = new Filter();
    f->mGridSize = mGridSize;
    f->mOutputMode = mOutputMode;
    return f;
}

extern "C" QHash<QString, QString> getIBPPluginInfo();
QHash<QString, QString> Filter::info()
{
    return getIBPPluginInfo();
}

QImage Filter::process(const QImage &inputImage)
{
    if (inputImage.isNull() || inputImage.format() != QImage::Format_ARGB32)
        return inputImage;

    register int x, y, w = inputImage.width(), h = inputImage.height(), mean = 0, sw, sh;
    register HSL * bitsHSL = (HSL *)malloc(w * h * sizeof(HSL)), * bitsHSLsl;
    cv::Mat mlchannel(h, w, CV_8UC1);
    register unsigned char * mbits8;

    // Convert to HSL
    convertBGRToHSL(inputImage.bits(), (unsigned char *)bitsHSL, w * h);

    // Separate L channel
    for (y = 0; y < h; y++)
    {
        bitsHSLsl = bitsHSL + y * w;
        mbits8 = mlchannel.ptr(y);
        for (x = 0; x < w; x++)
        {
            *mbits8 = bitsHSLsl->l;
            bitsHSLsl++;
            mbits8++;
        }
    }

    // Resample
    cv::Mat mInitial;
    if (w != MAX_IMAGE_SIZE || h != MAX_IMAGE_SIZE)
    {
        if (w > h)
        {
            sw = MAX_IMAGE_SIZE;
            sh = h * MAX_IMAGE_SIZE / w;
        }
        else
        {
            sh = MAX_IMAGE_SIZE;
            sw = w * MAX_IMAGE_SIZE / h;
        }
        sw = sw < 1 ? 1 : sw;
        sh = sh < 1 ? 1 : sh;

        cv::Mat mresized(sh, sw, CV_8UC1);
        cv::resize(mlchannel, mresized, cv::Size(sw, sh), 0, 0, cv::INTER_CUBIC);
        mInitial = mresized;
    }
    else
    {
        sw = w;
        sh = h;
        mInitial = mlchannel;
    }

    // -----------------------
    // ITK N4 Bias Correction
    // -----------------------

    // typedef declaration
    typedef itk::Image<float, 2> ImageType;
    typedef ImageType::Pointer ImagePointer;
    typedef itk::Image<unsigned char, 2> MaskImageType;
    typedef itk::N4BiasFieldCorrectionImageFilter<ImageType, MaskImageType, ImageType> CorrecterType;
    typedef itk::BSplineControlPointImageFilter <CorrecterType::BiasFieldControlPointLatticeType,
                                                 CorrecterType::ScalarImageType> BSplinerType;

    // initial image
    ImageType::SizeType initialImageSize;
    initialImageSize[0] = mInitial.cols;
    initialImageSize[1] = mInitial.rows;
    ImageType::IndexType initialImageStart;
    initialImageStart.Fill(0);
    ImageType::RegionType initialImageRegion;
    initialImageRegion.SetIndex(initialImageStart);
    initialImageRegion.SetSize(initialImageSize);

    ImagePointer initialImage = ImageType::New();
    initialImage->SetRegions(initialImageRegion);
    initialImage->Allocate();

    float * initialImagePtr = initialImage->GetBufferPointer();
    for (y = 0; y < sh; y++)
    {
        mbits8 = mInitial.ptr(y);
        for (x = 0; x < sw; x++)
        {
            *initialImagePtr = ((*mbits8) + 1) / 255.;
            mbits8++;
            initialImagePtr++;
        }
    }

    // correcter
    CorrecterType::Pointer correcter = CorrecterType::New();
    CorrecterType::ArrayType numberOfControlPoints;
    numberOfControlPoints.Fill(mGridSize + 1);
    correcter->SetNumberOfControlPoints(numberOfControlPoints);
    correcter->SetSplineOrder(IBP_minimum(mGridSize, 3));
    correcter->SetInput(initialImage);

    try
    {
        correcter->Update();
    }
    catch (itk::ExceptionObject &excep)
    {
        Q_UNUSED(excep)
        return inputImage;
    }

    // spline
    BSplinerType::Pointer bspliner = BSplinerType::New();
    bspliner->SetInput(correcter->GetLogBiasFieldControlPointLattice());
    bspliner->SetSplineOrder(correcter->GetSplineOrder());
    bspliner->SetSize(initialImage->GetLargestPossibleRegion().GetSize());
    bspliner->SetOrigin(initialImage->GetOrigin());
    bspliner->SetDirection(initialImage->GetDirection());
    bspliner->SetSpacing(initialImage->GetSpacing());
    bspliner->Update();

    itk::ImageRegionIterator<CorrecterType::ScalarImageType> ItB(bspliner->GetOutput(),
                                                                 bspliner->GetOutput()->GetLargestPossibleRegion());
    itk::ImageRegionIterator<ImageType> ItF(initialImage, initialImage->GetLargestPossibleRegion());
    for (ItB.GoToBegin(), ItF.GoToBegin(); !ItB.IsAtEnd(); ++ItB, ++ItF)
        ItF.Set(exp(ItB.Get()[0]));

    // ------------------------------
    // End of ITK N4 Bias Correction
    // ------------------------------

    // Set up the matrix A and the vector b for least squares fitting with the initial image
    register int row = 0, totalPixels = sw * sh;
    Eigen::MatrixXf ls_A = Eigen::MatrixXf(totalPixels, 2);
    Eigen::VectorXf ls_b = Eigen::VectorXf(totalPixels);

    initialImagePtr = initialImage->GetBufferPointer();
    for (y = 0; y < sh; y++)
    {
        mbits8 = mInitial.ptr(y);
        for (x = 0; x < sw; x++)
        {
            ls_A(row, 0) = 1;
            ls_A(row, 1) = *initialImagePtr;
            ls_b(row) = *mbits8;

            initialImagePtr++;
            mbits8++;
            row++;
        }
    }
    // Solve...
    Eigen::VectorXf ls_x = (ls_A).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ls_b);

    initialImagePtr = initialImage->GetBufferPointer();
    for (y = 0; y < sh; y++)
    {
        for (x = 0; x < sw; x++)
        {
            *initialImagePtr = ls_x(0) + (*initialImagePtr) * ls_x(1);
            initialImagePtr++;
        }
    }

    // Remap due to out of range values and scale
    double minIIH = 10000, maxIIH = -10000, minIIHc, maxIIHc, range1, range2;

    initialImagePtr = initialImage->GetBufferPointer();
    for (y = 0; y < sh; y++)
    {
        for (x = 0; x < sw; x++)
        {
            if ((*initialImagePtr) > maxIIH)
                maxIIH = (*initialImagePtr);
            if ((*initialImagePtr) < minIIH)
                minIIH = (*initialImagePtr);
            initialImagePtr++;
        }
    }
    minIIHc = IBP_clamp(0., minIIH, 255.);
    maxIIHc = IBP_clamp(0., maxIIH, 255.);
    range1 = maxIIH - minIIH;
    range2 = maxIIHc - minIIHc;
    initialImagePtr = initialImage->GetBufferPointer();
    for (y = 0; y < sh; y++)
    {
        mbits8 = mInitial.ptr(y);
        for (x = 0; x < sw; x++)
        {
            *mbits8 = round((minIIHc + ((*initialImagePtr) - minIIH) * range2 / range1));
            mean += *mbits8;
            mbits8++;
            initialImagePtr++;
        }
    }
    mean /= totalPixels;

    // Resample
    if (w != sw || h != sh)
        cv::resize(mInitial, mlchannel, cv::Size(w, h), 0, 0, cv::INTER_CUBIC);
    else
        mlchannel = mInitial;

    // Make output image
    if (mOutputMode == CorrectedImageMode1)
    {
        // Divide lightness channel
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->l = IBP_clamp(0, lut02[bitsHSLsl->l][IBP_clamp(1, *mbits8, 255)] * mean / 255, 255);
                bitsHSLsl++;
                mbits8++;
            }
        }
    }
    else if (mOutputMode == CorrectedImageMode2)
    {
        // Divide lightness channel
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->l = IBP_clamp(0, lut02[bitsHSLsl->l][IBP_clamp(1, *mbits8, 255)], 255);
                bitsHSLsl++;
                mbits8++;
            }
        }
    }
    else
    {
        for (y = 0; y < h; y++)
        {
            bitsHSLsl = bitsHSL + y * w;
            mbits8 = mlchannel.ptr(y);
            for (x = 0; x < w; x++)
            {
                bitsHSLsl->h = bitsHSLsl->s = 0;
                bitsHSLsl->l = *mbits8;
                bitsHSLsl++;
                mbits8++;
            }
        }
    }

    // Convert to RGB
    QImage finalImage = inputImage.copy();
    convertHSLToBGR((const unsigned char *)bitsHSL, finalImage.bits(), w * h);
    free(bitsHSL);

    return finalImage;
}

bool Filter::loadParameters(QSettings &s)
{
    int gridSize;
    QString outputModeStr;
    OutputMode outputMode;
    bool ok;

    gridSize = s.value("gridsize", 3).toUInt(&ok);
    if (!ok || gridSize > 10 || gridSize < 1)
        return false;

    outputModeStr = s.value("outputmode", "correctedimagemode1").toString();
    if (outputModeStr == "correctedimagemode1")
        outputMode = CorrectedImageMode1;
    else if (outputModeStr == "correctedimagemode2")
        outputMode = CorrectedImageMode2;
    else if (outputModeStr == "iihcorrectionmodel")
        outputMode = IIHCorrectionModel;
    else
        return false;

    setOutputMode(outputMode);
    setGridSize(gridSize);

    return true;
}

bool Filter::saveParameters(QSettings &s)
{
    s.setValue("gridsize", mGridSize);
    s.setValue("outputmode", mOutputMode == CorrectedImageMode1 ? "correctedimagemode1" :
                             mOutputMode == CorrectedImageMode2 ? "correctedimagemode2" : "iihcorrectionmodel");
    return true;
}

QWidget *Filter::widget(QWidget *parent)
{
    FilterWidget * fw = new FilterWidget(parent);
    fw->setGridSize(mGridSize);
    fw->setOutputMode(mOutputMode);
    connect(this, SIGNAL(gridSizeChanged(int)), fw, SLOT(setGridSize(int)));
    connect(this, SIGNAL(outputModeChanged(Filter::OutputMode)), fw, SLOT(setOutputMode(Filter::OutputMode)));
    connect(fw, SIGNAL(gridSizeChanged(int)), this, SLOT(setGridSize(int)));
    connect(fw, SIGNAL(outputModeChanged(Filter::OutputMode)), this, SLOT(setOutputMode(Filter::OutputMode)));
    return fw;
}

void Filter::setGridSize(int gs)
{
    if (gs == mGridSize)
        return;
    mGridSize = gs;
    emit gridSizeChanged(gs);
    emit parametersChanged();
}

void Filter::setOutputMode(Filter::OutputMode om)
{
    if (om == mOutputMode)
        return;
    mOutputMode = om;
    emit outputModeChanged(om);
    emit parametersChanged();
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/sum.cpp
 *
 * \brief Test suite for the \c sum operation.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublasx/operation/sum.hpp>
#include <boost/numeric/ublasx/tags.hpp>
#include <functional>
#include <iostream>
#include "libs/numeric/ublasx/test/utils.hpp"

static const double tol = 1.0e-5;

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( test_vector_container )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Vector Container" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublas::zero_vector<value_type> zero_vector_type;
    typedef ublas::vector_traits<vector_type>::size_type size_type;

    size_type n(5);

    vector_type v(n);

    v(0) = 0.0;
    v(1) = 0.108929;
    v(2) = 0.0;
    v(3) = 0.0;
    v(4) = 1.023787;

    value_type sum_all = v(0)+v(1)+v(2)+v(4);

    zero_vector_type z(n);

    value_type expect(0);
    value_type res(0);
    ublas::vector<value_type> vexpect(0);
    ublas::vector<value_type> vres(0);

    // sum(z)
    expect = value_type(0);
    res = ublasx::sum(z);
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << z << ") = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK( res == expect );

    // sum(v)
    expect = sum_all;
    res = ublasx::sum(v);
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << v << ") = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

    // sum<1>(v)
    vexpect = ublas::vector<value_type>(1, sum_all);
    vres = ublasx::sum<1>(v);
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << v << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( test_vector_expression )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Vector Expression" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublas::vector_traits<vector_type>::size_type size_type;

    size_type n(5);

    vector_type v(n);

    v(0) = 0.0;
    v(1) = 0.108929;
    v(2) = 0.0;
    v(3) = 0.0;
    v(4) = 1.023787;

    value_type sum_all = v(0)+v(1)+v(2)+v(4);

    value_type expect(0);
    value_type res(0);
    ublas::vector<value_type> vexpect(0);
    ublas::vector<value_type> vres(0);

    // sum(-v)
    expect = -sum_all;
    res = ublasx::sum(-v);
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << -v << ") = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

    // sum<1>(-v)
    vexpect = ublas::vector<value_type>(1, -sum_all);
    vres = ublasx::sum<1>(-v);
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << -v << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( test_vector_reference )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Vector Reference" );

    typedef double value_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublas::vector_reference<vector_type> vector_reference_type;
    typedef ublas::vector_traits<vector_type>::size_type size_type;

    size_type n(5);

    vector_type v(n);

    v(0) = 0.0;
    v(1) = 0.108929;
    v(2) = 0.0;
    v(3) = 0.0;
    v(4) = 1.023787;

    value_type sum_all = v(0)+v(1)+v(2)+v(4);

    value_type expect(0);
    value_type res(0);
    ublas::vector<value_type> vexpect(0);
    ublas::vector<value_type> vres(0);

    // sum(ref(v))
    expect = sum_all;
    res = ublasx::sum(vector_reference_type(v));
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << vector_reference_type(v) << ") = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

    // sum<1>(ref(v))
    vexpect = ublas::vector<value_type>(1, sum_all);
    vres = ublasx::sum<1>(vector_reference_type(v));
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << vector_reference_type(v) << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, 1, tol );
}

BOOST_UBLASX_TEST_DEF( test_row_major_matrix_container )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Row-major Matrix Container" );

    typedef double value_type;
    typedef ublas::matrix<value_type, ublas::row_major> matrix_type;
    typedef ublas::zero_matrix<value_type> zero_matrix_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublas::matrix_traits<matrix_type>::size_type size_type;

    size_type nr = 5;
    size_type nc = 4;

    matrix_type A(nr, nc);

    A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
    A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
    A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

    zero_matrix_type Z(nr, nc);

    value_type sum_all =   A(0,0)+A(0,1)+A(0,2)+A(0,3)
                         + A(1,0)+A(1,1)+A(1,2)+A(1,3)
                         + A(2,0)+A(2,1)+A(2,2)+A(2,3)
                         + A(3,0)+A(3,1)+A(3,2)+A(3,3)
                         + A(4,0)+A(4,1)+A(4,2)+A(4,3);

    vector_type sum_rows(nc);
    sum_rows(0) = A(0,0)+A(1,0)+A(2,0)+A(3,0)+A(4,0);
    sum_rows(1) = A(0,1)+A(1,1)+A(2,1)+A(3,1)+A(4,1);
    sum_rows(2) = A(0,2)+A(1,2)+A(2,2)+A(3,2)+A(4,2);
    sum_rows(3) = A(0,3)+A(1,3)+A(2,3)+A(3,3)+A(4,3);

    vector_type sum_cols(nr);
    sum_cols(0) = A(0,0)+A(0,1)+A(0,2)+A(0,3);
    sum_cols(1) = A(1,0)+A(1,1)+A(1,2)+A(1,3);
    sum_cols(2) = A(2,0)+A(2,1)+A(2,2)+A(2,3);
    sum_cols(3) = A(3,0)+A(3,1)+A(3,2)+A(3,3);
    sum_cols(4) = A(4,0)+A(4,1)+A(4,2)+A(4,3);

    value_type expect(0);
    value_type res(0);
    vector_type vexpect(0);
    vector_type vres(0);

    // sum_all(Z)
    expect = value_type(0);
    res = ublasx::sum_all(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum_all(" << Z << ") = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK( res == expect );

    // sum(Z)
    vexpect = ublas::vector<value_type>(nc, 0);
    vres = ublasx::sum(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

    // sum_rows(Z)
    vexpect = ublas::vector<value_type>(nc, 0);
    vres = ublasx::sum_rows(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum_rows(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

    // sum_columns(Z)
    vexpect = ublas::vector<value_type>(nr, 0);
    vres = ublasx::sum_columns(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum_columns(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

    // sum<1>(Z)
    vexpect = ublas::vector<value_type>(nc, 0);
    vres = ublasx::sum<1>(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

    // sum<2>(Z)
    vexpect = ublas::vector<value_type>(nr, 0);
    vres = ublasx::sum<2>(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

    // sum<tag::major>(Z)
    vexpect = ublas::vector<value_type>(nc, 0);
    vres = ublasx::sum_by_tag<ublas::tag::major>(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::major>(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nc, tol );

    // sum<tag::minor>(Z)
    vexpect = ublas::vector<value_type>(nr, 0);
    vres = ublasx::sum_by_tag<ublas::tag::minor>(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::minor>(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

    // sum<tag::leading>(Z)
    vexpect = ublas::vector<value_type>(nr, 0);
    vres = ublasx::sum_by_tag<ublas::tag::leading>(Z);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::leading>(" << Z << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, nr, tol );

    // sum_all(A)
    expect = value_type(sum_all);
    res = ublasx::sum_all(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_all(" << A << ") = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

    // sum(A)
    vexpect = sum_rows;
    vres = ublasx::sum(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_rows(A)
    vexpect = sum_rows;
    vres = ublasx::sum_rows(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_rows(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_columns(A)
    vexpect = sum_cols;
    vres = ublasx::sum_columns(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_columns(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<1>(A)
    vexpect = sum_rows;
    vres = ublasx::sum<1>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<2>(A)
    vexpect = sum_cols;
    vres = ublasx::sum<2>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum<2>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::major>(A)
    vexpect = sum_rows;
    vres = ublasx::sum_by_tag<ublasx::tag::major>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::major>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::minor>(A)
    vexpect = sum_cols;
    vres = ublasx::sum_by_tag<ublasx::tag::minor>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::minor>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::leading>(A)
    vexpect = sum_cols;
    vres = ublasx::sum_by_tag<ublasx::tag::leading>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::leading>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );
}

BOOST_UBLASX_TEST_DEF( test_col_major_matrix_container )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Column-major Matrix Container" );

    typedef double value_type;
    typedef ublas::matrix<value_type, ublas::column_major> matrix_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublas::matrix_traits<matrix_type>::size_type size_type;

    size_type nr = 5;
    size_type nc = 4;

    matrix_type A(nr, nc);

    A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
    A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
    A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

    value_type sum_all =   A(0,0)+A(0,1)+A(0,2)+A(0,3)
                         + A(1,0)+A(1,1)+A(1,2)+A(1,3)
                         + A(2,0)+A(2,1)+A(2,2)+A(2,3)
                         + A(3,0)+A(3,1)+A(3,2)+A(3,3)
                         + A(4,0)+A(4,1)+A(4,2)+A(4,3);

    vector_type sum_rows(nc);
    sum_rows(0) = A(0,0)+A(1,0)+A(2,0)+A(3,0)+A(4,0);
    sum_rows(1) = A(0,1)+A(1,1)+A(2,1)+A(3,1)+A(4,1);
    sum_rows(2) = A(0,2)+A(1,2)+A(2,2)+A(3,2)+A(4,2);
    sum_rows(3) = A(0,3)+A(1,3)+A(2,3)+A(3,3)+A(4,3);

    vector_type sum_cols(nr);
    sum_cols(0) = A(0,0)+A(0,1)+A(0,2)+A(0,3);
    sum_cols(1) = A(1,0)+A(1,1)+A(1,2)+A(1,3);
    sum_cols(2) = A(2,0)+A(2,1)+A(2,2)+A(2,3);
    sum_cols(3) = A(3,0)+A(3,1)+A(3,2)+A(3,3);
    sum_cols(4) = A(4,0)+A(4,1)+A(4,2)+A(4,3);

    value_type expect(0);
    value_type res(0);
    vector_type vexpect(0);
    vector_type vres(0);

    // sum_all(A)
    expect = value_type(sum_all);
    res = ublasx::sum_all(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_all(" << A << ") = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

    // sum(A)
    vexpect = sum_rows;
    vres = ublasx::sum(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_rows(A)
    vexpect = sum_rows;
    vres = ublasx::sum_rows(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_rows(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_columns(A)
    vexpect = sum_cols;
    vres = ublasx::sum_columns(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_columns(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<1>(A)
    vexpect = sum_rows;
    vres = ublasx::sum<1>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<2>(A)
    vexpect = sum_cols;
    vres = ublasx::sum<2>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum<2>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::major>(A)
    vexpect = sum_cols;
    vres = ublasx::sum_by_tag<ublasx::tag::major>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::major>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::minor>(A)
    vexpect = sum_rows;
    vres = ublasx::sum_by_tag<ublasx::tag::minor>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::minor>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::leading>(A)
    vexpect = sum_rows;
    vres = ublasx::sum_by_tag<ublasx::tag::leading>(A);
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::leading>(" << A << ") = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );
}

BOOST_UBLASX_TEST_DEF( test_matrix_expression )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Matrix Expression" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublas::matrix_traits<matrix_type>::size_type size_type;

    size_type nr = 5;
    size_type nc = 4;

    matrix_type A(nr, nc);

    A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
    A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
    A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

    value_type sum_all =   A(0,0)+A(0,1)+A(0,2)+A(0,3)
                         + A(1,0)+A(1,1)+A(1,2)+A(1,3)
                         + A(2,0)+A(2,1)+A(2,2)+A(2,3)
                         + A(3,0)+A(3,1)+A(3,2)+A(3,3)
                         + A(4,0)+A(4,1)+A(4,2)+A(4,3);

    vector_type sum_rows(nc);
    sum_rows(0) = A(0,0)+A(1,0)+A(2,0)+A(3,0)+A(4,0);
    sum_rows(1) = A(0,1)+A(1,1)+A(2,1)+A(3,1)+A(4,1);
    sum_rows(2) = A(0,2)+A(1,2)+A(2,2)+A(3,2)+A(4,2);
    sum_rows(3) = A(0,3)+A(1,3)+A(2,3)+A(3,3)+A(4,3);

    vector_type sum_cols(nr);
    sum_cols(0) = A(0,0)+A(0,1)+A(0,2)+A(0,3);
    sum_cols(1) = A(1,0)+A(1,1)+A(1,2)+A(1,3);
    sum_cols(2) = A(2,0)+A(2,1)+A(2,2)+A(2,3);
    sum_cols(3) = A(3,0)+A(3,1)+A(3,2)+A(3,3);
    sum_cols(4) = A(4,0)+A(4,1)+A(4,2)+A(4,3);

    value_type expect(0);
    value_type res(0);
    vector_type vexpect(0);
    vector_type vres(0);

    // sum_all(A')
    expect = value_type(sum_all);
    res = ublasx::sum_all(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_all(" << A << "') = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect, tol );

    // sum(A')
    vexpect = sum_cols;
    vres = ublasx::sum(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_rows(A')
    vexpect = sum_cols;
    vres = ublasx::sum_rows(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_rows(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_columns(A')
    vexpect = sum_rows;
    vres = ublasx::sum_columns(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_columns(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<1>(A')
    vexpect = sum_cols;
    vres = ublasx::sum<1>(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<2>(A')
    vexpect = sum_rows;
    vres = ublasx::sum<2>(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum<2>(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::major>(A')
    vexpect = sum_rows;
    vres = ublasx::sum_by_tag<ublasx::tag::major>(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::major>(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::minor>(A')
    vexpect = sum_cols;
    vres = ublasx::sum_by_tag<ublasx::tag::minor>(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::minor>(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::leading>(A')
    vexpect = sum_cols;
    vres = ublasx::sum_by_tag<ublasx::tag::leading>(ublas::trans(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::leading>(" << A << "') = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );
}

BOOST_UBLASX_TEST_DEF( test_matrix_reference )
{
    BOOST_UBLASX_DEBUG_TRACE( "TEST Matrix Reference" );

    typedef double value_type;
    typedef ublas::matrix<value_type> matrix_type;
    typedef ublas::matrix_reference<matrix_type> matrix_reference_type;
    typedef ublas::vector<value_type> vector_type;
    typedef ublas::matrix_traits<matrix_type>::size_type size_type;

    size_type nr = 5;
    size_type nc = 4;

    matrix_type A(nr, nc);

    A(0,0) = 0.0;      A(0,1) = 0.274690; A(0,2) = 0.0;      A(0,3) = 0.798938;
    A(1,0) = 0.108929; A(1,1) = 0.0;      A(1,2) = 0.891726; A(1,3) = 0.0;
    A(2,0) = 0.0;      A(2,1) = 0.0;      A(2,2) = 0.0;      A(2,3) = 0.0;
    A(3,0) = 0.0;      A(3,1) = 0.675382; A(3,2) = 0.0;      A(3,3) = 0.450332;
    A(4,0) = 1.023787; A(4,1) = 1.0;      A(4,2) = 1.231751; A(4,3) = 1.0;

    value_type sum_all =  A(0,0)+A(0,1)+A(0,2)+A(0,3)
                         + A(1,0)+A(1,1)+A(1,2)+A(1,3)
                         + A(2,0)+A(2,1)+A(2,2)+A(2,3)
                         + A(3,0)+A(3,1)+A(3,2)+A(3,3)
                         + A(4,0)+A(4,1)+A(4,2)+A(4,3);

    vector_type sum_rows(nc);
    sum_rows(0) = A(0,0)+A(1,0)+A(2,0)+A(3,0)+A(4,0);
    sum_rows(1) = A(0,1)+A(1,1)+A(2,1)+A(3,1)+A(4,1);
    sum_rows(2) = A(0,2)+A(1,2)+A(2,2)+A(3,2)+A(4,2);
    sum_rows(3) = A(0,3)+A(1,3)+A(2,3)+A(3,3)+A(4,3);

    vector_type sum_cols(nr);
    sum_cols(0) = A(0,0)+A(0,1)+A(0,2)+A(0,3);
    sum_cols(1) = A(1,0)+A(1,1)+A(1,2)+A(1,3);
    sum_cols(2) = A(2,0)+A(2,1)+A(2,2)+A(2,3);
    sum_cols(3) = A(3,0)+A(3,1)+A(3,2)+A(3,3);
    sum_cols(4) = A(4,0)+A(4,1)+A(4,2)+A(4,3);

    value_type expect(0);
    value_type res(0);
    vector_type vexpect(0);
    vector_type vres(0);

    // sum_all(ref(A))
    expect = value_type(sum_all);
    res = ublasx::sum_all(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_all(reference(" << A << ")) = " << res << " ==> " << expect );
    BOOST_UBLASX_TEST_CHECK( res == expect );

    // sum(ref(A))
    vexpect = sum_rows;
    vres = ublasx::sum(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_rows(ref(A))
    vexpect = sum_rows;
    vres = ublasx::sum_rows(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_rows(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum_columns(ref(A))
    vexpect = sum_cols;
    vres = ublasx::sum_columns(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_columns(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<1>(ref(A))
    vexpect = sum_rows;
    vres = ublasx::sum<1>(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum<1>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<2>(ref(A))
    vexpect = sum_cols;
    vres = ublasx::sum<2>(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum<2>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::major>(ref(A))
    vexpect = sum_rows;
    vres = ublasx::sum_by_tag<ublasx::tag::major>(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::major>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::minor>(ref(A))
    vexpect = sum_cols;
    vres = ublasx::sum_by_tag<ublasx::tag::minor>(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::minor>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );

    // sum<tag::leading>(ref(A))
    vexpect = sum_cols;
    vres = ublasx::sum_by_tag<ublasx::tag::leading>(matrix_reference_type(A));
    BOOST_UBLASX_DEBUG_TRACE( "sum_by_tag<tag::leading>(reference(" << A << ")) = " << vres << " ==> " << vexpect );
    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( vres, vexpect, vexpect.size(), tol );
}

int main()
{
    BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'sum' operation");

    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( test_vector_container );
    BOOST_UBLASX_TEST_DO( test_vector_expression );
    BOOST_UBLASX_TEST_DO( test_vector_reference );
    BOOST_UBLASX_TEST_DO( test_row_major_matrix_container );
    BOOST_UBLASX_TEST_DO( test_col_major_matrix_container );
    BOOST_UBLASX_TEST_DO( test_matrix_expression );
    BOOST_UBLASX_TEST_DO( test_matrix_reference );

    BOOST_UBLASX_TEST_END();
}


#ifndef HYBRID_ASTAR_TEST_NAVIGATION_NAVIGATION_PURE_PURSUIT_HPP
#define HYBRID_ASTAR_TEST_NAVIGATION_NAVIGATION_PURE_PURSUIT_HPP

#include <ros/ros.h>
#include <Eigen/Dense>
#include <mutex>
#include <thread>
#include <memory>

#include "hybrid_astar_test/subscriber/start_subscriber.hpp"
#include "hybrid_astar_test/subscriber/path_subscriber.hpp"

class PurePursuitControl{
public:
    PurePursuitControl(ros::NodeHandle& nh);
    ~PurePursuitControl();

public:
    bool Run();

private:
    Eigen::Vector2f CalculationVelocity(const Eigen::Vector3f& last_pose, const Eigen::Vector3f& now_pose, float delta_time);
    Eigen::Vector2f CalculationAcceleration(const Eigen::Vector2f& last_v, const Eigen::Vector2f& now_v, const Eigen::Vector3f& last_pose, const Eigen::Vector3f& now_pose);

private:
    std::shared_ptr<StartSubscriber>     localization_subscriber_ptr_;
    std::shared_ptr<PathSubscriber>      path_subscriber_ptr_;

private:
    ros::NodeHandle nh_;

    std::mutex global_mtx_;
    Eigen::VectorXf start_point(6);
    Eigen::VectorXf robot_localization(7);  // x y yaw liner_v angle_v liner_acc angle_acc
    std::vector<Eigen::Vector3f> Trajectory;
};

#endif


// Copyright (C) 2015 National ICT Australia (NICTA)
//
// 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/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_find_unique_1")
  {
  mat A =
    {
    {  1,  3,  5,  6,  7 },
    {  2,  4,  5,  7,  8 },
    {  3,  5,  5,  6,  9 },
    };

  uvec indices = find_unique(A);

  uvec indices2 = { 0, 1, 2, 4, 5, 9, 10, 13, 14 };

  REQUIRE( indices.n_elem == indices2.n_elem );

  bool same = true;

  for(uword i=0; i < indices.n_elem; ++i)
    {
    if(indices(i) != indices2(i))  { same = false; break; }
    }

  REQUIRE( same == true );

  vec unique_elem = { 1, 2, 3, 4, 5, 6, 7, 8, 9 };

  REQUIRE( accu(abs( A.elem(indices) - unique_elem )) == Approx(0.0) );

  // REQUIRE_THROWS(  );
  }

TEST_CASE("fn_find_unique_2")
  {
  cx_mat A =
    {
    { cx_double(1,-1), cx_double(3, 2), cx_double(5, 2), cx_double(6, 1), cx_double(7,-1) },
    { cx_double(2, 1), cx_double(4, 4), cx_double(5, 2), cx_double(7,-1), cx_double(8, 1) },
    { cx_double(3, 2), cx_double(5, 1), cx_double(5, 3), cx_double(6, 1), cx_double(9,-9) }
    };

  uvec indices = find_unique(A);

  uvec indices2 = { 0, 1, 2, 4, 5, 6, 8, 9, 10, 13, 14 };

  REQUIRE( indices.n_elem == indices2.n_elem );

  bool same = true;

  for(uword i=0; i < indices.n_elem; ++i)
    {
    if(indices(i) != indices2(i))  { same = false; break; }
    }

  REQUIRE( same == true );

  cx_vec unique_elem =
    {
    cx_double(1,-1),
    cx_double(2, 1),
    cx_double(3, 2),
    cx_double(4, 4),
    cx_double(5, 1),
    cx_double(5, 2),
    cx_double(5, 3),
    cx_double(6, 1),
    cx_double(7,-1),
    cx_double(8, 1),
    cx_double(9,-9)
    };

  REQUIRE( accu(abs( A.elem(indices) - unique_elem )) == Approx(0.0) );

  // REQUIRE_THROWS(  );
  }


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.ieee toolbox - fast_ldexp/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.ieee components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 04/12/2010
///
#include <boost/simd/toolbox/ieee/include/functions/fast_ldexp.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

NT2_TEST_CASE_TPL ( fast_ldexp_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::fast_ldexp;
  using boost::simd::tag::fast_ldexp_;
  using boost::simd::native;
  using boost::simd::meta::cardinal_of;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                                     vT;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef native<iT,ext_t>                                    ivT;

  for(unsigned int i = 0; i < cardinal_of<vT>::value; ++i)
    {
      std::cout << i << std::endl;
      NT2_TEST_EQUAL( boost::simd::fast_ldexp( boost::simd::One<vT>()
                                              , boost::simd::Two<ivT>()
                                              )[i]
                    , boost::simd::fast_ldexp( boost::simd::One<T>()
                                              , boost::simd::Two<iT>()
                                              )
                    );
    }
} // end of test for floating_


/*
 * Software License Agreement (New BSD License)
 *
 * Copyright (c) 2013, Keith Leung, Felipe Inostroza
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Advanced Mining Technology Center (AMTC), the
 *       Universidad de Chile, nor the names of its contributors may be
 *       used to endorse or promote products derived from this software without
 *       specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AMTC, UNIVERSIDAD DE CHILE, OR THE COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

// State class
// Keith Leung 2013

#ifndef STATE_HPP
#define STATE_HPP

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/LU>
//#include <Eigen/StdVector>
#include <iostream>
#include <stdio.h>

double const PI = acos(-1);

/**
 * \class RandomVec
 * Representation of a Gaussian random vector, with mean and covariance. A time variable is also included for time-stamping.
 * \brief An abstract base class for deriving pose and measurement classes
 * \tparam VecType An Eigen vector of dimension n
 * \tparam MatType An Eigen matrix or dimension n x n
 * \author Keith Leung
 */
template<class VecType, class MatType>
class RandomVec
{

public:

  typedef VecType Vec;
  typedef MatType Mat;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  /** Default constructor */
  RandomVec() :
    isValid_Sx_L_(false),
    isValid_Sx_inv_(false),
    isValid_Sx_det_(false),
    gen_(NULL)
  {

    if ( !dimCheck() ){
      exit(-1);
    }
    x_.setZero();
    Sx_.setZero();
    t_ = -1;

  }

  /**
   * Constructor
   * \param[in] x vector
   * \param[in] Sx covariance
   * \param[in] t time
   */
  RandomVec(VecType x, MatType Sx, double t = -1) :
    isValid_Sx_L_(false),
    isValid_Sx_inv_(false),
    isValid_Sx_det_(false),
    gen_(NULL)
  {
    if ( !dimCheck() ){
      exit(-1);
    }
    set(x);
    setCov(Sx);
    t_ = t;

  }

  /**
   * Constructor
   * \param[in] x vector
   * \param[in] t time
   */
  RandomVec(VecType x, double t = -1) :
    isValid_Sx_L_(false),
    isValid_Sx_inv_(false),
    isValid_Sx_det_(false),
    gen_(NULL)
  {
    if ( !dimCheck() ){
      exit(-1);
    }
    set(x);
    Sx_.setZero();
    t_ = t;

  }

  /**
   * Copy constructor
   * \param[in] other the RandomVec being copied from
   */
  RandomVec( const RandomVec& other ):

    x_( other.x_ ),
    nDim_( other.nDim_ ),
    Sx_( other.Sx_ ),
    Sx_inv_( other.Sx_inv_ ),
    isValid_Sx_inv_( other.isValid_Sx_inv_),
    Sx_det_( other.Sx_det_ ),
    isValid_Sx_det_( other.isValid_Sx_det_),
    Sx_L_( other.Sx_L_ ),
    isValid_Sx_L_( other.isValid_Sx_L_),
    t_(other.t_)
  {
    gen_ = NULL;
  }

  /**
   * Assignment operator
   * \param[in] rhs the right-hand-side from which data is copied
   */
  RandomVec& operator=( const RandomVec& rhs ){

    x_ = rhs.x_;
    nDim_ = rhs.nDim_;
    Sx_ = rhs.Sx_;
    Sx_inv_ = rhs.Sx_inv_;
    isValid_Sx_inv_ = rhs.isValid_Sx_inv_;
    Sx_det_ = rhs.Sx_det_;
    isValid_Sx_det_ = rhs.isValid_Sx_det_;
    Sx_L_ = rhs.Sx_L_;
    isValid_Sx_L_ = rhs.isValid_Sx_L_;
    t_ = rhs.t_;

    gen_ = NULL;
  }

  /** Default destructor */
  ~RandomVec(){
    if( gen_ != NULL )
      delete gen_;
  };

  /**
   * Set the vector
   * \param[in] x vector to be set
   */
  void set( VecType &x ){x_ = x;}

  /**
   * Set the covariance for uncertainty
   * \param[in] Sx uncertainty to be set
   */
  void setCov( MatType &Sx){
    Sx_ = Sx;
    isValid_Sx_L_ = false;
    isValid_Sx_inv_ = false;
    isValid_Sx_det_ =false;
  }

  /**
   * Set the time
   * \param[in] t time
   */
  void setTime( double t ){
    t_ = t;
  }

  /**
   * Set the vector with a covariance matrix
   * \param[in] x vector to be set
   * \param[in] Sx covariance to be set
   */
  void set( VecType &x, MatType &Sx){
    set(x);
    setCov(Sx);
  }

  /**
   * Set the vector with a time
   * \param[in] x vector to be set
   * \param[in] t time
   */
  void set( VecType &x, double t){
    set(x);
    t_ = t;
  }

  /**
   * Set the vector with a covariance matrix, and time
   * \param[in] x vector to be set
   * \param[in] Sx covariance to be set
   * \param[in] t time
   */
  void set( VecType &x, MatType &Sx, double t){
    set(x);
    setCov(Sx);
    t_ = t;
  }

  /**
   * Get the vector
   * \param[out] x vector
   */
  void get( VecType &x ){x = x_;}

  /**
   * Get the covariance matrix
   * \param[out] Sx uncertainty
   */
  void getCov( MatType &Sx){
    Sx = Sx_;
  }

  /**
   * Get the lower triangular part of the Cholesky decomposition
   * on the covariance matrx Sx_
   * \param[out] Sx_Chol_L the lower triangular part of the Choloesky decomposition
   */
  void getCovCholeskyDecompLower( MatType &Sx_Chol_L){
    if(!isValid_Sx_L_){
      Eigen::LLT<MatType> cholesky( Sx_ );
      Sx_L_ = cholesky.matrixL();
      isValid_Sx_L_ = true;
    }
    Sx_Chol_L = Sx_L_;
  }

  /**
   * Get the invserve covariance matrix
   * \param[out] Sx_inv inverse covariance
   */
  void getCovInv( MatType &Sx_inv){
    if(!isValid_Sx_inv_){
      Sx_inv_ = Sx_.inverse();
      isValid_Sx_inv_ = true;
    }
    Sx_inv = Sx_inv_;
  }

  /**
   * Get the determinant of the covariance
   * \return determinant
   */
  double getCovDet(){
    if(!isValid_Sx_det_){
      Sx_det_ = Sx_.determinant();
      isValid_Sx_det_ = true;
    }
    return Sx_det_;
  }

  /**
   * Get the vector and covariance matrix
   * \param[out] x vector
   * \param[out] Sx uncertainty
   */
  void get( VecType &x, MatType &Sx){
    get(x);
    getCov(Sx);
  }

  /**
   * Get the vector and time
   * \param[out] x vector
   * \param[out] t time
   */
  void get( VecType &x, double &t){
    get(x);
    t = t_;
  }

  /**
   * Get the vector, covariance matrix, and time
   * \param[out] x vector
   * \param[out] Sx uncertainty
   * \param[out] t time
   */
  void get( VecType &x, MatType &Sx, double &t){
    get(x);
    getCov(Sx);
    t = t_;
  }

  /**
   * Get an element of the vector
   * \param[in] n element index
   * \return element n
   */
  double get( int n ){ return x_(n);}

  /**
   * Get the time
   * \return time
   */
  double getTime(){
    return t_;
  }

  /**
   * Get the dimension
   * \return dimension
   */
  unsigned int getNDim(){ return nDim_; }

  /**
   * Calculate the squared Mahalanobis distance to another random vector of the same type
   * \param[in] to the RandomVec containing the vector we are measuring to
   */
  double mahalanobisDist2( RandomVec<VecType, MatType> &to ){
    if(!isValid_Sx_inv_){
      Sx_inv_ = Sx_.inverse();
      isValid_Sx_inv_ = true;
    }
    e_ = to.x_ - x_;
    return (e_.transpose() * Sx_inv_ * e_);
  }

  /**
   * Calculate the squared Mahalanobis distance to another random vector of the same type
   * \param[in] to_x the vector we are measuring to
   */
  double mahalanobisDist2( typename RandomVec<VecType, MatType>::Vec &to_x ){
    if(!isValid_Sx_inv_){
      Sx_inv_ = Sx_.inverse();
      isValid_Sx_inv_ = true;
    }
    e_ = to_x - x_;
    return (e_.transpose() * Sx_inv_ * e_);
  }

  /**
   * Calculate the Gaussian likelihood of a given evaluation point
   * \param[in] x_eval the evaluation point
   * \param[out] if not NULL, the pointed to variable will be overwritten by the
   * squared mahalanobis distance used to calculate the likelihood
   */
  double evalGaussianLikelihood( RandomVec<VecType, MatType> &x_eval,
				 double* mDist2 = NULL){
    if(!isValid_Sx_det_){
      Sx_det_ = Sx_.determinant();
      isValid_Sx_det_ = true;
    }
    double md2 = mahalanobisDist2( x_eval );
    double l = ( exp(-0.5 * md2 ) / sqrt( pow( 2*PI, nDim_ ) * Sx_det_ ) );
    if( l != l) //If md2 is very large, l will become NAN;
      l = 0;
    if(mDist2 != NULL)
      *mDist2 = md2;
    return l;
  }

  /**
   * Calculate likelihood
   * \param[in] x_eval the evaluation point
   * \param[out] if not NULL, the pointed to variable will be overwritten by the
   * squared mahalanobis distance used to calculate the likelihood
   */
  double evalGaussianLikelihood( typename RandomVec<VecType, MatType>::Vec &x_eval,
				 double* mDist2 = NULL){
    if(!isValid_Sx_det_){
      Sx_det_ = Sx_.determinant();
      isValid_Sx_det_ = true;
    }
    double md2 = mahalanobisDist2( x_eval );
    double l = ( exp(-0.5 * md2 ) / sqrt( pow( 2*PI, nDim_ ) * Sx_det_ ) );
    if( l != l) //If md2 is very large, l will become NAN;
      l = 0;
    if(mDist2 != NULL)
      *mDist2 = md2;
    return l;
  }

  /**
   * Sample this random vector
   * \param[out] s_sample The sampled vector, with time and covariance copied from this random vector
   */
  void sample( RandomVec<VecType, MatType> &s_sample ){

    VecType x_sample, indep_noise;

    if(!isValid_Sx_L_){
      Eigen::LLT<MatType> cholesky( Sx_ );
      Sx_L_ = cholesky.matrixL();
      isValid_Sx_L_ = true;
    }

    if(gen_ == NULL){
      gen_ = new boost::variate_generator< boost::mt19937,
					   boost::normal_distribution<double> >
	(boost::mt19937(rand()), boost::normal_distribution<double>());
    }

    int n = Sx_L_.cols();
    for(int i = 0; i < n; i++){
      indep_noise(i) = (*gen_)();
    }
    x_sample = x_ + Sx_L_ * indep_noise;
    s_sample.set( x_sample, Sx_, t_ );

  }

  /**
   * Sample this random vector and write the result over the mean x_;
   */
  void sample(){

    VecType x_sample, indep_noise;

    if(!isValid_Sx_L_){
      Eigen::LLT<MatType> cholesky( Sx_ );
      Sx_L_ = cholesky.matrixL();
      isValid_Sx_L_ = true;
    }

    if(gen_ == NULL){
      gen_ = new boost::variate_generator< boost::mt19937,
					   boost::normal_distribution<double> >
	(boost::mt19937(rand()), boost::normal_distribution<double>());
    }

    int n = Sx_L_.cols();
    for(int i = 0; i < n; i++){
      indep_noise(i) = (*gen_)();
    }
    x_ += Sx_L_ * indep_noise;

  }

private:

  VecType x_; /**< State */
  unsigned int nDim_; /**< Number of dimensions */
  MatType Sx_; /**< Covariance */
  MatType Sx_inv_; /**< Inverse covariance */
  bool isValid_Sx_inv_; /**< Inverse covariance is up to date */
  double Sx_det_; /**< Determinant of Sx_ */
  bool isValid_Sx_det_; /**< Determinant of Sx_ is up to date */
  MatType Sx_L_; /**< Lower triangular part of Cholesky decomposition on Sx_ */
  bool isValid_Sx_L_; /**< Lower triangular part of Cholesky decomposition on Sx_ is up to date */
  double t_; /**< time */

  VecType e_; /**< temporary */

  boost::variate_generator< boost::mt19937,
			    boost::normal_distribution<double> >* gen_;/**< normal distribution random number generator */

  /** Dimensionality check during initialization */
  bool dimCheck(){

    if( Sx_.rows() != Sx_.cols() ){
      std::cerr << "Error: MatType must be a square matrix \n";
      return false;
    }
    if( Sx_.rows() != x_.size() ){
      std::cerr << "Error: VecType and MatType dimension mismatch \n";
      return false;
    }
    nDim_ = x_.size();
    return true;
  }

};

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/swar/include/functions/sort.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <iostream>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

NT2_TEST_CASE_TPL ( sort, BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::native;
  using boost::simd::sort;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t> vT;

  vT ordered, reversed, mixed;
  for(std::size_t i=0; i!=vT::static_size; ++i)
  {
    ordered[i] = T(i);
    reversed[vT::static_size-1-i] = T(i);
    mixed[(i % 2) ? (vT::static_size-i) : i] = T(i);
  }

  NT2_TEST_EQUAL(sort(ordered), ordered);
  NT2_TEST_EQUAL(sort(reversed), ordered);

  std::cout << "mixed = " << mixed << std::endl;
  NT2_TEST_EQUAL(sort(mixed), ordered);
 }


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@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;

static void test_simple_reshape()
{
  Tensor<float, 5> tensor1(2,3,1,7,1);
  tensor1.setRandom();

  Tensor<float, 3> tensor2(2,3,7);
  Tensor<float, 2> tensor3(6,7);
  Tensor<float, 2> tensor4(2,21);

  Tensor<float, 3>::Dimensions dim1(2,3,7);
  tensor2 = tensor1.reshape(dim1);
  Tensor<float, 2>::Dimensions dim2(6,7);
  tensor3 = tensor1.reshape(dim2);
  Tensor<float, 2>::Dimensions dim3(2,21);
  tensor4 = tensor1.reshape(dim1).reshape(dim3);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_EQUAL(tensor1(i,j,0,k,0), tensor2(i,j,k));
        VERIFY_IS_EQUAL(tensor1(i,j,0,k,0), tensor3(i+2*j,k));
        VERIFY_IS_EQUAL(tensor1(i,j,0,k,0), tensor4(i,j+3*k));
      }
    }
  }
}

static void test_reshape_in_expr() {
  MatrixXf m1(2,3*5*7*11);
  MatrixXf m2(3*5*7*11,13);
  m1.setRandom();
  m2.setRandom();
  MatrixXf m3 = m1 * m2;

  TensorMap<Tensor<float, 5>> tensor1(m1.data(), 2,3,5,7,11);
  TensorMap<Tensor<float, 5>> tensor2(m2.data(), 3,5,7,11,13);
  Tensor<float, 2>::Dimensions newDims1(2,3*5*7*11);
  Tensor<float, 2>::Dimensions newDims2(3*5*7*11,13);
  typedef Tensor<float, 1>::DimensionPair DimPair;
  array<DimPair, 1> contract_along{{DimPair(1, 0)}};
  Tensor<float, 2> tensor3(2,13);
  tensor3 = tensor1.reshape(newDims1).contract(tensor2.reshape(newDims2), contract_along);

  Map<MatrixXf> res(tensor3.data(), 2, 13);
  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 13; ++j) {
      VERIFY_IS_APPROX(res(i,j), m3(i,j));
    }
  }
}

static void test_reshape_as_lvalue()
{
  Tensor<float, 3> tensor(2,3,7);
  tensor.setRandom();

  Tensor<float, 2> tensor2d(6,7);
  Tensor<float, 3>::Dimensions dim(2,3,7);
  tensor2d.reshape(dim) = tensor;

  float scratch[2*3*1*7*1];
  TensorMap<Tensor<float, 5>> tensor5d(scratch, 2,3,1,7,1);
  tensor5d.reshape(dim).device(Eigen::DefaultDevice()) = tensor;

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_EQUAL(tensor2d(i+2*j,k), tensor(i,j,k));
        VERIFY_IS_EQUAL(tensor5d(i,j,0,k,0), tensor(i,j,k));
      }
    }
  }
}

template<int DataLayout>
static void test_simple_slice()
{
  Tensor<float, 5, DataLayout> tensor(2,3,5,7,11);
  tensor.setRandom();

  Tensor<float, 5, DataLayout> slice1(1,1,1,1,1);
  Eigen::DSizes<ptrdiff_t, 5> indices(1,2,3,4,5);
  Eigen::DSizes<ptrdiff_t, 5> sizes(1,1,1,1,1);
  slice1 = tensor.slice(indices, sizes);
  VERIFY_IS_EQUAL(slice1(0,0,0,0,0), tensor(1,2,3,4,5));

  Tensor<float, 5, DataLayout> slice2(1,1,2,2,3);
  Eigen::DSizes<ptrdiff_t, 5> indices2(1,1,3,4,5);
  Eigen::DSizes<ptrdiff_t, 5> sizes2(1,1,2,2,3);
  slice2 = tensor.slice(indices2, sizes2);
  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 2; ++j) {
      for (int k = 0; k < 3; ++k) {
        VERIFY_IS_EQUAL(slice2(0,0,i,j,k), tensor(1,1,3+i,4+j,5+k));
      }
    }
  }
}

template<int DataLayout>
static void test_slice_in_expr() {
  typedef Matrix<float, Dynamic, Dynamic, DataLayout> Mtx;
  Mtx m1(7,7);
  Mtx m2(3,3);
  m1.setRandom();
  m2.setRandom();

  Mtx m3 = m1.block(1, 2, 3, 3) * m2.block(0, 2, 3, 1);

  TensorMap<Tensor<float, 2, DataLayout>> tensor1(m1.data(), 7, 7);
  TensorMap<Tensor<float, 2, DataLayout>> tensor2(m2.data(), 3, 3);
  Tensor<float, 2, DataLayout> tensor3(3,1);
  typedef Tensor<float, 1>::DimensionPair DimPair;
  array<DimPair, 1> contract_along{{DimPair(1, 0)}};

  Eigen::DSizes<ptrdiff_t, 2> indices1(1,2);
  Eigen::DSizes<ptrdiff_t, 2> sizes1(3,3);
  Eigen::DSizes<ptrdiff_t, 2> indices2(0,2);
  Eigen::DSizes<ptrdiff_t, 2> sizes2(3,1);
  tensor3 = tensor1.slice(indices1, sizes1).contract(tensor2.slice(indices2, sizes2), contract_along);

  Map<Mtx> res(tensor3.data(), 3, 1);
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 1; ++j) {
      VERIFY_IS_APPROX(res(i,j), m3(i,j));
    }
  }

  // Take an arbitrary slice of an arbitrarily sized tensor.
  TensorMap<Tensor<const float, 2, DataLayout>> tensor4(m1.data(), 7, 7);
  Tensor<float, 1, DataLayout> tensor6 = tensor4.reshape(DSizes<ptrdiff_t, 1>(7*7)).exp().slice(DSizes<ptrdiff_t, 1>(0), DSizes<ptrdiff_t, 1>(35));
  for (int i = 0; i < 35; ++i) {
    VERIFY_IS_APPROX(tensor6(i), expf(tensor4.data()[i]));
  }
}

template<int DataLayout>
static void test_slice_as_lvalue()
{
  Tensor<float, 3, DataLayout> tensor1(2,2,7);
  tensor1.setRandom();
  Tensor<float, 3, DataLayout> tensor2(2,2,7);
  tensor2.setRandom();
  Tensor<float, 3, DataLayout> tensor3(4,3,5);
  tensor3.setRandom();
  Tensor<float, 3, DataLayout> tensor4(4,3,2);
  tensor4.setRandom();
  Tensor<float, 3, DataLayout> tensor5(10,13,12);
  tensor5.setRandom();

  Tensor<float, 3, DataLayout> result(4,5,7);
  Eigen::DSizes<ptrdiff_t, 3> sizes12(2,2,7);
  Eigen::DSizes<ptrdiff_t, 3> first_slice(0,0,0);
  result.slice(first_slice, sizes12) = tensor1;
  Eigen::DSizes<ptrdiff_t, 3> second_slice(2,0,0);
  result.slice(second_slice, sizes12).device(Eigen::DefaultDevice()) = tensor2;

  Eigen::DSizes<ptrdiff_t, 3> sizes3(4,3,5);
  Eigen::DSizes<ptrdiff_t, 3> third_slice(0,2,0);
  result.slice(third_slice, sizes3) = tensor3;

  Eigen::DSizes<ptrdiff_t, 3> sizes4(4,3,2);
  Eigen::DSizes<ptrdiff_t, 3> fourth_slice(0,2,5);
  result.slice(fourth_slice, sizes4) = tensor4;

  for (int j = 0; j < 2; ++j) {
    for (int k = 0; k < 7; ++k) {
      for (int i = 0; i < 2; ++i) {
        VERIFY_IS_EQUAL(result(i,j,k), tensor1(i,j,k));
        VERIFY_IS_EQUAL(result(i+2,j,k), tensor2(i,j,k));
      }
    }
  }
  for (int i = 0; i < 4; ++i) {
    for (int j = 2; j < 5; ++j) {
      for (int k = 0; k < 5; ++k) {
        VERIFY_IS_EQUAL(result(i,j,k), tensor3(i,j-2,k));
      }
      for (int k = 5; k < 7; ++k) {
        VERIFY_IS_EQUAL(result(i,j,k), tensor4(i,j-2,k-5));
      }
    }
  }

  Eigen::DSizes<ptrdiff_t, 3> sizes5(4,5,7);
  Eigen::DSizes<ptrdiff_t, 3> fifth_slice(0,0,0);
  result.slice(fifth_slice, sizes5) = tensor5.slice(fifth_slice, sizes5);
  for (int i = 0; i < 4; ++i) {
    for (int j = 2; j < 5; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_EQUAL(result(i,j,k), tensor5(i,j,k));
      }
    }
  }
}

template<int DataLayout>
static void test_slice_raw_data()
{
  Tensor<float, 4, DataLayout> tensor(3,5,7,11);
  tensor.setRandom();

  Eigen::DSizes<ptrdiff_t, 4> offsets(1,2,3,4);
  Eigen::DSizes<ptrdiff_t, 4> extents(1,1,1,1);
  typedef TensorEvaluator<decltype(tensor.slice(offsets, extents)), DefaultDevice> SliceEvaluator;
  auto slice1 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
  VERIFY_IS_EQUAL(slice1.dimensions().TotalSize(), 1);
  VERIFY_IS_EQUAL(slice1.data()[0], tensor(1,2,3,4));

  if (DataLayout == ColMajor) {
    extents = Eigen::DSizes<ptrdiff_t, 4>(2,1,1,1);
    auto slice2 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
    VERIFY_IS_EQUAL(slice2.dimensions().TotalSize(), 2);
    VERIFY_IS_EQUAL(slice2.data()[0], tensor(1,2,3,4));
    VERIFY_IS_EQUAL(slice2.data()[1], tensor(2,2,3,4));
  } else {
    extents = Eigen::DSizes<ptrdiff_t, 4>(1,1,1,2);
    auto slice2 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
    VERIFY_IS_EQUAL(slice2.dimensions().TotalSize(), 2);
    VERIFY_IS_EQUAL(slice2.data()[0], tensor(1,2,3,4));
    VERIFY_IS_EQUAL(slice2.data()[1], tensor(1,2,3,5));
  }

  extents = Eigen::DSizes<ptrdiff_t, 4>(1,2,1,1);
  auto slice3 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
  VERIFY_IS_EQUAL(slice3.dimensions().TotalSize(), 2);
  VERIFY_IS_EQUAL(slice3.data(), static_cast<float*>(0));

  if (DataLayout == ColMajor) {
    offsets = Eigen::DSizes<ptrdiff_t, 4>(0,2,3,4);
    extents = Eigen::DSizes<ptrdiff_t, 4>(3,2,1,1);
    auto slice4 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
    VERIFY_IS_EQUAL(slice4.dimensions().TotalSize(), 6);
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 2; ++j) {
        VERIFY_IS_EQUAL(slice4.data()[i+3*j], tensor(i,2+j,3,4));
      }
    }
  } else {
    offsets = Eigen::DSizes<ptrdiff_t, 4>(1,2,3,0);
    extents = Eigen::DSizes<ptrdiff_t, 4>(1,1,2,11);
    auto slice4 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
    VERIFY_IS_EQUAL(slice4.dimensions().TotalSize(), 22);
    for (int l = 0; l < 11; ++l) {
      for (int k = 0; k < 2; ++k) {
        VERIFY_IS_EQUAL(slice4.data()[l+11*k], tensor(1,2,3+k,l));
      }
    }
  }

  if (DataLayout == ColMajor) {
    offsets = Eigen::DSizes<ptrdiff_t, 4>(0,0,0,4);
    extents = Eigen::DSizes<ptrdiff_t, 4>(3,5,7,2);
    auto slice5 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
    VERIFY_IS_EQUAL(slice5.dimensions().TotalSize(), 210);
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 5; ++j) {
        for (int k = 0; k < 7; ++k) {
          for (int l = 0; l < 2; ++l) {
            int slice_index = i + 3 * (j + 5 * (k + 7 * l));
            VERIFY_IS_EQUAL(slice5.data()[slice_index], tensor(i,j,k,l+4));
          }
        }
      }
    }
  } else {
    offsets = Eigen::DSizes<ptrdiff_t, 4>(1,0,0,0);
    extents = Eigen::DSizes<ptrdiff_t, 4>(2,5,7,11);
    auto slice5 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
    VERIFY_IS_EQUAL(slice5.dimensions().TotalSize(), 770);
    for (int l = 0; l < 11; ++l) {
      for (int k = 0; k < 7; ++k) {
        for (int j = 0; j < 5; ++j) {
          for (int i = 0; i < 2; ++i) {
            int slice_index = l + 11 * (k + 7 * (j + 5 * i));
            VERIFY_IS_EQUAL(slice5.data()[slice_index], tensor(i+1,j,k,l));
          }
        }
      }
    }

  }

  offsets = Eigen::DSizes<ptrdiff_t, 4>(0,0,0,0);
  extents = Eigen::DSizes<ptrdiff_t, 4>(3,5,7,11);
  auto slice6 = SliceEvaluator(tensor.slice(offsets, extents), DefaultDevice());
  VERIFY_IS_EQUAL(slice6.dimensions().TotalSize(), 3*5*7*11);
  VERIFY_IS_EQUAL(slice6.data(), tensor.data());
}

template<int DataLayout>
static void test_composition()
{
  Eigen::Tensor<float, 2, DataLayout> matrix(7, 11);
  matrix.setRandom();

  const DSizes<ptrdiff_t, 3> newDims(1, 1, 11);
  Eigen::Tensor<float, 3, DataLayout> tensor =
      matrix.slice(DSizes<ptrdiff_t, 2>(2, 0), DSizes<ptrdiff_t, 2>(1, 11)).reshape(newDims);

  VERIFY_IS_EQUAL(tensor.dimensions().TotalSize(), 11);
  VERIFY_IS_EQUAL(tensor.dimension(0), 1);
  VERIFY_IS_EQUAL(tensor.dimension(1), 1);
  VERIFY_IS_EQUAL(tensor.dimension(2), 11);
  for (int i = 0; i < 11; ++i) {
    VERIFY_IS_EQUAL(tensor(0,0,i), matrix(2,i));
  }
}

void test_cxx11_tensor_morphing()
{
  CALL_SUBTEST(test_simple_reshape());
  CALL_SUBTEST(test_reshape_in_expr());
  CALL_SUBTEST(test_reshape_as_lvalue());

  CALL_SUBTEST(test_simple_slice<ColMajor>());
  CALL_SUBTEST(test_simple_slice<RowMajor>());
  CALL_SUBTEST(test_slice_in_expr<ColMajor>());
  CALL_SUBTEST(test_slice_in_expr<RowMajor>());
  CALL_SUBTEST(test_slice_as_lvalue<ColMajor>());
  CALL_SUBTEST(test_slice_as_lvalue<RowMajor>());
  CALL_SUBTEST(test_slice_raw_data<ColMajor>());
  CALL_SUBTEST(test_slice_raw_data<RowMajor>());

  CALL_SUBTEST(test_composition<ColMajor>());
  CALL_SUBTEST(test_composition<RowMajor>());
}


#ifndef SLIDING_WINDOW_H
#define SLIDING_WINDOW_H

#include <queue>
#include <map>
#include <vector>
#include <stdexcept>
#include <string>
#include <iostream>

#include <boost/lexical_cast.hpp>

#include <sam/ActiveWindow.hpp>
#include <sam/DormantWindow.hpp>

namespace sam {

template <typename K>
class SlidingWindow
{
private:
  size_t N; ///> The total number of elements in the sliding window
  size_t b; ///> The number elements represented in a block
  size_t k; ///> The number of top elements to keep track of
  size_t counter = 0; ///>how many elements have been processed in active window
  ActiveWindow<K> active; ///> The active window
  std::queue<DormantWindow<K>> queue; ///> All the dormant windows
  std::map<K, size_t> globalInfo; ///> Global counts on frequent keys
  int numDormant; ///> The number of dormant windows (N/b - 1)

public:
  SlidingWindow(int N, int b, int k) : active(b)
  {
    this->N = N;
    this->b = b;
    this->k = k;
    numDormant = N / b - 1;
    if (numDormant <= 0) {
      std::string message = "Num dormant was less than or equal to zero " +
                            boost::lexical_cast<string>(numDormant);
      throw std::invalid_argument(message);
    }
  }

  int getNumDormant() const { return numDormant; }

  /**
   * Adds the given key to the sliding window.
   * \param key
   */
  void add(K key)
  {
    // If the counter is less than b, we add the key to the active window
    if (counter < b) {
      active.update(key);
      counter++;
    }
    // When the active window is full
    else {
      // Create a new dormant window based on the current active window
      DormantWindow<K> newDormant(k, active);
      queue.push(newDormant); // Add the dormant window to the queue
      addToGlobal(newDormant); // Update the global stats
      active = ActiveWindow<K>(b); // Create a new active window
      active.update(key);
      counter = 1;
    }

    // Check to see if we need to get rid of the oldest dormant window.
    if (queue.size() > numDormant) {
      DormantWindow<K> oldest = queue.front();
      queue.pop();
      removeFromGlobal(oldest);
    }
  }

  size_t getNumActiveElements() {
    return active.getNumElements();
  }

  size_t getNumDormantElements() {
    return queue.size() * b;
  }

  std::pair<K, size_t> getIthElement(size_t i) {

    if (i >= globalInfo.size()) {
      std::string message = "Error in GetIth Element: Size of global info: " +
        boost::lexical_cast<std::string>(globalInfo.size()) +
        " Requested element " + boost::lexical_cast<std::string>(i);
      throw std::out_of_range(message);
    }

    std::vector<std::pair<K, size_t>> pairs;
    for (auto itr = globalInfo.begin(); itr != globalInfo.end(); ++itr)
      pairs.push_back(*itr);

    std::sort(pairs.begin(), pairs.end(),
     [=](std::pair<K, size_t> const & a, std::pair<K, size_t> const & b)
              {
               return a.second > b.second;
              }
    );

    return pairs[i];
  }

  /**
   * Returns a vector of the keys in string format in descending order.
   */
  std::vector<string> getKeys() {
    std::vector<string> keys;
    int limit = globalInfo.size();
    for (int i = 0; i < limit; i++) {
      auto p = getIthElement(i);
      string key = boost::lexical_cast<string>(p.first);
      keys.push_back(key);
    }
    return keys;
  }

  std::vector<double> getFrequencies() {
    std::vector<double> frequencies;
    int limit = globalInfo.size();
    for (int i = 0; i < limit; i++) {
      auto p = getIthElement(i);
      double count = boost::lexical_cast<double>(p.second);
      frequencies.push_back(count);
    }

    double total = static_cast<double>(getNumDormantElements());

    std::transform(frequencies.begin(),
                  frequencies.end(),
                  frequencies.begin(),
                  [total](double &item){ return item / total; });

    return frequencies;
  }

private:

  /**
   * Addes the specified Dormant window's stats to the global stats.
   * \param newDormant
   */
  void addToGlobal(DormantWindow<K> newDormant)
  {
    int actualK = newDormant.getNumKeys() < k ? newDormant.getNumKeys() : k;
    for (int i = 0; i < actualK; i++) {
      std::pair<K, size_t> t = newDormant.getIthMostFrequent(i);
      K key = t.first;
      size_t value = t.second;
      if (globalInfo.count(key) > 0) {
        globalInfo[key] = globalInfo[key] + value;
      } else {
        globalInfo[key] = value;
      }
    }
  }

  /**
   * Removes the specified dormant window's statistics from the
   * global stats.
   * \param oldest
   */
  void removeFromGlobal(DormantWindow<K> oldest)
  {
    int actualK = oldest.getNumKeys() < k ? oldest.getNumKeys() : k;
    for (int i = 0; i < actualK; i++) {
      std::pair<K, size_t> t = oldest.getIthMostFrequent(i);
      K key = t.first;
      size_t value = t.second;
      if (globalInfo.count(key) > 0) {
        size_t newValue = globalInfo[key] - value;
        globalInfo[key] = newValue;

        if (globalInfo[key] <= 0) {
          globalInfo.erase(key);
        }
      }
    }
  }

};

}

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef NT2_TOOLBOX_BOOST_MATH_FUNCTIONS_LEGENDRE_NEXT_HPP_INCLUDED
#define NT2_TOOLBOX_BOOST_MATH_FUNCTIONS_LEGENDRE_NEXT_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <boost/math/special_functions/legendre.hpp>

/*!
 * \ingroup boost_math
 * \defgroup boost_math_legendre_next legendre_next
 *
 * \par Description
 * Please for details consult the proper documentation of the external
 * library boost_math.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/toolbox/boost_math/include/functions/legendre_next.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace nt2
 * {
 *   namespace boost_math
 *   {
 *     template <class A0>
 *       meta::call<tag::legendre_next_(A0,A0,A0,A0,A0)>::type
 *       legendre_next(const A0 & a0,const A0 & a1,const A0 & a2,const A0 & a3,const A0 & a4);
 *   }
 * }
 * \endcode
 *
 * \param a0 the first parameter of legendre_next
 * \param a1 the second parameter of legendre_next
 * \param a2 the third parameter of legendre_next
 * \param a3 the fourth parameter of legendre_next
 * \param a4 the fith parameter of legendre_next
 *
 * \return a value of the common type of the parameters
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 * When calling external library, nt2 simply encapsulates the
 * original proper call to provide easy use.
 * \par
 * Remenber that SIMD implementation is therefore merely
 * mapping the scalar function to each SIMD vectors elements
 * and will not provide acceleration, but ease.
 *
**/

namespace nt2 { namespace boost_math { namespace tag
  {
    /*!
     * \brief Define the tag legendre_next_ of functor legendre_next
     *        in namespace nt2::boost_math::tag for toolbox boost_math
    **/
    struct legendre_next_ : ext::elementwise_<legendre_next_> { typedef ext::elementwise_<legendre_next_> parent; };
  }
  NT2_FUNCTION_IMPLEMENTATION(boost_math::tag::legendre_next_, legendre_next, 5)
  } }

#include <nt2/toolbox/boost_math/functions/scalar/legendre_next.hpp>
// #include <nt2/toolbox/boost_math/functions/simd/all/legendre_next.hpp>

#endif

// modified by jt the 29/12/2010