// Details for templated Spreadsort-based integer_sort.

//          Copyright Steven J. Ross 2001 - 2014.
// 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/libs/sort for library home page.

/*
Some improvements suggested by:
Phil Endecott and Frank Gennari
*/

#ifndef BOOST_SORT_SPREADSORT_DETAIL_INTEGER_SORT_HPP
#define BOOST_SORT_SPREADSORT_DETAIL_INTEGER_SORT_HPP
#include <algorithm>
#include <boost/cstdint.hpp>
#include <boost/serialization/static_warning.hpp>
#include <boost/sort/spreadsort/detail/constants.hpp>
#include <boost/sort/spreadsort/detail/spreadsort_common.hpp>
#include <boost/static_assert.hpp>
#include <boost/utility/enable_if.hpp>
#include <functional>
#include <limits>
#include <vector>

namespace boost {
namespace sort {
namespace spreadsort {
namespace detail {
// Return true if the list is sorted.  Otherwise, find the minimum and
// maximum using <.
template <class RandomAccessIter>
inline bool
is_sorted_or_find_extremes(RandomAccessIter current, RandomAccessIter last,
                           RandomAccessIter &max, RandomAccessIter &min) {
  min = max = current;
  // This assumes we have more than 1 element based on prior checks.
  while (!(*(current + 1) < *current)) {
    // If everything is in sorted order, return
    if (++current == last - 1)
      return true;
  }

  // The maximum is the last sorted element
  max = current;
  // Start from the first unsorted element
  while (++current < last) {
    if (*max < *current)
      max = current;
    else if (*current < *min)
      min = current;
  }
  return false;
}

// Return true if the list is sorted.  Otherwise, find the minimum and
// maximum.
// Use a user-defined comparison operator
template <class RandomAccessIter, class Compare>
inline bool is_sorted_or_find_extremes(RandomAccessIter current,
                                       RandomAccessIter last,
                                       RandomAccessIter &max,
                                       RandomAccessIter &min, Compare comp) {
  min = max = current;
  while (!comp(*(current + 1), *current)) {
    // If everything is in sorted order, return
    if (++current == last - 1)
      return true;
  }

  // The maximum is the last sorted element
  max = current;
  while (++current < last) {
    if (comp(*max, *current))
      max = current;
    else if (comp(*current, *min))
      min = current;
  }
  return false;
}

// Gets a non-negative right bit shift to operate as a logarithmic divisor
template <unsigned log_mean_bin_size>
inline int get_log_divisor(size_t count, int log_range) {
  int log_divisor;
  // If we can finish in one iteration without exceeding either
  //(2 to the max_finishing_splits) or n bins, do so
  if ((log_divisor = log_range - rough_log_2_size(count)) <= 0 &&
      log_range <= max_finishing_splits)
    log_divisor = 0;
  else {
    // otherwise divide the data into an optimized number of pieces
    log_divisor += log_mean_bin_size;
    // Cannot exceed max_splits or cache misses slow down bin lookups
    if ((log_range - log_divisor) > max_splits)
      log_divisor = log_range - max_splits;
  }
  return log_divisor;
}

// Implementation for recursive integer sorting
template <class RandomAccessIter, class Div_type, class Size_type>
inline void spreadsort_rec(RandomAccessIter first, RandomAccessIter last,
                           std::vector<RandomAccessIter> &bin_cache,
                           unsigned cache_offset, size_t *bin_sizes) {
  // This step is roughly 10% of runtime, but it helps avoid worst-case
  // behavior and improve behavior with real data
  // If you know the maximum and minimum ahead of time, you can pass those
  // values in and skip this step for the first iteration
  RandomAccessIter max, min;
  if (is_sorted_or_find_extremes(first, last, max, min))
    return;
  RandomAccessIter *target_bin;
  unsigned log_divisor = get_log_divisor<int_log_mean_bin_size>(
      last - first, rough_log_2_size(Size_type((*max >> 0) - (*min >> 0))));
  Div_type div_min = *min >> log_divisor;
  Div_type div_max = *max >> log_divisor;
  unsigned bin_count = unsigned(div_max - div_min) + 1;
  unsigned cache_end;
  RandomAccessIter *bins =
      size_bins(bin_sizes, bin_cache, cache_offset, cache_end, bin_count);

  // Calculating the size of each bin; this takes roughly 10% of runtime
  for (RandomAccessIter current = first; current != last;)
    bin_sizes[size_t((*(current++) >> log_divisor) - div_min)]++;
  // Assign the bin positions
  bins[0] = first;
  for (unsigned u = 0; u < bin_count - 1; u++)
    bins[u + 1] = bins[u] + bin_sizes[u];

  RandomAccessIter nextbinstart = first;
  // Swap into place
  // This dominates runtime, mostly in the swap and bin lookups
  for (unsigned u = 0; u < bin_count - 1; ++u) {
    RandomAccessIter *local_bin = bins + u;
    nextbinstart += bin_sizes[u];
    // Iterating over each element in this bin
    for (RandomAccessIter current = *local_bin; current < nextbinstart;
         ++current) {
      // Swapping elements in current into place until the correct
      // element has been swapped in
      for (target_bin = (bins + ((*current >> log_divisor) - div_min));
           target_bin != local_bin;
           target_bin = bins + ((*current >> log_divisor) - div_min)) {
        // 3-way swap; this is about 1% faster than a 2-way swap
        // The main advantage is less copies are involved per item
        // put in the correct place
        typename std::iterator_traits<RandomAccessIter>::value_type tmp;
        RandomAccessIter b = (*target_bin)++;
        RandomAccessIter *b_bin = bins + ((*b >> log_divisor) - div_min);
        if (b_bin != local_bin) {
          RandomAccessIter c = (*b_bin)++;
          tmp = *c;
          *c = *b;
        } else
          tmp = *b;
        *b = *current;
        *current = tmp;
      }
    }
    *local_bin = nextbinstart;
  }
  bins[bin_count - 1] = last;

  // If we've bucketsorted, the array is sorted and we should skip recursion
  if (!log_divisor)
    return;
  // log_divisor is the remaining range; calculating the comparison threshold
  size_t max_count =
      get_min_count<int_log_mean_bin_size, int_log_min_split_count,
                    int_log_finishing_count>(log_divisor);

  // Recursing
  RandomAccessIter lastPos = first;
  for (unsigned u = cache_offset; u < cache_end; lastPos = bin_cache[u], ++u) {
    Size_type count = bin_cache[u] - lastPos;
    // don't sort unless there are at least two items to Compare
    if (count < 2)
      continue;
    // using boost::sort::pdqsort if its worst-case is better
    if (count < max_count)
      boost::sort::pdqsort(lastPos, bin_cache[u]);
    else
      spreadsort_rec<RandomAccessIter, Div_type, Size_type>(
          lastPos, bin_cache[u], bin_cache, cache_end, bin_sizes);
  }
}

// Generic bitshift-based 3-way swapping code
template <class RandomAccessIter, class Div_type, class Right_shift>
inline void inner_swap_loop(RandomAccessIter *bins,
                            const RandomAccessIter &next_bin_start, unsigned ii,
                            Right_shift &rshift, const unsigned log_divisor,
                            const Div_type div_min) {
  RandomAccessIter *local_bin = bins + ii;
  for (RandomAccessIter current = *local_bin; current < next_bin_start;
       ++current) {
    for (RandomAccessIter *target_bin =
             (bins + (rshift(*current, log_divisor) - div_min));
         target_bin != local_bin;
         target_bin = bins + (rshift(*current, log_divisor) - div_min)) {
      typename std::iterator_traits<RandomAccessIter>::value_type tmp;
      RandomAccessIter b = (*target_bin)++;
      RandomAccessIter *b_bin = bins + (rshift(*b, log_divisor) - div_min);
      // Three-way swap; if the item to be swapped doesn't belong
      // in the current bin, swap it to where it belongs
      if (b_bin != local_bin) {
        RandomAccessIter c = (*b_bin)++;
        tmp = *c;
        *c = *b;
      }
      // Note: we could increment current once the swap is done in this case
      // but that seems to impair performance
      else
        tmp = *b;
      *b = *current;
      *current = tmp;
    }
  }
  *local_bin = next_bin_start;
}

// Standard swapping wrapper for ascending values
template <class RandomAccessIter, class Div_type, class Right_shift>
inline void swap_loop(RandomAccessIter *bins, RandomAccessIter &next_bin_start,
                      unsigned ii, Right_shift &rshift, const size_t *bin_sizes,
                      const unsigned log_divisor, const Div_type div_min) {
  next_bin_start += bin_sizes[ii];
  inner_swap_loop<RandomAccessIter, Div_type, Right_shift>(
      bins, next_bin_start, ii, rshift, log_divisor, div_min);
}

// Functor implementation for recursive sorting
template <class RandomAccessIter, class Div_type, class Right_shift,
          class Compare, class Size_type, unsigned log_mean_bin_size,
          unsigned log_min_split_count, unsigned log_finishing_count>
inline void spreadsort_rec(RandomAccessIter first, RandomAccessIter last,
                           std::vector<RandomAccessIter> &bin_cache,
                           unsigned cache_offset, size_t *bin_sizes,
                           Right_shift rshift, Compare comp) {
  RandomAccessIter max, min;
  if (is_sorted_or_find_extremes(first, last, max, min, comp))
    return;
  unsigned log_divisor = get_log_divisor<log_mean_bin_size>(
      last - first,
      rough_log_2_size(Size_type(rshift(*max, 0) - rshift(*min, 0))));
  Div_type div_min = rshift(*min, log_divisor);
  Div_type div_max = rshift(*max, log_divisor);
  unsigned bin_count = unsigned(div_max - div_min) + 1;
  unsigned cache_end;
  RandomAccessIter *bins =
      size_bins(bin_sizes, bin_cache, cache_offset, cache_end, bin_count);

  // Calculating the size of each bin
  for (RandomAccessIter current = first; current != last;)
    bin_sizes[unsigned(rshift(*(current++), log_divisor) - div_min)]++;
  bins[0] = first;
  for (unsigned u = 0; u < bin_count - 1; u++)
    bins[u + 1] = bins[u] + bin_sizes[u];

  // Swap into place
  RandomAccessIter next_bin_start = first;
  for (unsigned u = 0; u < bin_count - 1; ++u)
    swap_loop<RandomAccessIter, Div_type, Right_shift>(
        bins, next_bin_start, u, rshift, bin_sizes, log_divisor, div_min);
  bins[bin_count - 1] = last;

  // If we've bucketsorted, the array is sorted
  if (!log_divisor)
    return;

  // Recursing
  size_t max_count = get_min_count<log_mean_bin_size, log_min_split_count,
                                   log_finishing_count>(log_divisor);
  RandomAccessIter lastPos = first;
  for (unsigned u = cache_offset; u < cache_end; lastPos = bin_cache[u], ++u) {
    size_t count = bin_cache[u] - lastPos;
    if (count < 2)
      continue;
    if (count < max_count)
      boost::sort::pdqsort(lastPos, bin_cache[u], comp);
    else
      spreadsort_rec<RandomAccessIter, Div_type, Right_shift, Compare,
                     Size_type, log_mean_bin_size, log_min_split_count,
                     log_finishing_count>(lastPos, bin_cache[u], bin_cache,
                                          cache_end, bin_sizes, rshift, comp);
  }
}

// Functor implementation for recursive sorting with only Shift overridden
template <class RandomAccessIter, class Div_type, class Right_shift,
          class Size_type, unsigned log_mean_bin_size,
          unsigned log_min_split_count, unsigned log_finishing_count>
inline void spreadsort_rec(RandomAccessIter first, RandomAccessIter last,
                           std::vector<RandomAccessIter> &bin_cache,
                           unsigned cache_offset, size_t *bin_sizes,
                           Right_shift rshift) {
  RandomAccessIter max, min;
  if (is_sorted_or_find_extremes(first, last, max, min))
    return;
  unsigned log_divisor = get_log_divisor<log_mean_bin_size>(
      last - first,
      rough_log_2_size(Size_type(rshift(*max, 0) - rshift(*min, 0))));
  Div_type div_min = rshift(*min, log_divisor);
  Div_type div_max = rshift(*max, log_divisor);
  unsigned bin_count = unsigned(div_max - div_min) + 1;
  unsigned cache_end;
  RandomAccessIter *bins =
      size_bins(bin_sizes, bin_cache, cache_offset, cache_end, bin_count);

  // Calculating the size of each bin
  for (RandomAccessIter current = first; current != last;)
    bin_sizes[unsigned(rshift(*(current++), log_divisor) - div_min)]++;
  bins[0] = first;
  for (unsigned u = 0; u < bin_count - 1; u++)
    bins[u + 1] = bins[u] + bin_sizes[u];

  // Swap into place
  RandomAccessIter nextbinstart = first;
  for (unsigned ii = 0; ii < bin_count - 1; ++ii)
    swap_loop<RandomAccessIter, Div_type, Right_shift>(
        bins, nextbinstart, ii, rshift, bin_sizes, log_divisor, div_min);
  bins[bin_count - 1] = last;

  // If we've bucketsorted, the array is sorted
  if (!log_divisor)
    return;

  // Recursing
  size_t max_count = get_min_count<log_mean_bin_size, log_min_split_count,
                                   log_finishing_count>(log_divisor);
  RandomAccessIter lastPos = first;
  for (unsigned u = cache_offset; u < cache_end; lastPos = bin_cache[u], ++u) {
    size_t count = bin_cache[u] - lastPos;
    if (count < 2)
      continue;
    if (count < max_count)
      boost::sort::pdqsort(lastPos, bin_cache[u]);
    else
      spreadsort_rec<RandomAccessIter, Div_type, Right_shift, Size_type,
                     log_mean_bin_size, log_min_split_count,
                     log_finishing_count>(lastPos, bin_cache[u], bin_cache,
                                          cache_end, bin_sizes, rshift);
  }
}

// Holds the bin vector and makes the initial recursive call
template <class RandomAccessIter, class Div_type>
// Only use spreadsort if the integer can fit in a size_t
inline
    typename boost::enable_if_c<sizeof(Div_type) <= sizeof(size_t), void>::type
    integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type) {
  size_t bin_sizes[1 << max_finishing_splits];
  std::vector<RandomAccessIter> bin_cache;
  spreadsort_rec<RandomAccessIter, Div_type, size_t>(first, last, bin_cache, 0,
                                                     bin_sizes);
}

// Holds the bin vector and makes the initial recursive call
template <class RandomAccessIter, class Div_type>
// Only use spreadsort if the integer can fit in a uintmax_t
inline typename boost::enable_if_c<(sizeof(Div_type) > sizeof(size_t)) &&
                                       sizeof(Div_type) <=
                                           sizeof(boost::uintmax_t),
                                   void>::type
integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type) {
  size_t bin_sizes[1 << max_finishing_splits];
  std::vector<RandomAccessIter> bin_cache;
  spreadsort_rec<RandomAccessIter, Div_type, boost::uintmax_t>(
      first, last, bin_cache, 0, bin_sizes);
}

template <class RandomAccessIter, class Div_type>
inline typename boost::disable_if_c<sizeof(Div_type) <= sizeof(size_t) ||
                                        sizeof(Div_type) <=
                                            sizeof(boost::uintmax_t),
                                    void>::type
// defaulting to boost::sort::pdqsort when integer_sort won't work
integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type) {
  // Warning that we're using boost::sort::pdqsort, even though integer_sort was
  // called
  BOOST_STATIC_WARNING(sizeof(Div_type) <= sizeof(size_t));
  boost::sort::pdqsort(first, last);
}

// Same for the full functor version
template <class RandomAccessIter, class Div_type, class Right_shift,
          class Compare>
// Only use spreadsort if the integer can fit in a size_t
inline
    typename boost::enable_if_c<sizeof(Div_type) <= sizeof(size_t), void>::type
    integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type,
                 Right_shift shift, Compare comp) {
  size_t bin_sizes[1 << max_finishing_splits];
  std::vector<RandomAccessIter> bin_cache;
  spreadsort_rec<RandomAccessIter, Div_type, Right_shift, Compare, size_t,
                 int_log_mean_bin_size, int_log_min_split_count,
                 int_log_finishing_count>(first, last, bin_cache, 0, bin_sizes,
                                          shift, comp);
}

template <class RandomAccessIter, class Div_type, class Right_shift,
          class Compare>
// Only use spreadsort if the integer can fit in a uintmax_t
inline typename boost::enable_if_c<(sizeof(Div_type) > sizeof(size_t)) &&
                                       sizeof(Div_type) <=
                                           sizeof(boost::uintmax_t),
                                   void>::type
integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type,
             Right_shift shift, Compare comp) {
  size_t bin_sizes[1 << max_finishing_splits];
  std::vector<RandomAccessIter> bin_cache;
  spreadsort_rec<RandomAccessIter, Div_type, Right_shift, Compare,
                 boost::uintmax_t, int_log_mean_bin_size,
                 int_log_min_split_count, int_log_finishing_count>(
      first, last, bin_cache, 0, bin_sizes, shift, comp);
}

template <class RandomAccessIter, class Div_type, class Right_shift,
          class Compare>
inline typename boost::disable_if_c<sizeof(Div_type) <= sizeof(size_t) ||
                                        sizeof(Div_type) <=
                                            sizeof(boost::uintmax_t),
                                    void>::type
// defaulting to boost::sort::pdqsort when integer_sort won't work
integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type,
             Right_shift shift, Compare comp) {
  // Warning that we're using boost::sort::pdqsort, even though integer_sort was
  // called
  BOOST_STATIC_WARNING(sizeof(Div_type) <= sizeof(size_t));
  boost::sort::pdqsort(first, last, comp);
}

// Same for the right shift version
template <class RandomAccessIter, class Div_type, class Right_shift>
// Only use spreadsort if the integer can fit in a size_t
inline
    typename boost::enable_if_c<sizeof(Div_type) <= sizeof(size_t), void>::type
    integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type,
                 Right_shift shift) {
  size_t bin_sizes[1 << max_finishing_splits];
  std::vector<RandomAccessIter> bin_cache;
  spreadsort_rec<RandomAccessIter, Div_type, Right_shift, size_t,
                 int_log_mean_bin_size, int_log_min_split_count,
                 int_log_finishing_count>(first, last, bin_cache, 0, bin_sizes,
                                          shift);
}

template <class RandomAccessIter, class Div_type, class Right_shift>
// Only use spreadsort if the integer can fit in a uintmax_t
inline typename boost::enable_if_c<(sizeof(Div_type) > sizeof(size_t)) &&
                                       sizeof(Div_type) <=
                                           sizeof(boost::uintmax_t),
                                   void>::type
integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type,
             Right_shift shift) {
  size_t bin_sizes[1 << max_finishing_splits];
  std::vector<RandomAccessIter> bin_cache;
  spreadsort_rec<RandomAccessIter, Div_type, Right_shift, boost::uintmax_t,
                 int_log_mean_bin_size, int_log_min_split_count,
                 int_log_finishing_count>(first, last, bin_cache, 0, bin_sizes,
                                          shift);
}

template <class RandomAccessIter, class Div_type, class Right_shift>
inline typename boost::disable_if_c<sizeof(Div_type) <= sizeof(size_t) ||
                                        sizeof(Div_type) <=
                                            sizeof(boost::uintmax_t),
                                    void>::type
// defaulting to boost::sort::pdqsort when integer_sort won't work
integer_sort(RandomAccessIter first, RandomAccessIter last, Div_type,
             Right_shift shift) {
  // Warning that we're using boost::sort::pdqsort, even though integer_sort was
  // called
  BOOST_STATIC_WARNING(sizeof(Div_type) <= sizeof(size_t));
  boost::sort::pdqsort(first, last);
}
} // namespace detail
} // namespace spreadsort
} // namespace sort
} // namespace boost

#endif


// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright (c) 2018, FZI Forschungszentrum Informatik
//
// Redistribution and use in source and binary forms, with or without modification, are permitted
// provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this list of conditions
//    and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder 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 HOLDER 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.
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
/*!\file
 *
 * \author  Julius Ziegler <ziegler@fzi.de>
 * \date    2012-03-06
 *
 */
//----------------------------------------------------------------------

#if !defined(LOCALGEOGRAPHICCS_HPP)
#define LOCALGEOGRAPHICCS_HPP

#include "convert_coordinates.hpp"

#include <boost/tuple/tuple.hpp>

#include <utility>

struct LocalGeographicCS
{
  LocalGeographicCS();
  LocalGeographicCS( double lat0, double lon0 );

  void set_origin( double lat0, double lon0 );

  void ll2xy( double lat, double lon, double& x, double& y ) const;
  void xy2ll( double x, double y, double& lat, double& lon ) const;

  boost::tuple<double, double> ll2xy( double lat, double lon ) const;
  boost::tuple<double, double> xy2ll( double x, double y ) const;

  // operate on containers
  template<class ItIn, class ItOut>
  void ll2xy( const ItIn& lat_begin, const ItIn& lat_end, const ItIn& lon_begin, const ItOut& x_begin, const ItOut& y_begin ) const;

  template<class ItIn, class ItOut>
  void xy2ll( const ItIn& x_begin, const ItIn& x_end, const ItIn& y_begin, const ItOut& lat_begin, const ItOut& lon_begin ) const;

private:
  double _scale;
  double _x0, _y0;
};

inline LocalGeographicCS::LocalGeographicCS( double lat0, double lon0 )
{
  set_origin( lat0, lon0 );
}

inline LocalGeographicCS::LocalGeographicCS()
{}

inline void LocalGeographicCS::set_origin( double lat0, double lon0 )
{
  _scale = convert_coordinates::lat_to_scale( lat0 );
  convert_coordinates::latlon_to_mercator( lat0, lon0, _scale, _x0, _y0 );
}

inline void LocalGeographicCS::ll2xy( double lat, double lon, double& x, double& y ) const
{
  convert_coordinates::latlon_to_mercator( lat, lon, _scale, x, y );
  x -= _x0;
  y -= _y0;
}

inline boost::tuple<double, double> LocalGeographicCS::ll2xy( double lat, double lon ) const
{
  double x, y;
  ll2xy( lat, lon, x, y );
  return boost::make_tuple( x, y );
}

inline void LocalGeographicCS::xy2ll( double x, double y, double& lat, double& lon ) const
{
  x += _x0;
  y += _y0;

  convert_coordinates::mercator_to_latlon( x, y, _scale, lat, lon );
}

inline boost::tuple<double, double> LocalGeographicCS::xy2ll( double x, double y ) const
{
  double lat, lon;
  xy2ll( x, y, lat, lon );
  return boost::make_tuple( lat, lon );
}

// operate on containers
template<class ItIn, class ItOut>
void LocalGeographicCS::ll2xy( const ItIn& lat_begin, const ItIn& lat_end, const ItIn& lon_begin, const ItOut& x_begin, const ItOut& y_begin ) const
{
  ItIn lat = lat_begin;
  ItIn lon = lon_begin;
  ItOut x = x_begin;
  ItOut y = y_begin;

  for( ; lat != lat_end; lat++, lon++, x++, y++ )
    {
      ll2xy( *lat, *lon, *x, *y );
    }
}

template<class ItIn, class ItOut>
void LocalGeographicCS::xy2ll( const ItIn& x_begin, const ItIn& x_end, const ItIn& y_begin, const ItOut& lat_begin, const ItOut& lon_begin ) const
{
  ItIn x = x_begin;
  ItIn y = y_begin;

  ItOut lat = lat_begin;
  ItOut lon = lon_begin;

  for( ; x != x_end; lat++, lon++, x++, y++ )
    xy2ll( *x, *y, *lat, *lon );
}

#endif


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
//!
//! @file
//! @ingroup openOR_core

#ifndef openOR_core_math_traits_hpp
#define openOR_core_math_traits_hpp

#include <openOR/Utility/if_const.hpp>
#include <openOR/Utility/Types.hpp>

#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>

#include <boost/numeric/ublas/traits.hpp>

// boost ublas scalar traits adjustments
namespace boost {
   namespace numeric {
      namespace ublas {

         template<>
         struct type_traits<char> : scalar_traits<char> {
            typedef type_traits<char> self_type;
            typedef int value_type;
            typedef const value_type &const_reference;
            typedef value_type &reference;
            typedef double real_type;
            typedef real_type precision_type;
         };

         template<>
         struct type_traits<unsigned char> : scalar_traits<unsigned char> {
            typedef type_traits<unsigned char> self_type;
            typedef int value_type;
            typedef const value_type &const_reference;
            typedef value_type &reference;
            typedef double real_type;
            typedef real_type precision_type;
         };

         template<>
         struct type_traits<short> : scalar_traits<short> {
            typedef type_traits<short> self_type;
            typedef int value_type;
            typedef const value_type &const_reference;
            typedef value_type &reference;
            typedef double real_type;
            typedef real_type precision_type;
         };

         template<>
         struct type_traits<unsigned short> : scalar_traits<unsigned short> {
            typedef type_traits<unsigned short> self_type;
            typedef int value_type;
            typedef const value_type &const_reference;
            typedef value_type &reference;
            typedef double real_type;
            typedef real_type precision_type;
         };

         template<>
         struct type_traits<int> : scalar_traits<int> {
            typedef type_traits<int> self_type;
            typedef int value_type;
            typedef const value_type &const_reference;
            typedef value_type &reference;
            typedef double real_type;
            typedef real_type precision_type;
         };

         template<>
         struct type_traits<unsigned int> : scalar_traits<unsigned int> {
            typedef type_traits<unsigned int> self_type;
            typedef int value_type;
            typedef const value_type &const_reference;
            typedef value_type &reference;
            typedef double real_type;
            typedef real_type precision_type;
         };

      }
   }
}

namespace openOR {
   namespace Math {

      /**
       * @brief traits for scalar types
       * @ingroup openOR_core
       */
      template<class Type>
      struct ScalarTraits {
         typedef typename boost::numeric::ublas::type_traits<Type>::value_type ValueType;
         typedef typename boost::numeric::ublas::type_traits<Type>::real_type RealType;
         typedef typename boost::numeric::ublas::type_traits<Type>::precision_type PrecisionType;
      };

      /**
       * @brief Functor for Vector element access
       * @ingroup openOR_core
       */
      template <typename Type>
      struct DefaultVectorAccess {
         typedef invalid_type AccessType;
         typedef invalid_type ConstAccessType;

         template <int I, typename PP>
            static typename if_const<PP, ConstAccessType, AccessType>::
         type get(PP& p) {
            return p.template get<I>();
         }
      };

      /**
      * \brief Default traits for non-vector types.
      * @ingroup openOR_core
      *
      * This traits class has to be specialized for concrete vector implementations.
      */
      template<class Type>
      struct VectorTraits {
         typedef Type VectorType;                     //!< self type
         typedef Type RealVectorType;                 //!< vector type with a floating point value type
         typedef invalid_type ValueType;                   //!< type of a vector element
         typedef invalid_type Vector3Type;                 //!< type of a vector with dimension 3 with same value type
		 typedef invalid_type Vector2Type;                 //!< type of a vector with dimension 2 with same value type
         typedef boost::mpl::int_<0> Dimension;       //!< number of elements (boost::mpl::int_<n>)
         typedef DefaultVectorAccess<Type> Access;    //!< functor type for element access
         typedef boost::mpl::false_ IsVector;         //!< flag to indicate that is is a valid vector type (must be set to boost::mpl::true_ if specialized for vector implementations.

         static VectorType ZEROS;                     //!< vector filled with zeros
      };

      /**
       * @brief Functor for matrix element access
       * @ingroup openOR_core
       */
      template <typename Type>
      struct DefaultMatrixAccess {
         typedef invalid_type AccessType;
         typedef invalid_type ConstAccessType;

         template <int I, int J, typename PP>
            static typename if_const<PP, ConstAccessType, AccessType>::type
         get(PP& p) {
            return p.template get<I, J>();
         }
      };

      /**
       * \brief Default traits for non-matrix types.
       * @ingroup openOR_core
       *
       * This traits class has to be specialized for concrete matrix implementations.
       */
      template<class Type>
      struct MatrixTraits {
         typedef Type MatrixType;                     //!< self type
         typedef Type RealMatrixType;                 //!< matrix type with a floating point value type
         typedef invalid_type ValueType;                   //!< type of a matrix element
         typedef boost::mpl::int_<0> RowDimension;    //!< number of rows (boost::mpl::int_<n>)
         typedef boost::mpl::int_<0> ColDimension;    //!< number of columns (boost::mpl::int_<n>)
         typedef invalid_type RowVectorType;               //!< type of a row vector
         typedef invalid_type ColVectorType;               //!< type of a column vector
         typedef invalid_type Vector3Type;                 //!< type of a vector with dimension 3 with same value type
		 typedef invalid_type Vector2Type;                 //!< type of a vector with dimension 2 with same value type
         typedef invalid_type Matrix33Type;                //!< type of a 3x3 matrix with same value type
         typedef DefaultMatrixAccess<Type> Access;    //!< functor type for element access
         typedef boost::mpl::false_ IsMatrix;         //!< flag to indicate that is is a valid matrix type (must be set to boost::mpl::true_ if specialized for matrix implementations.

         static MatrixType ZEROS;                     //!< matrix filled with zeros
         static MatrixType IDENTITY;                  //!< identify matrix
      };

      /**
       * @brief Traits for math type
       * @ingroup openOR_core
       */
      template<class Type>
      struct MathTraits {
        typedef typename boost::mpl::if_<typename MatrixTraits<Type>::IsMatrix,
                                           typename MatrixTraits<Type>::ValueType,
                                           typename boost::mpl::if_<typename VectorTraits<Type>::IsVector,
                                             typename VectorTraits<Type>::ValueType,
                                             typename ScalarTraits<Type>::ValueType>::type>::type ValueType;
        typedef ValueType *Pointer;
        typedef const ValueType *ConstPointer;
      };
   }
}

#endif


/// @file mopcap_pose.cpp : Implementation Qualisys Motion Capture convertor

#include "mocap_pose/mocap_pose.hpp"
#include "geodesy/utm.h"
#include "geodesy/wgs84.h"
#include <Eigen/Geometry>
#include <RTPacket.h>
#include <RTProtocol.h>
#include <px4_msgs/msg/sensor_gps.hpp>
#include <chrono>
#include <sstream>
#include <thread>
using namespace std::chrono;

struct MocapPose::Impl
{
    std::string serverAddress = "";
    unsigned short basePort = 22222;
    int sdkMajorVersion = 1;
    int sdkMinorVersion = 19;
    bool bigEndian = false;
    std::string bodyName = "drone";
    int velocity_type = 1;

    // "Home" point on Earth, from where we "linearize" our coordinates
    geodesy::UTMPoint home;
    // step in [sec] between publishing (setup with frequency/FPS parameter)
    double publishing_timestep = 0.0;

    rclcpp::Time last_timestamp{};
    rclcpp::Time last_published_timestamp{};
    Eigen::Vector3f last_position{};
    Eigen::Vector3f last_velocity{};

    rclcpp::Publisher<px4_msgs::msg::SensorGps>::SharedPtr publisher;
    std::thread worker_thread;
    std::atomic_bool worker_thread_running{};

    Eigen::Vector3f FixNans(Eigen::Vector3f vec)
    {
        if (std::isnan(vec[0]) || std::isnan(vec[1]) || std::isnan(vec[0]))
        {
            return Eigen::Vector3f(0, 0, 0);
        }
        else
        {
            return vec;
        }
    }

    px4_msgs::msg::SensorGps PrepareGpsMessage(const Eigen::Vector3f& position,
                                               const Eigen::Quaternionf& q,
                                               const rclcpp::Time& timestamp)
    {
        px4_msgs::msg::SensorGps sensor_gps{};

        // Position
        geodesy::UTMPoint utm = geodesy::UTMPoint(home);

        utm.easting += position[0];
        utm.northing += position[1];
        utm.altitude += position[2];

        const geographic_msgs::msg::GeoPoint point = toMsg(utm);

        Eigen::Vector3f velocity{};  // meters per second

        // Velocity
        if (last_timestamp.nanoseconds() > 0)
        {
            velocity = (position - last_position) / (timestamp.seconds() - last_timestamp.seconds());
        }
        velocity = FixNans(velocity);

        if (velocity_type > 1)
        {
            // filter velocity
            const float avg_factor = 0.1F;
            velocity = velocity * avg_factor + last_velocity * (1.F - avg_factor);
            velocity = FixNans(velocity);
        }

        last_timestamp = timestamp;
        last_position = position;
        last_velocity = velocity;

        // PX4 uses "steady_clock" timer, while Qualisys Mocap data comes with "system_clock" timestamps
        // Here we convert one to another, such that PX4 could use our message
        const auto system_now = time_point_cast<microseconds>(system_clock::now()).time_since_epoch().count();
        const auto steady_now = time_point_cast<microseconds>(steady_clock::now()).time_since_epoch().count();
        steady_clock::rep steady_stamp = timestamp.nanoseconds() / 1000ULL - system_now + steady_now;

        const double siny_cosp = 2.0 * (q.w() * q.z() + q.x() * q.y());
        const double cosy_cosp = 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
        const double heading_rad = -atan2(siny_cosp, cosy_cosp);

        sensor_gps.timestamp = steady_stamp;
        sensor_gps.lat = (uint32_t)std::round(point.latitude * 10000000);
        sensor_gps.lon = (uint32_t)std::round(point.longitude * 10000000);
        sensor_gps.alt = (uint32_t)std::round(point.altitude * 1000);
        sensor_gps.s_variance_m_s = 0.2f;

        sensor_gps.fix_type = 3;
        sensor_gps.eph = 0.5f;
        sensor_gps.epv = 0.8f;
        sensor_gps.hdop = 0.0f;
        sensor_gps.vdop = 0.0f;

        sensor_gps.time_utc_usec = timestamp.nanoseconds() / 1000ULL;  // system time (UTF) in millis
        sensor_gps.satellites_used = 16;
        sensor_gps.heading = heading_rad;
        sensor_gps.heading_offset = 0.0f;

        if (velocity_type > 0)
        {
            sensor_gps.vel_m_s = velocity.norm();
            sensor_gps.vel_n_m_s = velocity[1];
            sensor_gps.vel_e_m_s = velocity[0];
            sensor_gps.vel_d_m_s = -velocity[2];
            sensor_gps.cog_rad = atan2(velocity[0], velocity[1]);
            sensor_gps.vel_ned_valid = true;
        }
        else  // do not send velocity if velocity_type parameter == 0
        {
            sensor_gps.vel_m_s = NAN;
            sensor_gps.vel_n_m_s = NAN;
            sensor_gps.vel_e_m_s = NAN;
            sensor_gps.vel_d_m_s = NAN;
            sensor_gps.cog_rad = NAN;
            sensor_gps.vel_ned_valid = false;
        }

        return sensor_gps;
    }
};

MocapPose::MocapPose() : Node("MocapPose"), impl_(new MocapPose::Impl())
{
    RCLCPP_INFO(this->get_logger(), "MocapPose (Motion Capture Positioning Service)");

    declare_parameter<double>("home_lat", 61.50341);
    declare_parameter<double>("home_lon", 23.77509);
    declare_parameter<double>("home_alt", 110.0);
    declare_parameter<double>("frequency", 10.0);
    declare_parameter<int>("velocity_type", 1);
    declare_parameter<std::string>("server_address", "172.18.32.20");
    declare_parameter<std::string>("body_name", "sad");

    double frequency = 0.0;
    auto point = geographic_msgs::msg::GeoPoint();
    get_parameter("home_lat", point.latitude);
    get_parameter("home_lon", point.longitude);
    get_parameter("home_alt", point.altitude);
    get_parameter("server_address", impl_->serverAddress);
    get_parameter("body_name", impl_->bodyName);
    get_parameter("frequency", frequency);
    get_parameter("velocity_type", impl_->velocity_type);
    impl_->home = geodesy::UTMPoint(point);
    impl_->publishing_timestep = (frequency > 0.0) ? 1.0 / frequency : 0.0;

    RCLCPP_INFO(get_logger(), "Looking for body with name: %s", impl_->bodyName.c_str());
    RCLCPP_INFO(this->get_logger(),
                "Frequency: %lf, delay: %lf, Velocity type: %d",
                frequency,
                impl_->publishing_timestep,
                impl_->velocity_type);
    RCLCPP_INFO(get_logger(),
                "Initial Home coordinates: lat: %lf, lon: %lf, alt: %lf",
                point.latitude,
                point.longitude,
                point.altitude);

    RCLCPP_INFO(get_logger(),
                "east: %lf north: %lf alt: %lf  -  zone: %u%c",
                impl_->home.easting,
                impl_->home.northing,
                impl_->home.altitude,
                impl_->home.zone,
                impl_->home.band);

    impl_->publisher = create_publisher<px4_msgs::msg::SensorGps>("fmu/sensor_gps/in", 10);
    impl_->worker_thread_running = true;
    impl_->worker_thread = std::thread(&MocapPose::WorkerThread, this);
}

void MocapPose::Stop()
{
    impl_->worker_thread_running = false;
    if(impl_->worker_thread.joinable())
    {
        impl_->worker_thread.join();
    }
}

void MocapPose::WorkerThread()
{
    while (impl_->worker_thread_running)
    {
        auto latest_succesfull_receive = std::chrono::system_clock::now();
        CRTProtocol rtProtocol;

        bool dataAvailable = false;
        bool streamFrames = false;
        unsigned short udpPort = 6734;
        bool very_first_message = true;
        while (impl_->worker_thread_running)
        {
            auto now = std::chrono::system_clock::now();
            if (now > latest_succesfull_receive + std::chrono::seconds(10))
            {
                RCLCPP_WARN(get_logger(),
                            "Have not succesfully received for more than 10 secs - restarting receiver");
                break;
            }

            if (!rtProtocol.Connected())
            {
                if (!rtProtocol.Connect(impl_->serverAddress.c_str(),
                                        impl_->basePort,
                                        &udpPort,
                                        impl_->sdkMajorVersion,
                                        impl_->sdkMinorVersion,
                                        impl_->bigEndian))
                {
                    RCLCPP_WARN(get_logger(),
                                "Trying to connect to %s:%d : %s",
                                impl_->serverAddress.c_str(),
                                impl_->basePort,
                                rtProtocol.GetErrorString());
                    std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    continue;
                }
                else
                {
                    RCLCPP_INFO(
                        get_logger(), "Succesfully connected to %s:%d", impl_->serverAddress.c_str(), impl_->basePort);
                }
            }

            if (!dataAvailable)
            {
                if (!rtProtocol.Read6DOFSettings(dataAvailable))
                {
                    RCLCPP_WARN(get_logger(), "6DoF data is not available: %s", rtProtocol.GetErrorString());
                    std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    continue;
                }
                else
                {
                    RCLCPP_INFO(get_logger(), "6DoF data is available (server configured well)");
                }
            }

            if (!streamFrames)
            {
                if (!rtProtocol.StreamFrames(
                        CRTProtocol::RateAllFrames, 0, udpPort, nullptr, CRTProtocol::cComponent6d))
                {
                    RCLCPP_WARN(get_logger(), "Cannot start data streaming: %s", rtProtocol.GetErrorString());
                    std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    continue;
                }
                else
                {
                    RCLCPP_INFO(get_logger(), "Data streaming succesfully started");
                }
                streamFrames = true;
            }

            CRTPacket::EPacketType packetType;

            if (rtProtocol.Receive(packetType, true) == CNetwork::ResponseType::success)
            {
                if (packetType == CRTPacket::PacketData)
                {
                    float fX, fY, fZ;
                    float rotation[9];

                    CRTPacket* rtPacket = rtProtocol.GetRTPacket();
                    bool body_found = false;

                    for (unsigned int i = 0; i < rtPacket->Get6DOFBodyCount(); i++)
                    {
                        if (rtPacket->Get6DOFBody(i, fX, fY, fZ, rotation))
                        {
                            auto name = std::string(rtProtocol.Get6DOFBodyName(i));

                            if (name == impl_->bodyName)
                            {
                                body_found = true;

                                // convert millimeters into meters
                                const Eigen::Vector3f Pos = Eigen::Vector3f(fX, fY, fZ) / 1000.F;
                                const Eigen::Map<Eigen::Matrix3f> R(rotation);
                                const Eigen::Quaternionf Q(R);

                                RCLCPP_INFO(this->get_logger(),
                                            "Position: [%6.2f %6.2f %6.2f]\t Quaternion: [%6.3f %6.3f %6.3f %6.3f]",
                                            Pos[0],
                                            Pos[1],
                                            Pos[2],
                                            Q.w(),
                                            Q.x(),
                                            Q.y(),
                                            Q.z());

                                const auto timestamp = rclcpp::Clock().now();
                                const auto gps_msg = impl_->PrepareGpsMessage(Pos, Q, timestamp);
                                const bool time_to_publish = (impl_->last_published_timestamp.seconds() +
                                                              impl_->publishing_timestep) <= timestamp.seconds();

                                const bool translation_is_valid =
                                    !std::isnan(Pos[0]) && !std::isnan(Pos[1]) && !std::isnan(Pos[2]);
                                const bool rotation_is_valid = !std::isnan(Q.w()) && !std::isnan(Q.x()) &&
                                                               !std::isnan(Q.y()) && !std::isnan(Q.z());
                                const bool data_is_valid = translation_is_valid && rotation_is_valid;

                                latest_succesfull_receive = std::chrono::system_clock::now();

                                if (data_is_valid)
                                {
                                    if (very_first_message || time_to_publish)
                                    {
                                        RCLCPP_INFO(this->get_logger(),
                                                    "Publish GPS at time %lf, previous_time: %lf",
                                                    timestamp.seconds(),
                                                    impl_->last_published_timestamp.seconds());
                                        impl_->publisher->publish(gps_msg);
                                        impl_->last_published_timestamp = timestamp;
                                        very_first_message = false;
                                    }
                                }
                                else
                                {
                                    RCLCPP_WARN(get_logger(), "Data is full of NaNs and thus NOT PUBLISHED!");
                                }
                            }
                        }
                    }

                    if (!body_found)
                    {
                        RCLCPP_WARN(get_logger(),
                                    "Cannot find body named \"%s\" in Qualisys Data Stream",
                                    impl_->bodyName.c_str());
                    }
                }
            }
        }
        rtProtocol.StopCapture();
        rtProtocol.Disconnect();
    }
}

MocapPose::~MocapPose()
{
    impl_->worker_thread_running = false;
    if (impl_->worker_thread.joinable())
    {
        impl_->worker_thread.join();
    }
}


#include <immintrin.h>
#include <cassert>
#include <cstdlib>
#include <iostream>
#include <boost/hana.hpp>

#include "blend256.h"

namespace hana = boost::hana;
using namespace hana::literals;

int main() {
#ifdef mask_1100_0011_0000_0000_1100_0011_0000_0000
  // clang-format off
  auto mask = hana::make_tuple(
      hana::true_c,  hana::true_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::true_c,  hana::true_c,

      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,

      hana::true_c,  hana::true_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::true_c,  hana::true_c,

      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c
 );
  // clang-format on
#endif

#ifdef mask_1111_0000_1111_0000_0000_0000_0000_0000
  // clang-format off
  auto mask = hana::make_tuple(
      hana::true_c,  hana::true_c,
      hana::true_c,  hana::true_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,

      hana::true_c,  hana::true_c,
      hana::true_c,  hana::true_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,

      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,

      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c
 );
  // clang-format on
#endif

#ifdef mask_1000_0000_0000_0000_0000_0000_0000_0000
  // clang-format off
  auto mask = hana::make_tuple(
      hana::true_c,  hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,

      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,

      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,

      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c,
      hana::false_c, hana::false_c
 );
  // clang-format on
#endif

  __m256i a = _mm256_set1_epi8(0);
  // clang-format off
  __m256i b = _mm256_setr_epi8( 1,  2,  3,  4,  5,  6,  7,  8,
                                9, 10, 11, 12, 13, 14, 15, 16,
                               17, 18, 19, 20, 21, 22, 23, 24,
                               25, 26, 27, 28, 29, 30, 31, 32);
  // clang-format on
  auto blend_result = blend256(a, b, mask);

  int return_value = EXIT_SUCCESS;
  try {
    verify_blend_result(blend_result, a, b, mask);
  } catch (const std::runtime_error& e) {
    std::cerr << "std::runtime_error = " << e.what() << std::endl;
    return_value = EXIT_FAILURE;
  } catch (...) {
    assert(false);  // Should never happen
  }
  return return_value;
}


#pragma once

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <unordered_map>

// Type declarations for node ids and way ids.  We don't need the full 64 bits
// for ways as the highest way number isn't close to 2^32 yet.
typedef std::uint64_t external_nodeid_t;
typedef std::uint32_t internal_nodeid_t;
typedef std::uint32_t wayid_t;
typedef std::uint8_t segment_speed_t;

constexpr float kKmPerMile = 1.609344f;

// Way ID, and whether it the node pair for it is stored forward or backward
typedef struct
{
    wayid_t id;
    bool forward;
} way_storage_t;

static constexpr segment_speed_t INVALID_SPEED = std::numeric_limits<segment_speed_t>::max();

static constexpr internal_nodeid_t INVALID_INTERNAL_NODEID =
    std::numeric_limits<internal_nodeid_t>::max();
static constexpr wayid_t INVALID_WAYID = std::numeric_limits<wayid_t>::max();

typedef boost::geometry::model::
    point<double, 2, boost::geometry::cs::spherical_equatorial<boost::geometry::degree>>
        point_t;
typedef std::pair<point_t, internal_nodeid_t> value_t;

// Data types for our lookup tables
typedef std::pair<internal_nodeid_t, internal_nodeid_t> internal_nodepair_t;

typedef std::unordered_map<external_nodeid_t, internal_nodeid_t> external_internal_map_t;

typedef std::vector<wayid_t> annotated_route_t;

// Every unique string gets an ID of this type
typedef std::uint32_t stringid_t;

// A way has several tags.  The tagrange is the index of the
// first tag table entry, second is the index of the last
// tag table entry
typedef std::pair<std::uint32_t, std::uint32_t> tagrange_t;

// Pairs of string ids in the string table.  first = string ID
// of the key, second = string ID of the value
typedef std::pair<stringid_t, stringid_t> keyvalue_index_t;

// Indexes of strings in the char buffer.  first = first character
// position, second = length of string
typedef std::pair<std::uint32_t, std::uint32_t> stringoffset_t;

// Custom hashing functions for our data types to use in standard containers (sets, maps, etc).
namespace std
{
// This is so that we have reasonable hashing of pairs of nodeids
template <> struct hash<internal_nodepair_t>
{
    inline size_t operator()(const internal_nodepair_t &v) const
    {
        return (static_cast<std::uint64_t>(v.first) << 32) + v.second;
    }
};
} // namespace std


//=============================================================================================================
/**
* @file     spectrogram.cpp
* @author   Martin Henfling <martin.henfling@tu-ilmenau.de>;
*           Daniel Knobl <daniel.knobl@tu-ilmenau.de>;
* @version  1.0
* @date     September, 2015
*
* @section  LICENSE
*
* Copyright (C) 2014, Martin Henfling and Daniel Knobl. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors 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.
*
*
* @brief    Implementation of spectrogram class.
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "spectrogram.h"
#include "math.h"

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/SparseCore>
#include <unsupported/Eigen/FFT>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

VectorXd Spectrogram::gauss_window (qint32 sample_count, qreal scale, quint32 translation)
{
    VectorXd gauss = VectorXd::Zero(sample_count);

    for(qint32 n = 0; n < sample_count; n++)
    {
        qreal t = (qreal(n) - translation) / scale;
        gauss[n] = exp(-3.14 * pow(t, 2))*pow(sqrt(scale),(-1))*pow(qreal(2),(0.25));
    }

    return gauss;
}

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

MatrixXd Spectrogram::make_spectrogram(VectorXd signal, qint32 window_size = 0)
{
    if(window_size == 0)
        window_size = signal.rows()/4;

    Eigen::FFT<double> fft;
    MatrixXd tf_matrix = MatrixXd::Zero(signal.rows()/2, signal.rows());

    for(qint32 translate = 0; translate < signal.rows(); translate++)
    {
        VectorXd envelope = gauss_window(signal.rows(), window_size, translate);

        VectorXd windowed_sig = VectorXd::Zero(signal.rows());
        VectorXcd fft_win_sig = VectorXcd::Zero(signal.rows());

        VectorXd real_coeffs = VectorXd::Zero(signal.rows()/2);

        for(qint32 sample = 0; sample < signal.rows(); sample++)
            windowed_sig[sample] = signal[sample] * envelope[sample];

        fft.fwd(fft_win_sig, windowed_sig);

        for(qint32 i= 0; i<signal.rows()/2; i++)
        {
            qreal value = pow(abs(fft_win_sig[i]), 2.0);
            real_coeffs[i] = value;
        }

        tf_matrix.col(translate) = real_coeffs;
    }
    return tf_matrix;
}


//==================================================================================================
/*!
  @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_CONSTANT_DEFINITION_VALMAX_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_VALMAX_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/as_unsigned.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>
#include <climits>

namespace boost { namespace simd
{
  namespace tag
  {
    struct valmax_ : boost::dispatch::constant_value_<valmax_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,valmax_,boost::dispatch::constant_value_<valmax_>);

      struct value_map
      {
        template<typename X>
        static std::integral_constant<X,0x7F> value(boost::dispatch::int8_<X> const&);

        template<typename X>
        static std::integral_constant<X,0x7FFF> value(boost::dispatch::int16_<X> const&);

        template<typename X>
        static std::integral_constant<X,0x7FFFFFFF> value(boost::dispatch::int32_<X> const&);

        template<typename X>
        static std::integral_constant<X,0x7FFFFFFFFFFFFFFF> value(boost::dispatch::int64_<X> const&);

        template<typename X>
        static std::integral_constant<X,X(0xFFFFFFFFFFFFFFFFULL)> value(boost::dispatch::uint_<X> const&);

        template<typename X>
        static brigand::single_<0x7F7FFFFF> value(boost::dispatch::single_<X> const&);

        template<typename X>
        static brigand::double_<0x7FEFFFFFFFFFFFFFULL> value(boost::dispatch::double_<X> const&);
      };
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,valmax_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::valmax_,valmax);
  }

  template<typename T> BOOST_FORCEINLINE auto Valmax()
  -> decltype(detail::valmax(boost::dispatch::as_<T>{}))
  {
    return detail::valmax( boost::dispatch::as_<T>{} );
  }
} }

#endif


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_AVX_DEINTERLEAVE_FIRST_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_AVX_DEINTERLEAVE_FIRST_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/swar/functions/deinterleave_first.hpp>
#include <boost/simd/sdk/simd/meta/retarget.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( deinterleave_first_
                                   , boost::simd::tag::avx_
                                   , (A0)
                                   , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      result_type x = _mm256_permute2f128_ps(a0,a1,0x20);
      result_type y = _mm256_permute2f128_ps(a0,a1,0x31);

      return _mm256_unpacklo_ps ( _mm256_unpacklo_ps(x,y)
                                , _mm256_unpackhi_ps(x,y)
                                );
     }
  };

  BOOST_DISPATCH_IMPLEMENT         ( deinterleave_first_
                                   , boost::simd::tag::avx_
                                   , (A0)
                                   , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      return _mm256_unpacklo_pd ( _mm256_permute2f128_pd(a0,a1,0x20)
                                , _mm256_permute2f128_pd(a0,a1,0x31)
                                );
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( deinterleave_first_
                                   , boost::simd::tag::avx_
                                   , (A0)
                                   , ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<arithmetic_<A0>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      typedef typename meta::retarget<A0, boost::simd::tag::sse_ >::type svtype;

      svtype a00 = _mm256_extractf128_si256(a0, 0);
      svtype a01 = _mm256_extractf128_si256(a0, 1);
      svtype a10 = _mm256_extractf128_si256(a1, 0);
      svtype a11 = _mm256_extractf128_si256(a1, 1);
      __m256i that = _mm256_castsi128_si256(boost::simd::deinterleave_first(a00, a01));
      return  _mm256_insertf128_si256(that,  boost::simd::deinterleave_first(a10, a11), 1);
     }
  };
} } }

#endif
#endif


#ifndef NEPOMUK_SEARCH_COORDINATE_TO_STOP_HPP_
#define NEPOMUK_SEARCH_COORDINATE_TO_STOP_HPP_

#include "geometric/coordinate.hpp"
#include "id/stop.hpp"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <utility>
#include <vector>

namespace nepomuk
{

namespace geometric
{
class WGS84BoundingBox;
}

namespace search
{

class CoordinateToStop
{
  public:
    CoordinateToStop(std::vector<std::pair<geometric::WGS84Coordinate, StopID>> const &);

    using value_type = std::pair<StopID, geometric::WGS84Coordinate>;

    // find the nearest stop to a location
    value_type nearest(geometric::WGS84Coordinate const &location) const;
    std::vector<value_type> nearest(geometric::WGS84Coordinate const &location,
                                    std::uint32_t count) const;
    std::vector<value_type> all(geometric::WGS84BoundingBox const &bounding_box) const;
    std::vector<value_type> all(geometric::WGS84Coordinate const &location,
                                double const radius) const;

    using PointT = boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian>;

  private:
    using RTreeEntry = std::pair<PointT, StopID>;
    using RTree = boost::geometry::index::rtree<RTreeEntry, boost::geometry::index::quadratic<64>>;

    RTree tree;
};

} // namespace search
} // namespace nepomuk

#endif // NEPOMUK_SEARCH_COORDINATE_TO_STOP_HPP_


/*
 * prefilter_view_pairs.cc
 *
 *  Created on: Jul 12, 2016
 *      Author: LeonMing
 */

#include "theia/sfm/view_graph/prefilter_view_pairs.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <glog/logging.h>
#include <unordered_map>
#include <unordered_set>
#include <cereal/access.hpp>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <cmath>

#include "theia/math/util.h"
#include "theia/math/graph/minimum_spanning_tree.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/util/filesystem.h"

#include <lemon/lp.h>
#include <lemon/lp_base.h>

namespace theia {

std::ofstream fileWriter("cycle_error_report.txt", std::ios::out);

bool PrefilterViewPairs(ViewGraph* viewGraph) {
  CHECK_NOTNULL(viewGraph);

// Declare MST extractor and MST and edges
  MinimumSpanningTree<ViewId, int> mst_extractor;
  std::unordered_set<ViewIdPair> mst;
  std::unordered_map<ViewIdPair, TwoViewInfo> edges;

// Create MST
  edges = viewGraph->GetAllEdges();

  for (const auto& edge : edges) {
    mst_extractor.AddEdge(edge.first.first, edge.first.second, 1);
  }

// Add values to MST
  mst_extractor.Extract(&mst);

// Preparing variables for creating chains
  std::vector<std::vector<ViewIdPair> > chainList;
  std::unordered_set<ViewIdPair> visited;
  std::vector<ViewIdPair> oldChain;
// Create chains
  CreateChains(chainList, mst, visited, oldChain, mst.begin()->first);

// Preparing variables for creating cycles
// Store cycles as vectors of ViewIdPairs, with a big vector holding all the cycles
  std::vector<std::vector<ViewIdPair> > cycleList;
// Create cycles
  CreateCycles(edges, chainList, mst, cycleList);

// Preparing variables for calculating errors
  std::unordered_set<ViewIdPair> allUsedEdges;
  std::vector<double> errors;
// Calculate and print cycle errors
 CalculateErrors(cycleList, edges, allUsedEdges, errors);

// Preparing variables for calculating translation errors
  std::unordered_set<ViewIdPair> allUsedTranslationEdges;
  std::vector<std::vector<ViewIdPair> > translationCycles;
  std::vector<double> translationErrors;
// Calculate translation cycle errors
  CalculateTranslationErrors(cycleList, translationCycles, edges, allUsedTranslationEdges, translationErrors);

// Preparing variables for inferencing
  std::unordered_map<int, ViewIdPair> number2Edge;
  std::unordered_map<ViewIdPair, int> edge2Number;
  int it = 0;
  for (auto& edge : allUsedEdges) {
	  number2Edge.emplace(it, edge);
	  edge2Number.emplace(edge, it);
	  ++it;
  }

  std::unordered_map<int, ViewIdPair> number2TranslationEdge;
  std::unordered_map<ViewIdPair, int> translationEdge2Number;
  int it2 = 0;
  for (auto& edge : allUsedTranslationEdges) {
	  number2TranslationEdge.emplace(it2, edge);
	  translationEdge2Number.emplace(edge, it2);
	  ++it2;
  }

  double rotSigma = 2*M_PI/180;
  double transSigma = 0.011;

// Edges that we need to remove
  std::unordered_set<ViewIdPair> viewPairsToRemove;
// Do inferences to figure out what edges to remove
  DoInference(rotSigma, cycleList, errors, edge2Number, number2Edge, allUsedEdges, viewPairsToRemove);
// Do inferences based on relative translations
  DoInference(transSigma, translationCycles, translationErrors, translationEdge2Number, number2TranslationEdge, allUsedTranslationEdges, viewPairsToRemove);

// Remove the edge if it is incorrect
  for (const ViewIdPair viewIdPair : viewPairsToRemove) {
    viewGraph->RemoveEdge(viewIdPair.first, viewIdPair.second);
  }
  VLOG(1) << "Removed " << viewPairsToRemove.size()
          << " view pairs by rotation prefiltering.";
  return true;
}

void CreateChains(std::vector<std::vector<ViewIdPair> >& chainList,
			      std::unordered_set<ViewIdPair>& mst,
				  std::unordered_set<ViewIdPair>& visited,
				  std::vector<ViewIdPair> oldChain,
				  ViewId root) {
	for (ViewIdPair e : mst) {
		if (visited.find(e) != visited.end()) {
			continue;
		}
		if (e.first == root) {
			oldChain.emplace_back(e);
			std::vector<ViewIdPair> chain = oldChain;
			oldChain.pop_back();
			chainList.emplace_back(chain);
			visited.insert(e);

			CreateChains(chainList, mst, visited, chain, e.second);
		}
		else if (e.second == root) {
			oldChain.emplace_back(e);
			std::vector<ViewIdPair> chain = oldChain;
			oldChain.pop_back();
			chainList.emplace_back(chain);
			visited.insert(e);

			CreateChains(chainList, mst, visited, chain, e.first);
		}
	}
}

void CreateCycles(std::unordered_map<ViewIdPair, TwoViewInfo>& edges,
				  std::vector<std::vector<ViewIdPair> >& chainList,
				  std::unordered_set<ViewIdPair>& mst,
				  std::vector<std::vector<ViewIdPair> >& cycleList) {

	std::unordered_set<ViewIdPair> otherEdges; // not in MST

	for (auto& e : edges) {
		if (mst.find(e.first) == mst.end()) {
			otherEdges.insert(e.first);
		}
	}

	for (ViewIdPair e : otherEdges) {
		if (e.first == mst.begin()->first || e.second == mst.begin()->first) {
			continue;
		}
		std::vector<ViewIdPair> cycle;
		cycle.emplace_back(e);
		std::vector<ViewIdPair> chain1;
		std::vector<ViewIdPair> chain2;

		for (std::vector<ViewIdPair> chain : chainList) {
			ViewIdPair lastE = chain.end()[-1];
			ViewIdPair secondtolastE = chain.end()[-2];
			if (
				((e.first == lastE.first || e.first == lastE.second)
				&& !(e.first == secondtolastE.first || e.first == secondtolastE.second))
				||
				((e.first == lastE.first || e.first == lastE.second)
				&& (chain.size() == 1))
				) {
				chain1 = chain;
				continue;
			}
			if (
				((e.second == lastE.first || e.second == lastE.second)
				&& !(e.second == secondtolastE.first || e.second == secondtolastE.second))
				||
				((e.second == lastE.first || e.second == lastE.second)
				&& (chain.size() == 1))
				) {
				chain2 = chain;
				continue;
			}
		}

		std::set_symmetric_difference(chain1.begin(), chain1.end(),
				                      chain2.begin(), chain2.end(),
				                      std::back_inserter(cycle));
		cycleList.emplace_back(cycle);
	}
}

void CalculateErrors(std::vector<std::vector<ViewIdPair> >& cycleList,
					 std::unordered_map<ViewIdPair, TwoViewInfo>& edges,
					 std::unordered_set<ViewIdPair>& allUsedEdges,
					 std::vector<double>& errors) {
	for (std::vector<ViewIdPair> cycle : cycleList) {

		double error;
		Eigen::Matrix3d combinedRotation;
		ceres::AngleAxisToRotationMatrix(
			edges[cycle[0]].rotation_2.data(),
			ceres::RowMajorAdapter3x3(combinedRotation.data()));

		std::vector<ViewIdPair> usedEdges;
		usedEdges.push_back(cycle.front());
		allUsedEdges.insert(cycle.front());
		std::vector<ViewId> nextEdge;
		nextEdge.push_back(cycle.front().second);
		while (usedEdges.size() < cycle.size()) {
			for (ViewIdPair edge : cycle) {

				TwoViewInfo info = edges[edge];
				Eigen::Matrix3d rotation_matrix;

				if (std::find(usedEdges.begin(), usedEdges.end(), edge) != usedEdges.end()) {
					continue;
				} else if (edge.first == nextEdge.back()) {
					ceres::AngleAxisToRotationMatrix(
						info.rotation_2.data(),
						ceres::RowMajorAdapter3x3(rotation_matrix.data()));
					combinedRotation *= rotation_matrix;
					usedEdges.push_back(edge);
					if (allUsedEdges.find(edge) == allUsedEdges.end()) {
						allUsedEdges.insert(edge);
					}
					nextEdge.push_back(edge.second);
					fileWriter << rotation_matrix;
					fileWriter << "\n\n";
				} else if (edge.second == nextEdge.back()) {
					ceres::AngleAxisToRotationMatrix(
						info.rotation_2.data(),
						ceres::RowMajorAdapter3x3(rotation_matrix.data()));
					combinedRotation *= rotation_matrix.transpose();
					usedEdges.push_back(edge);
					if (allUsedEdges.find(edge) == allUsedEdges.end()) {
						allUsedEdges.insert(edge);
					}
					nextEdge.push_back(edge.first);
					fileWriter << rotation_matrix.transpose();
					fileWriter << "\n\n";
				} else {
					continue;
				}
			}
		}
		fileWriter << "Combined Rotation:\n";
		fileWriter << combinedRotation;

		fileWriter << "\n\nError:\n";
		error = acos((combinedRotation.trace()-1)/2);
		fileWriter << error << "\n\n\n\n";
		errors.push_back(error);
	}
}

void CalculateTranslationErrors(std::vector<std::vector<ViewIdPair> >& cycleList,
								std::vector<std::vector<ViewIdPair> >& translationCycles,
		 	 	 	 	 	 	std::unordered_map<ViewIdPair, TwoViewInfo>& edges,
								std::unordered_set<ViewIdPair>& allUsedTranslationEdges,
								std::vector<double>& translationErrors) {
	for (auto& cycle : cycleList) {
		if (cycle.size() == 3) {
			translationCycles.push_back(cycle);
		}
	}
	std::cout << "Singular Values: " << "\n\n";
	for (auto& cycle : translationCycles) {
		Eigen::Matrix3d translationsMatrix;
		for (int i = 0; i < 3; ++i) {
			translationsMatrix(i, 0) = edges[cycle.front()].position_2[i];
		}
		std::vector<ViewIdPair> usedEdges;
		usedEdges.push_back(cycle.front());
		allUsedTranslationEdges.insert(cycle.front());
		std::vector<ViewId> nextEdge;
		nextEdge.push_back(cycle.front().second);
		while (usedEdges.size() < 3) {
			for (int j = 1; j < 3; ++j) {
				if (std::find(usedEdges.begin(), usedEdges.end(), cycle[j]) != usedEdges.end()) {
					continue;
				} else if (cycle[j].first == nextEdge.back()) {
					for (int i = 0; i < 3; ++i) {
						translationsMatrix(i, j) = edges[cycle[j]].position_2[i];
					}
					usedEdges.push_back(cycle[j]);
					if (allUsedTranslationEdges.find(cycle[j]) == allUsedTranslationEdges.end()) {
						allUsedTranslationEdges.insert(cycle[j]);
					}
					nextEdge.push_back(cycle[j].second);
				} else if (cycle[j].second == nextEdge.back()) {
					for (int i = 0; i < 3; ++i) {
						translationsMatrix(i, j) = -(edges[cycle[j]].position_2[i]);
					}
					usedEdges.push_back(cycle[j]);
					if (allUsedTranslationEdges.find(cycle[j]) == allUsedTranslationEdges.end()) {
						allUsedTranslationEdges.insert(cycle[j]);
					}
					nextEdge.push_back(cycle[j].first);
				}
			}
		}
//		std::cout << translationsMatrix << "\n";
		Eigen::JacobiSVD<Eigen::Matrix3d> svd(translationsMatrix);
		double error = svd.singularValues()[2];
		translationErrors.push_back(error);
	}
	for (int i = 0; i < translationErrors.size(); ++i) {
		std::cout << "Cycle " << i << "\n" << translationErrors[i] << "\n\n";
	}
}

void DoInference(double sigma,
				 std::vector<std::vector<ViewIdPair> >& cycleList,
				 std::vector<double>& errors,
				 std::unordered_map<ViewIdPair, int>& edge2Number,
				 std::unordered_map<int, ViewIdPair>& number2Edge,
				 std::unordered_set<ViewIdPair>& allUsedEdges,
				 std::unordered_set<ViewIdPair>& viewPairsToRemove) {
	const double rhoe = (-log(0.1)) - (-log(0.9));
	double rhol;

	const double sigmaInliers = sigma;
	const double eps = pow(2,-52);

	using namespace lemon;

	// Create an instance of the default LP solver class
	// (it will represent an "empty" problem at first)
	Lp lp;

	// Set up variables
	std::vector<Lp::Col> xe;
	std::vector<Lp::Col> xl;
	Lp::Expr totalEdges;
	Lp::Expr totalCycles;
	std::map<std::vector<ViewIdPair>, std::vector<int> > index;
	for (auto& cycle : cycleList) {
		std::vector<int> tempVector;
		for (auto& edge : cycle) {
			tempVector.push_back(edge2Number[edge]);
		}
		index.emplace(cycle, tempVector);
	}

	// Add columns (variables) to the problem
	for (int i = 0; i < allUsedEdges.size(); ++i) { // For every edge used in any cycle, make a new xe
		xe.push_back(lp.addCol());
	}
	for (int i = 0; i < cycleList.size(); ++i) { // For every cycle in the cycleList, make a new xl
		xl.push_back(lp.addCol());
	}

	// Add rows (constraints) to the problem
	for (int cycle = 0; cycle < cycleList.size(); ++cycle) { // For every cycle in the cycleList

		Lp::Expr total;
		for (int edge = 0; edge < cycleList[cycle].size(); ++edge) {
			lp.addRow(xl[cycle] >= xe[(index[cycleList[cycle]])[edge]]); // xl for this cycle is greater than each xe
			total += xe[(index[cycleList[cycle]])[edge]];
			totalEdges += total;
		}
		lp.addRow(xl[cycle] <= total);

		// Calculate rhol for this cycle
		double pErrorInliers = exp(-errors[cycle]/sigmaInliers)/(sigmaInliers*(1-exp(-M_PI/sigmaInliers)));
		double pErrorOutliers = (1-exp(-errors[cycle]/sigmaInliers))/(M_PI-sigmaInliers*(1-exp(-M_PI/sigmaInliers)));
		rhol = (-log(pErrorOutliers + eps))-(-log(pErrorInliers + eps));
		totalCycles += rhol * xl[cycle];
	}

	// Set lower and upper bounds for the columns (variables)
	for (int k = 0; k < xe.size(); ++k) {
		lp.colBounds(xe[k], 0, 1);
	}
	for (int k = 0; k < xl.size(); ++k) {
		lp.colBounds(xl[k], 0, 1);
	}

	// Specify the objective function
	lp.min();
	lp.obj(rhoe * totalEdges + totalCycles);

	// Solve the problem using the underlying LP solver
	lp.solve();

	// Print the results
	if (lp.primalType() == Lp::OPTIMAL) {
		std::cout << "Objective function value: " << lp.primal() << "\n\n";
		for (int i = 0; i < xe.size(); ++i) {
			std::cout << "Edge " << i << ": " << number2Edge[i].first << " " << number2Edge[i].second << ":\t" << lp.primal(xe[i]) << "\n";
			if (lp.primal(xe[i]) == 1) {
				viewPairsToRemove.insert(number2Edge[i]);
			}
		}
		for (int i = 0; i < xl.size(); ++i) {
			std::cout << "Cycle " << i << ": " << lp.primal(xl[i]) << "\n";
		}
	} else {
		std::cout << "Optimal solution not found." << "\n";
	}
}

}  // namespace theia


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "armnnOnnxParser/IOnnxParser.hpp"
#include  "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(OnnxParser)

struct ReluMainFixture : public armnnUtils::ParserPrototxtFixture<armnnOnnxParser::IOnnxParser>
{
    ReluMainFixture()
    {
        m_Prototext = R"(
                   ir_version: 3
                   producer_name:  "CNTK"
                   producer_version:  "2.5.1"
                   domain:  "ai.cntk"
                   model_version: 1
                   graph {
                     name:  "CNTKGraph"
                     input {
                        name: "Input"
                        type {
                          tensor_type {
                            elem_type: 1
                            shape {
                              dim {
                                dim_value: 4
                              }
                            }
                          }
                        }
                      }
                     node {
                         input: "Input"
                         output: "Output"
                         name: "ActivationLayer"
                         op_type: "Relu"
                    }
                      output {
                          name: "Output"
                          type {
                             tensor_type {
                               elem_type: 1
                               shape {
                                   dim {
                                       dim_value: 4
                                   }
                               }
                            }
                         }
                      }
                    }
                   opset_import {
                      version: 7
                    })";
        Setup();
    }
};

BOOST_FIXTURE_TEST_CASE(ValidReluTest, ReluMainFixture)
{
    RunTest<1>({{"Input",  { -1.0f, -0.5f, 1.25f, -3.0f}}},
               {{ "Output", { 0.0f, 0.0f, 1.25f, 0.0f}}});
}

BOOST_AUTO_TEST_SUITE_END()


/***************************************************************************************************************************
 * payload_drop.cpp
 *
 * Author: Qyp
 *
 * Update Time: 2019.06.07
 *
 * Introduction: payload_drop.cpp
 * 1.Takeoff
 * 2.Fly to the target position
 * 3.Decrease the altitude
 * 4.Drop
 * 5.Increase the altitude
 * 6.Back to the takeoff point
 * 7.Land
***************************************************************************************************************************/

#include <ros/ros.h>
#include <fstream>
#include <math.h>
#include <string>
#include <time.h>
#include <queue>
#include <vector>
#include <cstdlib>
#include <stdlib.h>
#include <iostream>
#include <stdio.h>
#include <std_msgs/Bool.h>
#include <px4_command/command.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <geometry_msgs/PoseStamped.h>

#include <Eigen/Eigen>

using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
enum Command
{
    Idle,
    Takeoff,
    Move_ENU,
    Move_Body,
    Hold,
    Land,
    Disarm,
    Failsafe_land,
};
px4_command::command Command_now;
//---------------------------------------正方形参数---------------------------------------------
float fly_height;
float target_x,target_y;
float drop_height;
float drop_flag;
float limit_time;
float min_distance;
float distance_to_target;

int time_sec = 0;
int comid = 0;
int switch_flag = 0;

Eigen::Vector3f drone_pos;
Eigen::Vector3f point1;
Eigen::Vector3f point2;
Eigen::Vector3f point3;

float cal_distance(Eigen::Vector3f a,Eigen::Vector3f b);
void printf_result(Eigen::Vector3f target_point);

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    drone_pos  = Eigen::Vector3f(msg->pose.position.x,msg->pose.position.y,msg->pose.position.z);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "payload_drop");
    ros::NodeHandle nh("~");

    //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>参数读取<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    nh.param<float>("fly_height", fly_height, 1.0);
    nh.param<float>("target_x", target_x, 0.0);
    nh.param<float>("target_y", target_y, 0.0);
    nh.param<float>("drop_height", drop_height, 1.5);
    nh.param<float>("limit_time", limit_time, 10.0);
    nh.param<float>("min_distance", min_distance, 0.2);

    // 频率 [1hz]
    ros::Rate rate(1.0);

    //Subscribe the drone position
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, pos_cb);

    // 【发布】发送给position_control.cpp的命令
    ros::Publisher move_pub = nh.advertise<px4_command::command>("/px4/command", 10);

    // Drop cmd send to mavros
    ros::Publisher drop_pub = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", 10);

    mavros_msgs::OverrideRCIn drop_cmd;

    drop_cmd.channels[0] = 0;
    drop_cmd.channels[1] = 0;
    drop_cmd.channels[2] = 0;
    drop_cmd.channels[3] = 0;
    drop_cmd.channels[4] = 0;
    drop_cmd.channels[5] = 0;
    drop_cmd.channels[6] = 0;
    drop_cmd.channels[7] = 1600;

    drop_flag = 0;

    int check_flag;
    // Check the parameter
    cout << "fly_height: "<<fly_height<<"[m]"<<endl;
    cout << "target_x: "<<target_x<<"[m]"<<endl;
    cout << "target_y: "<<target_y<<"[m]"<<endl;
    cout << "drop_height: "<<drop_height<<"[m]"<<endl;
    cout << "limit_time: "<<limit_time<<"[s]"<<endl;
    cout << "min_distance: "<<min_distance<<"[m]"<<endl;

    cout << "Please check the parameter and setting，1 for go on， else for quit: "<<endl;
    cin >> check_flag;

    if(check_flag != 1)
    {
        return -1;
    }

    //Read the takeoff point
    int i =0;
    for(i=0;i<5;i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    point1[0] = drone_pos[0];
    point1[1] = drone_pos[1];
    point1[2] = fly_height;

    point2[0] = target_x;
    point2[1] = target_y;
    point2[2] = fly_height;

    point3[0] = target_x;
    point3[1] = target_y;
    point3[2] = drop_height;

    //Takeoff - fly to point 1
    while (switch_flag == 0)
    {
        Command_now.command = Move_ENU;         //Move模式
        Command_now.sub_mode = 0;               //子模式：位置控制模式
        Command_now.pos_sp[0] = point1[0];
        Command_now.pos_sp[1] = point1[1];
        Command_now.pos_sp[2] = point1[2];
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;

        move_pub.publish(Command_now);

        distance_to_target = cal_distance(drone_pos,point1);

        if(time_sec > limit_time ||  distance_to_target< min_distance )
        {
            switch_flag =1;
        }

        time_sec++;

        printf_result(point1);

        cout << "Takeoff: flying to Point 1"<<endl;

        rate.sleep();
        ros::spinOnce();

    }

    //wait 1s more
    sleep(1.0);
    //reset the flag
    switch_flag = 0;
    time_sec = 0;

    //Fly to target - to point 2
    while (switch_flag == 0)
    {
        Command_now.command = Move_ENU;         //Move模式
        Command_now.sub_mode = 0;               //子模式：位置控制模式
        Command_now.pos_sp[0] = point2[0];
        Command_now.pos_sp[1] = point2[1];
        Command_now.pos_sp[2] = point2[2];
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;

        move_pub.publish(Command_now);

        distance_to_target = cal_distance(drone_pos,point2);

        if(time_sec > limit_time ||  distance_to_target< min_distance )
        {
            switch_flag =1;
        }

        time_sec++;

        printf_result(point2);

        cout << "Fly to target - Flying to Point 2"<<endl;

        rate.sleep();
        ros::spinOnce();

    }

    //wait 1s more
    sleep(1.0);
    //reset the flag
    switch_flag = 0;
    time_sec = 0;

    //Point 3 get target and decrease the height

    while (switch_flag == 0)
    {
        Command_now.command = Move_ENU;         //Move模式
        Command_now.sub_mode = 0;               //子模式：位置控制模式
        Command_now.pos_sp[0] = point3[0];
        Command_now.pos_sp[1] = point3[1];
        Command_now.pos_sp[2] = point3[2];
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;

        move_pub.publish(Command_now);

        distance_to_target = cal_distance(drone_pos,point3);

        if(time_sec > limit_time ||  distance_to_target< min_distance )
        {
            switch_flag =1;
        }

        time_sec++;

        printf_result(point3);

        cout << "Get target and decrease the height- flying to Point 3"<<endl;

        rate.sleep();
        ros::spinOnce();

    }

    //wait 1s more
    sleep(1.0);
    //reset the flag
    switch_flag = 0;
    time_sec = 0;

    //Drop
    while (time_sec < 3)
    {
        drop_pub.publish(drop_cmd);

        cout << "Droping......"<<endl;

        time_sec++;
        rate.sleep();
        ros::spinOnce();
    }

    //wait 1s more
    sleep(1.0);
    //reset the flag
    switch_flag = 0;
    time_sec = 0;

    //Finsh drop and increase the altitude,fly to point2
    while (switch_flag == 0)
    {
        Command_now.command = Move_ENU;         //Move模式
        Command_now.sub_mode = 0;               //子模式：位置控制模式
        Command_now.pos_sp[0] = point2[0];
        Command_now.pos_sp[1] = point2[1];
        Command_now.pos_sp[2] = point2[2];
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;

        move_pub.publish(Command_now);

        distance_to_target = cal_distance(drone_pos,point2);

        if(time_sec > limit_time ||  distance_to_target< min_distance )
        {
            switch_flag =1;
        }

        time_sec++;

        printf_result(point2);

        cout << "Finsh drop and increase the altitude...fly to point2"<<endl;

        rate.sleep();
        ros::spinOnce();

    }

    //wait 1s more
    sleep(1.0);
    //reset the flag
    switch_flag = 0;
    time_sec = 0;

    //Go back to the takeoff point - point1
    while (switch_flag == 0)
    {
        Command_now.command = Move_ENU;         //Move模式
        Command_now.sub_mode = 0;               //子模式：位置控制模式
        Command_now.pos_sp[0] = point1[0];
        Command_now.pos_sp[1] = point1[1];
        Command_now.pos_sp[2] = point1[2];
        Command_now.yaw_sp = 0;
        Command_now.comid = comid;
        comid++;

        move_pub.publish(Command_now);

        distance_to_target = cal_distance(drone_pos,point1);

        if(time_sec > limit_time ||  distance_to_target< min_distance )
        {
            switch_flag =1;
        }

        time_sec++;

        printf_result(point1);

        cout << "Go back to the takeoff point...flying to point 1"<<endl;

        rate.sleep();
        ros::spinOnce();

    }

    //wait 1s more
    sleep(1.0);
    time_sec = 0;

    // Land
    while (time_sec < 5)
    {
        Command_now.command = Land;
        move_pub.publish(Command_now);

        cout << "Finsh the payload drop mission, Landing..."<<endl;

        time_sec++;
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

float cal_distance(Eigen::Vector3f a,Eigen::Vector3f b)
{
    float distance;
    distance = sqrt(  (a[0] - b[0])*(a[0] - b[0]) + (a[1] - b[1])*(a[1] - b[1]) + (a[2] - b[2])*(a[2] - b[2]) );
    return distance;
}

void printf_result(Eigen::Vector3f target_point)
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Payload Drop Mission<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(2);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout << "Drone_Pos  [X Y Z] : " << drone_pos[0] << " [ m ] "<< drone_pos[1]<<" [ m ] "<<drone_pos[2]<<" [ m ] "<<endl;
    cout << "Target_Pos [X Y Z] : " << target_point[0] << " [ m ] "<< target_point[1]<<" [ m ] "<<target_point[2]<<" [ m ] "<<endl;
    cout << "Distance_to_target : " << distance_to_target <<" [ m ] "<<endl;
    cout << "Flying_Time        : " << time_sec <<" [ s ] "<<endl;
    cout << "Drop_Flag          : " << drop_flag <<endl;

}