//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file ir_steiner_tree_long_test.cpp
 * @brief
 * @author Maciej Andrejczuk
 * @version 1.0
 * @date 2013-02-04
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_steinlib.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/iterative_rounding/steiner_tree/steiner_tree.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/range/adaptor/sliced.hpp>

#include <fstream>
#include <vector>

using namespace paal;
using namespace paal::ir;

using Vertex = int;
using Dist = int;
using Terminals = std::vector<int>;
using Metric = GraphMT;

static const double APPROXIMATION_RATIO = 1.39;

template <typename OracleStrategy = paal::lp::random_violated_separation_oracle,
          typename Strategy>
void run_test(const steiner_tree_test_with_metric& test, const Strategy& strategy) {
    std::vector<int> result;
    auto status =
        paal::ir::steiner_tree_iterative_rounding<OracleStrategy>(
            test.metric, test.terminals, test.steiner_points,
            std::back_inserter(result), strategy);
    BOOST_CHECK_EQUAL(status, paal::lp::OPTIMAL);
    int res = paal::ir::steiner_utils::count_cost(result, test.terminals, test.metric);

    LOG("RES " << res << "\n");
    LOG("APPROXIMATION_RATIO:" << double(res) / double(test.optimal) << "\n");
    check_result(res, test.optimal, APPROXIMATION_RATIO);
}

BOOST_AUTO_TEST_CASE(ir_steiner_tree_long_test) {
    std::vector<steiner_tree_test_with_metric> data;
    read_steinlib_tests(data);
    paal::ir::steiner_tree_random_generator strategy_rand(50, 5);
    // First tests only
    for (auto const &test : data | boost::adaptors::sliced(0, 10)) {
        LOG("TEST " << test.test_name << "\n");
        LOG("OPT " << test.optimal << "\n");

        for (int i : paal::irange(5)) {
            LOGLN("random violated/, seed " << i);
            srand(i);
            run_test(test, strategy_rand);

            LOGLN("random violated + graph strategy, seed " << i);
            srand(i);
            auto strategy_graph = paal::ir::make_steiner_tree_graph_all_generator<Vertex>(
                    test.graph, test.terminals, 5);
            run_test(test, strategy_graph);
        }

        LOGLN("most violated");
        run_test<paal::lp::max_violated_separation_oracle>(test, strategy_rand);

        LOGLN("first violated");
        run_test<paal::lp::first_violated_separation_oracle>(test, strategy_rand);
    }
}


#pragma once

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/range/numeric.hpp>
#include "rng.hpp"
#include "utils.hpp"

/**
 * Returns a randomly selected element of a vector.
 */
template <class T> T select_random_element(std::vector<T> v) {
  using namespace std;
  mt19937 gen = RNG::rng()->get_RNG();
  T element;
  if (v.size() == 0) {
    throw "Vector is empty, so we cannot select a random element from it. (function: select_random_element)\n";
  }
  else if (v.size() == 1){
    element = v[0];
  }
  else {
    boost::random::uniform_int_distribution<> dist(0, v.size() - 1);
    element = v[dist(gen)];
  }
  return element;
}

double sample_from_normal(double mu, double sd);

/**
 * The KDE class implements 1-D Gaussian kernel density estimators.
 */
class KDE {
public:
  KDE () {};
  std::vector<double> dataset;
  double bw; // bandwidth
  KDE(std::vector<double>);

  // TODO: Made this public just to initialize β.
  // Not sure this is the correct way to do it.
  double mu;

  std::vector<double> resample(int n_samples);
  double pdf(double x);
  std::vector<double> pdf(std::vector<double> v);
  double logpdf(double x);
};


///////////////////////////////////////////////////////////////////////////////
// accumulator.hpp                                                           //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_TOOLS_DETAIL_SERIES_ACCUMULATOR_HPP_ER_2010
#define BOOST_STATISTICS_TOOLS_DETAIL_SERIES_ACCUMULATOR_HPP_ER_2010
#include <boost/type_traits/remove_reference.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/statistics/tools/series/traits.hpp>
#include <boost/statistics/tools/series/assign_data.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace series{

    // This is an accumulater_set<> bound two objets : one generating the
    // cumulative sample size (S), the other generating data (G) to be accumu-
    // lated. The features of interest are specified by an mpl vector F. Each
    // nullary call generates a new set of samples which are passed to the accu-
    // mulator and the features of interest are then evaluated and returned in a
    // data-structure.
    template<typename F,typename S,typename G>
    class accumulator{

        typedef typename G::distribution_type random_;
        typedef typename boost::remove_reference<
            S::result_type
        >::type size_;

        public:
        typedef typename boost::remove_reference<
            typename random_::result_type>::type sample_type;
        typedef F features_type;
        typedef boost::accumulators::accumulator_set<
            sample_type,features_type> accumulator_type;

        typedef typename detail::series::data_of<
            acc_,features_type>::type result_type;

        accumulator():n_( 0 ){}

        accumulator(const S& s,const G& g)
        :n_series_( s ),gen_( g ),n_( 0 ){}

        accumulator(const Args& args,const S& s,const G& g)
        :acc_( args ),n_series_( s ),gen_( g ),n_( 0 ){}

        result_type operator()()const
        {
            this->generate_accumulate();
            result_type result;
            detail::series::assign_data<F>( this->acc_, result );
            return result;
        }

        template<typename Args>
        result_type operator()(const Args& args)const
        {
            this->generate_accumulate();
            result_type result;
            detail::series::assign_data<F>( this->acc_, args,result );
            return result;
        }

        private:
        void generate_accumulate()const{
            size_ new_n = this->series_();
            for(size_ i = this->n_; i<new_n; i++){ this->acc( this->gen() ); }
            this->n_ = new_n;
        }

        A acc_;
        S n_series_;
        G gen_;
        size_ n_;
    };

    template<typename F,typename Args,typename S,typename G>
    detail::series::accumulator<F,S,G>
    make_accumulator(const S& series,const G& gen)
    {
        typedef detail::series::accumulator<F,S,G> result_;
        return result_(series,gen);
    }

    template<typename F,typename Args,typename S,typename G>
    detail::series::accumulator<F,S,G>
    make_accumulator(const Args& args,const S& series,const G& gen)
    {
        typedef detail::series::accumulator<F,S,G> result_;
        return result_(series,gen);
    }

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

#endif


//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

// clang-format off

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include "PyImathFun.h"
#include "PyImathFunOperators.h"
#include "PyImathDecorators.h"
#include "PyImathExport.h"
#include "PyImathAutovectorize.h"

namespace PyImath {

using namespace boost::python;

namespace
{

struct RegisterFloatDoubleOps
{
    template <typename T>
    void operator() (T)
    {
        // nb: MSVC gets confused about which arg we want (it thinks it
        // might be boost::arg), so telling it which one explicitly here.
        typedef boost::python::arg arg;

        generate_bindings<abs_op<T>,boost::mpl::true_>(
            "abs",
            "return the absolute value of 'value'",
            (arg("value")));

        generate_bindings<sign_op<T>,boost::mpl::true_>(
            "sign",
            "return 1 or -1 based on the sign of 'value'",
            (arg("value")));

        generate_bindings<log_op<T>,boost::mpl::true_>(
            "log",
            "return the natural log of 'value'",
            (arg("value")));

        generate_bindings<log10_op<T>,boost::mpl::true_>(
            "log10",
            "return the base 10 log of 'value'",
            (arg("value")));

        generate_bindings<lerp_op<T>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
            "lerp",
            "return the linear interpolation of 'a' to 'b' using parameter 't'",
            (arg("a"),arg("b"),arg("t")));

        generate_bindings<lerpfactor_op<T>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
            "lerpfactor",
            "return how far m is between a and b, that is return t such that\n"
            "if:\n"
            "    t = lerpfactor(m, a, b);\n"
            "then:\n"
            "    m = lerp(a, b, t);\n"
            "\n"
            "If a==b, return 0.\n",
            (arg("m"),arg("a"),arg("b")));

        generate_bindings<clamp_op<T>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
            "clamp",
            "return the value clamped to the range [low,high]",
            (arg("value"),arg("low"),arg("high")));

        generate_bindings<floor_op<T>,boost::mpl::true_>(
            "floor",
            "return the closest integer less than or equal to 'value'",
            (arg("value")));

        generate_bindings<ceil_op<T>,boost::mpl::true_>(
            "ceil",
            "return the closest integer greater than or equal to 'value'",
            (arg("value")));

        generate_bindings<trunc_op<T>,boost::mpl::true_>(
            "trunc",
            "return the closest integer with magnitude less than or equal to 'value'",
            (arg("value")));

        generate_bindings<rgb2hsv_op<T>,boost::mpl::true_>(
            "rgb2hsv",
            "return the hsv version of an rgb color",
            args("rgb"));

        generate_bindings<hsv2rgb_op<T>,boost::mpl::true_>(
            "hsv2rgb",
            "return the rgb version of an hsv color",
            args("hsv"));

        generate_bindings<sin_op<T>,boost::mpl::true_>(
            "sin",
            "return the sine of the angle theta",
            args("theta"));

        generate_bindings<cos_op<T>,boost::mpl::true_>(
            "cos",
            "return the cosine of the angle theta",
            args("theta"));

        generate_bindings<tan_op<T>,boost::mpl::true_>(
            "tan",
            "return the tangent of the angle theta",
            args("theta"));

        generate_bindings<asin_op<T>,boost::mpl::true_>(
            "asin",
            "return the arcsine of the value x",
            args("x"));

        generate_bindings<acos_op<T>,boost::mpl::true_>(
            "acos",
            "return the arccosine of the value x",
            args("x"));

        generate_bindings<atan_op<T>,boost::mpl::true_>(
            "atan",
            "return the arctangent of the value x",
            args("x"));

        generate_bindings<atan2_op<T>,boost::mpl::true_,boost::mpl::true_>(
            "atan2",
            "return the arctangent of the coordinate x,y - note the y "
            "is the first argument for consistency with libm ordering",
            args("y","x"));

        generate_bindings<sqrt_op<T>,boost::mpl::true_>(
            "sqrt",
            "return the square root of x",
            args("x"));

        generate_bindings<pow_op<T>,boost::mpl::true_,boost::mpl::true_>(
            "pow",
            "return x**y",
            args("x","y"));

        generate_bindings<exp_op<T>,boost::mpl::true_>(
             "exp",
             "return exp(x)",
             args("x"));

        generate_bindings<sinh_op<T>,boost::mpl::true_>(
             "sinh",
             "return sinh(x)",
             args("x"));

        generate_bindings<cosh_op<T>,boost::mpl::true_>(
             "cosh",
             "return cosh(x)",
             args("x"));

        def("cmp", IMATH_NAMESPACE::cmp<T>);
        def("cmpt", IMATH_NAMESPACE::cmpt<T>);
        def("iszero", IMATH_NAMESPACE::iszero<T>);
        def("equal", IMATH_NAMESPACE::equal<T, T, T>);
    }
};

} // namespace

void register_functions()
{
    //
    // Utility Functions
    //

    // nb: MSVC gets confused about which arg we want (it thinks it
    // might be boost::arg), so telling it which one explicitly here.
    typedef boost::python::arg arg;

    using fp_types = boost::mpl::vector<float, double>;
    boost::mpl::for_each<fp_types>(RegisterFloatDoubleOps());

    generate_bindings<abs_op<int>,boost::mpl::true_>(
        "abs",
        "return the absolute value of 'value'",
        (arg("value")));

    generate_bindings<sign_op<int>,boost::mpl::true_>(
        "sign",
        "return 1 or -1 based on the sign of 'value'",
        (arg("value")));

    generate_bindings<clamp_op<int>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
        "clamp",
        "return the value clamped to the range [low,high]",
        (arg("value"),arg("low"),arg("high")));

    generate_bindings<divs_op,boost::mpl::true_,boost::mpl::true_>(
        "divs",
        "return x/y where the remainder has the same sign as x:\n"
        "    divs(x,y) == (abs(x) / abs(y)) * (sign(x) * sign(y))\n",
        (arg("x"),arg("y")));
    generate_bindings<mods_op,boost::mpl::true_,boost::mpl::true_>(
        "mods",
        "return x%y where the remainder has the same sign as x:\n"
        "    mods(x,y) == x - y * divs(x,y)\n",
        (arg("x"),arg("y")));

    generate_bindings<divp_op,boost::mpl::true_,boost::mpl::true_>(
        "divp",
        "return x/y where the remainder is always positive:\n"
        "    divp(x,y) == floor (double(x) / double (y))\n",
        (arg("x"),arg("y")));
    generate_bindings<modp_op,boost::mpl::true_,boost::mpl::true_>(
        "modp",
        "return x%y where the remainder is always positive:\n"
        "    modp(x,y) == x - y * divp(x,y)\n",
        (arg("x"),arg("y")));

    generate_bindings<bias_op,boost::mpl::true_,boost::mpl::true_>(
         "bias",
         "bias(x,b) is a gamma correction that remaps the unit interval such that bias(0.5, b) = b.",
         (arg("x"),arg("b")));

    generate_bindings<gain_op,boost::mpl::true_,boost::mpl::true_>(
         "gain",
         "gain(x,g) is a gamma correction that remaps the unit interval with the property that gain(0.5, g) = 0.5.\n"
         "The gain function can be thought of as two scaled bias curves forming an 'S' shape in the unit interval.",
         (arg("x"),arg("g")));

    //
    // Vectorized utility functions
    //
    generate_bindings<rotationXYZWithUpDir_op<float>,boost::mpl::true_,boost::mpl::true_,boost::mpl::true_>(
        "rotationXYZWithUpDir",
        "return the XYZ rotation vector that rotates 'fromDir' to 'toDir'"
        "using the up vector 'upDir'",
        args("fromDir","toDir","upDir"));
}

} // namespace PyImath


#include <iostream>
#include <iterator>
#include <string>
#include <vector>
#include <algorithm>
#include <map>
#include <set>
#include <boost/algorithm/string.hpp>

using namespace std;

int main() {

  map<string, set<string>> allergen_map;
  set<string> no_allergen_ing;
  set<string> all_allergens;
  vector<set<string>> all_foods;

  for (string line; getline(cin, line);){
    int start_paren = line.find('(');
    string ig_str = line.substr(0, start_paren);
    int start_allergens = start_paren + 9;
    string al_str = line.substr(start_allergens, line.size()-1-start_allergens);
    boost::trim(al_str);
    boost::trim(ig_str);
    vector<string> ingredients;
    boost::split(ingredients, ig_str, boost::is_any_of(" "));
    vector<string> allergens;
    boost::split(allergens, al_str, boost::is_any_of(","));
    set<string> ig_set;
    for (string ig : ingredients){
      boost::trim(ig);
      no_allergen_ing.emplace(ig);
      ig_set.emplace(ig);
    }
    all_foods.push_back(ig_set);
    for (string al : allergens) {
      boost::trim(al);
      all_allergens.emplace(al);
      if (allergen_map.find(al) == allergen_map.end()) {
        allergen_map[al] = ig_set;
      } else {
        set<string> intersection;
        set<string> existing_set = allergen_map[al];
        set_intersection(ig_set.begin(), ig_set.end(),
                         existing_set.begin(), existing_set.end(),
                         inserter(intersection, intersection.begin()));
        allergen_map[al] = intersection;
      }
    }
  }

  // Collect results
  map<string, string> allergen_result;
  bool not_done = true;
  while (not_done){
    for (auto al : all_allergens){
      if (allergen_map[al].size()==1){
        string ing = *(allergen_map[al].begin());
        allergen_result[al] = ing;
        no_allergen_ing.erase(ing);
        allergen_map.erase(al);
        for (auto al : all_allergens) {
          allergen_map[al].erase(ing);
        }
      }
    }
    if (allergen_result.size() == all_allergens.size()){
      not_done = false;
    }
  }

  int num_appearances = 0;
  for (auto ing_set: all_foods){
    for (auto ing : no_allergen_ing){
      if (ing_set.find(ing) != ing_set.end()){
        num_appearances++;
      }
    }
  }
  cout << "Part 1: " << num_appearances << endl;

  // I used an ordered map so...
  cout << "Part 2: |";
  unsigned count = 0;
  for (auto [al, ing] : allergen_result){
    cout << ing;
    if (count != allergen_result.size()-1){
      cout << ",";
    }
    count++;
 }
  cout << "|" << endl;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing oneminus capabilities

    Returns the one minus the entry, saturated in the entry type.

    @par semantic:
    For any given value @c x of type @c T:

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

    For signed type is similar to:

    @code
    T r = 1-x
    @endcode

    For unsigned type it is equivalent (due to saturation) to:

    @code
    T r = x == 0 ? 1 : 0
    @endcode

  **/
  const boost::dispatch::functor<tag::oneminus_> oneminus = {};
} }
#endif

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

#endif


// Copyright (c) 2012 Christopher Lux <christopherlux@gmail.com>
// Distributed under the Modified BSD License, see license.txt.

#include "patch_grid_mesh.h"

#include <cassert>
#include <exception>
#include <stdexcept>

#include <boost/assign/list_of.hpp>

#include <scm/log.h>
#include <scm/core/memory.h>

#include <scm/gl_core/render_device.h>
#include <scm/gl_core/buffer_objects/buffer.h>
#include <scm/gl_core/render_device/context.h>
#include <scm/gl_core/buffer_objects/vertex_array.h>
#include <scm/gl_core/buffer_objects/vertex_format.h>

namespace {

struct vertex {
    scm::math::vec3f pos;
    scm::math::vec2f tex_hf; // height map coord
    scm::math::vec2f tex_dm; // density map coord
}; //  struct vertex

} // namespace

namespace scm {
namespace data {

patch_grid_mesh::patch_grid_mesh(const gl::render_device_ptr& device,
                                 const math::vec2ui&          full_resolution,
                                 const math::vec2ui&          cell_size,
                                 const math::vec2f&           mesh_extends)
  : _full_resolution(full_resolution)
  , _mesh_extends(mesh_extends)
{
    using namespace scm::math;
    using namespace scm::gl;
    using boost::assign::list_of;

    // neighboring cells share texels, so we have to account for the 1 texel overlap
    _grid_resolution = vec2ui(floor(vec2f(_full_resolution) / vec2f(cell_size - 1)));
    //out() << "_full_resolution: " << _full_resolution << log::end;
    //out() << "cell_size: " << cell_size << log::end;
    //out() << "_grid_resolution: " << _grid_resolution << log::end;
    //out() << "_mesh_extends: " << _mesh_extends << log::end;

    size_t num_vertices     = (_grid_resolution.x + 1) * (_grid_resolution.y + 1);
    size_t num_quad_patches = _grid_resolution.x * _grid_resolution.y;
    size_t num_tri_patches  = _grid_resolution.x * _grid_resolution.y * 2;

    _vertices         = device->create_buffer(BIND_VERTEX_BUFFER, USAGE_STATIC_DRAW, num_vertices * sizeof(vertex), 0);
    _quad_indices     = device->create_buffer(BIND_INDEX_BUFFER,  USAGE_STATIC_DRAW, num_quad_patches * 4 * sizeof(unsigned), 0);
    _triangle_indices = device->create_buffer(BIND_INDEX_BUFFER,  USAGE_STATIC_DRAW, num_tri_patches * 3 * sizeof(unsigned), 0);

    if (   !_vertices
        || !_quad_indices) {
        throw (std::runtime_error("patch_grid_mesh::patch_grid_mesh(): unable to create vertex or index buffer."));
    }

    render_context_ptr ctx = device->main_context();

    { // initialize vertices
        vertex* data = static_cast<vertex*>(ctx->map_buffer(_vertices, ACCESS_WRITE_INVALIDATE_BUFFER));

        if (!data) {
            throw (std::runtime_error("patch_grid_mesh::patch_grid_mesh(): unable map vertex buffer."));
        }

        const vec2f dm_extends   = vec2f(_full_resolution) / (vec2f(_grid_resolution) * (cell_size - 1));

        const vec2f pos_delta    = (_mesh_extends * vec2f(cell_size - 1)) / _full_resolution;//_grid_dimension / _grid_resolution;
        const vec2f hf_tex_delta = (vec2f(1.0f)   * vec2f(cell_size - 1)) / _full_resolution;//vec2f(1.0, 1.0f) / _grid_resolution;
        const vec2f dm_tex_delta = (dm_extends    * vec2f(cell_size - 1)) / _full_resolution;//vec2f(1.0, 1.0f) / _grid_resolution;

        for(unsigned y = 0; y < (_grid_resolution.y + 1); ++y){
            for(unsigned x = 0; x < (_grid_resolution.x + 1); ++x){
                data[x + y * (_grid_resolution.x + 1)].pos    = vec3f(pos_delta.x * x,    pos_delta.y * y, 0.0f);
                data[x + y * (_grid_resolution.x + 1)].tex_hf = vec2f(hf_tex_delta.x * x, hf_tex_delta.y * y) + vec2f(0.5f) / _full_resolution;
                data[x + y * (_grid_resolution.x + 1)].tex_dm = vec2f(dm_tex_delta.x * x, dm_tex_delta.y * y) + vec2f(0.5f) / _full_resolution;
            }
        }
        ctx->unmap_buffer(_vertices);
    }

    _vertex_array = device->create_vertex_array(vertex_format(0, 0, TYPE_VEC3F, sizeof(vertex))  // vertex positions
                                                             (0, 2, TYPE_VEC2F, sizeof(vertex))  // texture coordinates height  map
                                                             (0, 3, TYPE_VEC2F, sizeof(vertex)), // texture coordinates density map
                                                list_of(_vertices));

    { // initialize quad list index buffer
        unsigned int* data = static_cast<unsigned int*>(ctx->map_buffer(_quad_indices, ACCESS_WRITE_INVALIDATE_BUFFER));

        if (!data) {
            throw (std::runtime_error("patch_grid_mesh::patch_grid_mesh(): unable map quad index buffer."));
        }

        for(unsigned y = 0; y < _grid_resolution.y; ++y){
            for(unsigned x = 0; x < _grid_resolution.x; ++x){
                unsigned i00 =  x      + y       * (_grid_resolution.x + 1);
                unsigned i10 = (x + 1) + y       * (_grid_resolution.x + 1);
                unsigned i11 = (x + 1) + (y + 1) * (_grid_resolution.x + 1);
                unsigned i01 =  x      + (y + 1) * (_grid_resolution.x + 1);

                size_t off = 4 * (x + y * _grid_resolution.x);

                data[off + 0] = i00;
                data[off + 1] = i10;
                data[off + 2] = i11;
                data[off + 3] = i01;
            }
        }
        ctx->unmap_buffer(_quad_indices);

        _quad_prim_topology = PRIMITIVE_PATCH_LIST_4_CONTROL_POINTS;
        _quad_vertex_count  = 4 * _grid_resolution.x * _grid_resolution.y;
    }
    { // initialize triangle list index buffer
        unsigned int* data = static_cast<unsigned int*>(ctx->map_buffer(_triangle_indices, ACCESS_WRITE_INVALIDATE_BUFFER));

        if (!data) {
            throw (std::runtime_error("patch_grid_mesh::patch_grid_mesh(): unable map triangle index buffer."));
        }

        for(unsigned y = 0; y < _grid_resolution.y; ++y){
            for(unsigned x = 0; x < _grid_resolution.x; ++x){
                unsigned i00 =  x      + y       * (_grid_resolution.x + 1);
                unsigned i10 = (x + 1) + y       * (_grid_resolution.x + 1);
                unsigned i11 = (x + 1) + (y + 1) * (_grid_resolution.x + 1);
                unsigned i01 =  x      + (y + 1) * (_grid_resolution.x + 1);

                size_t off = 6 * (x + y * _grid_resolution.x);

                data[off + 0] = i00;
                data[off + 1] = i01;
                data[off + 2] = i10;

                data[off + 3] = i10;
                data[off + 4] = i01;
                data[off + 5] = i11;
            }
        }
        ctx->unmap_buffer(_triangle_indices);

        _triangle_prim_topology = PRIMITIVE_PATCH_LIST_3_CONTROL_POINTS;
        _triangle_vertex_count  = 6 * _grid_resolution.x * _grid_resolution.y;
    }
}

patch_grid_mesh::~patch_grid_mesh()
{
    _vertices.reset();
    _quad_indices.reset();
    _quad_indices.reset();
    _triangle_indices.reset();
}

void
patch_grid_mesh::draw(const gl::render_context_ptr& context,
                      const mesh_mode               mode)
{
    using namespace scm::gl;
    context_vertex_input_guard vig(context);

    if (mode == MODE_QUAD_PATCHES) {
        context->bind_vertex_array(_vertex_array);
        context->bind_index_buffer(_quad_indices, _quad_prim_topology, TYPE_UINT);

        context->apply();
        context->draw_elements(_quad_vertex_count);
    }
    else if (mode == MODE_TRIANGLE_PATCHES) {
        context->bind_vertex_array(_vertex_array);
        context->bind_index_buffer(_triangle_indices, _triangle_prim_topology, TYPE_UINT);

        context->apply();
        context->draw_elements(_triangle_vertex_count);
    }
    else {
        err() << log::warning << "patch_grid_mesh::draw(): unknown mesh mode." << log::end;
    }
}

} // namespace data
} // namespace scm


//
//  AWT.hpp
//
//  Created by r. on 10/05/14 (using code from 2013.12.06)
//

#ifndef round1_AWT_hpp
#define round1_AWT_hpp

#include <boost/numeric/ublas/vector_of_vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>

//#include <algorithm>
#include <numeric>
#include <vector>

#include "../include/stopwatch.hpp"

namespace spacetime
{
	namespace ublas = boost::numeric::ublas;

	struct AWT
	{
	public:
		typedef boost::numeric::ublas::compressed_matrix<double> matrix;
		typedef boost::numeric::ublas::vector<double> vector;
	public:
		class Filter
		{
		public:
			const bool wave;
			const unsigned int n, N;
			const matrix C, F;
		public:
			Filter(unsigned int n, unsigned int N, const matrix& C, const matrix& F)
			:
			wave(n!=0), n(n), N(N), C(C), F(F)
			{
				assert(C.size1() == n);
				assert(C.size2() == N);
				assert(F.size1() == N-n);
				assert(F.size2() == N);
			}
		};
	public:
		// Should be hidden
		std::vector<Filter> filterbank;
	public:
		// These should be hidden
		matrix V;
		vector gamma;
		unsigned int n;
		unsigned int nu;
		double eta;
	public:
		AWT() : n(0), V(0,0), gamma(0), nu(0), eta(0) { }
	public:
		AWT(const matrix& A, const matrix& M, unsigned int nu = 2, double eta = 1.9)
		:
		V(0, 0), gamma(0), n((unsigned int)(A.size1())), nu(nu), eta(eta), filterbank()
		// Pre: A and M are square matrices of equal size
		{
			stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT()");

			assert((A.size1() == n) && (A.size2() == n));
			assert((M.size1() == n) && (M.size2() == n));
			assert(eta > 0);

			// Will hold the square roots of generalized "eigenvalues"
			gamma = vector(n);
			gamma.clear();

			typedef ublas::compressed_matrix<double, ublas::row_major> S_type;
			typedef ublas::compressed_matrix<double, ublas::column_major> T_type;

			// Note:
			// prod(S_type, T_type) is more efficient than prod(T_type, S_type)

			T_type T = ublas::identity_matrix<>(n);
			T_type PRO = ublas::identity_matrix<>(n);
			{
				typedef ublas::compressed_matrix<double> acmc_type;
				acmc_type mc = M, ac = A;
				unsigned int m = n;
				// Invariant: m is size1 and size2 of mc and ac

				// Iterate over levels
				while (m >= 3)
				{
					const unsigned int m0 = m;
					S_type M = mc;
					S_type A = ac;

					// Step 1. Identify fine hats

					typedef ublas::compressed_vector<double> compressed_vector;
					typedef std::vector<compressed_vector> P_type;
					P_type P;
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().P");

						for (unsigned int i = 0; i != m0; ++i) {
							P.push_back(compressed_vector(m0));
							(P.back())[i] = 1;
						}
					}

					std::vector<unsigned int> I(m0);
					{
						for (unsigned int i = 0; i != m0; ++i) I[i] = i;
					}

					double e0 = 0;
					while (m >= 3)
					{
						unsigned int j; // index of most energetic hat
						double e1; // and its energy
						{
							//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().e1");
							std::vector<double> E1(m);
							{
								ublas::range ALL(0, m0);
								ublas::matrix_vector_range<acmc_type> dac(ac, ALL, ALL);
								ublas::matrix_vector_range<acmc_type> dmc(mc, ALL, ALL);
								for (unsigned i = 0; i != m; ++i)
									E1[i] = dac(I[i]) / dmc(I[i]);
								E1.front() = E1.back() = 0;
							}
							auto i = std::max_element(E1.begin(), E1.end());
							e1 = *i;
							j = (unsigned int)(std::distance(E1.begin(), i));
							assert(E1[j] == e1);
						}
						assert(e1 > 0);

						if (e1 <= e0 / eta) break;
						if (e0 == 0) e0 = e1;

						double p = -ac(I[j-1], I[j]) / ac(I[j], I[j]);
						double q = -ac(I[j+1], I[j]) / ac(I[j], I[j]);

						// Manipulate ac
						{
							//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().prod1");

							row(ac, I[j-1]) += p * row(ac, I[j]);
							row(ac, I[j+1]) += q * row(ac, I[j]);

							I.erase(I.begin() + j);

							// New effective size of mc and ac
							m--;
						}

						// Pre-Manipulate T by means of P
						{
							//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().mani");
							P[j-1] += p * P[j];
							P[j+1] += q * P[j];
							P.push_back(P[j]);
							P.erase(P.begin() + j);
						}
					} // Finished identifying level

					S_type SC(m, m0), SF(m0-m, m0);
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().perm");

						for (unsigned int i = 0; i != m; ++i) row(SC, i) = P[i];
						for (unsigned int i = m; i != m0; ++i) row(SF, i-m) = P[i];
					}

					T_type MSC;
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().mcac");
						T_type tSC(ublas::trans(SC));
						MSC = prod(M, tSC);
						mc = prod(SC, MSC);
						ac = prod(SC, T_type(prod(A, tSC)));
					}

					// Step 2. Orthogonalize level
					{
						T_type DSC(SC);
						{
							//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().mass");

							for (unsigned int i = 0; i != m; ++i)
							{
								ublas::matrix_row<acmc_type> r(mc, i);
								row(DSC, i) *= (1. / std::accumulate(r.begin(), r.end(), 0.));
							}
						}

						ublas::range IF(m, m0), ALL(0, m0);
						for (unsigned int k = 0; k != nu; ++k)
						{
							//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().proj2");
							assert(SF.size2() == MSC.size1());
							assert(MSC.size2() == DSC.size1());
							assert(SF.size2() == DSC.size2());
							SF -= prod(S_type(prod(SF, MSC)), DSC);
						}
					}

					// Normalize wavelets
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().norm");
						for (unsigned int i = 0; i != m0-m; ++i)
						{
							auto c = row(SF, i);
							c *= (1. / sqrt(ublas::inner_prod(c, prod(M, ublas::trans(c)))));
							double d = ublas::inner_prod(c, prod(A, ublas::trans(c)));
							gamma[i + m] = sqrt(std::max(0., d));
						}
					}

					// Update filterbank
					{
						Filter filter(m, m0, SC, SF);
						filterbank.push_back(filter);
					}

					// Copy S into T
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().copy1");
						noalias(project(T, ublas::range(m, m0), ublas::range(0, n))) = prod(SF, PRO);
					}

					// Update prolongation onto finest hats
					{
						//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().PRO2");
						PRO = prod(SC, PRO);
					}

				} // end while(m >= 3)

				{
					S_type SF = ublas::identity_matrix<>(m);;
					{
						for (unsigned int i = 0; i != m; ++i)
						{
							auto c = row(SF, i);
							c *= (1. / sqrt(mc(i, i)));
							double d = ublas::inner_prod(c, prod(ac, ublas::trans(c)));
							gamma[i] = sqrt(std::max(0., d));
						}
					}

					// Update filterbank
					{
						Filter filter(0, m, matrix(0, m), SF);
						filterbank.push_back(filter);
					}

					//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().copy2");
					noalias(project(T, ublas::range(0, m), ublas::range(0, n))) = prod(SF, PRO);
				}
			} // Matrix T computed

			// Compute V
			{
				//stopwatch::Time time(stopwatch::watches, "spacetime::AWT::AWT().V");

				V = ublas::trans(T);
			}
		}

		AWT& operator=(const AWT&) = default;
	};

}

#endif


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>
#include <limits>

TEST(AgradFwdAbs, Fvar) {
  using stan::math::fvar;
  using std::abs;
  using std::isnan;

  fvar<double> x(2);
  fvar<double> y(-3);
  x.d_ = 1.0;
  y.d_ = 2.0;

  fvar<double> a = abs(x);
  EXPECT_FLOAT_EQ(abs(2), a.val_);
  EXPECT_FLOAT_EQ(1.0, a.d_);

  fvar<double> b = abs(-x);
  EXPECT_FLOAT_EQ(abs(-2), b.val_);
  EXPECT_FLOAT_EQ(1.0, b.d_);

  fvar<double> c = abs(y);
  EXPECT_FLOAT_EQ(abs(-3), c.val_);
  EXPECT_FLOAT_EQ(-2.0, c.d_);

  fvar<double> d = abs(2 * x);
  EXPECT_FLOAT_EQ(abs(2 * 2), d.val_);
  EXPECT_FLOAT_EQ(2 * 1.0, d.d_);

  fvar<double> e = abs(y + 4);
  EXPECT_FLOAT_EQ(abs(-3 + 4), e.val_);
  EXPECT_FLOAT_EQ(2.0, e.d_);

  fvar<double> f = abs(x - 2);
  EXPECT_FLOAT_EQ(abs(2 - 2), f.val_);
  EXPECT_FLOAT_EQ(0, f.d_);

  fvar<double> z = std::numeric_limits<double>::quiet_NaN();
  fvar<double> g = abs(z);
  EXPECT_TRUE(stan::math::is_nan(g.val_));
  EXPECT_TRUE(stan::math::is_nan(g.d_));

  fvar<double> w = 0;
  fvar<double> h = abs(w);
  EXPECT_FLOAT_EQ(0.0, h.val_);
  EXPECT_FLOAT_EQ(0.0, h.d_);
}

TEST(AgradFwdAbs, FvarFvarDouble) {
  using stan::math::fvar;
  using std::abs;

  fvar<fvar<double> > x;
  x.val_.val_ = 4.0;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > a = abs(x);

  EXPECT_FLOAT_EQ(4.0, a.val_.val_);
  EXPECT_FLOAT_EQ(1.0, a.val_.d_);
  EXPECT_FLOAT_EQ(0.0, a.d_.val_);
  EXPECT_FLOAT_EQ(0.0, a.d_.d_);
}

struct abs_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return abs(arg1);
  }
};

TEST(AgradFwdAbs, abs_NaN) {
  abs_fun abs_;
  test_nan_fwd(abs_, false);
}


/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  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 Universitaet Bremen 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 OWNER 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.
 */
/**
 * @file mtk/src/mtkmath.hpp
 * @brief several math utility functions.
 */

#ifndef MTKMATH_H_
#define MTKMATH_H_

#include <cmath>
#include <boost/math/tools/precision.hpp>

#include "../types/vect.hpp"

#ifndef M_PI
#define M_PI  3.1415926535897932384626433832795
#endif

namespace MTK {

namespace internal {

template<class Manifold>
struct traits {
	typedef typename Manifold::scalar scalar;
	enum {DOF = Manifold::DOF};
	typedef vect<DOF, scalar> vectorized_type;
	typedef Eigen::Matrix<scalar, DOF, DOF> matrix_type;
};

template<>
struct traits<float> : traits<Scalar<float> > {};
template<>
struct traits<double> : traits<Scalar<double> > {};

}  // namespace internal

/**
 * \defgroup MTKMath Mathematical helper functions
 */
//@{

//! constant @f$ \pi @f$
const double pi = M_PI;

template<class scalar> inline scalar tolerance();

template<> inline float  tolerance<float >() { return 1e-5f; }
template<> inline double tolerance<double>() { return 1e-11; }

/**
 * normalize @a x to @f$[-bound, bound] @f$.
 *
 * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$.
 */
template<class scalar>
inline scalar normalize(scalar x, scalar bound){
	if(std::fabs(x) <= bound) return x;
	int r = (int)(x *(scalar(1.0)/ bound));
	return x - ((r + (r>>31) + 1) & ~1)*bound;
}

//TODO this implementation might be faster?
inline double normalize2(double x, double interval){
	return x - interval * (rint(x * (1.0/interval)));
}

/**
 * Calculate cosine and sinc of sqrt(x2).
 * @param x2 the squared angle must be non-negative
 * @return a pair containing cos and sinc of sqrt(x2)
 */
template<class scalar>
std::pair<scalar, scalar> cos_sinc_sqrt(const scalar &x2){
	using std::sqrt;
	using std::cos;
	using std::sin;
	static scalar const taylor_0_bound = boost::math::tools::epsilon<scalar>();
	static scalar const taylor_2_bound = sqrt(taylor_0_bound);
	static scalar const taylor_n_bound = sqrt(taylor_2_bound);

	assert(x2>=0 && "argument must be non-negative");

	// FIXME check if bigger bounds are possible
	if(x2>=taylor_n_bound) {
		// slow fall-back solution
		scalar x = sqrt(x2);
		return std::make_pair(cos(x), sin(x)/x); // x is greater than 0.
	}

	// FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd)
	// TODO Find optimal coefficients using Remez algorithm
	static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.};
	scalar cosi = 1., sinc=1;
	scalar term = -1/2. * x2;
	for(int i=0; i<3; ++i) {
		cosi += term;
		term *= inv[2*i];
		sinc += term;
		term *= -inv[2*i+1] * x2;
	}

	return std::make_pair(cosi, sinc);

}

template<class scalar, int n>
scalar exp(vectview<scalar, n> result, vectview<const scalar, n> vec, const scalar& scale = 1) {
	scalar norm2 = vec.squaredNorm();
	std::pair<scalar, scalar> cos_sinc = cos_sinc_sqrt(scale*scale * norm2);
	scalar mult = cos_sinc.second * scale; // == std::sin(alpha) / norm;
	result = mult * vec;
	return cos_sinc.first;
}

/**
 * Inverse function to @c exp.
 *
 * @param result @c vectview to the result
 * @param w      scalar part of input
 * @param vec    vector part of input
 * @param scale  scale result by this value
 * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result
 */
template<class scalar, int n>
void log(vectview<scalar, n> result,
		const scalar &w, const vectview<const scalar, n> vec,
		const scalar &scale, bool plus_minus_periodicity)
{
	// FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division
	scalar nv = vec.norm();
	if(nv < tolerance<scalar>()) {
		if(!plus_minus_periodicity && w < 0) {
			// find the maximal entry:
			int i;
			nv = vec.cwiseAbs().maxCoeff(&i);
			result = scale * std::atan2(nv, w) * vect<n, scalar>::Unit(i);
			return;
		}
		nv = tolerance<scalar>();
	}
	scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) );

	result = s * vec;
}

//@}

} // namespace MTK

#endif /* MTKMATH_H_ */


#pragma once
#include <ros/ros.h>
#include <Eigen/Dense>
#include <geometry_msgs/Point.h>
#include <planner_types.hpp>

namespace Baxter
{

class Kinematics
{

public:
    Kinematics(ros::NodeHandle &pnh, const ArmState &state, const std::string &arm);
    ~Kinematics();

    std::vector<Eigen::Matrix4d> getTransforms(const ArmState &state);

private:
    void createTransforms(const std::vector<double> &angles);
    void updateTransforms(const std::vector<double> &angles);
    const std::vector<Eigen::Matrix4d> getJointTransforms() const;
    void createTbab();
    void createTab0();
    void createT01(const double &angle);
    void createT12(const double &angle);
    void createT23(const double &angle);
    void createT34(const double &angle);
    void createT45(const double &angle);
    void createT56(const double &angle);
    void createT67(const double &angle);
    void createT7g();
    void updateT01(const double &angle);
    void updateT12(const double &angle);
    void updateT23(const double &angle);
    void updateT34(const double &angle);
    void updateT45(const double &angle);
    void updateT56(const double &angle);
    void updateT67(const double &angle);

    std::string m_arm;

    Eigen::Matrix4d tbab;
    Eigen::Matrix4d tab0;
    Eigen::Matrix4d t01;
    Eigen::Matrix4d t12;
    Eigen::Matrix4d t23;
    Eigen::Matrix4d t34;
    Eigen::Matrix4d t45;
    Eigen::Matrix4d t56;
    Eigen::Matrix4d t67;
    Eigen::Matrix4d t7g;

    double l0;
    double l1;
    double l2;
    double l3;
    double l4;
    double l5;
    double l6;
    double H;
    double L;
    double h;
};

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_REM_PIO2_CEPHES_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_GENERIC_REM_PIO2_CEPHES_HPP_INCLUDED
#include <nt2/trigonometric/functions/rem_pio2_cephes.hpp>
#include <nt2/include/functions/simd/round.hpp>
#include <nt2/include/functions/simd/fast_toint.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/bitwise_and.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/twoopi.hpp>
#include <nt2/include/constants/pio2_1.hpp>
#include <nt2/include/constants/pio2_2.hpp>
#include <nt2/include/constants/pio2_3.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::rem_pio2_cephes_, tag::cpu_
                            , (A0)(A1)
                            , (generic_ < floating_<A0> > )
                              (generic_ < integer_<A1>  > )
                              (generic_ < floating_<A0> > )
                            )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& x, A1 & n, A0 & xr)
    {
      n = nt2::rem_pio2_cephes(x, xr);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::rem_pio2_cephes_, tag::cpu_
                            , (A0)
                            , (generic_ < floating_<A0> > )
                            )
  {
    typedef typename nt2::meta::as_integer<A0>::type int_t;
    typedef std::pair<int_t, A0>               result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      result_type res;
      res.first = nt2::rem_pio2_cephes(a0,res.second);
      return res;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::rem_pio2_cephes_, tag::cpu_
                            , (A0)
                            , (generic_< floating_<A0> >)
                              (generic_< floating_<A0> >)
                            )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& x, A0 & xr) const
    {
      A0 xi =  nt2::round(x*nt2::Twoopi<A0>());
      xr  = x-xi*nt2::Pio2_1<A0>();
      xr -= xi*nt2::Pio2_2<A0>();
      xr -= xi*nt2::Pio2_3<A0>();
      return nt2::bitwise_and(nt2::fast_toint(xi), Three<result_type>());
    }
  };
} }

#endif


#include "mtf/SM/NT/PF.h"
#include "mtf/Utilities/miscUtils.h"
#include <boost/random/random_device.hpp>
#include <boost/random/seed_seq.hpp>
#include "opencv2/highgui/highgui.hpp"
#include "mtf/Utilities/histUtils.h"
#ifndef DISABLE_GRAPH_UTILS
#include "mtf/Utilities/graphUtils.h"
#endif

#define reset_file(fname) fclose(fopen(fname, "w"));

_MTF_BEGIN_NAMESPACE
namespace nt{
	PF::PF(AM _am, SSM _ssm, const ParamType *pf_params) :
		SearchMethod(_am, _ssm), params(pf_params), max_wt_id(0),
		enable_adaptive_resampling(false), min_eff_particles(0){
		printf("\n");
		printf("Using Particle Filter (NT) SM with:\n");
		printf("max_iters: %d\n", params.max_iters);
		printf("n_particles: %d\n", params.n_particles);
		printf("epsilon: %f\n", params.epsilon);
		printf("dynamic_model: %s\n", PFParams::toString(params.dynamic_model));
		printf("update_type: %s\n", PFParams::toString(params.update_type));
		printf("likelihood_func: %s\n", PFParams::toString(params.likelihood_func));
		printf("resampling_type: %s\n", PFParams::toString(params.resampling_type));
		printf("adaptive_resampling_thresh: %f\n", params.adaptive_resampling_thresh);
		printf("mean_type: %s\n", PFParams::toString(params.mean_type));
		printf("reset_to_mean: %d\n", params.reset_to_mean);
		printf("update_distr_wts: %d\n", params.update_distr_wts);
		printf("min_distr_wt: %f\n", params.min_distr_wt);
		printf("measurement_sigma: %f\n", params.measurement_sigma);
		printf("show_particles: %d\n", params.show_particles);
		printf("enable_learning: %d\n", params.enable_learning);
		printf("jacobian_as_sigma: %d\n", params.jacobian_as_sigma);
		printf("debug_mode: %d\n", params.debug_mode);
		printf("appearance model: %s\n", am->name.c_str());
		printf("state space model: %s\n", ssm->name.c_str());
		printf("\n");

		name = "pf_nt";
		log_fname = "log/pf_debug.txt";
		time_fname = "log/pf_times.txt";
		wts_fname = "log/pf_particle_wts.txt";
		cum_wts_fname = "log/pf_particle_cum_wts.txt";
		state_fname = "log/pf_states.txt";
		corners_fname = "log/pf_corners.txt";

		frame_id = 0;

		ssm_state_size = ssm->getStateSize();
		printf("ssm_state_size: %d\n", ssm_state_size);

		using_pix_sigma = params.processDistributions(state_sigma, state_mean,
			distr_n_particles, n_distr, ssm_state_size);

		if(params.jacobian_as_sigma){
			dI_dp.resize(am->getPatchSize(), ssm_state_size);
			df_dp.resize(ssm_state_size);
			d2f_dp2.resize(ssm_state_size, ssm_state_size);
			n_distr = 1;
			using_pix_sigma = false;
		}
		printf("n_distr: %d\n", n_distr);

		if(n_distr == 1){ params.update_distr_wts = false; }

		const double pi = 3.14159265358979323846;
		measurement_factor = 1.0 / sqrt(2 * pi * params.measurement_sigma);

		for(int set_id = 0; set_id < 2; ++set_id){
			particle_states[set_id].resize(params.n_particles);
			particle_distr[set_id].resize(params.n_particles);
			particle_ar[set_id].resize(params.n_particles);
			for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
				particle_states[set_id][particle_id].resize(ssm_state_size);
				particle_ar[set_id][particle_id].resize(ssm_state_size);
			}
		}

		min_particles = static_cast<int>((params.n_particles*params.min_distr_wt) /
			static_cast<double>(n_distr));
		dynamic_particles = (params.n_particles - min_particles*n_distr);

		curr_set_id = 0;
		particle_wts.resize(params.n_particles);
		particle_cum_wts.setZero(params.n_particles);
		distr_wts.resize(n_distr);
		distr_n_particles.resize(n_distr);

		perturbed_state.resize(ssm_state_size);
		perturbed_ar.resize(ssm_state_size);
		mean_state.resize(ssm_state_size);

		state_sigma.resize(n_distr);
		state_mean.resize(n_distr);

		//! initialize random number generators for resampling and measurement function
		boost::random_device r;
		boost::random::seed_seq measurement_seed{ r(), r(), r(), r(), r(), r(), r(), r() };

		measurement_gen = RandGenT(measurement_seed);
		measurement_dist = MeasureDistT(0, params.measurement_sigma);

		boost::random::seed_seq resample_seed{ r(), r(), r(), r(), r(), r(), r(), r() };
		resample_gen = RandGenT(resample_seed);
		resample_dist = ResampleDistT(0, 1);

		distr_id_gen = RandGenT(r());

		if(params.adaptive_resampling_thresh > 0 && params.adaptive_resampling_thresh <= 1){
			printf("Using adaptive resampling\n");
			enable_adaptive_resampling = true;
			min_eff_particles = params.adaptive_resampling_thresh*params.n_particles;
		}

		if(params.debug_mode){
			reset_file(log_fname);
			reset_file(wts_fname);
			reset_file(cum_wts_fname);
			resample_ids.resize(params.n_particles);
			resample_ids_hist.resize(params.n_particles);
			uniform_rand_nums.resize(params.n_particles);
		}
		if(params.show_particles){
			reset_file(state_fname);
			reset_file(corners_fname);
			printf("Writing particle states to; %s\n", state_fname);
			printf("Writing particle corners to; %s\n", corners_fname);
		}
	}

	void PF::initialize(const cv::Mat &corners){
		am->clearInitStatus();
		ssm->clearInitStatus();

		ssm->initialize(corners, am->getNChannels());

		if(using_pix_sigma){
			//! estimate SSM parameter sigma from pixel sigma
			for(unsigned int distr_id = 0; distr_id < n_distr; ++distr_id){
				state_sigma[distr_id].resize(ssm_state_size);
				state_mean[distr_id] = VectorXd::Zero(ssm_state_size);
				ssm->estimateStateSigma(state_sigma[distr_id], params.pix_sigma[distr_id]);
			}
		}
		//! initialize SSM sampler with the first distribution
		ssm->initializeSampler(state_sigma[0], state_mean[0]);
		am->initializePixVals(ssm->getPts());
		am->initializeSimilarity();
		max_similarity = am->getSimilarity();

		if(params.jacobian_as_sigma){
			am->initializeGrad();
			am->initializePixGrad(ssm->getPts());
			if(params.update_type == UpdateType::Additive){
				ssm->cmptPixJacobian(dI_dp, am->getInitPixGrad());
			} else{
				ssm->cmptWarpedPixJacobian(dI_dp, am->getInitPixGrad());
			}
			am->cmptSelfHessian(d2f_dp2, dI_dp);
		}

		initializeParticles();
		initializeDistributions();

		prev_corners = ssm->getCorners();
		ssm->getCorners(cv_corners_mat);

		if(params.debug_mode){
			//! print the sigma for the SSM distributions
			printf("state_sigma:\n");
			for(unsigned int distr_id = 0; distr_id < n_distr; ++distr_id){
				if(n_distr > 1){ printf("%d: ", distr_id); }
				utils::printMatrix(state_sigma[distr_id].transpose(), nullptr, "%e");
			}
			utils::printScalar(max_similarity, "max_similarity");
		}
	}

	void PF::initializeParticles(){
		double init_wt = 1.0 / params.n_particles;
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
			particle_states[curr_set_id][particle_id] = ssm->getState();
			particle_wts[particle_id] = init_wt;
			if(particle_id > 0){
				particle_cum_wts[particle_id] = particle_wts[particle_id] + particle_cum_wts[particle_id - 1];
			} else{
				particle_cum_wts[particle_id] = particle_wts[particle_id];
			}
			particle_ar[curr_set_id][particle_id].setZero();
		}

	}
	void PF::initializeDistributions(){
		double init_distr_wt = 1.0 / n_distr;
		for(unsigned int distr_id = 0; distr_id < n_distr; ++distr_id){
			distr_wts[distr_id] = init_distr_wt;
			distr_n_particles[distr_id] = 0;
		}
	}

	void PF::update(){
		++frame_id;
		if(params.debug_mode){
			utils::printScalarToFile(frame_id, "\n\n-------------------\nframe_id", log_fname, "%d");
		}
		am->setFirstIter();
		int pause_after_show = 1;
		if(params.jacobian_as_sigma){
			am->updatePixVals(ssm->getPts());
			am->updateSimilarity();
			am->updateCurrGrad();
			am->updatePixGrad(ssm->getPts());
			if(params.update_type == UpdateType::Additive){
				ssm->cmptPixJacobian(dI_dp, am->getCurrPixGrad());
			} else{
				ssm->cmptWarpedPixJacobian(dI_dp, am->getCurrPixGrad());
			}
			am->cmptCurrJacobian(df_dp, dI_dp);
			state_sigma[0] = -d2f_dp2.colPivHouseholderQr().solve(df_dp.transpose());

			ssm->setSampler(state_sigma[0], state_mean[0]);
		}
		if(params.show_particles){
			utils::printScalarToFile(frame_id, "frame_id", state_fname, "%d");
			utils::printScalarToFile(frame_id, "frame_id", corners_fname, "%d");
			utils::printMatrixToFile(ssm->getState().transpose(), nullptr, state_fname);
			utils::printMatrixToFile(ssm->getCorners(), nullptr, corners_fname);
		}
		int distr_id = 0;
		for(int iter_id = 0; iter_id < params.max_iters; ++iter_id){
			if(params.show_particles){
				am->getCurrImg().convertTo(curr_img_uchar, CV_8UC1);
			}
			if(n_distr > 1){
				distr_id_dist = DistrDistT(distr_wts.begin(), distr_wts.end());
				if(params.show_particles){
					cout << "Distribution weights:\n";
					for(unsigned int i = 0; i < n_distr; ++i){
						std::cout << distr_wts[i] << ' ';
					}
					std::cout << "\n";
					cout << "Distribution probablities:\n";
					for(unsigned int i = 0; i < n_distr; ++i){
						std::cout << distr_id_dist.probabilities()[i] << ' ';
					}
					std::cout << "\n";
				}
				for(unsigned int i = 0; i < n_distr; ++i){
					distr_wts[i] = 0;
					distr_n_particles[i] = 0;
				}
			}
			double max_wt = std::numeric_limits<double>::lowest();
			for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
				if(n_distr > 1){
					int new_distr_id = distr_id_dist(distr_id_gen);
					if(new_distr_id != distr_id){
						distr_id = new_distr_id;
						//! need to reset SSM sampler only if multiple distributions are in use
						ssm->setSampler(state_sigma[distr_id], state_mean[distr_id]);
					}
				}
				particle_distr[curr_set_id][particle_id] = distr_id;
				switch(params.dynamic_model){
				case DynamicModel::AutoRegression1:
					switch(params.update_type){
					case UpdateType::Additive:
						ssm->additiveAutoRegression1(perturbed_state, perturbed_ar,
							particle_states[curr_set_id][particle_id], particle_ar[curr_set_id][particle_id]);
						break;
					case UpdateType::Compositional:
						ssm->compositionalAutoRegression1(perturbed_state, perturbed_ar,
							particle_states[curr_set_id][particle_id], particle_ar[curr_set_id][particle_id]);
						break;
					}
					particle_ar[curr_set_id][particle_id] = perturbed_ar;
					break;
				case DynamicModel::RandomWalk:
					switch(params.update_type){
					case UpdateType::Additive:
						ssm->additiveRandomWalk(perturbed_state, particle_states[curr_set_id][particle_id]);
						break;
					case UpdateType::Compositional:
						ssm->compositionalRandomWalk(perturbed_state, particle_states[curr_set_id][particle_id]);
						break;
					}
					break;
				}
				particle_states[curr_set_id][particle_id] = perturbed_state;

				//printf("PF:: calling setState for particle %d set %d with: \n", particle_id, curr_set_id);
				//utils::printMatrix(perturbed_state.transpose(), "perturbed_state");
				//utils::printMatrix(particle_states[curr_set_id][particle_id].transpose(), "ssm_state");

				ssm->setState(particle_states[curr_set_id][particle_id]);
				am->updatePixVals(ssm->getPts());
				am->updateSimilarity(false);
				/**
				a positive number that measures the dissimilarity between the
				template and the patch corresponding to this particle
				*/
				double measuremnt_val = max_similarity - am->getSimilarity();

				// convert this dissimilarity to a likelihood proportional to the dissimilarity
				switch(params.likelihood_func){
				case LikelihoodFunc::AM:
					measurement_likelihood = am->getLikelihood();
					break;
				case LikelihoodFunc::Gaussian:
					measurement_likelihood = measurement_factor * exp(-0.5*measuremnt_val / params.measurement_sigma);
					break;
				case LikelihoodFunc::Reciprocal:
					measurement_likelihood = 1.0 / (1.0 + measuremnt_val);
					break;
				}
				if(params.show_particles){
					cv::Point2d corners[4];
					ssm->getCorners(corners);
					utils::drawCorners(curr_img_uchar, corners,
						cv::Scalar(0, 0, 255), cv::format("%d: %5.3e", particle_id + 1, measurement_likelihood));
					//printf("measurement_likelihood: %e\n", measurement_likelihood);
					if((particle_id + 1) % params.show_particles == 0){
						cv::imshow("Particles", curr_img_uchar);
						int key = cv::waitKey(1 - pause_after_show);
						if(key == 27){
							cv::destroyWindow("Particles");
							params.show_particles = 0;
						} else if(key == 32){
							pause_after_show = 1 - pause_after_show;
						}
						am->getCurrImg().convertTo(curr_img_uchar, CV_8UC1);
					}
					utils::printMatrixToFile(ssm->getState().transpose(), nullptr, state_fname);
					utils::printMatrixToFile(ssm->getCorners(), nullptr, corners_fname);
				}
				particle_wts[particle_id] = measurement_likelihood;
				particle_cum_wts[particle_id] = particle_id == 0 ? particle_wts[particle_id] :
					particle_wts[particle_id] + particle_cum_wts[particle_id - 1];
				if(params.update_distr_wts){
					distr_wts[distr_id] += particle_wts[particle_id];
					distr_n_particles[distr_id] += 1;
				}
				if(particle_wts[particle_id] >= max_wt){
					max_wt = particle_wts[particle_id];
					max_wt_id = particle_id;
				}
			}
			if(params.update_distr_wts){
				double wt_sum = 0;
				//! set distribution weights to average particle weights
				for(unsigned int i = 0; i < n_distr; ++i){
					if(distr_n_particles[i]>0){
						distr_wts[i] /= distr_n_particles[i];
						wt_sum += distr_wts[i];
					}
				}
				//! normalize weights to sum to 1 while maintaining the minimum threshold
				for(unsigned int i = 0; i < n_distr; ++i){
					distr_wts[i] /= wt_sum;
					if(distr_wts[i] < params.min_distr_wt){
						distr_wts[i] = params.min_distr_wt;
					}
				}
				if(params.debug_mode){
					utils::printMatrixToFile(particle_wts.transpose(), nullptr, wts_fname, "%e");
					utils::printMatrixToFile(particle_cum_wts.transpose(), nullptr, cum_wts_fname, "%e");
					utils::printMatrixToFile(distr_n_particles.transpose(), "distr_n_particles", log_fname, "%d");
#ifndef DISABLE_GRAPH_UTILS
					VectorXf particle_wts_float = particle_wts.cast<float>();
					cv::imshow("Particle Weights", utils::drawFloatGraph(particle_wts_float.data(), params.n_particles));
#endif
				}
			}
			bool perform_resampling = true;
			if(enable_adaptive_resampling){
				double n_eff_particles = (particle_wts / particle_wts.sum()).squaredNorm();
				n_eff_particles = n_eff_particles == 0 ? 0 :
					1.0 / n_eff_particles;
				if(n_eff_particles > min_eff_particles){
					perform_resampling = false;
				}
			}
			if(perform_resampling){
				switch(params.resampling_type){
				case ResamplingType::None:
					break;
				case ResamplingType::BinaryMultinomial:
					binaryMultinomialResampling();
					break;
				case ResamplingType::LinearMultinomial:
					linearMultinomialResampling();
					break;
				case ResamplingType::Residual:
					residualResampling();
					break;
				}
			}
			switch(params.mean_type){
			case MeanType::None:
				//printf("PF:: calling setState with: \n");
				//printf("max_wt_id :%d\n", max_wt_id);
				//utils::printMatrix(particle_wts.transpose(), "particle_wts");
				//utils::printMatrix(particle_states[curr_set_id][max_wt_id].transpose(), "ssm_state");

				//! set the SSM state to that of the highest weighted particle
				ssm->setState(particle_states[curr_set_id][max_wt_id]);
				break;
			case MeanType::SSM:
				ssm->estimateMeanOfSamples(mean_state, particle_states[curr_set_id], params.n_particles);
				ssm->setState(mean_state);
				break;
			case MeanType::Corners:
				updateMeanCorners();
				ssm->setCorners(mean_corners);
			}
			double update_norm = (prev_corners - ssm->getCorners()).squaredNorm();
			prev_corners = ssm->getCorners();
			if(update_norm < params.epsilon){
				if(params.debug_mode){
					printf("n_iters: %d\n", iter_id + 1);
				}
				break;
			}
			am->clearFirstIter();
		}
		if(params.reset_to_mean){
			initializeParticles();
		}
		if(params.enable_learning){
			am->updateModel(ssm->getPts());
			max_similarity = am->getSimilarity();
		}
		ssm->getCorners(cv_corners_mat);
	}

	/**
	uses binary search to find the particle with the smallest
	index whose cumulative weight is greater than the provided
	random number supposedly drawn from a uniform distribution
	between 0 and max cumulative weight
	*/
	void PF::binaryMultinomialResampling(){
		//! change the range of the uniform distribution used for resampling instead of normalizing the weights
		//resample_dist.param(ResampleDistParamT(0, particle_cum_wts[params.n_particles - 1]));

		//! normalize the cumulative weights and leave the uniform distribution range to (0, 1]
		particle_cum_wts /= particle_cum_wts[params.n_particles - 1];
		if(params.debug_mode){
			utils::printMatrixToFile(particle_cum_wts.transpose(), "normalized particle_cum_wts", log_fname, "%e");
		}
		double max_wt = std::numeric_limits<double>::lowest();
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
			double uniform_rand_num = resample_dist(resample_gen);
			int lower_id = 0, upper_id = params.n_particles - 1;
			int resample_id = (lower_id + upper_id) / 2;
			int iter_id = 0;
			while(upper_id > lower_id){
				if(particle_cum_wts[resample_id] >= uniform_rand_num){
					upper_id = resample_id;
				} else{
					lower_id = resample_id + 1;
				}
				resample_id = (lower_id + upper_id) / 2;
				//if(params.debug_mode){
				//	printf("iter_id: %d upper_id: %d lower_id: %d resample_id: %d\n", iter_id, upper_id,
				//		lower_id, resample_id);
				//}
				++iter_id;
			}

			if(params.debug_mode){
				resample_ids[particle_id] = resample_id;
				uniform_rand_nums[particle_id] = uniform_rand_num;
			}

			//! place the resampled particle states into the other set so as not to overwrite the current one
			particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
			particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
			if(particle_wts[resample_id] >= max_wt){
				max_wt = particle_wts[resample_id];
				max_wt_id = particle_id;
			}
		}
		if(params.debug_mode){
#ifndef DISABLE_GRAPH_UTILS

			utils::getDiracHist(resample_ids_hist, resample_ids, params.n_particles);
			cv::imshow("Resampled ID Histogram", utils::drawIntGraph(resample_ids_hist.data(), resample_ids_hist.size()));
#endif
			utils::printMatrixToFile(resample_ids.transpose(), "resample_ids", log_fname, "%d");
			utils::printMatrixToFile(uniform_rand_nums.transpose(), "uniform_rand_nums", log_fname, "%e");
		}
		//! make the other particle set the current one
		curr_set_id = 1 - curr_set_id;
	}

	void PF::linearMultinomialResampling(){
		//! change the range of the uniform distribution used for resampling instead of normalizing the weights
		//resample_dist.param(ResampleDistParamT(0, particle_cum_wts[params.n_particles - 1]));

		//! normalize the cumulative weights and leave the uniform distribution range to (0, 1]
		particle_cum_wts /= particle_cum_wts[params.n_particles - 1];
		//if(params.debug_mode){
		//	utils::printMatrix(particle_cum_wts.transpose(), "normalized particle_cum_wts");
		//}
		double max_wt = std::numeric_limits<double>::lowest();
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id){
			double uniform_rand_num = resample_dist(resample_gen);
			int resample_id = 0;
			while(particle_cum_wts[resample_id] < uniform_rand_num){ ++resample_id; }

			//if(params.debug_mode){
			//	utils::printScalar(uniform_rand_num, "uniform_rand_num");
			//	utils::printScalar(resample_id, "resample_id", "%d");
			//}

			// place the resampled particle states into the other set so as not to overwrite the current one
			particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
			particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
			if(particle_wts[resample_id] >= max_wt){
				max_wt = particle_wts[resample_id];
				max_wt_id = particle_id;
			}
		}
		//! make the other particle set the current one
		curr_set_id = 1 - curr_set_id;
	}

	void PF::residualResampling() {
		// normalize the weights
		particle_wts /= particle_cum_wts[params.n_particles - 1];
		// vector of particle indies
		VectorXi particle_idx = VectorXi::LinSpaced(params.n_particles, 0, params.n_particles - 1);
		//if(params.debug_mode){
		//	utils::printMatrix(particle_wts.transpose(), "normalized particle_wts");
		//	utils::printMatrix(particle_idx.transpose(), "particle_idx", "%d");
		//}
		// sort, with highest weight first
		std::sort(particle_idx.data(), particle_idx.data() + params.n_particles - 1,
			[&](int a, int b){
			return particle_wts[a] > particle_wts[b];
		});
		//if(params.debug_mode){
		//	utils::printMatrix(particle_idx.transpose(), "sorted particle_idx", "%d");
		//}

		//! now we append
		int particles_found = 0;
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id) {
			int resample_id = particle_idx[particle_id];
			int particle_copies = static_cast<int>(round(particle_wts[resample_id] * params.n_particles));
			for(int copy_id = 0; copy_id < particle_copies; ++copy_id) {
				particle_states[1 - curr_set_id][particles_found] = particle_states[curr_set_id][resample_id];
				particle_ar[1 - curr_set_id][particles_found] = particle_ar[curr_set_id][resample_id];
				if(++particles_found == params.n_particles) { break; }
			}
			if(particles_found == params.n_particles) { break; }
		}
		int resample_id = particle_idx[0];
		for(int particle_id = particles_found; particle_id < params.n_particles; ++particle_id) {
			//! duplicate particle with highest weight to get exactly same number again
			particle_states[1 - curr_set_id][particle_id] = particle_states[curr_set_id][resample_id];
			particle_ar[1 - curr_set_id][particle_id] = particle_ar[curr_set_id][resample_id];
		}
		curr_set_id = 1 - curr_set_id;
		max_wt_id = particle_idx[0];
	}

	void PF::updateMeanCorners(){
		mean_corners.setZero();
		for(int particle_id = 0; particle_id < params.n_particles; ++particle_id) {
			// compute running average of corners corresponding to the resampled particle states
			ssm->setState(particle_states[curr_set_id][particle_id]);
			mean_corners += (ssm->getCorners() - mean_corners) / (particle_id + 1);
		}
	}

	void PF::setRegion(const cv::Mat& corners){
		ssm->setCorners(corners);
		ssm->getCorners(cv_corners_mat);
		initializeParticles();
	}
}

_MTF_END_NAMESPACE


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) { return list(x, -x); };

    BOOST_HANA_CONSTEXPR_ASSERT((list(1, 2, 3) | f) == list(1, -1, 2, -2, 3, -3));

    BOOST_HANA_CONSTEXPR_ASSERT(
        flatten(list(list(1, 2), list(3, 4), list(list(5, 6)))) == list(1, 2, 3, 4, list(5, 6))
    );
    //! [main]
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2011 Gael Guennebaud <g.gael@free.fr>
//
// 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/.

#define EIGEN_NO_DEBUG_SMALL_PRODUCT_BLOCKS
#include "sparse_solver.h"

#include <Eigen/CholmodSupport>

template<typename SparseType> void test_cholmod_ST()
{
  CholmodDecomposition<SparseType, Lower> g_chol_colmajor_lower; g_chol_colmajor_lower.setMode(CholmodSupernodalLLt);
  CholmodDecomposition<SparseType, Upper> g_chol_colmajor_upper; g_chol_colmajor_upper.setMode(CholmodSupernodalLLt);
  CholmodDecomposition<SparseType, Lower> g_llt_colmajor_lower;  g_llt_colmajor_lower.setMode(CholmodSimplicialLLt);
  CholmodDecomposition<SparseType, Upper> g_llt_colmajor_upper;  g_llt_colmajor_upper.setMode(CholmodSimplicialLLt);
  CholmodDecomposition<SparseType, Lower> g_ldlt_colmajor_lower; g_ldlt_colmajor_lower.setMode(CholmodLDLt);
  CholmodDecomposition<SparseType, Upper> g_ldlt_colmajor_upper; g_ldlt_colmajor_upper.setMode(CholmodLDLt);

  CholmodSupernodalLLT<SparseType, Lower> chol_colmajor_lower;
  CholmodSupernodalLLT<SparseType, Upper> chol_colmajor_upper;
  CholmodSimplicialLLT<SparseType, Lower> llt_colmajor_lower;
  CholmodSimplicialLLT<SparseType, Upper> llt_colmajor_upper;
  CholmodSimplicialLDLT<SparseType, Lower> ldlt_colmajor_lower;
  CholmodSimplicialLDLT<SparseType, Upper> ldlt_colmajor_upper;

  check_sparse_spd_solving(g_chol_colmajor_lower);
  check_sparse_spd_solving(g_chol_colmajor_upper);
  check_sparse_spd_solving(g_llt_colmajor_lower);
  check_sparse_spd_solving(g_llt_colmajor_upper);
  check_sparse_spd_solving(g_ldlt_colmajor_lower);
  check_sparse_spd_solving(g_ldlt_colmajor_upper);

  check_sparse_spd_solving(chol_colmajor_lower);
  check_sparse_spd_solving(chol_colmajor_upper);
  check_sparse_spd_solving(llt_colmajor_lower);
  check_sparse_spd_solving(llt_colmajor_upper);
  check_sparse_spd_solving(ldlt_colmajor_lower);
  check_sparse_spd_solving(ldlt_colmajor_upper);

  check_sparse_spd_determinant(chol_colmajor_lower);
  check_sparse_spd_determinant(chol_colmajor_upper);
  check_sparse_spd_determinant(llt_colmajor_lower);
  check_sparse_spd_determinant(llt_colmajor_upper);
  check_sparse_spd_determinant(ldlt_colmajor_lower);
  check_sparse_spd_determinant(ldlt_colmajor_upper);
}

template<typename T, int flags, typename IdxType> void test_cholmod_T()
{
    test_cholmod_ST<SparseMatrix<T, flags, IdxType> >();
}

void test_cholmod_support()
{
  CALL_SUBTEST_11( (test_cholmod_T<double              , ColMajor, int >()) );
  CALL_SUBTEST_12( (test_cholmod_T<double              , ColMajor, long>()) );
  CALL_SUBTEST_13( (test_cholmod_T<double              , RowMajor, int >()) );
  CALL_SUBTEST_14( (test_cholmod_T<double              , RowMajor, long>()) );
  CALL_SUBTEST_21( (test_cholmod_T<std::complex<double>, ColMajor, int >()) );
  CALL_SUBTEST_22( (test_cholmod_T<std::complex<double>, ColMajor, long>()) );
  // TODO complex row-major matrices do not work at the moment:
  // CALL_SUBTEST_23( (test_cholmod_T<std::complex<double>, RowMajor, int >()) );
  // CALL_SUBTEST_24( (test_cholmod_T<std::complex<double>, RowMajor, long>()) );
}


/**
 *  testReachset.cpp
 *
 *  Test reachable set computation for simplified car kinematics model:
 *  compare two ways of computing reachable set.
 *
 *  Created by Yinan Li on Oct. 29, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <cmath>
#include <array>
#include <boost/numeric/odeint.hpp>

#include "RungeKutta4.hh"
#include "src/system.hpp"
#include "src/csolver.h"

#include "car.hpp"

/* state space dim */
const int state_dim=3;
/* input space dim */
const int input_dim=2;
/* sampling time */
const double tau = 0.3;

/*
 * data types for the state space elements and input space
 * elements used in uniform grid and ode solvers
 */
using state_type = std::array<double,state_dim>;
using input_type = std::array<double,input_dim>;

/* we integrate the vehicle ode by tau sec (the result is stored in x)  */
auto  vehicle_post = [](state_type &x, const input_type &u) {
    /* the ode describing the vehicle */
    auto rhs =[](state_type& xx,  const state_type &x, const input_type &u) {
        xx[0] = u[0]*std::cos(x[2]);
        xx[1] = u[0]*std::sin(x[2]);
        xx[2] = u[1];
    };
    /* simulate (use 10 intermediate steps in the ode solver) */
    scots::runge_kutta_fixed4(rhs,x,u,state_dim,tau,10);
};

/* we integrate the growth bound by 0.3 sec (the result is stored in r)  */
auto radius_post = [](state_type &r, const state_type &, const input_type &u) {
    double c = std::abs(u[0]);
    r[0] = r[0]+c*r[2]*tau;
    r[1] = r[1]+c*r[2]*tau;
};

/* For reachable set computation */
struct twoagent {
    static const int n = 3;  // system dimension
    static const int nu = 2;  // control dimension
    rocs::ivec d{rocs::interval(-0.8, 0.8),
		 rocs::interval(-0.8, 0.8)};

    /* template constructor
     * @param[out] dx
     * @param[in] x = [xr, yr, psir]
     * @param u = [v, w]
     * @param d = [v', w']
     */
    template<typename S>
    twoagent(S *dx, const S *x, rocs::Rn u) {
	dx[0] = -u[0] + d[0]*cos(x[2]) + u[1]*x[1];
	dx[1] = d[0]*sin(x[2]) - u[1]*x[0];
	dx[2] = d[1] - u[1];
    }
};

/* For solving ode */
struct twoagent_ode {
    rocs::Rn u;
    rocs::Rn d;
    twoagent_ode (const rocs::Rn p1, const rocs::Rn p2): u(p1), d(p2){}
    /**
     * ODE model
     * @param x system state: [x,y,theta], n=3
     * @param dxdt vector field
     * @param t time
     */
    void operator() (rocs::Rn &x, rocs::Rn &dxdt, double t) const
    {
	dxdt[0] = -u[0] + d[0]*cos(x[2]) + u[1]*x[1];
	dxdt[1] = d[0]*sin(x[2]) - u[1]*x[0];
	dxdt[2] = d[1] - u[1];
    }
};

struct car_dynamics {
    rocs::Rn u;
    car_dynamics (const rocs::Rn param): u (param) {}

    void operator() (rocs::Rn &x, rocs::Rn &dxdt, double t) const
    {
	dxdt[0] = u[0]*std::cos(x[2]);
	dxdt[1] = u[0]*std::sin(x[2]);
	dxdt[2] = u[1];
    }
};

int main()
{
    // /* set the state space */
    // const double theta = 3.5;
    // double xlb[] = {0, 0, -theta};
    // double xub[] = {10, 10, theta};
    // const double eta[] = {.2,.2,.2}; /* set precision */

    // /* set the control values */
    // double ulb[] = {-1.0, -1.0};
    // double uub[] = {1.0, 1.0};
    // double mu[] = {0.3, 0.3};

    // /* define the control system */
    // rocs::DTCntlSys<carde> car("reach goal", h, carde::n, carde::m);
    // car.init_workspace(xlb, xub);
    // car.init_inputset(mu, ulb, uub);

    // /* Test */
    // state_type x_test{-1.4, 0.6, -1.9};

    // /* interval method */
    // rocs::ivec x0 = {rocs::interval(x_test[0]-eta[0]/2., x_test[0]+eta[0]/2.),
    // 		     rocs::interval(x_test[1]-eta[1]/2., x_test[1]+eta[1]/2.),
    // 		     rocs::interval(x_test[2]-eta[2]/2., x_test[2]+eta[2]/2.)};
    // std::vector<rocs::ivec> y0(car._ugrid._nv, rocs::ivec(3));
    // car.get_reach_set(y0, x0);

    // std::cout << "The initial interval: " << x0 << '\n';
    // std::cout << "The integrating time: " << tau << '\n';
    // for(size_t i = 0; i < car._ugrid._nv; ++i) {
    // 	/* nominal trajectory + growth bound */
    // 	state_type r{0.1, 0.1, 0.1};
    // 	state_type x = x_test;
    // 	input_type u{car._ugrid._data[i][0], car._ugrid._data[i][1]};
    // 	radius_post(r, x, u);
    // 	vehicle_post(x, u);
    // 	rocs::ivec y = {rocs::interval(x[0]-r[0], x[0]+r[0]),
    // 			rocs::interval(x[1]-r[1], x[1]+r[1]),
    // 			rocs::interval(x[2]-r[2], x[2]+r[2])};
    // 	std::cout << "R(t, x0, [" << car._ugrid._data[i][0] << ','
    // 		  << car._ugrid._data[i][1] << "])= ";
    // 	std::cout << y << "(gb), or " << y0[i] << "(interval).\n";
    // }

    boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;
    rocs::Rn z0{5.08561,10.1089,1.8}, z1{3.7, 10.8, 0.0};
    rocs::Rn zr(3);
    for(int i = 0; i < 3; ++i) {
	zr[i] = z1[i]-z0[i];
    }
    rocs::Rn u{0.9, -0.9}, d{0.6, 0.6/4.61};

    const double h = 0.3;  // sampling time
    const double dt = 0.001; //integration step size for odeint
    /* Integrate z0 and z1 separately */
    boost::numeric::odeint::integrate_const(rk45, car_dynamics(u), z0, 0.0, h, dt);
    boost::numeric::odeint::integrate_const(rk45, car_dynamics(d), z1, 0.0, h, dt);
    std::cout << "z1-z0 = ";
    for(int i = 0; i < 3; ++i) {
	std::cout << z1[i]-z0[i] << ' ';
    }
    std::cout << '\n';
    /* Integrate by the local model */
    boost::numeric::odeint::integrate_const(rk45, twoagent_ode(u,d), zr, 0.0, h, dt);
    std::cout << "zr = ";
    for(int i = 0; i < 3; ++i) {
	std::cout << zr[i] << ' ';
    }
    std::cout << '\n';

    return 0;
}


#ifndef __TDINDEXSET__CLASS__
#define __TDINDEXSET__CLASS__

// include for C++ sort function
#include <algorithm>

// Eigen Library
#include <Eigen/Dense>

// include base class
#include "SparseIndexSet.hpp"

#include "CONSTANTS.hpp"

class TDindexSet : public SparseIndexSet {
 public:
  // override base class methods
  void computeIndexSet(int q, const Eigen::VectorXd &w);

  const Eigen::VectorXi &get_sortW(void) const;

 protected:
  // override base class methods
  void combiIndexSet(int maxBit, int *k, double q, Eigen::VectorXi &currInd);
  int combiWeights(double q, int maxBit, int cw, int lvl);

  // new methods and variables related to weight vector
  void init_sortW(void);
  void set_w(const Eigen::VectorXd &w);

  Eigen::VectorXd _w;
  Eigen::VectorXi _sortW;
  double _sumW;
  // comparison functors for C++ sort routine
  template <typename T>
  struct myCompareInc {
    const T &m;
    myCompareInc(T &p) : m(p){};

    bool operator()(const int &i, const int &j) { return (m(i) < m(j)); }
  };
};
#endif

#if 0
template <typename T>
struct myCompareDec {
    const T &m;
    myCompareDec(T &p) : m(p){};

    bool operator()(const int &i, const int &j) { return (m(i) > m(j)); }
};
#endif