#include <fstream>
#include <iostream>
#include <vector>

#include "baldr/rapidjson_utils.h"
#include <boost/filesystem/operations.hpp>
#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>

#include "baldr/graphreader.h"
#include "baldr/graphtile.h"
#include "baldr/tilehierarchy.h"
#include "midgard/aabb2.h"
#include "midgard/pointll.h"
#include "midgard/tiles.h"

#include "config.h"

namespace bpo = boost::program_options;

using namespace valhalla::baldr;
using namespace valhalla::midgard;

// Iterate over all Valhalla tiles within the coverage (specified in the
// mjolnir properties) and identify the 1 degree tiles that include
// any Valhalla edge. Returns a list of the base (SW) lat,lng of the tiles
std::vector<PointLL> get_land_tiles(const boost::property_tree::ptree& pt) {
  AABB2<PointLL> world(-180.0f, -90.0f, 180.f, 90.f);
  Tiles<PointLL> one_degree_tiles(world, 1.0f);
  Tiles<PointLL> local_tiles(world, 0.25f);

  std::set<int> land_tile_ids;
  boost::property_tree::ptree hierarchy_properties = pt.get_child("mjolnir");
  auto local_level = TileHierarchy::levels().rbegin()->second.level;
  GraphReader reader(hierarchy_properties);
  auto local_tile_ids = reader.GetTileSet(local_level);
  for (const auto& tile_id : local_tile_ids) {
    // Convert the local tile to a 1 degree tile ID. Add to the set.
    PointLL center = local_tiles.Center(tile_id.tileid());
    land_tile_ids.insert(one_degree_tiles.TileId(center));
  }

  // Iterate through the set and convert to SW corner lat,lngs
  std::vector<PointLL> ll;
  for (auto id : land_tile_ids) {
    ll.push_back(one_degree_tiles.Base(id));
  }
  return ll;
}

boost::filesystem::path config_file_path;

bool ParseArguments(int argc, char* argv[]) {
  std::vector<std::string> input_files;

  bpo::options_description options(
      " Usage: valhalla_land_tiles [options]\n"
      "valhalla_land_tiles is a program that creates a list of base lat,lng for 1 degree "
      "tiles that include Valhalla data.\n");

  options.add_options()("help,h", "Print this help message.")("version,v",
                                                              "Print the version of this software.")(
      "config,c",
      boost::program_options::value<boost::filesystem::path>(&config_file_path)->required(),
      "Path to the json configuration file.")
      // positional arguments
      ("input_files",
       boost::program_options::value<std::vector<std::string>>(&input_files)->multitoken());

  bpo::positional_options_description pos_options;
  pos_options.add("input_files", 16);

  bpo::variables_map vm;
  try {
    bpo::store(bpo::command_line_parser(argc, argv).options(options).positional(pos_options).run(),
               vm);
    bpo::notify(vm);

  } catch (std::exception& e) {
    std::cerr << "Unable to parse command line options because: " << e.what() << "\n"
              << "This is a bug, please report it at " PACKAGE_BUGREPORT << "\n";
    return false;
  }
  if (vm.count("version")) {
    std::cout << "ways_to_edges " << VALHALLA_VERSION << "\n";
    return true;
  }

  if (vm.count("config")) {
    if (boost::filesystem::is_regular_file(config_file_path)) {
      return true;
    } else {
      std::cerr << "Configuration file is required\n\n" << options << "\n\n";
    }
  }
  return false;
}

int main(int argc, char** argv) {
  // Parse command line arguments
  if (!ParseArguments(argc, argv)) {
    return EXIT_FAILURE;
  }

  // Get the config to see which coverage we are using
  boost::property_tree::ptree pt;
  rapidjson::read_json(config_file_path.c_str(), pt);

  // Get the base lat,lng of all 1 degree tiles that include a Valhalla tile
  auto lls = get_land_tiles(pt);

  // Open output file, iterate through the list of land tiles and output
  // the elevation filename required for this tile. Uses N,S for latitude
  // and E,W for longitude and requires 2 digits for latitude and 3 digits
  // for longitude (padded with 0s).
  std::ofstream landtiles;
  landtiles.open("elevation_tiles.txt");
  for (const auto& ll : lls) {
    if (ll.lat() >= 0.0f) {
      landtiles << "N" << std::setfill('0') << std::setw(2) << ll.lat();
    } else {
      landtiles << "S" << std::setfill('0') << std::setw(2) << -ll.lat();
    }
    if (ll.lng() >= 0.0f) {
      landtiles << "E" << std::setfill('0') << std::setw(3) << ll.lng();
    } else {
      landtiles << "W" << std::setfill('0') << std::setw(3) << -ll.lng();
    }
    landtiles << std::endl;
  }
  landtiles.close();
}


#include <boost/simd/meta/is_aligned.hpp>
#include <boost/mpl/assert.hpp>

int main()
{
  BOOST_MPL_ASSERT    (( boost::simd::meta::is_aligned_c<0  , 2>::type  ));
  BOOST_MPL_ASSERT    (( boost::simd::meta::is_aligned_c<4  , 2>::type  ));
  BOOST_MPL_ASSERT_NOT(( boost::simd::meta::is_aligned_c<17 , 2>::type  ));
}


#ifndef COMMON_HPP_
#define COMMON_HPP_

#include <Eigen/Core>

#include <utility>

/**
 * @brief Functor that wraps a callable with multiple forms of operator()
 */
template<typename _Func, typename _InputType>
struct EigenFunctor
{
  using Scalar = typename _InputType::Scalar;
  using InputType = _InputType;
  using ValueType = std::invoke_result_t<_Func, InputType>;

  static constexpr int InputsAtCompileTime = InputType::SizeAtCompileTime;
  static constexpr int ValuesAtCompileTime = ValueType::SizeAtCompileTime;

  using JacobianType = Eigen::Matrix<
    Scalar, ValueType::SizeAtCompileTime, InputType::SizeAtCompileTime
  >;

  using JacobianTypeRowMajor = Eigen::Matrix<
    Scalar, ValueType::SizeAtCompileTime, InputType::SizeAtCompileTime,
    InputType::SizeAtCompileTime == 1 ? Eigen::ColMajor : Eigen::RowMajor
  >;

  int values_ = ValueType::SizeAtCompileTime;  // must be changed for dynamic sizing

  explicit EigenFunctor(_Func && func)
  : func_(std::forward<_Func>(func))
  {}

  /**
   * @brief Return (dynamic) dimension of output vector
   */
  int values() const
  {
    return values_;
  }

  /**
   * @brief Functor form used in unsupported/AdolcForward
   */
  template<typename Derived1, typename Derived2>
  void operator()(const Eigen::MatrixBase<Derived1> & x, Eigen::MatrixBase<Derived2> * y) const
  {
    * y = func_(x);
  }

  /**
   * @brief Functor form used in unsupported/NumericalDiff
   */
  template<typename Derived1, typename Derived2>
  void operator()(const Eigen::MatrixBase<Derived1> & x, Eigen::MatrixBase<Derived2> & y) const
  {
    y = func_(x);
  }

  /**
   * @brief Functor form used in Ceres
   */
  template<typename T>
  bool operator()(const T * x, T * y) const
  {
    // TODO: expose dynamic input size and add here
    Eigen::Map<const Eigen::Matrix<T, InputsAtCompileTime, 1>> x_map(x);
    Eigen::Map<Eigen::Matrix<T, ValuesAtCompileTime, 1>> y_map(y, values());

    y_map = func_(x_map);

    return true;
  }

private:
  _Func func_;
};

#endif  // COMMON_HPP_


// *****************************************************************************
/*!
  \file      src/RNG/RNGSSE.hpp
  \copyright 2012-2015 J. Bakosi,
             2016-2018 Los Alamos National Security, LLC.,
             2019 Triad National Security, LLC.
             All rights reserved. See the LICENSE file for details.
  \brief     Interface to RNGSSE random number generators
  \details   Interface to RNGSSE random number generators
*/
// *****************************************************************************
#ifndef RNGSSE_h
#define RNGSSE_h

#include <cstring>
#include <random>

#include "NoWarning/beta_distribution.hpp"
#include <boost/random/gamma_distribution.hpp>

#include "Make_unique.hpp"
#include "Exception.hpp"
#include "Macro.hpp"
#include "Options/RNGSSESeqLen.hpp"

namespace tk {

//! RNGSSE-based random number generator used polymorphically with tk::RNG
template< class State, typename SeqNumType, unsigned int (*Generate)(State*) >
class RNGSSE {

  private:
    using InitFn = void (*)( State*, SeqNumType );
    using ncomp_t = kw::ncomp::info::expect::type;

    //! Adaptor to use a std distribution with the RNGSSE generator
    //! \see C++ concepts: UniformRandomNumberGenerator
    struct Adaptor {
      using result_type = unsigned int;
      Adaptor( const std::unique_ptr< State[] >& s, int t ) : str(s), tid(t) {}
      static constexpr result_type min() { return 0u; }
      static constexpr result_type max() { return 4294967295u; }
      result_type operator()()
      { return Generate( &str[ static_cast<std::size_t>(tid) ] ); }
      const std::unique_ptr< State[] >& str;
      int tid;
    };

  public:
    //! Constructor
    //! \param[in] n Initialize RNG using this many independent streams
    //! \param[in] fnShort RNG initializer function for short streams
    //! \param[in] seqlen Sequence length enum: short, medium or long
    //! \param[in] fnLong RNG initializer function for long streams
    //! \param[in] fnMed RNG initializer function for medium streams
    explicit RNGSSE( SeqNumType n,
                     InitFn fnShort,
                     ctr::RNGSSESeqLenType seqlen = ctr::RNGSSESeqLenType::SHORT,
                     InitFn fnLong = nullptr,
                     InitFn fnMed = nullptr) :
       m_nthreads( n ),
       m_init( seqlen == ctr::RNGSSESeqLenType::LONG ? fnLong :
               seqlen == ctr::RNGSSESeqLenType::MEDIUM ? fnMed : fnShort ),
       m_stream()
    {
      Assert( m_init != nullptr, "nullptr passed to RNGSSE constructor" );
      Assert( n > 0, "Need at least one thread" );
      // Allocate array of stream-pointers for threads
      m_stream = tk::make_unique< State[] >( n );
      // Initialize thread-streams
      for (SeqNumType i=0; i<n; ++i) m_init( &m_stream[i], i );
    }

    //! Uniform RNG: Generate uniform random numbers
    //! \param[in] tid Thread (or more precisely) stream ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in,out] r Pointer to memory to write the random numbers to
    void uniform( int tid, ncomp_t num, double* r ) const {
      for (ncomp_t i=0; i<num; ++i)
        r[i] = static_cast<double>(
                 Generate( &m_stream[ static_cast<std::size_t>(tid) ] ) )
               / 4294967296.0;
    }

    //! Gaussian RNG: Generate Gaussian random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \details Generating Gaussian random numbers is implemented via an
    //!   adaptor, modeling std::UniformRandomNumberGenerator, outsourcing the
    //!   transformation of uniform random numbers to Gaussian ones, to the
    //!   standard library. The adaptor is instantiated here because a standard
    //!   distribution, such as e.g., std::normal_distribution, generates
    //!   numbers using operator() with no arguments, thus the RNG state and the
    //!   thread ID (this latter only known here) must be stored in the adaptor
    //!   functor's state. Even though creating the adaptor seems like a
    //!   potentially costly operation for every call, using the standard
    //!   library implementation is still faster than a hand-coded
    //!   implementation of the Box-Muller algorithm. Note that libc++ uses a
    //!   cache, as Box-Muller, implemented using the polar algorithm generates
    //!   2 Gaussian numbers for each pair of uniform ones, caching every 2nd.
    void gaussian( int tid, ncomp_t num, double* r ) const {
      Adaptor generator( m_stream, tid );
      std::normal_distribution<> gauss_dist( 0.0, 1.0 );
      for (ncomp_t i=0; i<num; ++i) r[i] = gauss_dist( generator );
    }

    //! \brief Multi-variate Gaussian RNG: Generate multi-variate Gaussian
    //!    random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in] d Dimension d ( d ≥ 1) of output random vectors
    //! \param[in] mean Mean vector of dimension d
    //! \param[in] cov Lower triangle of covariance matrix, stored as a vector
    //!   of length d(d+1)/2
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \warning Not implemented!
    void gaussianmv( int tid, ncomp_t num, ncomp_t d, const double* const mean,
                     const double* const cov, double* r ) const
    {
      IGNORE(tid);
      IGNORE(num);
      IGNORE(d);
      IGNORE(mean);
      IGNORE(cov);
      IGNORE(r);
    }

    //! Beta RNG: Generate beta random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in] p First beta shape parameter
    //! \param[in] q Second beta shape parameter
    //! \param[in] a Beta displacement parameter
    //! \param[in] b Beta scale factor
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \details Generating beta-distributed random numbers is implemented via
    //!   an adaptor, modeling boost::UniformRandomNumberGenerator, outsourcing
    //!   the transformation of uniform random numbers to beta-distributed ones,
    //!   to boost::random. The adaptor is instantiated here because a boost
    //!   random number distribution, such as e.g.,
    //!   boost::random::beta_distribution, generates numbers using operator()
    //!   with no arguments, thus the RNG state and the thread ID (this latter
    //!   only known here) must be stored in the adaptor functor's state.
    void beta( int tid, ncomp_t num, double p, double q, double a, double b,
               double* r ) const {
      Adaptor generator( m_stream, tid );
      boost::random::beta_distribution<> beta_dist( p, q );
      for (ncomp_t i=0; i<num; ++i) r[i] = beta_dist( generator ) * b + a;
    }

    //! Gamma RNG: Generate gamma random numbers
    //! \param[in] tid Thread (or more precisely stream) ID
    //! \param[in] num Number of RNGs to generate
    //! \param[in] a Gamma shape parameter
    //! \param[in] b Gamma scale factor
    //! \param[in,out] r Pointer to memory to write the random numbers to
    //! \details Generating gamma-distributed random numbers is implemented via
    //!   an adaptor, modeling boost::UniformRandomNumberGenerator, outsourcing
    //!   the transformation of uniform random numbers to gamma-distributed
    //!   ones, to boost::random. The adaptor is instantiated here because a
    //!   boost random number distribution, such as e.g.,
    //!   boost::random::gamma_distribution, generates numbers using operator()
    //!   with no arguments, thus the RNG state and the thread ID (this latter
    //!   only known here) must be stored in the adaptor functor's state.
    void gamma( int tid, ncomp_t num, double a, double b, double* r ) const {
      Adaptor generator( m_stream, tid );
      boost::random::gamma_distribution<> gamma_dist( a, b );
      for (ncomp_t i=0; i<num; ++i) r[i] = gamma_dist( generator );
    }

    //! Copy assignment
    RNGSSE& operator=( const RNGSSE& x ) {
      m_nthreads = x.m_nthreads;
      m_init = x.m_init;
      m_stream = tk::make_unique< State[] >( x.m_nthreads );
      for (SeqNumType i=0; i<x.m_nthreads; ++i) m_init( &m_stream[i], i );
      return *this;
    }

    //! Copy constructor: in terms of copy assignment
    RNGSSE( const RNGSSE& x ) { operator=(x); }

    //! Move assignment
    RNGSSE& operator=( RNGSSE&& x ) {
      m_nthreads = x.m_nthreads;
      m_init = x.m_init;
      m_stream = tk::make_unique< State[] >( x.m_nthreads );
      for (SeqNumType i=0; i<x.m_nthreads; ++i) {
        m_stream[i] = x.m_stream[i];
        std::memset( &x.m_stream[i], 0, sizeof(x.m_stream[i]) );
      }
      x.m_nthreads = 0;
      x.m_init = nullptr;
      x.m_stream.reset( nullptr );
      return *this;
    }

    //! Move constructor: in terms of move assignment
    RNGSSE( RNGSSE&& x ) :
      m_nthreads( 0 ),
      m_init( nullptr ),
      m_stream( nullptr )
    { *this = std::move( x ); }

    //! Accessor to the number of threads we operate on
    SeqNumType nthreads() const noexcept { return m_nthreads; }

  private:
    SeqNumType m_nthreads;                 //!< Number of threads
    InitFn m_init;                         //!< Sequence length initializer
    std::unique_ptr< State[] > m_stream;   //!< Random number stream for threads
};

} // tk::

#endif // RNGSSE_h


//==================================================================================================
/*!
  @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_COMPLEMENT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COMPLEMENT_HPP_INCLUDED

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

  /*!
    @ingroup group-operator
    This function object returns the return the bitwise complement of the entry
    Infix notation can be used with operator '~'

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

    @par Example:

      @snippet complement.cpp complement

    @par Possible output:

      @snippet complement.txt complement

    @par Alias
    bitwise_not

    @see  bitwise_and, bitwise_or, bitwise_xor, bitwise_notand,
    bitwise_andnot, bitwise_notor, bitwise_ornot

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

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

#endif


//
// Copyright 2010 Scott McMurray.
// 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_HASH_BLOCK_CYPHERS_THREEFISH_HPP
#define BOOST_HASH_BLOCK_CYPHERS_THREEFISH_HPP

#include <boost/hash/block_cyphers/detail/threefish_policy.hpp>

#include <boost/cstdint.hpp>

#ifdef BOOST_HASH_SHOW_PROGRESS
#include <cstdio>
#endif

//
// Encrypt implemented directly from the Skein standard as found at
// http://www.skein-hash.info/sites/default/files/skein1.2.pdf
//

namespace boost {
namespace hashes {
namespace block_cyphers {

template <unsigned Version>
class threefish {
  public:
    static unsigned const version = Version;
    typedef detail::threefish_policy<version> policy_type;

    static unsigned const word_bits = policy_type::word_bits;
    typedef typename policy_type::word_type word_type;

    static unsigned const key_bits = policy_type::key_bits;
    static unsigned const key_words = policy_type::key_words;
    typedef typename policy_type::key_type key_type;

    static unsigned const block_bits = policy_type::block_bits;
    static unsigned const block_words = policy_type::block_words;
    typedef typename policy_type::block_type block_type;

    static unsigned const tweak_bits = policy_type::tweak_bits;
    static unsigned const tweak_words = policy_type::tweak_words;
    typedef typename policy_type::tweak_type tweak_type;

    static unsigned const rounds = policy_type::rounds;
    typedef typename policy_type::key_schedule_type key_schedule_type;
    typedef typename policy_type::tweak_schedule_type tweak_schedule_type;

  public:
    threefish(key_type const &key, tweak_type const &tweak = tweak_type()) {
        set_key(key);
        set_tweak(tweak);
    }
  public:
    void
    set_tweak(tweak_type const &t) {
        tweak_schedule[0] = t[0];
        tweak_schedule[1] = t[1];
        tweak_schedule[2] = t[0] ^ t[1];
#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t <= tweak_words; ++t) {
            std::printf("t_%d = %.16lx\n",
                        t, tweak_schedule[t]);
        }
#endif
    }
  private:
    void
    set_key(key_type const &key) {
        word_type k_N_w = UINT64_C(0x5555555555555555);
        for (unsigned t = 0; t < key_words; ++t) {
            key_schedule[t] = key[t];
#ifdef BOOST_HASH_SHOW_PROGRESS
            std::printf("k_%-2d = %.16lx\n",
                        t, key_schedule[t]);
#endif
            k_N_w ^= key[t];
        }
        key_schedule[key_words] = k_N_w;
#ifdef BOOST_HASH_SHOW_PROGRESS
        std::printf("k_%-2d = %.16lx\n",
                    key_words, key_schedule[key_words]);
#endif
    }
    key_schedule_type key_schedule;
    tweak_schedule_type tweak_schedule;
  private:
    word_type k(unsigned s, unsigned i) {
        word_type x = key_schedule[ (s+i) % (key_words+1) ];
        switch (i) {
          default:
            return x;
          case block_words - 3:
            return x + tweak_schedule[ s % 3 ];
          case block_words - 2:
            return x + tweak_schedule[ (s+1) % 3 ];
          case block_words - 1:
            return x + s;
        }
    }

  public:
    block_type
    encypher(block_type const &plaintext) {
        return encypher_block(plaintext);
    }
  private:
    block_type
    encypher_block(block_type const &plaintext) {

        // Initialize working variables with block
        block_type v = plaintext;

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf("v_0,%-2d = %.16lx\n",
                        t, v[t]);
        }
#endif

        // Encypher block
        for (unsigned d = 0; d < rounds; ) {
            // Add a subkey (when d%4 == 0)
            for (unsigned i = 0; i < block_words; ++i) {
                v[i] += k(d/4, i);
#ifdef BOOST_HASH_SHOW_PROGRESS
                std::printf("e_%d,%-2d = %.16lx\n",
                            d, i, v[i]);
#endif
            }

            // Unrolling by 4 is also useful as the permutations
            // have a cycle of 2 or 4 (see 8.3)
            for (unsigned q = 0; q < 4; ++q, ++d) {
                block_type f;
                // MIX into f
                for (unsigned j = 0; j < block_words/2; ++j) {
                    word_type x0 = v[2*j+0];
                    word_type x1 = v[2*j+1];
                    word_type y0 = x0 + x1;
                    unsigned r = policy_type::rotation(d%8, j);
                    word_type y1 = policy_type::ROTL(x1, r) ^ y0;
                    f[2*j+0] = y0;
                    f[2*j+1] = y1;
#ifdef BOOST_HASH_SHOW_PROGRESS
                    std::printf("f_%d,%-2d = %.16lx\n",
                                d, 2*j+0, f[2*j+0]);
                    std::printf("f_%d,%-2d = %.16lx\n",
                                d, 2*j+1, f[2*j+1]);
#endif
                }
                // PERMUTE back into v
                for (unsigned i = 0; i < block_words; ++i) {
                    unsigned pi = policy_type::permutation(i);
                    v[i] = f[pi];
#ifdef BOOST_HASH_SHOW_PROGRESS
                    std::printf("v_%d,%-2d = %.16lx\n",
                                d+1, i, v[i]);
#endif
                }
            }
        }

        block_type cyphertext;
        // Add final subkey
        for (unsigned i = 0; i < block_words; ++i) {
            cyphertext[i] = v[i] + k(rounds/4, i);
#ifdef BOOST_HASH_SHOW_PROGRESS
                std::printf("c_%-2d = %.16lx\n",
                            i, cyphertext[i]);
#endif
        }
        return cyphertext;
    }

  public:
    block_type
    decypher(block_type const &plaintext) {
        return decypher_block(plaintext);
    }
  private:
    block_type
    decypher_block(block_type const &cyphertext) {
        for (unsigned i = 0; i < block_words; ++i) {
#ifdef BOOST_HASH_SHOW_PROGRESS
                std::printf("c_%-2d = %.16lx\n",
                            i, cyphertext[i]);
#endif
        }

        block_type v;

        // Remove final subkey
        for (unsigned i = 0; i < block_words; ++i) {
            v[i] = cyphertext[i] - k(rounds/4, i);
#ifdef BOOST_HASH_SHOW_PROGRESS
                std::printf("v_%d,%-2d = %.16lx\n",
                            rounds, i, v[i]);
#endif
        }

        // Decypher block
        for (unsigned d = rounds; d; ) {
            // Unrolling by 4 is also useful as the permutations
            // have a cycle of 2 or 4 (see 8.3)
            for (unsigned q = 4; q--; ) {
                --d;

                block_type f;
                // PERMUTE back into f
                for (unsigned i = 0; i < block_words; ++i) {
                    unsigned pi = policy_type::permutation(i);
                    f[pi] = v[i];
#ifdef BOOST_HASH_SHOW_PROGRESS
                    std::printf("f_%d,%-2d = %.16lx\n",
                                d, pi, f[pi]);
#endif
                }

                // UNMIX into v
                for (unsigned j = 0; j < block_words/2; ++j) {
                    word_type y0 = f[2*j+0];
                    word_type y1 = f[2*j+1];

                    unsigned r = policy_type::rotation(d%8, j);
                    word_type x1 = policy_type::ROTR(y0 ^ y1, r);
                    word_type x0 = y0 - x1;

                    v[2*j+0] = x0;
                    v[2*j+1] = x1;

#ifdef BOOST_HASH_SHOW_PROGRESS
                    std::printf("e_%d,%-2d = %.16lx\n",
                                d, 2*j+0, v[2*j+0]);
                    std::printf("e_%d,%-2d = %.16lx\n",
                                d, 2*j+1, v[2*j+1]);
#endif
                }
            }

            // Remove a subkey (when d%4 == 0)
            for (unsigned i = 0; i < block_words; ++i) {
                v[i] -= k(d/4, i);
#ifdef BOOST_HASH_SHOW_PROGRESS
                std::printf("d_%d,%-2d = %.16lx\n",
                            d, i, v[i]);
#endif
            }
        }

        block_type plaintext = v;
        return plaintext;
    }

};

} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_THREEFISH_HPP


#include "Positions.h"
#include "Config.h"
#include "Hash.h"
#include "Projection.h"
#include <boost/move/make_unique.hpp>
#include <engines/querydata/ParameterOptions.h>
#include <gis/CoordinateTransformation.h>
#include <gis/OGR.h>
#include <grid-files/identification/GridDef.h>
#include <macgyver/Exception.h>
#include <macgyver/NearTree.h>
#include <spine/Convenience.h>
#include <spine/ParameterFactory.h>
#include <iomanip>
#include <stdexcept>

namespace SmartMet
{
namespace Plugin
{
namespace Dali
{
// For NearTree calculations
class XY
{
 public:
  XY(double theX, double theY) : itsX(theX), itsY(theY) {}
  double x() const { return itsX; }
  double y() const { return itsY; }

 private:
  double itsX;
  double itsY;
};

// ----------------------------------------------------------------------
/*!
 * \brief Length of a segment
 */
// ----------------------------------------------------------------------

double length(double x1, double y1, double x2, double y2)
{
  return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

// ----------------------------------------------------------------------
/*!
 * \brief Test if the given graticule cell overlaps the bounding box
 */
// ----------------------------------------------------------------------

bool overlaps(const Fmi::Box& theBox,
              double x1,
              double y1,
              double x2,
              double y2,
              double x3,
              double y3,
              double x4,
              double y4)
{
  try
  {
    // extent of the grid cell
    double xmin = std::min(std::min(x1, x3), std::min(x2, x4));
    double xmax = std::max(std::max(x1, x3), std::max(x2, x4));
    double ymin = std::min(std::min(y1, y3), std::min(y2, y4));
    double ymax = std::max(std::max(y1, y3), std::max(y2, y4));

    // intersection with the box
    xmin = std::max(xmin, 0.0);
    xmax = std::min(xmax, static_cast<double>(theBox.width()));
    ymin = std::max(ymin, 0.0);
    ymax = std::min(ymax, static_cast<double>(theBox.height()));

    // is the intersection non-empty?
    return (xmax > xmin && ymax > ymin);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Apply direction based adjustments to the selected positions
 *
 * We know that only direction or u- and v-components are set
 */
// ----------------------------------------------------------------------

void apply_direction_offsets(Positions::Points& thePoints,
                             const Engine::Querydata::Q& theQ,
                             const boost::posix_time::ptime& theTime,
                             int theOffset,
                             int theRotation,
                             const std::string& theDirection,
                             const std::string& theU,
                             const std::string& theV)
{
  try
  {
    if (theOffset == 0 || !theQ)
      return;

    auto& q = *theQ;

    const double pi = 3.141592658979323;

    if (!theDirection.empty())
    {
      auto param = Spine::ParameterFactory::instance().parse(theDirection);
      if (!q.param(param.number()))
        throw Fmi::Exception(
            BCP, "Parameter '" + theDirection + "' is not available for position selection!");

      for (auto& point : thePoints)
      {
        double dir = q.interpolate(point.latlon, theTime, 180);
        if (dir != kFloatMissing)
        {
          point.x += lround(theOffset * cos((dir + 90 + theRotation) * pi / 180.0));
          point.y += lround(theOffset * sin((dir + 90 + theRotation) * pi / 180.0));
        }
      }
    }
    else
    {
      auto uparam = Spine::ParameterFactory::instance().parse(theU);
      auto vparam = Spine::ParameterFactory::instance().parse(theV);

      // Q API SUCKS
      boost::shared_ptr<Fmi::TimeFormatter> timeformatter(Fmi::TimeFormatter::create("iso"));
      boost::local_time::time_zone_ptr utc(new boost::local_time::posix_time_zone("UTC"));
      boost::local_time::local_date_time localdatetime(theTime, utc);
      std::string tmp;
      auto mylocale = std::locale::classic();
      NFmiPoint dummy;
	  SmartMet::Spine::TimeSeries::LocalTimePoolPtr localTimePool = nullptr;

      for (auto& point : thePoints)
      {
        // Q API SUCKS

        Spine::Location loc(point.latlon.X(), point.latlon.Y());

        auto up = Engine::Querydata::ParameterOptions(uparam,
                                                      tmp,
                                                      loc,
                                                      tmp,
                                                      tmp,
                                                      *timeformatter,
                                                      tmp,
                                                      tmp,
                                                      mylocale,
                                                      tmp,
                                                      false,
                                                      dummy,
                                                      dummy,
													  localTimePool);
        auto uresult = q.value(up, localdatetime);

        auto vp = Engine::Querydata::ParameterOptions(vparam,
                                                      tmp,
                                                      loc,
                                                      tmp,
                                                      tmp,
                                                      *timeformatter,
                                                      tmp,
                                                      tmp,
                                                      mylocale,
                                                      tmp,
                                                      false,
                                                      dummy,
                                                      dummy,
													  localTimePool);
        auto vresult = q.value(vp, localdatetime);

        if (boost::get<double>(&uresult) != nullptr && boost::get<double>(&vresult) != nullptr)
        {
          auto uspd = *boost::get<double>(&uresult);
          auto vspd = *boost::get<double>(&vresult);

          if (uspd != kFloatMissing && vspd != kFloatMissing && (uspd != 0 || vspd != 0))
          {
            double dir = fmod(180 + 180 / pi * atan2(uspd, vspd), 360);
            point.x += lround(theOffset * cos((dir + 90 + theRotation) * pi / 180.0));
            point.y += lround(theOffset * sin((dir + 90 + theRotation) * pi / 180.0));
          }
        }
      }
    }
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Initialize from JSON
 */
// ----------------------------------------------------------------------

void Positions::init(const Json::Value& theJson, const Config& theConfig)
{
  try
  {
    if (!theJson.isObject())
      throw Fmi::Exception(BCP, "Positions JSON is not a JSON object!");

    // Iterate through all the members

    const auto members = theJson.getMemberNames();
    for (const auto& name : members)
    {
      const Json::Value& json = theJson[name];

      if (name == "layout")
      {
        std::string tmp = json.asString();
        if (tmp == "grid")
          layout = Layout::Grid;
        else if (tmp == "data")
          layout = Layout::Data;
        else if (tmp == "graticule")
          layout = Layout::Graticule;
        else if (tmp == "graticulefill")
          layout = Layout::GraticuleFill;
        else if (tmp == "keyword")
          layout = Layout::Keyword;
        else if (tmp == "latlon")
          layout = Layout::LatLon;
        else if (tmp == "station")
          layout = Layout::Station;
        else
          throw Fmi::Exception(BCP, "Unknown layout type for positions: '" + tmp + "'!");
      }

      else if (name == "x")
        x = json.asInt();
      else if (name == "y")
        y = json.asInt();
      else if (name == "dx")
        dx = json.asInt();
      else if (name == "dy")
        dy = json.asInt();
      else if (name == "ddx")
        ddx = json.asInt();

      else if (name == "xmargin")
        xmargin = json.asInt();
      else if (name == "ymargin")
        ymargin = json.asInt();

      else if (name == "directionoffset")
        directionoffset = json.asInt();
      else if (name == "rotate")
        rotate = json.asInt();
      else if (name == "direction")
        direction = json.asString();
      else if (name == "u")
        u = json.asString();
      else if (name == "v")
        v = json.asString();

      else if (name == "size")
        size = json.asUInt();
      else if (name == "step")
        step = json.asUInt();

      else if (name == "mindistance")
        mindistance = json.asDouble();

      else if (name == "keyword")
        keyword = json.asString();

      else if (name == "locations")
        locations.init(json, theConfig);

      else if (name == "stations")
        stations.init(json, theConfig);

      else if (name == "outside")
      {
        outsidemap.reset(Map());
        outsidemap->init(json, theConfig);
      }
      else if (name == "inside")
      {
        insidemap.reset(Map());
        insidemap->init(json, theConfig);
      }
      else if (name == "intersect")
        intersections.init(json, theConfig);

      else
        throw Fmi::Exception(BCP, "Positions does not have a setting named '" + name + "'!");
    }

    if (size <= 0)
      throw Fmi::Exception(BCP, "Positions size-setting must be nonnegative!");

    if (step <= 0)
      throw Fmi::Exception(BCP, "Positions step-setting must be nonnegative!");

    if (step > size)
      throw Fmi::Exception(BCP, "Positions step-setting must be smaller than the size-setting!");

    if (180 % step != 0)
      throw Fmi::Exception(BCP, "Positions step-size must divide 180 degrees evenly");

    // we cannot allow stuff like 1e-6, that'll generate a huge amounts of points
    if (mindistance < 2)
      throw Fmi::Exception(BCP, "Minimum distance between positions must be at least 2 pixels!");

    if (!direction.empty() && (!u.empty() || !v.empty()))
      throw Fmi::Exception(
          BCP, "Cannot specify position offsets using both direction and the u- and v-components!");

    if ((!u.empty() && v.empty()) || (!v.empty() && u.empty()))
      throw Fmi::Exception(BCP,
                           "Cannot specify only one of u- and v-components for position offsets!");

    if (directionoffset != 0 && direction.empty() && u.empty() && v.empty())
      throw Fmi::Exception(BCP,
                           "Must specify direction parameter for direction offset of positions!");
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Initialize the geometries to be used for intersections
 */
// ----------------------------------------------------------------------

void Positions::init(const boost::optional<std::string>& theProducer,
                     const Projection& theProjection,
                     const boost::posix_time::ptime& theTime,
                     const State& theState)
{
  try
  {
    // For applying directional corrections to positions
    time = theTime;

    // Needed engines
    geonames = &theState.getGeoEngine();
    gisengine = &theState.getGisEngine();

    // Initialize clipping shapes

    if (insidemap)
    {
      inshape = gisengine->getShape(&theProjection.getCRS(), insidemap->options);
      if (!inshape)
        throw Fmi::Exception(BCP, "Positions received empty inside-shape from database!");

      // This does not obey layer margins, hence we disable this speed optimization
      // inshape.reset(Fmi::OGR::polyclip(*inshape, theProjection.getBox()));
    }

    if (outsidemap)
    {
      outshape = gisengine->getShape(&theProjection.getCRS(), outsidemap->options);

      // This does not obey layer margings, hence we disable this speed optimization
      // if (outshape)
      // outshape.reset(Fmi::OGR::polyclip(*outshape, theProjection.getBox()));
    }

    intersections.init(theProducer, theProjection, theTime, theState);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Add layer margins to position margins
 */
// ----------------------------------------------------------------------

void Positions::addMargins(int theXMargin, int theYMargin)
{
  xmargin = std::max(xmargin, theXMargin);
  ymargin = std::max(ymargin, theYMargin);
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate the locations to be rendered
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getPoints(const Engine::Querydata::Q& theQ,
                                       const Fmi::SpatialReference& theCRS,
                                       const Fmi::Box& theBox,
                                       bool forecastMode) const
{
  try
  {
    switch (layout)
    {
      case Layout::Grid:
        return getGridPoints(theQ, theCRS, theBox, forecastMode);
      case Layout::Data:
        return getDataPoints(theQ, theCRS, theBox, forecastMode);
      case Layout::Graticule:
        return getGraticulePoints(theQ, theCRS, theBox, forecastMode);
      case Layout::GraticuleFill:
        return getGraticuleFillPoints(theQ, theCRS, theBox, forecastMode);
      case Layout::Keyword:
        return getKeywordPoints(theQ, theCRS, theBox, forecastMode);
      case Layout::LatLon:
        return getLatLonPoints(theQ, theCRS, theBox, forecastMode);
      case Layout::Station:
        return getStationPoints(theQ, theCRS, theBox, forecastMode);
    }
    // Dummy to prevent g++ from complaining
    return Points();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate the locations to be rendered
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getPoints(const char* originalCrs,
                                       int originalWidth,
                                       int originalHeight,
                                       uint originalGeometryId,
                                       const Fmi::SpatialReference& theCRS,
                                       const Fmi::Box& theBox) const
{
  try
  {
    switch (layout)
    {
      case Layout::Grid:
        return getGridPoints(nullptr, theCRS, theBox, true);
      case Layout::Data:
        return getDataPoints(
            originalCrs, originalWidth, originalHeight, originalGeometryId, theCRS, theBox);
      case Layout::Graticule:
        return getGraticulePoints(nullptr, theCRS, theBox, true);
      case Layout::GraticuleFill:
        return getGraticuleFillPoints(nullptr, theCRS, theBox, true);
      case Layout::Keyword:
        return getKeywordPoints(nullptr, theCRS, theBox, true);
      case Layout::LatLon:
        return getLatLonPoints(nullptr, theCRS, theBox, true);
      case Layout::Station:
        return getStationPoints(nullptr, theCRS, theBox, true);
    }
    // Dummy to prevent g++ from complaining
    return Points();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate a grid of locations
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getGridPoints(const Engine::Querydata::Q& theQ,
                                           const Fmi::SpatialReference& theCRS,
                                           const Fmi::Box& theBox,
                                           bool forecastMode) const
{
  try
  {
    // Get the data projection

    Fmi::CoordinateTransformation transformation(theCRS, "WGS84");

    // Use defaults for this layout if nothing is specified

    if (!!dx && *dx <= 0)
      throw Fmi::Exception(BCP, "Positions dx-setting must be nonnegative for grid-layouts!");

    if (!!dy && *dy <= 0)
      throw Fmi::Exception(BCP, "Positions dy-setting must be nonnegative for grid-layouts!");

    int xstart = (!!x ? *x : 5);
    int ystart = (!!y ? *y : 5);
    int deltax = (!!dx ? *dx : 20);
    int deltay = (!!dy ? *dy : 20);
    int deltaxx = (!!ddx ? *ddx : 0);

    // Sanity check
    if (deltaxx >= deltax)
      throw Fmi::Exception(BCP, "ddx must be smaller than dx when generating a grid layout!");

    // Generate the grid coordinates

    Points points;

    // Cover margins too
    while (xstart - deltax >= -xmargin)
      xstart -= deltax;
    while (ystart - deltay >= -ymargin)
      ystart -= deltay;

    int row = 0;

    // TODO(mheiskan): Must alter these if there is a margin
    const int height = theBox.height();
    const int width = theBox.width();

    // This is for backwards compatibility - original code which did not handle
    // the margins had a logic error
    xstart -= deltaxx;

    for (int ypos = ystart; ypos < (height + ymargin); ypos += deltay)
    {
      // Stagger every other row
      if (row++ % 2 == 0)
        xstart += deltaxx;
      else
        xstart -= deltaxx;

      for (int xpos = xstart; xpos < (width + xmargin); xpos += deltax)
      {
        // Convert pixel coordinate to world coordinate (or latlon for geographic spatial
        // references)
        auto xcoord = static_cast<double>(xpos);
        auto ycoord = static_cast<double>(ypos);

        theBox.itransform(xcoord, ycoord);

        // Skip if the coordinate is outside the desired shapes

        if (!inside(xcoord, ycoord, forecastMode))
          continue;

        // Convert world coordinate to latlon, skipping points which cannot be handled

        if (!transformation.transform(xcoord, ycoord))
          continue;

        points.emplace_back(Point(xpos, ypos, NFmiPoint(xcoord, ycoord)));
      }
    }

    apply_direction_offsets(points, theQ, time, directionoffset, rotate, direction, u, v);

    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate querydata locations
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getDataPoints(const Engine::Querydata::Q& theQ,
                                           const Fmi::SpatialReference& theCRS,
                                           const Fmi::Box& theBox,
                                           bool forecastMode) const
{
  // Cannot generate any points without any querydata. If layout=data,
  // the layers will handle stations and flashes by themselves.

  if (!theQ)
    return {};

  try
  {
    // Create the coordinate transformation from querydata world coordinates
    // to image world coordinates

    Fmi::CoordinateTransformation transformation(theQ->SpatialReference(), theCRS);

    // Generate the grid coordinates

    Points points;

    for (theQ->resetLocation(); theQ->nextLocation();)
    {
      NFmiPoint latlon = theQ->latLon();
      NFmiPoint worldxy = theQ->worldXY();

      // Date world coordinate to image world coordinate
      double xcoord = worldxy.X();
      double ycoord = worldxy.Y();

      if (!transformation.transform(xcoord, ycoord))
        continue;

      // Image world coordinate to pixel coordinate
      theBox.transform(xcoord, ycoord);

      int deltax = 0;
      if (dx)
        deltax += *dx;
      int deltay = 0;
      if (dy)
        deltay += *dy;

      // Skip if not inside desired shapes
      if (inside(latlon.X(), latlon.Y(), forecastMode))
      {
        points.emplace_back(Point(xcoord, ycoord, latlon, deltax, deltay));
      }
    }

    apply_direction_offsets(points, theQ, time, directionoffset, rotate, direction, u, v);

    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

Positions::Points Positions::getDataPoints(const char* originalCrs,
                                           int originalWidth,
                                           int originalHeight,
                                           uint originalGeometryId,
                                           const Fmi::SpatialReference& theCRS,
                                           const Fmi::Box& theBox) const
{
  try
  {
    // Create the coordinate transformation from image world coordinates
    // to querydata world coordinates

    Fmi::CoordinateTransformation transformation("WGS84", theCRS);
    Fmi::CoordinateTransformation transformation2(theCRS, "WGS84");

    int deltax = (!!dx ? *dx : 10);
    int deltay = (!!dy ? *dy : 10);

    T::Coordinate_svec originalCoordinates;
    if (originalGeometryId > 0)
      originalCoordinates =
          Identification::gridDef.getGridLatLonCoordinatesByGeometryId(originalGeometryId);

    Points points;
    if (originalCoordinates)
    {
      uint pos = 0;
      uint sz = originalCoordinates->size();
      for (int y = 0; y < originalHeight; y = y + deltay)
      {
        for (int x = 0; x < originalWidth; x = x + deltax)
        {
          pos = y * originalWidth + x;
          if (pos < sz)
          {
            auto cc = (*originalCoordinates)[pos];
            if (inside(cc.x(), cc.y(), false))
            {
              double xx = cc.x();
              double yy = cc.y();

              transformation.transform(xx, yy);

              double xp = xx;
              double yp = yy;

              theBox.transform(xp, yp);

              NFmiPoint coordinate(cc.x(), cc.y());
              points.emplace_back(Point(xp, yp, coordinate, deltax, deltay));
            }
          }
        }
      }
    }
    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate graticule locations
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getGraticulePoints(const Engine::Querydata::Q& theQ,
                                                const Fmi::SpatialReference& theCRS,
                                                const Fmi::Box& theBox,
                                                bool forecastMode) const
{
  try
  {
    // Create the coordinate transformation from WGS84 to projection coordinates

    Fmi::CoordinateTransformation transformation("WGS84", theCRS);

    // Generate the graticule coordinates.

    Points points;

    // This loop order makes it easier to handle the poles only once
    for (int lat = -90; lat <= 90; lat += step)
      for (int lon = -180; lon < 180; lon += step)
      {
        if (lon % size == 0 || lat % size == 0)
        {
          // latlon to world coordinate
          double xcoord = lon;
          double ycoord = lat;
          if (!transformation.transform(xcoord, ycoord))
            continue;

          // to pixel coordinate
          theBox.transform(xcoord, ycoord);

          int deltax = 0;
          if (dx)
            deltax += *dx;

          int deltay = 0;
          if (dy)
            deltay += *dy;

          // Skip if not inside desired areas
          if (inside(lon, lat, forecastMode))
            points.emplace_back(Point(xcoord, ycoord, NFmiPoint(lon, lat), deltax, deltay));
        }
        // Handle the poles only once
        if (lat == -90 || lat == 90)
          break;
      }

    apply_direction_offsets(points, theQ, time, directionoffset, rotate, direction, u, v);

    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate graticule fill locations
 *
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getGraticuleFillPoints(const Engine::Querydata::Q& theQ,
                                                    const Fmi::SpatialReference& theCRS,
                                                    const Fmi::Box& theBox,
                                                    bool forecastMode) const
{
  try
  {
    // Create the coordinate transformation from WGS84 to projection coordinates

    Fmi::CoordinateTransformation transformation("WGS84", theCRS);

    // Generate the graticule coordinates. Algorithm:
    //
    // 1. Loop over all graticule cells
    // 2. Project the 4 corner coordinates
    // 3. If the cell overlaps the output
    //    a) calculate how many points would fit onto each edge
    //    b) generate the points for the edges
    //    c) choose the minimum of the number of points generated for the top & bottom edges
    //    d) choose the minimum of the number of points generated for the left & right edges
    //    e) generate the points inside the cell using dimensions determined in c) and d)
    //
    // Details: - we start from bottom left, and output only bottom & left edges.
    //          - the poles are handled separately

    Points points;

    // Do not allow too close pixels
    Fmi::NearTree<XY> selected_coordinates;

    for (int lat = -90; lat < 90; lat += size)
      for (int lon = -180; lon < 180; lon += size)
      {
        double x1 = lon;         // p3---p4
        double y1 = lat;         // |    |
        double x2 = lon + size;  // p1---p2
        double y2 = lat;
        double x3 = lon;
        double y3 = lat + size;
        double x4 = lon + size;
        double y4 = lat + size;

        // to projection coordinates
        if (!transformation.transform(x1, y1))
          continue;
        if (!transformation.transform(x2, y2))
          continue;
        if (!transformation.transform(x3, y3))
          continue;
        if (!transformation.transform(x4, y4))
          continue;

        // to pixel coordinates
        theBox.transform(x1, y1);
        theBox.transform(x2, y2);
        theBox.transform(x3, y3);
        theBox.transform(x4, y4);

        // We make the assumption that the bounding box very closely
        // matches the bounding box formed by the graticule cell.
        // One could use a margin-setting to allow positions slightly
        // outside the box to fix that.

        if (!overlaps(theBox, x1, y1, x2, y2, x3, y3, x4, y4))
          continue;

        double bottom = length(x1, y1, x2, y2);
        double left = length(x1, y1, x3, y3);
        double top = length(x3, y3, x4, y4);
        double right = length(x2, y2, x4, y4);

        int nbottom = std::floor(bottom / mindistance);
        int nleft = std::floor(left / mindistance);
        int ntop = std::floor(top / mindistance);
        int nright = std::floor(right / mindistance);

        int nparallel = std::min(nbottom, ntop);
        int nmeridian = std::min(nleft, nright);

        // Left edge

        for (int j = 0; j < nleft; j++)
        {
          double newlon = lon;
          double newlat = lat + 1.0 * j / nleft * size;
          double newx = newlon;
          double newy = newlat;

          if (!transformation.transform(newx, newy))
            continue;
          theBox.transform(newx, newy);

          int deltax = 0;
          if (dx)
            deltax += *dx;
          int deltay = 0;
          if (dy)
            deltay += *dy;

          // Skip if not inside desired areas
          if (inside(newlon, newlat, forecastMode))
          {
            XY xy(newx, newy);
            auto match = selected_coordinates.nearest(xy, mindistance);
            if (!match)
            {
              points.emplace_back(Point(newx, newy, NFmiPoint(newlon, newlat), deltax, deltay));
              selected_coordinates.insert(xy);
            }
          }
        }

        // Bottom edge
        for (int i = 1; i < nbottom; i++)
        {
          double newlon = lon + 1.0 * i / nbottom * size;
          double newlat = lat;
          double newx = newlon;
          double newy = newlat;

          if (!transformation.transform(newx, newy))
            continue;
          theBox.transform(newx, newy);

          int deltax = 0;
          if (dx)
            deltax += *dx;
          int deltay = 0;
          if (dy)
            deltay += *dy;

          // Skip if not inside desired shapes
          if (inside(newlon, newlat, forecastMode))
          {
            XY xy(newx, newy);
            auto match = selected_coordinates.nearest(xy, mindistance);
            if (!match)
            {
              points.emplace_back(Point(newx, newy, NFmiPoint(newlon, newlat), deltax, deltay));
              selected_coordinates.insert(xy);
            }
          }

          // Handle the south pole only once
          if (newlat == -90)
            break;
        }

        // Inner points

        for (int i = 1; i < nparallel; i++)
          for (int j = 1; j < nmeridian; j++)
          {
            double newlon = lon + 1.0 * i / nparallel * size;
            double newlat = lat + 1.0 * j / nmeridian * size;
            double newx = newlon;
            double newy = newlat;

            if (!transformation.transform(newx, newy))
              continue;
            theBox.transform(newx, newy);

            int deltax = 0;
            if (dx)
              deltax += *dx;
            int deltay = 0;
            if (dy)
              deltay += *dy;

            // Skip if not inside desired area
            if (inside(newlon, newlat, forecastMode))
            {
              XY xy(newx, newy);
              auto match = selected_coordinates.nearest(xy, mindistance);
              if (!match)
              {
                points.emplace_back(Point(newx, newy, NFmiPoint(newlon, newlat), deltax, deltay));
                selected_coordinates.insert(xy);
              }
            }
          }
      }

    // All that remains is the north pole
    double newlon = 0;
    double newlat = 90;
    double newx = newlon;
    double newy = newlat;

    if (!transformation.transform(newx, newy))
    {
      int deltax = 0;
      if (dx)
        deltax += *dx;
      int deltay = 0;
      if (dy)
        deltay += *dy;

      // Skip if not inside desired areas
      if (inside(newlon, newlat, forecastMode))
      {
        XY xy(newx, newy);
        auto match = selected_coordinates.nearest(xy, mindistance);
        if (!match)
        {
          points.emplace_back(Point(newx, newy, NFmiPoint(newlon, newlat), deltax, deltay));
          selected_coordinates.insert(xy);
        }
      }
    }

    apply_direction_offsets(points, theQ, time, directionoffset, rotate, direction, u, v);

    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate keyword locations
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getKeywordPoints(const Engine::Querydata::Q& theQ,
                                              const Fmi::SpatialReference& theCRS,
                                              const Fmi::Box& theBox,
                                              bool forecastMode) const
{
  try
  {
    // Read the keyword

    if (keyword.empty())
      throw Fmi::Exception(
          BCP, "No keyword given when trying to use a keyword for location definitions!");

    Locus::QueryOptions options;
    auto locations = geonames->keywordSearch(options, keyword);

    // Keyword locations are in WGS84

    Fmi::CoordinateTransformation transformation("WGS84", theCRS);

    Points points;

    // This loop order makes it easier to handle the poles only once
    for (const auto& location : locations)
    {
      // keyword location latlon
      double lon = location->longitude;
      double lat = location->latitude;

      // To world coordinate
      double xcoord = lon;
      double ycoord = lat;
      if (!transformation.transform(xcoord, ycoord))
        continue;

      // to pixel coordinate
      theBox.transform(xcoord, ycoord);

      int deltax = 0;
      if (dx)
        deltax += *dx;
      int deltay = 0;
      if (dy)
        deltay += *dy;

      // Skip if not inside desired areas
      if (inside(lon, lat, forecastMode))
        points.emplace_back(Point(xcoord, ycoord, NFmiPoint(lon, lat), deltax, deltay));
    }

    apply_direction_offsets(points, theQ, time, directionoffset, rotate, direction, u, v);

    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate explicit locations
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getLatLonPoints(const Engine::Querydata::Q& theQ,
                                             const Fmi::SpatialReference& theCRS,
                                             const Fmi::Box& theBox,
                                             bool forecastMode) const
{
  try
  {
    Points points;

    if (locations.locations.empty())
      return points;

    // Create the coordinate transformation from WGS84 to projection coordinates

    Fmi::CoordinateTransformation transformation("WGS84", theCRS);

    // This loop order makes it easier to handle the poles only once
    for (const auto& location : locations.locations)
    {
      // keyword location latlon
      if (!location.longitude || !location.latitude)
        throw Fmi::Exception(BCP, "Incomplete location in the locations list!");

      double lon = *location.longitude;
      double lat = *location.latitude;

      // To world coordinate
      double xcoord = lon;
      double ycoord = lat;
      if (!transformation.transform(xcoord, ycoord))
        continue;

      // to pixel coordinate
      theBox.transform(xcoord, ycoord);

      // Global position adjustment
      int deltax = 0;
      if (dx)
        deltax += *dx;

      int deltay = 0;
      if (dy)
        deltay = *dy;

      // Individual adjustments
      if (location.dx)
        deltax += *location.dx;

      if (location.dy)
        deltay += *location.dy;

      // Skip if not inside desired areas
      if (inside(lon, lat, forecastMode))
        points.emplace_back(Point(xcoord, ycoord, NFmiPoint(lon, lat), deltax, deltay));
    }

    apply_direction_offsets(points, theQ, time, directionoffset, rotate, direction, u, v);

    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Generate station locations
 */
// ----------------------------------------------------------------------

Positions::Points Positions::getStationPoints(const Engine::Querydata::Q& theQ,
                                              const Fmi::SpatialReference& theCRS,
                                              const Fmi::Box& theBox,
                                              bool forecastMode) const
{
  try
  {
    Points points;

    // We pass the station numbers directly to obsengine

    if (!forecastMode)
      return points;

    if (stations.empty())
      return points;

    // Create the coordinate transformation from WGS84 to projection coordinates

    Fmi::CoordinateTransformation transformation("WGS84", theCRS);

    Locus::QueryOptions options;
    auto locations = geonames->keywordSearch(options, keyword);

    // This loop order makes it easier to handle the poles only once
    for (const auto& station : stations.stations)
    {
      Locus::QueryOptions options;
      options.SetFeatures("SYNOP");
      Spine::LocationList places;

      if (station.fmisid)
      {
        options.SetLanguage("fmisid");
        places = geonames->nameSearch(options, Fmi::to_string(*station.fmisid));
        if (places.empty())
          std::cerr << Spine::log_time_str() << " WMS could not find fmisid " << *station.fmisid
                    << std::endl;
      }
      else if (station.wmo)
      {
        options.SetLanguage("wmo");
        places = geonames->nameSearch(options, Fmi::to_string(*station.wmo));
        if (places.empty())
          std::cerr << Spine::log_time_str() << " WMS could not find wmo " << *station.wmo
                    << std::endl;
      }
      else if (station.lpnn)
      {
        options.SetLanguage("lpnn");
        places = geonames->nameSearch(options, Fmi::to_string(*station.lpnn));
        if (places.empty())
          std::cerr << Spine::log_time_str() << " WMS could not find lpnn " << *station.lpnn
                    << std::endl;
      }
      else if (station.geoid)
      {
        places = geonames->idSearch(options, *station.geoid);
        if (places.empty())
          std::cerr << Spine::log_time_str() << " WMS could not find geoid " << *station.geoid
                    << std::endl;
      }
      else if (station.longitude || station.latitude)
      {
        throw Fmi::Exception(
            BCP, "Use latlon instead of station layout for forecast data when listing coordinates");
      }
      else
        throw Fmi::Exception(BCP, "Station ID not set in the configuration");

      if (places.empty())
        continue;

      // We'll just use the first station instead of checking for duplicates,
      // which should never exist anyway

      auto location = *places.front();

      double lon = location.longitude;
      double lat = location.latitude;

      // To world coordinate
      double xcoord = lon;
      double ycoord = lat;
      if (!transformation.transform(xcoord, ycoord))
        continue;

      // to pixel coordinate
      theBox.transform(xcoord, ycoord);

      // Global position adjustment
      int deltax = 0;

      if (dx)
        deltax += *dx;

      int deltay = 0;
      if (dy)
        deltay += *dy;

      // Individual adjustments
      if (station.dx)
        deltax += *station.dx;

      if (station.dy)
        deltay += *station.dy;

      // Skip if not inside desired areas
      if (inside(lon, lat, forecastMode))
        points.emplace_back(Point(xcoord, ycoord, NFmiPoint(lon, lat), deltax, deltay));
    }

    apply_direction_offsets(points, theQ, time, directionoffset, rotate, direction, u, v);

    return points;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Test if the given point is to be rendered
 */
// ----------------------------------------------------------------------

bool Positions::insideShapes(double theX, double theY) const
{
  try
  {
    if (outshape && Fmi::OGR::inside(*outshape, theX, theY))
      return false;

    if (inshape && !Fmi::OGR::inside(*inshape, theX, theY))
      return false;

    return true;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Test if the given point is to be rendered
 */
// ----------------------------------------------------------------------

bool Positions::inside(double theX, double theY, bool forecastMode) const
{
  try
  {
    if (!insideShapes(theX, theY))
      return false;

    if (!forecastMode)
      return true;

    return intersections.inside(theX, theY);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Test if the given point is to be rendered or filtered out
 */
// ----------------------------------------------------------------------

bool Positions::inside(double theX,
                       double theY,
                       const Intersections::IntersectValues& theValues) const
{
  try
  {
    if (outshape && Fmi::OGR::inside(*outshape, theX, theY))
      return false;

    if (inshape && !Fmi::OGR::inside(*inshape, theX, theY))
      return false;

    return intersections.inside(theValues);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t Positions::hash_value(const State& theState) const
{
  try
  {
    auto hash = Fmi::hash_value(static_cast<int>(layout));

    Fmi::hash_combine(hash, Fmi::hash_value(x));
    Fmi::hash_combine(hash, Fmi::hash_value(y));
    Fmi::hash_combine(hash, Fmi::hash_value(dx));
    Fmi::hash_combine(hash, Fmi::hash_value(dy));
    Fmi::hash_combine(hash, Fmi::hash_value(ddx));

    Fmi::hash_combine(hash, Fmi::hash_value(size));

    Fmi::hash_combine(hash, Fmi::hash_value(step));

    Fmi::hash_combine(hash, Fmi::hash_value(mindistance));

    Fmi::hash_combine(hash, Fmi::hash_value(keyword));

    Fmi::hash_combine(hash, Dali::hash_value(locations, theState));
    Fmi::hash_combine(hash, Dali::hash_value(stations, theState));

    Fmi::hash_combine(hash, Fmi::hash_value(directionoffset));
    Fmi::hash_combine(hash, Fmi::hash_value(rotate));
    Fmi::hash_combine(hash, Fmi::hash_value(direction));
    Fmi::hash_combine(hash, Fmi::hash_value(u));
    Fmi::hash_combine(hash, Fmi::hash_value(v));

    Fmi::hash_combine(hash, Dali::hash_value(outsidemap, theState));
    Fmi::hash_combine(hash, Dali::hash_value(insidemap, theState));
    Fmi::hash_combine(hash, Dali::hash_value(intersections, theState));

    Fmi::hash_combine(hash, Fmi::hash_value(xmargin));
    Fmi::hash_combine(hash, Fmi::hash_value(ymargin));

    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

}  // namespace Dali
}  // namespace Plugin
}  // namespace SmartMet


/*! \brief Automatically configures and runs a Benchmark from a CFG file.

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <cmath>
#include <array>
#include <string>

#include <boost/variant.hpp>

#include "../fmm/fmdata/fmcell.h"
#include "../ndgridmap/ndgridmap.hpp"

#include "../fmm/fmm.hpp"
#include "../fmm/fim.hpp"
#include "../fmm/gmm.hpp"
#include "../fmm/ufmm.hpp"

#include "../fmm/fmdata/fmfibheap.hpp"
#include "../fmm/fmdata/fmdaryheap.hpp"
#include "../fmm/fmdata/fmpriorityqueue.hpp"

#include "../benchmark/benchmark.hpp"
#include "../benchmark/benchmarkcfg.hpp"

using namespace std;

int main(int argc, const char ** argv)
{
    // Parse input.
    if (argc < 2)
    {
        std::cerr << "Usage:\n\t " << argv[0] << " problem.cfg" << std::endl;
        return 1;
    }

    // Parse the CFG file.
    BenchmarkCFG bcfg;
    if (bcfg.readOptions(argv[1]))
    {
        // If FMCell is used...
        if(bcfg.getValue<std::string>("grid.cell") == "FMCell")
        {
            // ... and for dimensions...
            switch (bcfg.getValue<unsigned int>("grid.ndims"))
            {
                case 2:
                {
                    Benchmark<nDGridMap<FMCell,2> > b;
                    bcfg.configure<nDGridMap<FMCell,2>, FMCell>(b);
                    b.run();
                    break;
                }
                case 3:
                {
                    Benchmark<nDGridMap<FMCell,3> > b;
                    bcfg.configure<nDGridMap<FMCell,3>, FMCell>(b);
                    b.run();
                    break;
                }
                // Include here new dimensions copying, pasting and changing x.
                /*case x:
                {
                    Benchmark<nDGridMap<FMCell,x> > b;
                    bcfg.configure<nDGridMap<FMCell,x>, FMCell>(b);
                    b.run();
                    break;
                }*/
            }
        }
        else // else if (bcfg.getValue<std::string>("grid.cell") == "MyCell")
        {
            // Include here new celltypes and include the corresponding switch dimensions as for FMCell.
            // ... and for dimensions...
            /*switch (bcfg.getValue<unsigned int>("grid.ndims"))
            {
                case 2:
                {
                    Benchmark<nDGridMap<MyCell,2> > b;
                    bcfg.configure<nDGridMap<MyCell,2>, MyCell>(b);
                    b.run();
                    break;
                }
                //...
            }*/
        }
    }
}


////////////////////////////////////////////////////////////////////////////
//! \file    robot.cpp
//! \date    February 2018
//! \author  Eric Barnett
//!          Marc-Antoine Lacasse
//!
//! Copyright (C) 2018 Eric Barnett         <e.barnett@robotiq.com>
//! Copyright (C) 2018 Marc-Antoine Lacasse <malacasse@robotiq.com>
//! Copyright (C) 2018 Clément Gosselin     <gosselin@gmc.ulaval.ca>
//!
//! \brief Robot-specific kinematics and dynamics
//!
//! This file is part of BA, a bisection algorithm for time-optimal trajectory
//! planning
//!
//! This Source Code Form is subject to the terms of the Berkeley Software
//! Distribution (BSD) 3-clause license . If a copy of the BSD license was not
//! distributed with this file, you can obtain one at
//! https://opensource.org/licenses/BSD-3-Clause.
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
// Includes
////////////////////////////////////////////////////////////////////////////////
#include "util.h"
#include "robot.h"
#include "config.h"
#include <array>
#include <Eigen/Core> // for linear Algebra in fwdKinKuka()
#ifndef isWin
   #include <limits>
   #include "string.h"
#endif

using namespace Eigen;

namespace BATOTP{

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief set_robotType
//!
//! Set the robot type (define new robots here)
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_set_robotType(const std::string& robotTypeStr)
{
    return set_robotType(robotTypeStr);
}

int Robot::set_robotType(const std::string &robotTypeStr)
{
   _robotType=0;
   _robotTypeStr=robotTypeStr;
   if(_robotTypeStr=="KUKA")     _robotType=KUKA;
   if(_robotTypeStr=="UR")       _robotType=UR;
   if(_robotTypeStr=="RR")       _robotType=RR;
   if(_robotTypeStr=="CSPR3DOF") _robotType=CSPR3DOF;
   if(_robotTypeStr=="GENJNT")   _robotType=GENJNT;

   return(_robotType);
}

//! FORWARD KINEMATICS

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief fwdKin
//!
//! Forward displacement problem
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_fwdKin(const std::vector<std::vector<double> >& theta,
                       std::vector<std::vector<double>> &cart)
{return(fwdKin(theta,cart));}
int Robot::fwdKin(const std::vector<std::vector<double>> &theta,
                  std::vector<std::vector<double>> &cart)
{

   switch(_robotType)
   {
   case KUKA: //KUKA-LWR IV+
      fwdKinKuka(theta,cart);
      break;
   case RR: // planar RR positioning manipulator
      fwdKinRR(theta,cart);
      break;
   default:
      printf ("No forward Kinematics model provided for robotType=%s.\n",
              _robotTypeStr.c_str());
      return -1;
      assert(!"Unhandled case"); // Always assert unhandled cases
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief fwdKinKuka
//!
//! Forward displacement problem for the Kuka LWR IV+
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::fwdKinKuka(const std::vector<std::vector<double> >& theta,
                      std::vector<std::vector<double>> &cart)
{
   int i;
   Matrix3d Q12,Q34,Q567,Q1234,Q;
   //op=[0; 0; .282];  .204+78 Robotiq hand with Robotiq fingers (center of pads)
   //op=[0; 0; .3145]; Robotiq hand with Robotiq fingers (tips of pads)
   Vector3d ToolPointVector(0,-.08,.545); // .468+.078 Robotiq hand with long fingers

   int nPts=(int)theta[0].size();

   double a0=.3105,a1=.4,a2=.39; // link lengths

   double t1,t2,t3,t4,t5,t6,t7;
   double c1,c2,c3,c4,c5,c6,c7;
   double s1,s2,s3,s4,s5,s6,s7;
   double x1,y1,z1;
   double x2,y2,z2;
   double x3,y3,z3;

   cart.resize(3);
   for(i=0;i<3;i++) cart[i].resize(nPts);

   for(i=0; i<nPts; i++)
   {
      t1=_DEG2RAD*theta[0][i]; c1=cos(t1); s1=sin(t1);
      t2=_DEG2RAD*theta[1][i]; c2=cos(t2); s2=sin(t2);
      t3=_DEG2RAD*theta[2][i]; c3=cos(t3); s3=sin(t3);
      t4=_DEG2RAD*theta[3][i]; c4=cos(t4); s4=sin(t4);
      t5=_DEG2RAD*theta[4][i]; c5=cos(t5); s5=sin(t5);
      t6=_DEG2RAD*theta[5][i]; c6=cos(t6); s6=sin(t6);
      t7=_DEG2RAD*theta[6][i]; c7=cos(t7); s7=sin(t7);

      Q12 <<  c1*c2, -s1, -c1*s2,
            c2*s1,  c1, -s1*s2,
            s2,   0,     c2;

      Q34 << c3*c4, -s3, c3*s4,
            c4*s3,  c3, s3*s4,
            -s4,   0,    c4;

      Q567 << c5*c6*c7 - s5*s7, - c7*s5 - c5*c6*s7, -c5*s6,
            c5*s7 + c6*c7*s5,   c5*c7 - c6*s5*s7, -s5*s6,
            c7*s6,             -s6*s7,     c6;

      Q1234=Q12*Q34; Q=Q1234*Q567;

      // elbow
      x1=a1*Q12(0,2);
      y1=a1*Q12(1,2);
      z1=a1*Q12(2,2)+a0;

      // wrist
      x2=x1+a2*Q1234(0,2);
      y2=y1+a2*Q1234(1,2);
      z2=z1+a2*Q1234(2,2);

      // tool point
      x3=x2+Q.row(0)*ToolPointVector;
      y3=y2+Q.row(1)*ToolPointVector;
      z3=z2+Q.row(2)*ToolPointVector;

      // normal (tool-pointing direction)
      //n << x3-x2, y3-y2, z3-z2 ;
      //n=n.normalized();

      cart[0][i]=x3;
      cart[1][i]=y3;
      cart[2][i]=z3;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief fwdKinRR
//!
//! Forward displacement problem for a RR planar robot
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::fwdKinRR(const std::vector<std::vector<double> >& theta,
                    std::vector<std::vector<double>> &cart)
{
   int i,nPts=(int)theta[0].size();
   double a1=.4,a2=.6;
   double th1,th2;
   cart.resize(3);
   for(i=0;i<3;i++) cart[i].resize(nPts);

   for(i=0;i<nPts;i++)
   {
      th1=_DEG2RAD*theta[0][i];
      th2=_DEG2RAD*theta[1][i];
      cart[0][i]=a1*std::cos(th1)+a2*std::cos(th1+th2);
      cart[1][i]=a1*std::sin(th1)+a2*std::sin(th1+th2);
   }
   return 0;
}

//! INVERSE KINEMATICS

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief invKin
//!
//! Inverse displacement problem
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_invKin(std::vector<std::vector<double>> &theta,
                       const std::vector<std::vector<double>> &cart)
{return(invKin(theta,cart));}
int Robot::invKin(std::vector<std::vector<double>> &theta,
                  const std::vector<std::vector<double>> &cart)
{
   switch(_robotType)
   {
   case CSPR3DOF:
      invKinCSPR3DOF(theta,cart);
      break;
   default:
      printf ("No inverse Kinematics model provided for robotType=%s.\n",
              _robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief invKinCSPR3DOF
//!
//! Inverse kinematics for the large 3-DOF point-mass CSPR in PLT-00370:
//! finds cable lengths (theta) from Cartesian EE positions (cart) and
//! cable attacment points (pmat)
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::invKinCSPR3DOF(std::vector<std::vector<double>> &theta,
                          const std::vector<std::vector<double>> &cart)
{

   int i;
   int nDim=(int)cart.size();

   int nPts=(int)cart[0].size();
   theta.resize(3);

   for(i=0;i<nDim;i++) theta[i].resize(nPts);
   double x,y,z,rho1,rho2,rho3;
   std::array<double,3> rv1,rv2,rv3;

   if(_pmat.size()==0)
   {
      findCSPR3DOFpmat(_pmat);
   }

   for(i=0;i<nPts;i++)
   {
      x=cart[0][i];
      y=cart[1][i];
      z=cart[2][i];
      rv1={x-_pmat[0][0], y-_pmat[1][0], z-_pmat[2][0]};
      rv2={x-_pmat[0][1], y-_pmat[1][1], z-_pmat[2][1]};
      rv3={x-_pmat[0][2], y-_pmat[1][2], z-_pmat[2][2]};
      rho1=norm(rv1);
      rho2=norm(rv2);
      rho3=norm(rv3);
      theta[0][i]=rho1;
      theta[1][i]=rho2;
      theta[2][i]=rho3;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief findCSPR3DOFpmat
//!
//! Compute the cable attachment points for the large point-mass 3-DOF CSPR
//! in PLT-00370 at Laval University
//!
//! \param
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::findCSPR3DOFpmat(std::vector<std::vector<double>> &pmat)
{
   int i,j;
   std::array<double,3> cible1={1.0941, -4.9074, 2.5542};
   std::array<double,3> delta1={-0.765, 0.112, 3.74};
   std::array<double,3> cible3={0.2098, 5.3409, 2.6236};
   std::array<double,3> delta2={0.43, 0.125, 3.615};

   std::array<double,3> p1=cible1+delta1;
   std::array<double,3> p2=cible3+delta2;
   std::array<double,3> p3={-5.9751, 0.1399, 6.1543};

   pmat.resize(3); for(i=0;i<3;i++) pmat[i].resize(3);

   std::array<int,3> ind={1,0,2};
   for(i=0;i<3;i++)
   {
      int it=ind[i];
      pmat[i][0]=-p1[it];
      pmat[i][1]=-p2[it];
      pmat[i][2]=-p3[it];
   }

   std::array<double,3> centroide;
   for(i=0;i<3;i++) centroide[i]=1/3.0*(pmat[i][0]+pmat[i][1]+pmat[i][2]);

   for(i=0;i<3;i++)
   {
      for(j=0;j<3;j++) pmat[i][j]-=centroide[i];
   }
   return 0;
}

//! DYNAMICS

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynSerial
//!
//! Dynamics model for a serial robot
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_dynSerial(std::vector<std::vector<double>> &a1,
                   std::vector<std::vector<double>> &a2,
                   std::vector<std::vector<double>> &a3,
                   std::vector<std::vector<double>> &a4,
                   const std::vector<std::vector<double>> &theta,
                   const std::vector<std::vector<double>> &thetaD,
                   const std::vector<std::vector<double>> &thetaD2)
{return(dynSerial(a1,a2,a3,a4,theta,thetaD,thetaD2));}
int Robot::dynSerial(std::vector<std::vector<double>> &a1,
                     std::vector<std::vector<double>> &a2,
                     std::vector<std::vector<double>> &a3,
                     std::vector<std::vector<double>> &a4,
                     const std::vector<std::vector<double>> &theta,
                     const std::vector<std::vector<double>> &thetaD,
                     const std::vector<std::vector<double>> &thetaD2)
{
   switch(_robotType)
   {
   case RR:
      dynRR(a1,a2,a3,a4,theta,thetaD,thetaD2);
      break;
   default:
      printf ("No dynamics model provided for serial robotType=%s.\n",
              _robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynRR
//!
//! Dynamics model for an RR planar robot
//!  - links are approximated as point masses
//!  - tau = a1*sddot + a2*sdot^2 + a3*sdot + a4
//!  - thetaD and thetaD2 are partial derivatives wrt s
//!
//! \param
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::dynRR(
      std::vector<std::vector<double>> &a1,
      std::vector<std::vector<double>> &a2,
      std::vector<std::vector<double>> &a3,
      std::vector<std::vector<double>> &a4,
      const std::vector<std::vector<double>> &theta,
      const std::vector<std::vector<double>> &thetaD,
      const std::vector<std::vector<double>> &thetaD2)
{
   int i;
   int nPts=(int)theta[0].size();
   double A1=.4,A2=.6,m1=4,m2=8;
   double th1,th2,dth1,dth2,ddth1,ddth2,c1,c2,c12,ccFact;
   double A11,A12,A22;
   for(i=0;i<2;i++)
   {
      a1[i].resize(nPts);
      a2[i].resize(nPts);
      a3[i].resize(nPts);
      a4[i].resize(nPts);
   }

   for(i=0;i<nPts;i++)
   {
      th1  =_DEG2RAD*theta[0][i];
      th2  =_DEG2RAD*theta[1][i];
      dth1 =_DEG2RAD*thetaD[0][i];
      dth2 =_DEG2RAD*thetaD[1][i];
      ddth1=_DEG2RAD*thetaD2[0][i];
      ddth2=_DEG2RAD*thetaD2[1][i];

      c1 =std::cos(th1);
      c2 =std::cos(th2);
      c12=std::cos(th1+th2);

      A11=.25*m1*A1*A1 + m2*(A1*A1 + .25*A2*A2 + A1*A2*c2);
      A12=.5*m2*(.5*A2*A2 + A1*A2*c2);
      A22=.25*m2*A2*A2;

      a1[0][i]=A11*dth1 + A12*dth2;
      a1[1][i]=A12*dth1 + A22*dth2;

      ccFact=m2*A1*A2*sin(th2);
      a2[0][i]=A11*ddth1 + A12*ddth2-ccFact*dth2*(dth1+.5*dth2);
      a2[1][i]=A12*ddth1 + A22*ddth2-.5*ccFact*dth1*dth1;

      //friction terms: (friction is zero if left unspecified)
      a3[0][i]=10*dth1;
      a3[1][i]=10*dth2;

      a4[0][i]=.5*_g*(m1*A1*c1 + m2*(2.0*A1*c1 + A2*c12));
      a4[1][i]=.5*_g*m2*A2*c12;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynParallel
//!
//! Dynamics model for a parallel robot
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
///
int Robot::call_dynParallel(std::vector<std::vector<double>> &a1,
                   std::vector<std::vector<double>> &a2,
                   std::vector<std::vector<double>> &a3,
                   std::vector<std::vector<double>> &a4,
                   const std::vector<std::vector<double>> &cart,
                   const std::vector<std::vector<double>> &cartD,
                   const std::vector<std::vector<double>> &cartD2)
{
    return dynParallel(a1,a2,a3,a4,cart,cartD,cartD2);
}

int Robot::dynParallel(std::vector<std::vector<double>> &a1,
                     std::vector<std::vector<double>> &a2,
                     std::vector<std::vector<double>> &a3,
                     std::vector<std::vector<double>> &a4,
                     const std::vector<std::vector<double>> &cart,
                     const std::vector<std::vector<double>> &cartD,
                     const std::vector<std::vector<double>> &cartD2)
{
   (void)cart;

   switch(_robotType)
   {
   case CSPR3DOF: //CSPR3DOF
      dynCSPR3DOF(a1,a2,a3,a4,cartD,cartD2);
      break;
   default:
      printf ("No dynamics model provided for parallel robotType=%s.\n",
              _robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief dynCSPR3DOF
//!
//! Dynamics model for the large 3-DOF point-mass CSPR in PLT-00370
//!  - A*tau = a1*sddot + a2*sdot^2 + a3*sdot + a4
//!  - cartD and cartD2 are partial derivatives wrt s
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::dynCSPR3DOF(
      std::vector<std::vector<double>> &a1,
      std::vector<std::vector<double>> &a2,
      std::vector<std::vector<double>> &a3,
      std::vector<std::vector<double>> &a4,
      const std::vector<std::vector<double>> &cartD,
      const std::vector<std::vector<double>> &cartD2)
{
   int i,j;
   int nCart=3;
   int nPts =(int)cartD[0].size();

   for(i=0;i<nCart;i++)
   {
      a1[i].resize(nPts);
      a2[i].resize(nPts);
      a3[i].assign(nPts,0.0);
      a4[i].assign(nPts,0.0);
   }

   for(i=0;i<nPts;i++)
   {
      for(j=0;j<nCart;j++)
      {
         a1[j][i]=-cartD[j][i];
         a2[j][i]=-cartD2[j][i];
      }
      a4[2][i]=_g;
   }
   return 0;
}

//////////////////////////////////////////////////////////////////////////////
//!
//! \brief setA
//!
//! Find the A matrix for the dynamics equation of a parallel robot
//!
//! \param
//! \return
//!
//////////////////////////////////////////////////////////////////////////////
int Robot::call_setA(const std::vector<double> &theta,
         const std::vector<double> &cart,
         std::vector<std::vector<double>> &A)
{return(setA(theta,cart,A));}

int Robot::setA(const std::vector<double> &theta,
                const std::vector<double> &cart,
                std::vector<std::vector<double>> &A)
{
   switch(_robotType)
   {
   case CSPR3DOF:
      for(int i=0;i<3;i++)
      {
         for(int j=0;j<3;j++)
         {
            A[i][j]=(cart[i]-_pmat[i][j])/theta[j];
         }
      }
      break;

   default:
      printf ("isParallel=True and no code was provided to find the A matrix");
      printf("for robotType=%s.\n",_robotTypeStr.c_str());
      assert(!"Unhandled case"); // Always assert unhandled cases
      return -1;
      break;
   }
   return 0;
}

}