/* boost random/detail/gray_coded_qrng.hpp header file
 *
 * Copyright Justinas Vygintas Daugmaudis 2010-2018
 * 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_RANDOM_DETAIL_GRAY_CODED_QRNG_HPP
#define BOOST_RANDOM_DETAIL_GRAY_CODED_QRNG_HPP

#include <boost/random/detail/qrng_base.hpp>

#include <boost/multiprecision/integer.hpp> // lsb

#include <functional> // bit_xor

#include <boost/mpl/if.hpp>

#include <boost/integer/integer_mask.hpp>

//!\file
//!Describes the gray-coded quasi-random number generator base class template.

namespace boost {
namespace random {

namespace qrng_detail {

template<typename LatticeT>
class gray_coded_qrng
  : public qrng_base<
      gray_coded_qrng<LatticeT>
    , LatticeT
    , typename LatticeT::value_type
    >
{
public:
  typedef typename LatticeT::value_type result_type;
  typedef result_type size_type;

private:
  typedef gray_coded_qrng<LatticeT> self_t;
  typedef qrng_base<self_t, LatticeT, size_type> base_t;

  // The base needs to access modifying member f-ns, and we
  // don't want these functions to be available for the public use
  friend class qrng_base<self_t, LatticeT, size_type>;

  // Respect lattice bit_count here
  struct check_nothing {
    inline static void bit_pos(unsigned) {}
    inline static void code_size(size_type) {}
  };
  struct check_bit_range {
    static void raise_bit_count() {
      boost::throw_exception( std::range_error("gray_coded_qrng: bit_count") );
    }
    inline static void bit_pos(unsigned bit_pos) {
      if (bit_pos >= LatticeT::bit_count)
        raise_bit_count();
    }
    inline static void code_size(size_type code) {
      if (code > (self_t::max)())
        raise_bit_count();
    }
  };

  // We only want to check whether bit pos is outside the range if given bit_count
  // is narrower than the size_type, otherwise checks compile to nothing.
  BOOST_STATIC_ASSERT(LatticeT::bit_count <= std::numeric_limits<size_type>::digits);

  typedef typename mpl::if_c<
      ((LatticeT::bit_count) < std::numeric_limits<size_type>::digits)
    , check_bit_range
    , check_nothing
  >::type check_bit_range_t;

public:
  //!Returns: Tight lower bound on the set of values returned by operator().
  //!
  //!Throws: nothing.
  static BOOST_CONSTEXPR result_type min BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return 0; }

  //!Returns: Tight upper bound on the set of values returned by operator().
  //!
  //!Throws: nothing.
  static BOOST_CONSTEXPR result_type max BOOST_PREVENT_MACRO_SUBSTITUTION ()
  { return low_bits_mask_t<LatticeT::bit_count>::sig_bits; }

  explicit gray_coded_qrng(std::size_t dimension)
    : base_t(dimension)
  {}

  // default copy c-tor is fine

  // default assignment operator is fine

  void seed()
  {
    set_zero_state();
    update_quasi(0);
    base_t::reset_seq(0);
  }

  void seed(const size_type init)
  {
    if (init != this->curr_seq())
    {
      // We don't want negative seeds.
      check_seed_sign(init);

      size_type seq_code = boost::next(init);
      if (BOOST_UNLIKELY(!(init < seq_code)))
        boost::throw_exception( std::range_error("gray_coded_qrng: seed") );

      seq_code ^= (seq_code >> 1);
      // Fail if we see that seq_code is outside bit range.
      // We do that before we even touch engine state.
      check_bit_range_t::code_size(seq_code);

      set_zero_state();
      for (unsigned r = 0; seq_code != 0; ++r, seq_code >>= 1)
      {
        if (seq_code & static_cast<size_type>(1))
          update_quasi(r);
      }
    }
    // Everything went well, set the new seq count
    base_t::reset_seq(init);
  }

private:
  void compute_seq(size_type seq)
  {
    // Find the position of the least-significant zero in sequence count.
    // This is the bit that changes in the Gray-code representation as
    // the count is advanced.
    // Xor'ing with max() has the effect of flipping all the bits in seq,
    // except for the sign bit.
    unsigned r = multiprecision::lsb(seq ^ (self_t::max)());
    check_bit_range_t::bit_pos(r);
    update_quasi(r);
  }

  void update_quasi(unsigned r)
  {
    // Calculate the next state.
    std::transform(this->state_begin(), this->state_end(),
      this->lattice.iter_at(r * this->dimension()), this->state_begin(),
      std::bit_xor<result_type>());
  }

  void set_zero_state()
  {
    std::fill(this->state_begin(), this->state_end(), result_type /*zero*/ ());
  }
};

} // namespace qrng_detail

} // namespace random
} // namespace boost

#endif // BOOST_RANDOM_DETAIL_GRAY_CODED_QRNG_HPP


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-reduction
    This function object returns `all(x == y)`,
    i.e. return @c true if and only if all corresponding entries elements are equal.

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

    @par Example:

      @snippet compare_equal.cpp compare_equal

    @par Possible output:

      @snippet compare_equal.txt compare_equal

  **/
  bool compare_equal(Value const& x, Value const& y);
} }
#endif

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

#endif


#include <boost/iterator_adaptors.hpp>

template <typename ForwardIterator,
          typename Predicate>
ForwardIterator
first_max_element_if(ForwardIter first,
                     ForwardIter last,
                     Predicate pred)
{
    return first_max_element(
        boost::make_filter_iterator(first, last,
                                    pred),
        boost::make_filter_iterator(last, last,
                                    pred)
    );
}


#include "WDPConfig.h"

#include "wdp/Depth.h"

#include <boost/gil/extension/io/jpeg_io.hpp>
#include <boost/gil/typedefs.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/positional_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include <cmath>
#include <experimental/filesystem>
#include <fstream>
#include <iostream>

#define STR(X) #X
#define XSTR(X) STR(X)

namespace bpo = boost::program_options;
namespace bg = boost::gil;

namespace fs = std::experimental::filesystem;

bool approxEqual(double a, double b) { return std::abs(a - b) < 25.0; }

int main(int argc, char **argv) {
  std::vector<std::string> Tests;

  // Declare program options.
  bpo::options_description Desc("Depth perception application options");
  Desc.add_options()("help", "produce help message")(
      "all", "run all tests in test directory")(
      "tests", bpo::value<std::vector<std::string>>(&Tests),
      "which tests to run");

  bpo::variables_map VM;
  bpo::store(bpo::command_line_parser(argc, argv).options(Desc).run(), VM);
  bpo::notify(VM);

  if (VM.count("help")) {
    std::cout << Desc << std::endl;
    return 1;
  }

  if (VM.count("all")) {
    if (VM.count("tests")) {
      std::cout << "All tests specified but --tests provided" << std::endl;
      return 1;
    }
    for (const auto &File : fs::directory_iterator(XSTR(WDP_TEST_DIR)))
      Tests.push_back(File.path().filename());
  } else if (!VM.count("tests")) {
    std::cout << "No tests specified - see help for details" << std::endl;
    return 1;
  }

  wdp::Radii NeighbourhoodRadii(15, 15);
  wdp::Offset SearchOffset(-250, 0);
  wdp::Radii SearchRadii(500, 250);

  wdp::SearchParameters SearchParams = {NeighbourhoodRadii, SearchOffset,
                                        SearchRadii, 1000000UL};

  wdp::DepthParameters DepthParams = {24.0, 0.00694, 100.0};

  for (const std::string &TestName : Tests) {
    auto LHSPath = fs::path(XSTR(WDP_DATA_DIR)) / TestName / "lhs.jpg";
    auto RHSPath = fs::path(XSTR(WDP_DATA_DIR)) / TestName / "rhs.jpg";
    bg::rgb8_image_t LHSImg;
    bg::rgb8_image_t RHSImg;
    bg::jpeg_read_image(LHSPath, LHSImg);
    bg::jpeg_read_image(RHSPath, RHSImg);

    auto TestPath = fs::path(XSTR(WDP_TEST_DIR)) / TestName;
    std::fstream TestFS(TestPath);

    std::size_t X, Y;
    double ExpectedDepth;
    while (TestFS >> X >> Y >> ExpectedDepth) {
      wdp::Coordinates C(X, Y);
      double Depth =
          wdp::getDepth(LHSImg, RHSImg, C, SearchParams, DepthParams);
      if (!approxEqual(Depth, ExpectedDepth))
        std::cout << "Failed depth check, expected " << ExpectedDepth << " at ["
                  << X << ", " << Y << "], got " << Depth << std::endl;
    }
  }

  return 0;
}


/*

This runs the full visual odometry system.

Changelog:
    awadhut - 10/02 - Initial commit
    subbu   - 10/02 - Adding Eigen, camera plotter 3D toy
    subbu   - 10/08 - Adding data set parsing
    subbu   - 10/20 - adding reprojector
    subbu   - 11/19 - 3d threaded viewer
*/

// C++ Standard headers
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <thread>
#include <pangolin/pangolin.h>

// Custom headers
#include "frame.h"
#include "utils.h"
#include "line.h"
#include "viewer.h"
#include "ransac.h"

int main(int argc, char* argv[])
{
  Eigen::initParallel();

  // Start processing the data
  std::cout << "Starting system .." << std::endl;
  cout << "Loading data ..." << endl;

  // Loading the data (TUMfr1)
  string data_dir     = "../data/rgbd_dataset_freiburg1_xyz/";
  string synched_file = "synched_data.txt";

  string window_name = "ROBust LINE Visual Odometry";;
  viewer* robline_viewer = new viewer(window_name);
  std::thread render_thread(&viewer::run, robline_viewer);

  string image_pair_path, rgb_path, depth_path, rgb_time, depth_time;
  ifstream infile(data_dir + synched_file);

  bool isFirst = true;
  cv::Mat previous_rgb, previous_depth;

  // Vector to hold objects of FramePair class
  // Each object contains lines found in both images and matches between the frames
  std::vector<FramePair*> pairs;

  while(getline(infile, image_pair_path)){
    std::stringstream linestream(image_pair_path);
    linestream >> rgb_time >> rgb_path >> depth_time >> depth_path;

    cv::Mat rgb_img   = utils::ReadImage(data_dir + rgb_path, true);
    cv::Mat depth_img = utils::ReadImage(data_dir + depth_path, false);

    if (isFirst)
    {
      previous_rgb = rgb_img;
      previous_depth = depth_img;
      isFirst = false;
    }

    else
    {
      // Detect lines, match lines between frames, sample 2d line, reproject to 3d, remove outliers
      FramePair* fpair = new FramePair(previous_rgb, previous_depth, rgb_img, depth_img);
      // pairs.push_back(fpair);

      utils::DrawSampledLines2D(fpair->rgb_image1, fpair->sampled_lines_2d_im1);
      utils::DrawSampledLines2D(fpair->depth_image1, fpair->sampled_lines_2d_im1);
      utils::DisplayImage(fpair->rgb_image1);
      // Update the current vieweing frame
      robline_viewer->updateCurrentFrame(fpair);

      cv::waitKey(10);

      // Update previous frame
      previous_rgb = rgb_img;
      previous_depth = depth_img;
    }
  }

  // close file
  infile.close();

  // Close viewer completely
  while(!robline_viewer->hasViewerStopped())
    robline_viewer->requestViewerStop();
  // Kill thread
  render_thread.join();
  pangolin::DestroyWindow(window_name);

  return 0;
}


#include <3dUnitDriver.hpp>
#include <3dUnitTypeSerialization.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>

int main(int argc, char *argv[])
{
	if (argc != 4)
	{
		std::cout << "usage:\n";
		std::cout << argv[0] <<" rawMeasurmentInput Output cfgfile";
		return 1;
	}
	std::string fnIn;
	std::string fnOut;
	std::string fnCfg;

	if (argc == 4)
	{
		fnIn = argv[1];
		fnOut = argv[2];
		fnCfg = argv[3];
	}
	using boost::property_tree::ptree;
	ptree pt;
	std::cout <<"opening raw file \n";

	read_xml(fnIn, pt);
	m3d::rawPointcloud raw;
	m3d::typeSerialization::deserialize(pt,raw,"raw1");

	std::cout <<"opening 3dunit cfg file \n";
	m3d::_3dUnitConfig cfg;
	cfg.readConfigFromXML(fnCfg);
	std::cout <<"opening transfering pc \n";
	m3d::pointcloud normalPc;

	m3d::transferPc (raw, normalPc, cfg);
	m3d::typeSerialization::saveTXT(fnOut, normalPc);
	return 0;
}


#define BOOST_TEST_MODULE pcraster pcrxml bin_double_le
#include <boost/test/unit_test.hpp>
#include "pcrxml_bindoublele.h"

BOOST_AUTO_TEST_CASE(encoding)
{
  using namespace pcrxml;

  {
    BinDoubleLE v(0);
    BOOST_CHECK(v.attrValueStr() == "0000000000000000");
  }
  {
    BinDoubleLE v(1234.34);
    double cmpV = v();
    BOOST_CHECK(cmpV == BinDoubleLE::hexToDouble(v.attrValueStr()));
  }
  {
    BinDoubleLE v(0.001);
    double cmpV = v();
    BOOST_CHECK(cmpV == BinDoubleLE::hexToDouble(v.attrValueStr()));
    BOOST_CHECK(cmpV == 0.001);
  }
}


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.

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

#ifndef BOOST_GEOMETRY_INDEX_TEST_INTERSECTION_CONTENT_HPP
#define BOOST_GEOMETRY_INDEX_TEST_INTERSECTION_CONTENT_HPP

#include <geometry_index_test_common.hpp>

#include <boost/geometry/index/detail/algorithms/intersection_content.hpp>

template <typename Geometry>
void test_intersection_content(Geometry const& geometry1, Geometry const& geometry2,
            typename bgi::detail::default_content_result<Geometry>::type expected_value)
{
    typename bgi::detail::default_content_result<Geometry>::type value = bgi::detail::intersection_content(geometry1, geometry2);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry>::type).name()
        << " "
        << typeid(typename bgi::detail::default_content_result<Geometry>::type).name()
        << " "
        << "intersection_content : " << value
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(value, expected_value, 0.0001);
}

template <typename Geometry>
void test_geometry(std::string const& wkt1, std::string const& wkt2,
            typename bgi::detail::default_content_result<Geometry>::type expected_value)
{
    Geometry geometry1, geometry2;
    bg::read_wkt(wkt1, geometry1);
    bg::read_wkt(wkt2, geometry2);
    test_intersection_content(geometry1, geometry2, expected_value);
}

#endif


//==================================================================================================
/*!
  @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_CTZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CTZ_HPP_INCLUDED

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

 /*!

    @ingroup group-bitwise
    Function object implementing ctz capabilities

    Returns the bit count of trailing zeroes

    Finds the first bit set in x, and returns the index of that bit, 0 being
    the least significant bit index.
    @ ctz assert is the entry is 0

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

    @code
    as_integer_t<T> r = ctz(x);
    @endcode

    @see  clz, popcnt
  **/
  const boost::dispatch::functor<tag::ctz_> ctz = {};
} }
#endif

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

#endif


/* boost random/uniform_smallint.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_smallint.hpp 52492 2009-04-19 14:55:57Z steven_watanabe $
 *
 * Revision history
 *  2001-04-08  added min<max assertion (N. Becker)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_SMALLINT_HPP
#define BOOST_RANDOM_UNIFORM_SMALLINT_HPP

#include <cassert>
#include <iostream>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/detail/workaround.hpp>

namespace boost {

// uniform integer distribution on a small range [min, max]

template<class IntType = int>
class uniform_smallint
{
public:
  typedef IntType input_type;
  typedef IntType result_type;

  explicit uniform_smallint(IntType min_arg = 0, IntType max_arg = 9)
    : _min(min_arg), _max(max_arg)
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
    BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
#endif
 }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _min; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _max; }
  void reset() { }

  template<class Engine>
  result_type operator()(Engine& eng)
  {
    typedef typename Engine::result_type base_result;
    base_result _range = static_cast<base_result>(_max-_min)+1;
    base_result _factor = 1;

    // LCGs get bad when only taking the low bits.
    // (probably put this logic into a partial template specialization)
    // Check how many low bits we can ignore before we get too much
    // quantization error.
    base_result r_base = (eng.max)() - (eng.min)();
    if(r_base == (std::numeric_limits<base_result>::max)()) {
      _factor = 2;
      r_base /= 2;
    }
    r_base += 1;
    if(r_base % _range == 0) {
      // No quantization effects, good
      _factor = r_base / _range;
    } else {
      // carefully avoid overflow; pessimizing here
      for( ; r_base/_range/32 >= _range; _factor *= 2)
        r_base /= 2;
    }

    return ((eng() - (eng.min)()) / _factor) % _range + _min;
  }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_smallint& ud)
  {
    os << ud._min << " " << ud._max;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_smallint& ud)
  {
    is >> std::ws >> ud._min >> std::ws >> ud._max;
    return is;
  }
#endif

private:

  result_type _min;
  result_type _max;
};

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_SMALLINT_HPP


//==============================================================================
//         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
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEEV_W_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_LAPACK_GENERAL_GEEV_W_HPP_INCLUDED

#include <nt2/linalg/functions/geev_w.hpp>
#include <nt2/linalg/details/lapack/declare/geev.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/scalar/max.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/linalg/details/utility/envblocksize.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/linalg/details/utility/workspace.hpp>
#include <nt2/linalg/functions/details/combine.hpp>
#include <nt2/core/container/table/table.hpp>

namespace nt2 { namespace ext
{

  // the routines here compute only the eigenvalues and always in a complex colon vector
  //---------------------------------------------Real-double- w------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A2)(S2)
                            , ((container_<nt2::tag::table_,  double_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //w
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A2& w) const
    {
      char jobvl = 'N';
      char jobvr = 'N';
       result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvr = 1;
      nt2_la_int ldvl = 1;
      NT2_F77NAME(dgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0/*wr*/, 0/*wi*/
                         , 0/*vl*/, &ldvl
                         , 0/*vr*/, &ldvr
                         , wk.main(), details::query()
                         , &info);
      info = nt2::geev_w(a, w, wk);
      return info;
    }
  };

  //---------------------------------------------Real-single- w------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A2)(S2)
                            , ((container_<nt2::tag::table_,  single_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //w
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A2& w) const
    {
      char jobvl = 'N';
      char jobvr = 'N';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvr = 1;
      nt2_la_int ldvl = 1;
      NT2_F77NAME(sgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0/*wr*/, 0/*wi*/
                         , 0/*vl*/, &ldvl
                         , 0/*vr*/, &ldvr
                         , wk.main(), details::query()
                         , &info);
      info = nt2::geev_w(a, w, wk);
      return info;
     }
  };

//---------------------------------------------Complex-single- w------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A1)(S1)
                            , ((container_<nt2::tag::table_,  complex_<single_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w) const
    {
      char jobvl = 'N';
      char jobvr = 'N';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvl = 1;
      nt2_la_int ldvr = 1;
      NT2_F77NAME(cgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0 /*w*/
                         , 0 /*vl*/, &ldvl
                         , 0 /*vr*/, &ldvr
                         , wk.main(), details::query()
                         , wk.reals()
                         , &info);
     info = nt2::geev_w(a, w, wk);
      return info;
     }
  };

  //---------------------------------------------Complex-double- w------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A1)(S1)
                            , ((container_<nt2::tag::table_,  complex_<double_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w) const
    {
      char jobvl = 'N';
      char jobvr = 'N';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvl = 1;
      nt2_la_int ldvr = 1;
      NT2_F77NAME(zgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0 /*w*/
                         , 0 /*vl*/, &ldvl
                         , 0 /*vr*/, &ldvr
                         , wk.main(), details::query()
                         , wk.reals()
                         , &info);
      info = nt2::geev_w(a, w, wk);
      return info;
     }
  };

  //---------------------------------------------Real-double- w------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(WK)
                            , ((container_<nt2::tag::table_,  double_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              (unspecified_<WK>)                                              //workspace
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, WK& wk) const
    {
      char jobvl = 'N';
      char jobvr = 'N';
      result_type info;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2::container::table<double> wr(of_size(n, 1)),  wi(of_size(n, 1));
      nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int wn = wk.main_need();
      wk.resize_main(wn);
      nt2_la_int ldvl = 1;
      nt2_la_int ldvr = 1;
      NT2_F77NAME(dgeev) ( &jobvl, &jobvr
                         , &n
                         , a.data() , &lda
                         , wr.data(), wi.data()
                         , 0, &ldvl
                         , 0, &ldvr
                         , wk.main(), &wn
                         , &info);
      combine_eigens(wr, wi, w);
      return info;
    }
  };

  //---------------------------------------------Real-single- w------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(WK)
                            , ((container_<nt2::tag::table_,  single_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              (unspecified_<WK>)                                              //workspace
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, WK& wk) const
    {
      char jobvl = 'N';
      char jobvr = 'N';
      result_type info;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2::container::table<float> wr(of_size(n, 1)),  wi(of_size(n, 1));
      nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int wn = wk.main_need();
      wk.resize_main(wn);
      nt2_la_int ldvl = 1;
      nt2_la_int ldvr = 1;
      NT2_F77NAME(sgeev) ( &jobvl, &jobvr
                         , &n
                         , a.data() , &lda
                         , wr.data(), wi.data()
                         , 0, &ldvl
                         , 0, &ldvr
                         , wk.main(), &wn
                         , &info);
      combine_eigens(wr, wi, w);
      return info;
    }
  };

  //---------------------------------------------Complex-single- w------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(WK)
                            , ((container_<nt2::tag::table_,  complex_<single_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              (unspecified_<WK>)                                              //workspace
                            )
  {
     typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, WK& wk) const
     {
       char jobvl = 'N';
       char jobvr = 'N';
       result_type info;
       nt2_la_int n = nt2::width(a);
       BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
       nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
       nt2_la_int ldvl = 1;
       nt2_la_int ldvr = 1;
       nt2_la_int wn = wk.main_need();
       wk.resize_main(wn);
       wk.resize_reals(2*n);
       w.resize(of_size(n, 1));
       NT2_F77NAME(cgeev) ( &jobvl, &jobvr
                          , &n
                          , a.data() , &lda
                          , w.data()
                          , 0, &ldvl
                          , 0, &ldvr
                          , wk.main(), &wn
                          , wk.reals()
                          , &info);
        return info;
     }
  };

  //---------------------------------------------Complex-double- w------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_w_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(WK)
                            , ((container_<nt2::tag::table_,  complex_<double_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              (unspecified_<WK>)                                              //workspace
                            )
  {
     typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, WK& wk) const
     {
       char jobvl = 'N';
       char jobvr = 'N';
       result_type info;
       nt2_la_int n = nt2::width(a);
       BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
       nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
       nt2_la_int ldvl = 1;
       nt2_la_int ldvr = 1;
       nt2_la_int wn = wk.main_need();
       wk.resize_main(wn);
       wk.resize_reals(2*n);
       w.resize(of_size(n, 1));
       NT2_F77NAME(zgeev) ( &jobvl, &jobvr
                          , &n
                          , a.data() , &lda
                          , w.data()
                          , 0, &ldvl
                          , 0, &ldvr
                          , wk.main(), &wn
                          , wk.reals()
                          , &info);
        return info;
     }
  };
} }

#endif


//==============================================================================
//         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_COMMON_DIVIDES_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_SIMD_COMMON_DIVIDES_HPP_INCLUDED
#include <boost/simd/operator/functions/divides.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
// TODO are these necessary ?
//////////////////////////////////////////////////////////////////////////////
// mixed scalar/simd divides
//////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divides_, tag::cpu_,
                                    (A0)(A1)(X),
                                    ((simd_<arithmetic_ <A0>,X>))((scalar_<arithmetic_<A1> >))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return boost::simd::divides(a0, boost::simd::splat<A0>(a1));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divides_, tag::cpu_,
                                     (A0)(A1)(X),
                                     ((scalar_<arithmetic_<A0> >))((simd_<arithmetic_<A1>,X>))
                                   )
  {
    typedef A1 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return boost::simd::divides(boost::simd::splat<A1>(a0), a1);
    }
  };
} } }

#endif


#ifndef DSST_TRACKER_HPP_
#define DSST_TRACKER_HPP_

#include <boost/python.hpp>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/traits.hpp>
#include <memory>
#include <iostream>
#include <fstream>

#include "cv_ext.hpp"
#include "mat_consts.hpp"
#include "feature_channels.hpp"
#include "gradientMex.hpp"
#include "math_helper.hpp"
#include "scale_estimator.hpp"
#include "psr.hpp"
#include "gil.hpp"

using namespace boost::python;

namespace cf_tracking
{
	struct DsstParameters
	{
		double padding = static_cast<double>(1.6);
		double outputSigmaFactor = static_cast<double>(0.05);
		double lambda = static_cast<double>(0.01);
		double learningRate = static_cast<double>(0.012);
		int templateSize = 100;
		int cellSize = 2;

		bool enableTrackingLossDetection = false;
		double psrThreshold = 13.5;
		int psrPeakDel = 1;

		bool enableScaleEstimator = true;
		double scaleSigmaFactor = static_cast<double>(0.25);
		double scaleStep = static_cast<double>(1.02);
		int scaleCellSize = 4;
		int numberOfScales = 33;

		//testing
		bool originalVersion = false;
		int resizeType = cv::INTER_LINEAR;
		bool useFhogTranspose = false;
		double minArea = static_cast<double>(10);
		double maxAreaFactor = static_cast<double>(0.8);
		bool useCcs = true;
	};

	class DsstTracker
	{
	public:
		typedef double T; // set precision here double or float
		static const int CV_TYPE = cv::DataType<T>::type;
		typedef cv::Size_<T> Size;
		typedef cv::Point_<T> Point;
		typedef cv::Rect_<T> Rect;
		typedef FhogFeatureChannels<T> FFC;
		typedef DsstFeatureChannels<T> DFC;
		typedef mat_consts::constants<T> consts;
		typedef boost::python::tuple Tuple;

		DsstTracker(DsstParameters paras)
			: _isInitialized(false),
			_scaleEstimator(0),
			_PADDING(static_cast<T>(paras.padding)),
			_OUTPUT_SIGMA_FACTOR(static_cast<T>(paras.outputSigmaFactor)),
			_LAMBDA(static_cast<T>(paras.lambda)),
			_LEARNING_RATE(static_cast<T>(paras.learningRate)),
			_CELL_SIZE(paras.cellSize),
			_TEMPLATE_SIZE(paras.templateSize),
			_PSR_THRESHOLD(static_cast<T>(paras.psrThreshold)),
			_PSR_PEAK_DEL(paras.psrPeakDel),
			_MIN_AREA(static_cast<T>(paras.minArea)),
			_MAX_AREA_FACTOR(static_cast<T>(paras.maxAreaFactor)),
			_ENABLE_TRACKING_LOSS_DETECTION(paras.enableTrackingLossDetection),
			_ORIGINAL_VERSION(paras.originalVersion),
			_RESIZE_TYPE(paras.resizeType),
			_USE_CCS(paras.useCcs)
		{
			if (paras.enableScaleEstimator)
			{
				ScaleEstimatorParas<T> sp;
				sp.scaleCellSize = paras.scaleCellSize;
				sp.scaleStep = static_cast<T>(paras.scaleStep);
				sp.numberOfScales = paras.numberOfScales;
				sp.scaleSigmaFactor = static_cast<T>(paras.scaleSigmaFactor);
				sp.lambda = static_cast<T>(paras.lambda);
				sp.learningRate = static_cast<T>(paras.learningRate);
				sp.useFhogTranspose = paras.useFhogTranspose;
				sp.resizeType = paras.resizeType;
				sp.originalVersion = paras.originalVersion;
				_scaleEstimator = new ScaleEstimator<T>(sp);
			}

			if (paras.useFhogTranspose)
				cvFhog = &piotr::cvFhogT < T, DFC >;
			else
				cvFhog = &piotr::cvFhog < T, DFC >;

			if (_USE_CCS)
				calcDft = &cf_tracking::dftCcs;
			else
				calcDft = &cf_tracking::dftNoCcs;

			// init dft
			cv::Mat initDft = (cv::Mat_<T>(1, 1) << 1);
			calcDft(initDft, initDft, 0);
		}

		~DsstTracker()
		{
			delete _scaleEstimator;
		}

		bool reinit(const cv::Mat& image, Tuple boundingBox)
		{
			releaseGIL unlock;

			Rect bb = Rect(
				extract<T>(boundingBox[0]),
				extract<T>(boundingBox[1]),
				extract<T>(boundingBox[2]),
				extract<T>(boundingBox[3])
				);

			return reinit_(image, bb);
		}

		bool update(const cv::Mat& image)
		{
			releaseGIL unlock;

			Rect bb = Rect(
				position[0],
				position[1],
				position[2],
				position[3]
				);

			if (update_(image, bb) == false)
				return false;

			position[0] = static_cast<T>(bb.x);
			position[1] = static_cast<T>(bb.y);
			position[2] = static_cast<T>(bb.width);
			position[3] = static_cast<T>(bb.height);
			return true;
		}

		bool updateAt(const cv::Mat& image, Tuple boundingBox)
		{
			releaseGIL unlock;

			bool isValid = false;

			Rect bb = Rect(
				extract<T>(boundingBox[0]),
				extract<T>(boundingBox[1]),
				extract<T>(boundingBox[2]),
				extract<T>(boundingBox[3])
				);

			isValid = updateAt_(image, bb);

			position[0] = static_cast<T>(bb.x);
			position[1] = static_cast<T>(bb.y);
			position[2] = static_cast<T>(bb.width);
			position[3] = static_cast<T>(bb.height);

			return isValid;
		}

		Tuple getBoundingBox()
		{
			return boost::python::make_tuple(position[0], position[1], position[2], position[3]);
		}

		Tuple getCenter()
		{
			return boost::python::make_tuple(position[0] + position[2] / 2, position[1] + position[3] / 2);
		}

	private:
		DsstTracker& operator=(const DsstTracker&)
		{}

		bool reinit_(const cv::Mat& image, Rect& boundingBox)
		{
			_pos.x = floor(boundingBox.x) + floor(boundingBox.width * consts::c0_5);
			_pos.y = floor(boundingBox.y) + floor(boundingBox.height * consts::c0_5);
			Size targetSize = Size(boundingBox.width, boundingBox.height);

			_templateSz = Size(floor(targetSize.width * (1 + _PADDING)),
				floor(targetSize.height * (1 + _PADDING)));

			_scale = 1.0;

			if (!_ORIGINAL_VERSION)
			{
				// resize to fixed side length _TEMPLATE_SIZE to stabilize FPS
				if (_templateSz.height > _templateSz.width)
					_scale = _templateSz.height / _TEMPLATE_SIZE;
				else
					_scale = _templateSz.width / _TEMPLATE_SIZE;

				_templateSz = Size(floor(_templateSz.width / _scale), floor(_templateSz.height / _scale));
			}

			_baseTargetSz = Size(targetSize.width / _scale, targetSize.height / _scale);
			_templateScaleFactor = 1 / _scale;

			Size templateSzByCells = Size(floor((_templateSz.width) / _CELL_SIZE),
				floor((_templateSz.height) / _CELL_SIZE));

			// translation filter output target
			T outputSigma = sqrt(_templateSz.area() / ((1 + _PADDING) * (1 + _PADDING)))
				* _OUTPUT_SIGMA_FACTOR / _CELL_SIZE;
			_y = gaussianShapedLabels2D<T>(outputSigma, templateSzByCells);
			calcDft(_y, _yf, 0);

			// translation filter hann window
			cv::Mat cosWindowX;
			cv::Mat cosWindowY;
			cosWindowY = hanningWindow<T>(_yf.rows);
			cosWindowX = hanningWindow<T>(_yf.cols);
			_cosWindow = cosWindowY * cosWindowX.t();

			std::shared_ptr<DFC> hfNum(0);
			cv::Mat hfDen;

			if (getTranslationTrainingData(image, hfNum, hfDen, _pos) == false)
				return false;

			_hfNumerator = hfNum;
			_hfDenominator = hfDen;

			if (_scaleEstimator)
			{
				_scaleEstimator->reinit(image, _pos, targetSize,
					_scale * _templateScaleFactor);
			}

			_lastBoundingBox = boundingBox;
			_isInitialized = true;
			return true;
		}

		bool getTranslationTrainingData(const cv::Mat& image, std::shared_ptr<DFC>& hfNum,
			cv::Mat& hfDen, const Point& pos) const
		{
			std::shared_ptr<DFC> xt(0);

			if (getTranslationFeatures(image, xt, pos, _scale) == false)
				return false;

			std::shared_ptr<DFC> xtf;

			if (_USE_CCS)
				xtf = DFC::dftFeatures(xt);
			else
				xtf = DFC::dftFeatures(xt, cv::DFT_COMPLEX_OUTPUT);

			hfNum = DFC::mulSpectrumsFeatures(_yf, xtf, true);
			hfDen = DFC::sumFeatures(DFC::mulSpectrumsFeatures(xtf, xtf, true));

			return true;
		}

		bool getTranslationFeatures(const cv::Mat& image, std::shared_ptr<DFC>& features,
			const Point& pos, T scale) const
		{
			cv::Mat patch;
			Size patchSize = _templateSz * scale;

			if (getSubWindow(image, patch, patchSize, pos) == false)
				return false;

			if (_ORIGINAL_VERSION)
				depResize(patch, patch, _templateSz);
			else
				resize(patch, patch, _templateSz, 0, 0, _RESIZE_TYPE);

			cv::Mat floatPatch;
			patch.convertTo(floatPatch, CV_32FC(3));

			features.reset(new DFC());
			cvFhog(floatPatch, features, _CELL_SIZE, DFC::numberOfChannels() - 1);

			// append gray-scale image
			if (patch.channels() == 1)
			{
				if (_CELL_SIZE != 1)
					resize(patch, patch, features->channels[0].size(), 0, 0, _RESIZE_TYPE);

				features->channels[DFC::numberOfChannels() - 1] = patch / 255.0 - 0.5;
			}
			else
			{
				if (_CELL_SIZE != 1)
					resize(patch, patch, features->channels[0].size(), 0, 0, _RESIZE_TYPE);

				cv::Mat grayFrame;
				cvtColor(patch, grayFrame, cv::COLOR_BGR2GRAY);
				grayFrame.convertTo(grayFrame, CV_TYPE);
				grayFrame = grayFrame / 255.0 - 0.5;
				features->channels[DFC::numberOfChannels() - 1] = grayFrame;
			}

			DFC::mulFeatures(features, _cosWindow);
			return true;
		}

		bool update_(const cv::Mat& image, Rect& boundingBox)
		{
			return updateAtScalePos(image, _pos, _scale, boundingBox);
		}

		bool updateAt_(const cv::Mat& image, Rect& boundingBox)
		{
			bool isValid = false;
			T scale = 0;
			Point pos(boundingBox.x + boundingBox.width * consts::c0_5,
				boundingBox.y + boundingBox.height * consts::c0_5);

			// caller's box may have a different aspect ratio
			// compared to the _targetSize; use the larger side
			// to calculate scale
			if (boundingBox.width > boundingBox.height)
				scale = boundingBox.width / _baseTargetSz.width;
			else
				scale = boundingBox.height / _baseTargetSz.height;

			isValid = updateAtScalePos(image, pos, scale, boundingBox);
			return isValid;
		}

		bool updateAtScalePos(const cv::Mat& image, const Point& oldPos, const T oldScale,
			Rect& boundingBox)
		{
			++_frameIdx;

			if (!_isInitialized)
				return false;

			T newScale = oldScale;
			Point newPos = oldPos;
			cv::Point2i maxResponseIdx;
			cv::Mat response;

			// in case of error return the last box
			boundingBox = _lastBoundingBox;

			if (detectModel(image, response, maxResponseIdx, newPos, newScale) == false)
				return false;

			// return box
			Rect tempBoundingBox;
			tempBoundingBox.width = _baseTargetSz.width * newScale;
			tempBoundingBox.height = _baseTargetSz.height * newScale;
			tempBoundingBox.x = newPos.x - tempBoundingBox.width / 2;
			tempBoundingBox.y = newPos.y - tempBoundingBox.height / 2;

			if (_ENABLE_TRACKING_LOSS_DETECTION)
			{
				if (evalReponse(image, response, maxResponseIdx,
					tempBoundingBox) == false)
					return false;
			}

			if (updateModel(image, newPos, newScale) == false)
				return false;

			boundingBox &= Rect(0, 0, static_cast<T>(image.cols), static_cast<T>(image.rows));
			boundingBox = tempBoundingBox;
			_lastBoundingBox = tempBoundingBox;
			return true;
		}

		bool evalReponse(const cv::Mat &image, const cv::Mat& response,
			const cv::Point2i& maxResponseIdx,
			const Rect& tempBoundingBox) const
		{
			T peakValue = 0;
			T psrClamped = calcPsr(response, maxResponseIdx, _PSR_PEAK_DEL, peakValue);

			if (psrClamped < _PSR_THRESHOLD)
				return false;

			// check if we are out of image, too small or too large
			Rect imageRect(Point(0, 0), image.size());
			Rect intersection = imageRect & tempBoundingBox;
			double  bbArea = tempBoundingBox.area();
			double areaThreshold = _MAX_AREA_FACTOR * imageRect.area();
			double intersectDiff = std::abs(bbArea - intersection.area());

			if (intersectDiff > 0.01 || bbArea < _MIN_AREA
				|| bbArea > areaThreshold)
				return false;

			return true;
		}

		bool detectModel(const cv::Mat& image, cv::Mat& response,
			cv::Point2i& maxResponseIdx, Point& newPos,
			T& newScale) const
		{
			// find translation
			std::shared_ptr<DFC> xt(0);

			if (getTranslationFeatures(image, xt, newPos, newScale) == false)
				return false;

			std::shared_ptr<DFC> xtf;
			if (_USE_CCS)
				xtf = DFC::dftFeatures(xt);
			else
				xtf = DFC::dftFeatures(xt, cv::DFT_COMPLEX_OUTPUT);

			std::shared_ptr<DFC> sampleSpec = DFC::mulSpectrumsFeatures(_hfNumerator, xtf, false);
			cv::Mat sumXtf = DFC::sumFeatures(sampleSpec);
			cv::Mat hfDenLambda = addRealToSpectrum<T>(_LAMBDA, _hfDenominator);
			cv::Mat responseTf;

			if (_USE_CCS)
				divSpectrums(sumXtf, hfDenLambda, responseTf, 0, false);
			else
				divideSpectrumsNoCcs<T>(sumXtf, hfDenLambda, responseTf);

			cv::Mat translationResponse;
			idft(responseTf, translationResponse, cv::DFT_REAL_OUTPUT | cv::DFT_SCALE);

			cv::Point delta;
			double maxResponse;
			cv::Point_<T> subDelta;
			minMaxLoc(translationResponse, 0, &maxResponse, 0, &delta);
			subDelta = delta;

			if (_CELL_SIZE != 1)
				subDelta = subPixelDelta<T>(translationResponse, delta);

			T posDeltaX = (subDelta.x + 1 - floor(translationResponse.cols / consts::c2_0)) * newScale;
			T posDeltaY = (subDelta.y + 1 - floor(translationResponse.rows / consts::c2_0)) * newScale;
			newPos.x += round(posDeltaX * _CELL_SIZE);
			newPos.y += round(posDeltaY * _CELL_SIZE);

			if (_scaleEstimator)
			{
				//find scale
				T tempScale = newScale * _templateScaleFactor;

				if (_scaleEstimator->detectScale(image, newPos,
					tempScale) == false)
					return false;

				newScale = tempScale / _templateScaleFactor;
			}

			response = translationResponse;
			maxResponseIdx = delta;
			return true;
		}

		bool updateModel(const cv::Mat& image, const Point& newPos,
			T newScale)
		{
			_pos = newPos;
			_scale = newScale;
			std::shared_ptr<DFC> hfNum(0);
			cv::Mat hfDen;

			if (getTranslationTrainingData(image, hfNum, hfDen, _pos) == false)
				return false;

			_hfDenominator = (1 - _LEARNING_RATE) * _hfDenominator + _LEARNING_RATE * hfDen;
			DFC::mulValueFeatures(_hfNumerator, (1 - _LEARNING_RATE));
			DFC::mulValueFeatures(hfNum, _LEARNING_RATE);
			DFC::addFeatures(_hfNumerator, hfNum);

			if (_scaleEstimator)
			{
				if (_scaleEstimator->updateScale(image, newPos, newScale * _templateScaleFactor) == false)
					return false;
			}

			return true;
		}

	private:
		typedef void(*cvFhogPtr)
			(const cv::Mat& img, std::shared_ptr<DFC>& cvFeatures, int binSize, int fhogChannelsToCopy);
		cvFhogPtr cvFhog = 0;

		typedef void(*dftPtr)
			(const cv::Mat& input, cv::Mat& output, int flags);
		dftPtr calcDft = 0;

		cv::Mat _cosWindow;
		cv::Mat _y;
		std::shared_ptr<DFC> _hfNumerator;
		cv::Mat _hfDenominator;
		cv::Mat _yf;
		Point _pos;
		Size _templateSz;
		Size _templateSizeNoFloor;
		Size _baseTargetSz;
		Rect _lastBoundingBox;
		T _scale; // _scale is the scale of the template; not the target
		T _templateScaleFactor; // _templateScaleFactor is used to calc the target scale
		ScaleEstimator<T>* _scaleEstimator;
		int _frameIdx = 1;
		bool _isInitialized;

		const T _MIN_AREA;
		const T _MAX_AREA_FACTOR;
		const T _PADDING;
		const T _OUTPUT_SIGMA_FACTOR;
		const T _LAMBDA;
		const T _LEARNING_RATE;
		const T _PSR_THRESHOLD;
		const int _PSR_PEAK_DEL;
		const int _CELL_SIZE;
		const int _TEMPLATE_SIZE;
		const bool _ENABLE_TRACKING_LOSS_DETECTION;
		const int _RESIZE_TYPE;
		const bool _ORIGINAL_VERSION;
		const bool _USE_CCS;

		T position[4];
	};
}

#endif


// Copyright PinaPL
//
// main.cpp
// PinaPL
//

#include <Eigen/Dense>

#include "weights.hpp"
#include "cell.hpp"
#include "network.hpp"
#include "test.hpp"

int main(int argc, char **argv) {
    single_cell_test();
}


#include <aslam/calibration/model/fragments/PoseCv.h>

#include <boost/make_shared.hpp>

#include <aslam/calibration/model/Model.h>
#include <aslam/calibration/tools/DeprecationAlerter.h>

namespace aslam {
namespace calibration {

PoseCv::PoseCv(Module* module, boost::optional<std::string> defaultReferenceFrameName):
        referenceFrame_(module->getModel().getFrame(module->getMyConfig().get(
            {"referenceFrame",
            "!frame", "Use referenceFrame instead."},
            defaultReferenceFrameName, parameterDeprecationAlerter))),
        frame_(module->getModel().getOrCreateFrame(module->getName())), //TODO D make sure there is only one PoseCv in a module
        rotationVariable(module->createCVIfUsed<RotationQuaternionCv>("rotation", "R")),
        translationVariable(module->createCVIfUsed<EuclideanPointCv>("translation", "T")),
        transExp(translationVariable),
        rotExp(rotationVariable),
        trafoExp(rotExp, transExp)
{
}

Eigen::Vector3d PoseCv::NoTranslation = Eigen::Vector3d::Zero();
Eigen::Vector4d PoseCv::NoRotation = Eigen::Vector4d::Unit(3);

aslam::calibration::RelativeKinematicExpression PoseCv::calcRelativeKinematics() const
{
  return RelativeKinematicExpression(getRotationToParentExpression(),
                                     getTranslationToParentExpression());
}

} /* namespace calibration */
} /* namespace aslam */


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 4, 4> test_double_4_4(const Eigen::Matrix<double, 4, 4>& M) { return M; }
void export_double_4_4() { boost::python::def("test_double_4_4", test_double_4_4); }


#include "BlocksEngine/pch.h"
#include "BlocksEngine/Core/Math/Vector2.h"

#include <boost/container_hash/hash.hpp>

#include "BlocksEngine/Core/Math/Quaternion.h"

using namespace BlocksEngine;
using namespace DirectX;

//------------------------------------------------------------------------------
// Constructors
//------------------------------------------------------------------------------

template <class T>
Vector2<T>::Vector2() noexcept
    : Vector2Base<T>::Base{0, 0}
{
}

template <class T>
constexpr Vector2<T>::Vector2(const T a) noexcept
    : Vector2Base<T>::Base{a, a}
{
}

template <class T>
Vector2<T>::Vector2(_In_reads_(2) const T* pArray) noexcept
    : Vector2Base<T>::Base{pArray}
{
}

template <class T>
Vector2<T>::Vector2(FXMVECTOR v) noexcept // NOLINT(cppcoreguidelines-pro-type-member-init)
{
    Store(this, v);
}

template <class T>
Vector2<T>::Vector2(const typename Vector2Base<T>::Base& v) noexcept
    : Vector2Base<T>::Base{v.x, v.y}
{
}

template <>
Vector2<float>::Vector2(const Vector2Base<float>::Vector& v) noexcept
    : Vector2Base<float>::Base{v.f[0], v.f[1]}
{
}

template <>
Vector2<int32_t>::Vector2(const Vector2Base<int32_t>::Vector& v) noexcept
    : Vector2Base<int32_t>::Base{v.i[0], v.i[1]}
{
}

template <>
Vector2<uint32_t>::Vector2(const Vector2Base<uint32_t>::Vector& v) noexcept
    : Vector2Base<uint32_t>::Base{v.u[0], v.u[1]}
{
}

//------------------------------------------------------------------------------
// Converter operators
//------------------------------------------------------------------------------

template <class T>
Vector2<T>::operator DirectX::XMVECTOR() const noexcept
{
    return Load(this);
}

//------------------------------------------------------------------------------
// Comparision operators
//------------------------------------------------------------------------------

template <class T>
bool Vector2<T>::operator==(const Vector2<T>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    return XMVector2Equal(v1, v2);
}

template <class T>
bool Vector2<T>::operator!=(const Vector2<T>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    return XMVector2NotEqual(v1, v2);
}

//------------------------------------------------------------------------------
// Assignment operators
//------------------------------------------------------------------------------

template <>
Vector2<float>& Vector2<float>::operator=(const Vector2Base<float>::Vector& f) noexcept
{
    this->x = f.f[0];
    this->y = f.f[1];

    return *this;
}

template <>
Vector2<int32_t>& Vector2<int32_t>::operator=(const Vector2Base<int32_t>::Vector& f) noexcept
{
    this->x = f.i[0];
    this->y = f.i[1];

    return *this;
}

template <>
Vector2<uint32_t>& Vector2<uint32_t>::operator=(const Vector2Base<uint32_t>::Vector& f) noexcept
{
    this->x = f.u[0];
    this->y = f.u[1];

    return *this;
}

template <class T>
template <class U>
Vector2<T>& Vector2<T>::operator+=(const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVectorAdd(v1, v2);
    Store(this, x);

    return *this;
}

template <class T>
template <class U>
Vector2<T>& Vector2<T>::operator-=(const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVectorSubtract(v1, v2);
    Store(this, x);

    return *this;
}

template <class T>
template <class U>
Vector2<T>& Vector2<T>::operator*=(const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVectorMultiply(v1, v2);
    Store(this, x);

    return *this;
}

template <class T>
Vector2<T>& Vector2<T>::operator*=(const float s) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorScale(v1, s);
    Store(this, x);

    return *this;
}

template <class T>
Vector2<T>& Vector2<T>::operator/=(const float s) noexcept
{
    assert(s != 0.0f);

    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorScale(v1, 1.f / s);
    Store(this, x);

    return *this;
}

//------------------------------------------------------------------------------
// Unary operators
//------------------------------------------------------------------------------

template <class T>
Vector2<T> Vector2<T>::operator+() const noexcept
{
    return *this;
}

// TODO: Could be done with XMVectorNegate, but test it
template <>
Vector2<float> Vector2<float>::operator-() const noexcept
{
    return Vector2(-this->x, -this->y);
}

template <>
Vector2<int32_t> Vector2<int32_t>::operator-() const noexcept
{
    return Vector2(-this->x, -this->y);
}

template <>
Vector2<uint32_t> Vector2<uint32_t>::operator-() const noexcept
{
    return Vector2(this->x, this->y);
}

//------------------------------------------------------------------------------
// Hashing
//------------------------------------------------------------------------------

/*template <class T>
std::size_t hash_value(const Vector2<T>& v)
{
    std::size_t seed = 0;
    boost::hash_combine(seed, v.x);
    boost::hash_combine(seed, v.y);

    return seed;
}*/

//------------------------------------------------------------------------------
// Vector operations
//------------------------------------------------------------------------------

template <class T>
template <class U>
bool Vector2<T>::InBounds(const Vector2<U>& bounds) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&bounds);
    return XMVector2InBounds(v1, v2);
}

template <class T>
float Vector2<T>::Length() const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2Length(v1);
    return XMVectorGetX(x);
}

template <class T>
float Vector2<T>::LengthSquared() const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2LengthSq(v1);
    return XMVectorGetX(x);
}

template <class T>
template <class U>
float Vector2<T>::Dot(const Vector2<U>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVector2Dot(v1, v2);
    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
void Vector2<T>::Cross(const Vector2<U>& v, Vector2<V>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR r = XMVector2Cross(v1, v2);
    Store(&result, r);
}

template <class T>
template <class U, class V>
Vector2<U> Vector2<T>::Cross(const Vector2<V>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR r = XMVector2Cross(v1, v2);

    Vector2<U> result;
    Store(&result, r);
    return result;
}

template <class T>
void Vector2<T>::Normalize() noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2Normalize(v1);

    Store(this, x);
}

template <class T>
template <class U>
void Vector2<T>::Normalize(Vector2<U>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector2Normalize(v1);
    Store(&result, x);
}

template <class T>
template <class U, class V>
void Vector2<T>::Clamp(const Vector2<U>& vMin, const Vector2<V>& vMax) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&vMin);
    const XMVECTOR v3 = Load(&vMax);
    const XMVECTOR x = XMVectorClamp(v1, v2, v3);

    Store(this, x);
}

template <class T>
template <class U, class V, class W>
void Vector2<T>::Clamp(const Vector2<U>& vMin, const Vector2<V>& vMax, Vector2<W>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&vMin);
    const XMVECTOR v3 = Load(&vMax);
    const XMVECTOR x = XMVectorClamp(v1, v2, v3);

    Store(&result, x);
}

//------------------------------------------------------------------------------
// Static functions
//------------------------------------------------------------------------------

template <class T>
template <class U, class V>
float Vector2<T>::Distance(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR v = XMVectorSubtract(x2, x1);
    const XMVECTOR x = XMVector2Length(v);

    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
float Vector2<T>::DistanceSquared(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR v = XMVectorSubtract(x2, x1);
    const XMVECTOR x = XMVector2LengthSq(v);

    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
void Vector2<T>::Min(const Vector2<U>& v1, const Vector2<V>& v2, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMin(x1, x2);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Min(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMin(x1, x2);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Max(const Vector2<U>& v1, const Vector2<V>& v2, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMax(x1, x2);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Max(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMax(x1, x2);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Lerp(const Vector2<U>& v1, const Vector2<V>& v2, const float t, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Lerp(const Vector2<U>& v1, const Vector2<V>& v2, const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::SmoothStep(const Vector2<U>& v1, const Vector2<V>& v2, float t, Vector2<T>& result) noexcept
{
    t = t > 1.0f ? 1.0f : t < 0.0f ? 0.0f : t; // Clamp value to 0 to 1
    t = t * t * (3.f - 2.f * t);
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::SmoothStep(const Vector2<U>& v1, const Vector2<V>& v2, float t) noexcept
{
    t = t > 1.0f ? 1.0f : t < 0.0f ? 0.0f : t; // Clamp value to 0 to 1
    t = t * t * (3.f - 2.f * t);
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W>
void Vector2<T>::Barycentric(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3, const float f,
                             const float g, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x = XMVectorBaryCentric(x1, x2, x3, f, g);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W>
Vector2<T> Vector2<T>::Barycentric(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3, const float f,
                                   const float g) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x = XMVectorBaryCentric(x1, x2, x3, f, g);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W, class X>
void Vector2<T>::CatmullRom(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3, const Vector2<X>& v4,
                            const float t, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x4 = Load(&v4);
    const XMVECTOR x = XMVectorCatmullRom(x1, x2, x3, x4, t);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W, class X>
Vector2<T> Vector2<T>::CatmullRom(const Vector2<U>& v1, const Vector2<V>& v2, const Vector2<W>& v3,
                                  const Vector2<X>& v4, const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x4 = Load(&v4);
    const XMVECTOR x = XMVectorCatmullRom(x1, x2, x3, x4, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W, class X>
void Vector2<T>::Hermite(const Vector2<U>& v1, const Vector2<V>& t1, const Vector2<W>& v2, const Vector2<X>& t2,
                         const float t, Vector2<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&t1);
    const XMVECTOR x3 = Load(&v2);
    const XMVECTOR x4 = Load(&t2);
    const XMVECTOR x = XMVectorHermite(x1, x2, x3, x4, t);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W, class X>
Vector2<T> Vector2<T>::Hermite(const Vector2<U>& v1, const Vector2<V>& t1, const Vector2<W>& v2, const Vector2<X>& t2,
                               const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&t1);
    const XMVECTOR x3 = Load(&v2);
    const XMVECTOR x4 = Load(&t2);
    const XMVECTOR x = XMVectorHermite(x1, x2, x3, x4, t);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Reflect(const Vector2<U>& iVec, const Vector2<V>& nVec, Vector2<T>& result) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Reflect(i, n);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Reflect(const Vector2<U>& iVec, const Vector2<V>& nVec) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Reflect(i, n);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector2<T>::Refract(const Vector2<U>& iVec, const Vector2<V>& nVec, const float refractionIndex,
                         Vector2<T>& result) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Refract(i, n, refractionIndex);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector2<T> Vector2<T>::Refract(const Vector2<U>& iVec, const Vector2<V>& nVec, const float refractionIndex) noexcept
{
    const XMVECTOR i = Load(&iVec);
    const XMVECTOR n = Load(&nVec);
    const XMVECTOR x = XMVector2Refract(i, n, refractionIndex);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
void Vector2<T>::Transform(const Vector2<U>& v, const Quaternion& quat, Vector2<T>& result) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR q = XMLoadFloat4(&quat);
    const XMVECTOR x = XMVector3Rotate(v1, q);

    Store(&result, x);
}

template <class T>
template <class U>
Vector2<T> Vector2<T>::Transform(const Vector2<U>& v, const Quaternion& quat) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR q = XMLoadFloat4(&quat);
    const XMVECTOR x = XMVector3Rotate(v1, q);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
void Vector2<T>::Transform(const Vector2<U>& v, const Matrix& m, Vector2<T>& result) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformCoord(v1, m1);

    Store(&result, x);
}

template <class T>
template <class U>
Vector2<T> Vector2<T>::Transform(const Vector2<U>& v, const Matrix& m) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformCoord(v1, m1);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
_Use_decl_annotations_

void Vector2<T>::Transform(const Vector2<U>* varray, const size_t count, const Matrix& m,
                           Vector2<T>* resultArray) noexcept
{
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    XMVector2TransformCoordStream(resultArray, sizeof Vector2Base<U>::Vector, varray, sizeof Vector2Base<T>::Vector,
                                  count, m1);
}

template <class T>
template <class U>
void Vector2<T>::TransformNormal(const Vector2<U>& v, const Matrix& m, Vector2<T>& result) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformNormal(v1, m1);

    Store(&result, x);
}

template <class T>
template <class U>
Vector2<T> Vector2<T>::TransformNormal(const Vector2<U>& v, const Matrix& m) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector2TransformNormal(v1, m1);

    Vector2<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
_Use_decl_annotations_

void Vector2<T>::TransformNormal(const Vector2<U>* varray, const size_t count, const Matrix& m,
                                 Vector2<T>* resultArray) noexcept
{
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    XMVector2TransformNormalStream(resultArray, sizeof Vector2Base<U>::Vector, varray, sizeof Vector2Base<T>::Vector,
                                   count, m1);
}

//------------------------------------------------------------------------------
// Binary operators
//------------------------------------------------------------------------------

template <class T, class U, class V>
Vector2<T> operator+(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorAdd(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U, class V>
Vector2<T> operator-(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorSubtract(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U, class V>
Vector2<T> operator*(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMultiply(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U>
Vector2<T> operator*(const Vector2<U>& v, const float s) noexcept
{
    const XMVECTOR x1 = Load(&v);
    const XMVECTOR x = XMVectorScale(x1, s);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U, class V>
Vector2<T> operator/(const Vector2<U>& v1, const Vector2<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorDivide(x1, x2);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U>
Vector2<T> operator/(const Vector2<U>& v, const float s) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR x = XMVectorScale(v1, 1.f / s);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template <class T, class U>
Vector2<T> operator*(const float s, const Vector2<U>& v) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMVECTOR x = XMVectorScale(v1, s);

    Vector2<T> r;
    Vector2<T>::Store(&r, x);
    return r;
}

template struct Vector2<float>;
template struct Vector2<int32_t>;
template struct Vector2<uint32_t>;


#include "core_pch.hpp"
#include <glm/glm.hpp>

#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/matrix_inverse.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/noise.hpp>
#include <glm/gtc/packing.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtc/random.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/euler_angles.hpp>
#include <glm/gtx/matrix_interpolation.hpp>
#include <glm/gtx/matrix_major_storage.hpp>
#include <glm/gtx/matrix_query.hpp>
#include <glm/gtx/norm.hpp>
#include <glm/gtx/normal.hpp>
#include <glm/gtx/optimum_pow.hpp>
#include <glm/gtx/perpendicular.hpp>
#include <glm/gtx/perpendicular.hpp>
#include <glm/gtx/polar_coordinates.hpp>
#include <glm/gtx/projection.hpp>
#include <glm/gtx/projection.hpp>
#include <glm/gtx/rotate_vector.hpp>
#include <glm/gtx/vector_angle.hpp>
#include <glm/gtx/wrap.hpp>

#include <boost/core/noncopyable.hpp>

#include "Types.hpp"
#include "Constants.hpp"
#include "DefaultResourcePaths.hpp"

using namespace std::literals;

namespace fs = boost::filesystem;

uint frame(); // * returns frame number, should be defined elsewhere


#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;

int main() {
	boost::multiprecision::cpp_int L, R;
}


#include <Eigen/Dense>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <grppi/grppi.h>
#include <netcdf>
#include <omp.h>
#include <yaml-cpp/yaml.h>

#include "tula/eigen.h"
#include "tula/enum.h"
#include "tula/formatter/enum.h"
#include "tula/formatter/matrix.h"
#include "tula/formatter/utils.h"
#include "tula/grppiex.h"
#include "tula/logging.h"
#include "kids/core/kidsdata.h"

#include "core/TCData.h"
#include "core/lali.h"

// namespaces
namespace po = boost::program_options;
namespace pt = boost::property_tree;
//namespace ln = lali;

// TcData is the data structure of which RTCData and PTCData are a part
using timestream::TCData;

// Selects the type of TCData
using timestream::LaliDataKind;

// Command line parser
void po2pt(po::variables_map &in, pt::ptree &out)
{
    for (auto it = in.begin(); it != in.end(); ++it) {
        const auto &t = it->second.value().type();
        if ((t == typeid(int)) || (t == typeid(size_t))) {
            out.put<int>(it->first, in[it->first].as<int>());
        } else if ((t == typeid(float)) || (t == typeid(double))) {
            out.put<double>(it->first, in[it->first].as<double>());
        } else if (t == typeid(std::string)) {
            out.put<std::string>(it->first, in[it->first].as<std::string>());
        } else {
            throw std::runtime_error(fmt::format("unknown type in config: {}", t.name()));
        }
    }
}

int main([[maybe_unused]] int argc, [[maybe_unused]] char *argv[])
{
    SPDLOG_INFO("Starting Lali");
    logging::scoped_timeit timer("Lali");

    /*
  Declare Lali class *testing (to be moved outside of lali.cpp)*
  */

    SPDLOG_INFO("Making Lali Class");
    lali::Lali LC;

    /*
  Get Test data *testing*
  */
    // LC.makeTestData<lali::Tester::UseAzTEC>(argc, argv);
    /*
  Setup lali *testing (to be moved outside of lali.cpp)*
      - Calculate map dimensions
      - Allocate map matrices/tensors
      - Calculates Lowpass+Highpass Filter if requested
  */

    /*
  Begin main pipeline
      - Timestream Processing Stage 1 (Despking, Replacing Flagged Data,
  Lowpassing and Highpassing, Downsampling, Calibration
      - Timestream Processing Stage 2 (PCA Clean)
      - Mapmakaing Stage 1 (Populate map pixels)
  */

    // Get GRPPI execution policy (seq, omp, tbb, etc.)
    auto ex_name = LC.config->get_typed<std::string>("pipeline_ex_policy");

    // Get map type (Ra & Dec, Az & El, etc)
    std::string maptype = LC.config->get_typed<std::string>("mapType").c_str();

    auto data_type = LC.config->get_str("data_type");
    if (data_type == "toltec") {
        LC.init_from_cli<lali::DataType::TolTEC>(argc, argv);
        // open the kids files here
        namespace kids_spec = kids::toltec;
        auto kidsdata = kids_spec::read_data<>,
                                  rc.get_str("source"));
        // solve kidsdata in its entirety
        // and populate scans_data
        LC.Data.scans.resize(10, 10);
        LC.ndetectors = scans_data.cols();
        std::vector<std::string> detector_names(LC.ndetectors);
    } else if (data_type == "aztec") {
        LC.init_from_cli<lali::DataType::AzTEC>(argc, argv);
    }
    SPDLOG_INFO("Setting up Maps and Variables");
    {
        logging::scoped_timeit timer("setup()");
        LC.setup();
        // populate detector offsets.
    }

    SPDLOG_INFO("Starting Main Pipeline in {} mode", ex_name);
    {
        logging::scoped_timeit timer("Main pipeline");
        grppi::pipeline(
            tula::grppi_utils::dyn_ex(ex_name),
            [&]() -> std::optional<TCData<LaliDataKind::RTC, Eigen::MatrixXd>> {
                // Variable for current scan
                static auto scan = 0;
                // Current scanlength
                Eigen::Index scanlength;
                // Index of the start of the current scan
                Eigen::Index si = 0;

                // Loop through scans
                while (scan < LC.nscans) {
                    SPDLOG_INFO("----------------------------------------------------");
                    SPDLOG_INFO("Starting timestream -> map reduction for scan {}/{}",
                                scan + 1,
                                LC.nscans);
                    SPDLOG_INFO("----------------------------------------------------");
                    // Declare a TCData to hold data
                    TCData<LaliDataKind::RTC, Eigen::MatrixXd> rtc;

                    // First scan index for current scan
                    si = LC.Data.scanindex(2, scan);

                    // Get length of current scan (do we need the + 1?)
                    scanlength = LC.Data.scanindex(3, scan) - LC.Data.scanindex(2, scan) + 1;

                    // Get reference to BeammapData and put into the current RTC
                    if (auto data_type = LC.config->get_str("data_type");
                        data_type == "aztec") {
                        rtc.scans.data = LC.Data.scans.block(si, 0, scanlength,
                                                             LC.ndetectors);
                    } else if (data_type == "toltec") {
                        rtc.scans.data =
                            scans_data.block(si, 0, scanlength, ndetectors);
                    }

                    // Make flag matrix
                    rtc.flags.data.resize(rtc.scans.data.rows(),
                                          rtc.scans.data.cols());
                    rtc.flags.data.setOnes();

                    // Get scan indices and push into current RTC
                    rtc.scanindex.data = LC.Data.scanindex.col(scan);

                    // This index keeps track of which scan the RTC actually belongs to.
                    rtc.index.data = scan + 1;

                    // Get telescope pointings for scan (move to Eigen::Maps to save
                    // memory and time)
                    if (std::strcmp("RaDec", maptype.c_str()) == 0) {
                        rtc.telLat.data = LC.Data.telMetaData["TelRaPhys"].segment(si, scanlength);
                        rtc.telLon.data = LC.Data.telMetaData["TelDecPhys"].segment(si, scanlength);
                    }

                    else if (std::strcmp("AzEl", maptype.c_str()) == 0) {
                        rtc.telLat.data = LC.Data.telMetaData["TelAzPhys"].segment(si, scanlength);
                        rtc.telLon.data = LC.Data.telMetaData["TelElPhys"].segment(si, scanlength);
                    }

                    rtc.telElDes.data = LC.Data.telMetaData["TelElDes"].segment(si, scanlength);
                    rtc.ParAng.data = LC.Data.telMetaData["ParAng"].segment(si, scanlength);

                    // Increment scan
                    scan++;

                    // return RTC for the grppi:farm in Lali::run()
                    return rtc;
                }
                return {};
            },

            // Run the timestream - > map analysis.  This command contains a GRPPI farm that
            // parallelizes over the specified number of cores.
            LC.run());
    }

    /*
  Normalize maps
      - Mapmaking Stage 3 (Divide by weight map)
  */

    SPDLOG_INFO("Normalizing Maps by Weight Map");
    {
        logging::scoped_timeit timer("mapNormalize()");
        LC.Maps.mapNormalize();
    }

    /*
  Run after-mapmaking analyses
      - PSD
      - Histogram
  */

    /*
  Coadd maps
  */

    /*
  Run Wiener Filter
  */

    /*
  Additional Map Analyses
  */

    /*
  Output
  */

    SPDLOG_INFO("Outputing Maps to netCDF File");
    {
        logging::scoped_timeit timer("output()");
        LC.output(LC.config, LC.Maps);
    }

    return 0;
}


//==============================================================================
//         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 BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MANTISSA_HPP_INCLUDED
#include <boost/simd/ieee/functions/mantissa.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/is_invalid.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::mantissa_, tag::cpu_,
                           (A0)(X),
                           ((simd_<arithmetic_<A0>,X>))
                          )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      typedef typename dispatch::meta::as_integer<A0, unsigned>::type  int_type;
      typedef typename meta::scalar_of<int_type>::type      sint_type;
      typedef typename meta::scalar_of<A0>::type               s_type;
      const sint_type n1 = ((2*Maxexponent<s_type>()+1)<<Nbmantissabits<s_type>());
      const sint_type n2 = (sizeof(sint_type)-2);
      const int_type  mask0 = (splat<int_type>((n1<<2)>>2));
      const int_type  mask1 = (splat<int_type>((~n1)|n2));
      return select(logical_or(is_invalid(a0),is_eqz(a0)),a0,b_or(b_and(a0,mask1),mask0));
    }
  };
} } }
#endif