#include <crab/config.h>

#include "../program_options.hpp"
#include "../common.hpp"
#ifdef HAVE_LDD
#include <crab/domains/ldd/ldd.hpp>
#endif
#include <boost/optional.hpp>
#include <crab/domains/boxes.hpp>
using namespace std;
using namespace ikos;
using namespace crab::cfg_impl;
using namespace crab::domain_impl;
#ifdef HAVE_LDD
using namespace crab::domains::ldd;
#endif

#define RATIONALS

#ifdef RATIONALS
typedef q_number number_t;
#else
typedef z_number number_t;
#endif

typedef linear_constraint<number_t, varname_t> linear_constraint_t;
typedef linear_expression<number_t, varname_t> linear_expression_t;
typedef interval <number_t> interval_t;

#ifdef HAVE_LDD

LddManager* create_ldd_man (size_t num_vars) {
  DdManager* cudd = Cudd_Init (0, 0, CUDD_UNIQUE_SLOTS, 127, 0);
  #ifndef RATIONALS
  theory_t * theory = tvpi_create_boxz_theory (num_vars);
  #else
  theory_t * theory = tvpi_create_box_theory (num_vars);
  #endif

  LddManager* ldd = Ldd_Init (cudd, theory);

  const bool dvo = true;
  if(dvo) Cudd_AutodynEnable (cudd, CUDD_REORDER_GROUP_SIFT);
  return ldd;
}

void destroy_ldd_man (LddManager* ldd) {
  DdManager *cudd = NULL;
  theory_t *theory = NULL;
  if (ldd) {
    cudd = Ldd_GetCudd (ldd);
    theory = Ldd_GetTheory (ldd);
    Ldd_Quit(ldd);
  }

  if (theory) tvpi_destroy_theory(theory);
  if (cudd) Cudd_Quit(cudd);
}

void dump (LddNodePtr val) {
  LddManager *ldd = getLddManager (val);
  DdManager *cudd = Ldd_GetCudd (ldd);
  FILE *fp = Cudd_ReadStdout(cudd);
  Cudd_SetStdout(cudd,stderr);
  if (val.get () == Ldd_GetTrue (ldd)) crab::outs () << "true\n";
  else if (val.get () == Ldd_GetFalse (ldd)) crab::outs () << "false\n";
  else Ldd_PrintMinterm(ldd,val.get ());
  Cudd_SetStdout(cudd,fp);
}

LddNodePtr top (LddManager *ldd)
{
  return lddPtr (ldd, Ldd_GetTrue (ldd));
}

LddNodePtr bot (LddManager *ldd)
{
  return lddPtr (ldd, Ldd_GetFalse (ldd));
}

bool isBot (LddManager * ldd, LddNodePtr v)
{
  return &*v == Ldd_GetFalse (ldd);
}

bool isTop (LddManager * ldd, LddNodePtr v)
{
  return &*v == Ldd_GetTrue (ldd);
}

LddNodePtr approx (LddManager *ldd, LddNodePtr v)
{
  return lddPtr (ldd, Ldd_TermMinmaxApprox(ldd, &*v));
}

LddNodePtr meet (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return lddPtr (ldd, Ldd_And (ldd, &*n1, &*n2));
}

LddNodePtr convex_join (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return approx (ldd, lddPtr (ldd, Ldd_Or (ldd, &*n1, &*n2)));
}

LddNodePtr join (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return lddPtr (ldd, Ldd_Or (ldd, &*n1, &*n2));
}

bool isLeq (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return Ldd_TermLeq(ldd, &(*n1), &(*n2));
}

LddNodePtr widen (LddManager *ldd, LddNodePtr n1, LddNodePtr n2)
{
  return lddPtr (ldd, Ldd_IntervalWiden (ldd, &*n1, &*n2));
}

int getVarId (varname_t v) {
  return v.index ();
}

/** return term for variable v, neg for negation of variable */
linterm_t termForVal(LddManager* ldd, varname_t v, bool neg = false)
{
  int varId = getVarId (v);
  int sgn = neg ? -1 : 1;
  linterm_t term = Ldd_GetTheory (ldd)->create_linterm_sparse_si (&varId, &sgn, 1);
  return term;
}

/** v := k, where k is a constant */
LddNodePtr assign (LddManager *ldd, LddNodePtr n, varname_t v, number_t k) {
  if (isBot (ldd, n)) return n;

  #ifndef RATIONALS
  mpq_class kk ((mpz_class) k);
  #else
  mpq_class kk = ((mpq_class) k);
  #endif
  linterm_t t = termForVal (ldd, v);
  constant_t kkk = (constant_t) tvpi_create_cst (kk.get_mpq_t ());
  LddNodePtr newVal =
      lddPtr(ldd, Ldd_TermReplace(ldd, &(*n), t, NULL, NULL, kkk, kkk));
  Ldd_GetTheory (ldd)->destroy_cst(kkk);

  assert (!isBot (ldd, newVal));
  return newVal;
}

/** v := k, where k is an interval */
LddNodePtr assign (LddManager* ldd, LddNodePtr n, varname_t v, interval_t ival) {

  if (isBot (ldd, n)) return n;

  constant_t kmin = NULL, kmax = NULL;

  if (boost::optional <number_t> l = ival.lb ().number ()) {
    #ifndef RATIONALS
    mpq_class val ((mpz_class) (*l));
    #else
    mpq_class val ((mpq_class) (*l));
    #endif
    kmin = (constant_t)tvpi_create_cst (val.get_mpq_t ());
  }

  if (boost::optional <number_t> u = ival.ub ().number ()) {
    #ifndef RATIONALS
    mpq_class val ((mpz_class) (*u));
    #else
    mpq_class val ((mpq_class) (*u));
    #endif
    kmax = (constant_t)tvpi_create_cst (val.get_mpq_t ());
  }

  linterm_t t = termForVal(ldd, v);
  LddNodePtr newVal =
      lddPtr(ldd, Ldd_TermReplace(ldd, &(*n), t, NULL, NULL, kmin, kmax));

  if (kmin) Ldd_GetTheory (ldd)->destroy_cst(kmin);
  if (kmax) Ldd_GetTheory (ldd)->destroy_cst(kmax);

  assert (!isBot (ldd, newVal));
  return newVal;
}

/** v := a * u + k, where a, k are constants and u variable */
LddNodePtr apply (LddManager* ldd,
                  const LddNodePtr n,
                  varname_t v, varname_t u,
                  number_t a, number_t k)
{
  if (isTop (ldd, n) || isBot (ldd, n)) return n;

  linterm_t t = termForVal (ldd, v);
  linterm_t r = termForVal (ldd, u);

  #ifndef RATIONALS
  mpq_class aa ((mpz_class) a);
  mpq_class kk ((mpz_class) k);
  #else
  mpq_class aa ((mpq_class) a);
  mpq_class kk ((mpq_class) k);
  #endif

  constant_t aaa = (constant_t) tvpi_create_cst (aa.get_mpq_t ());
  constant_t kkk = (constant_t) tvpi_create_cst (kk.get_mpq_t ());
  LddNodePtr newVal =
      lddPtr(ldd, Ldd_TermReplace(ldd, &(*n), t, r, aaa, kkk, kkk));
  Ldd_GetTheory (ldd)->destroy_cst(aaa);
  Ldd_GetTheory (ldd)->destroy_cst(kkk);

  assert (!isBot (ldd, newVal));
  return newVal;
}

/** v := u */
LddNodePtr assign (LddManager* ldd, const LddNodePtr n,
                   varname_t v, varname_t u)
{
  return apply (ldd, n, v, u, 1, 0);
}
#endif

int main (int argc, char** argv )
{
#ifdef HAVE_LDD
  SET_TEST_OPTIONS(argc,argv)

  LddManager* man = create_ldd_man (3000);
  Ldd_SanityCheck (man);

  variable_factory_t vfac;
  varname_t x = vfac["x"];
  varname_t y = vfac["y"];

  {
    LddNodePtr s1  = top (man);
    LddNodePtr s11 = assign (man, s1, x, 5.5);
    LddNodePtr s111 = assign (man, s11, y, 8.3);

    LddNodePtr s2  = top (man);
    LddNodePtr s22 = assign (man, s2, x, 5.5);
    LddNodePtr s222 = assign (man, s22, y, 12.2);

    dump (s111);
    dump (s222);
    crab::outs () << "Join \n";
    LddNodePtr s3 = join (man, s111, s222);
    dump (s3);
    crab::outs () << "Widening \n";
    LddNodePtr s4 = widen (man, s111, s222);
    dump (s4);
  }

  #if 1
  {
    // To reproduce an old bug with boxes.
    // Assertion failed: (level < (unsigned) cuddI(unique,Cudd_Regular(E)->index)), function cuddUniqueInter, file /Users/E30338/Repos/crab/build_crab2/ldd/src/ldd/cudd-2.4.2/cudd/cuddTable.c, line 1143.

    typedef boxes_domain< number_t, varname_t > boxes_domain_t;
    typedef variable <number_t, varname_t> var_t;

    boxes_domain_t s1;
    s1 += linear_constraint_t (var_t(x) >= number_t(0));
    s1 += linear_constraint_t (var_t(x) <= number_t(1,4));
    s1 += linear_constraint_t (var_t(y) >= number_t(1,4));
    s1 += linear_constraint_t (var_t(y) <= number_t(1,2));
    boxes_domain_t s2;
    s2 += linear_constraint_t (var_t(x) == number_t(0));
    s2 += linear_constraint_t (var_t(y) == number_t(1,2));

    boxes_domain_t s3 = s2 | s1;
    ///////////////////
    boxes_domain_t s4;
    s4 += linear_constraint_t (var_t(x) >= number_t(1,4));
    s4 += linear_constraint_t (var_t(x) <= number_t(1));
    s4 += linear_constraint_t (var_t(y) >= number_t(1,8));
    s4 += linear_constraint_t (var_t(y) <= number_t(1,4));

    boxes_domain_t s5 = s4 | s2;
    //////
    crab::outs () << "Widening of \n" << s3;
    crab::outs () << "and\n" << s5 << " = \n";
    boxes_domain_t s6 = s3 || s5;
    crab::outs () << s6 << "\n";

  }
  #endif

  // Ldd_NodeSanityCheck (man, &(*s4));

  // FIXME: seg fault here
  // destroy_ldd_man (man);
#endif
  return 0;
}


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

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

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

#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_equal.hpp>
#include <boost/simd/detail/math.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( abss_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::unsigned_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a) const BOOST_NOEXCEPT
    {
      return a;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abss_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a) const BOOST_NOEXCEPT
    {
      return simd::abs(a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abss_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::signed_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a0) const BOOST_NOEXCEPT
    {
      T a =  bs::abs(a0);
      return if_else(is_equal(a0,Valmin<T>()),Valmax<T>(),a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abss_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::unspecified_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T const& a) const BOOST_NOEXCEPT
    {
      return abss(a);
    }
  };
} } }

#endif


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

#include "theia/sfm/filter_view_pairs_from_relative_translation.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <memory>
#include <mutex>  // NOLINT
#include <unordered_map>

#include "theia/math/util.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"
#include "theia/util/random.h"
#include "theia/util/threadpool.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"

namespace theia {
using Eigen::Vector3d;

namespace {

// Helper struct to maintain the graph for the translation projection problem.
struct MFASNode {
  std::unordered_map<ViewId, double> incoming_nodes;
  std::unordered_map<ViewId, double> outgoing_nodes;
  double incoming_weight = 0;
  double outgoing_weight = 0;
};

// Rotate the translation direction based on the known orientation such that the
// translation is in the global reference frame.
std::unordered_map<ViewIdPair, Vector3d>
      RotateRelativeTranslationsToGlobalFrame(
          const std::unordered_map<ViewId, Vector3d>& orientations,
          const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs) {
  std::unordered_map<ViewIdPair, Vector3d> rotated_translations;
  rotated_translations.reserve(orientations.size());

  for (const auto& view_pair : view_pairs) {
    const Vector3d view_to_world_rotation =
        -1.0 * FindOrDie(orientations, view_pair.first.first);
    Vector3d rotated_translation;
    ceres::AngleAxisRotatePoint(view_to_world_rotation.data(),
                                view_pair.second.position_2.data(),
                                rotated_translation.data());
    rotated_translations.emplace(view_pair.first, rotated_translation);
  }
  return rotated_translations;
}

// Find the next view to add to the order. We attempt to choose a source (i.e.,
// a node with no incoming edges) or choose a node based on a heuristic such
// that it has the most source-like properties.
ViewId FindNextViewInOrder(
    const std::unordered_map<ViewId, MFASNode>& degrees_for_view) {
  ViewId best_choice = kInvalidViewId;
  double best_score = 0;
  for (const auto& view : degrees_for_view) {
    // If the view is a source view, return it.
    if (view.second.incoming_nodes.size() == 0) {
      return view.first;
    }

    // Otherwise, keep track of the max score seen so far.
    const double score = (view.second.outgoing_weight + 1.0) /
                         (view.second.incoming_weight + 1.0);
    if (score > best_score) {
      best_choice = view.first;
      best_score = score;
    }
  }

  return best_choice;
}

// Based on the 1D translation projections, compute an ordering of the
// translations.
std::unordered_map<ViewId, int> OrderTranslationsFromProjections(
    const std::unordered_map<ViewIdPair, double>&
        translation_direction_projections) {
  // Compute the degrees of all vertices as the sum of weights coming in or out.
  std::unordered_map<ViewId, MFASNode> degrees_for_view;
  for (const auto& translation_projection : translation_direction_projections) {
    const ViewIdPair view_id_pair =
        (translation_projection.second > 0)
            ? translation_projection.first
            : ViewIdPair(translation_projection.first.second,
                         translation_projection.first.first);

    // Update the MFAS entry.
    const double weight = std::abs(translation_projection.second);
    degrees_for_view[view_id_pair.second].incoming_weight += weight;
    degrees_for_view[view_id_pair.first].outgoing_weight += weight;
    degrees_for_view[view_id_pair.second].incoming_nodes.emplace(
        view_id_pair.first, weight);
    degrees_for_view[view_id_pair.first].outgoing_nodes.emplace(
        view_id_pair.second, weight);
  }

  // Compute the ordering.
  const int num_views = degrees_for_view.size();
  std::unordered_map<ViewId, int> translation_ordering;
  for (int i = 0; i < num_views; i++) {
    // Find the next view to add.
    const ViewId next_view_in_order = FindNextViewInOrder(degrees_for_view);
    translation_ordering[next_view_in_order] = i;

    // Update the MFAS graph and remove the next view from the degrees_for_view.
    const auto& next_view_info =
        FindOrDie(degrees_for_view, next_view_in_order);
    for (auto& neighbor_info : next_view_info.incoming_nodes) {
      degrees_for_view[neighbor_info.first].outgoing_weight -=
          neighbor_info.second;
      degrees_for_view[neighbor_info.first].outgoing_nodes.erase(
          next_view_in_order);
    }
    for (auto& neighbor_info : next_view_info.outgoing_nodes) {
      degrees_for_view[neighbor_info.first].incoming_weight -=
          neighbor_info.second;
      degrees_for_view[neighbor_info.first].incoming_nodes.erase(
          next_view_in_order);
    }
    degrees_for_view.erase(next_view_in_order);
  }

  return translation_ordering;
}

// Projects all the of the translation onto the given axis.
std::unordered_map<ViewIdPair, double> ProjectTranslationsOntoAxis(
    const Vector3d& axis,
    const std::unordered_map<ViewIdPair, Vector3d>& relative_translations) {
  std::unordered_map<ViewIdPair, double> projection_weights;
  projection_weights.reserve(relative_translations.size());

  for (const auto& relative_translation : relative_translations) {
    const double projection_weight = relative_translation.second.dot(axis);
    projection_weights.emplace(relative_translation.first, projection_weight);
  }
  return projection_weights;
}

// This chooses a random axis based on the given relative translations.
void ComputeMeanVariance(
    const std::unordered_map<ViewIdPair, Vector3d>& relative_translations,
    Vector3d* mean,
    Vector3d* variance) {
  mean->setZero();
  variance->setZero();
  for (const auto& translation : relative_translations) {
    *mean += translation.second;
  }
  *mean /= static_cast<double>(relative_translations.size());

  for (const auto& translation : relative_translations) {
    *variance += (translation.second - *mean).cwiseAbs2();
  }
  *variance /= static_cast<double>(relative_translations.size());
}

// Performs a single iterations of the translation filtering. This method is
// thread-safe.
void TranslationFilteringIteration(
    const std::unordered_map<ViewIdPair, Vector3d>& relative_translations,
    const Vector3d& direction_mean,
    const Vector3d& direction_variance,
    const std::shared_ptr<RandomNumberGenerator>& rng,
    std::mutex* mutex,
    std::unordered_map<ViewIdPair, double>* bad_edge_weight) {
  // Create the random number generator within each thread. If the random number
  // generator is not supplied then create a new one within each thread.
  std::shared_ptr<RandomNumberGenerator> local_rng;
  if (rng.get() == nullptr) {
    local_rng = std::make_shared<RandomNumberGenerator>();
  } else {
    local_rng = rng;
  }

  // Get a random vector to project all relative translations on to.
  const Vector3d random_axis =
      Vector3d(
          local_rng->RandGaussian(direction_mean[0], direction_variance[0]),
          local_rng->RandGaussian(direction_mean[1], direction_variance[1]),
          local_rng->RandGaussian(direction_mean[2], direction_variance[2]))
          .normalized();

  // Project all vectors.
  const std::unordered_map<ViewIdPair, double>&
      translation_direction_projections =
          ProjectTranslationsOntoAxis(random_axis, relative_translations);

  // Compute ordering.
  const std::unordered_map<ViewId, int>& translation_ordering =
      OrderTranslationsFromProjections(translation_direction_projections);

  // Compute bad edge weights.
  for (auto& edge : *bad_edge_weight) {
    const int ordering_diff =
        FindOrDie(translation_ordering, edge.first.second) -
        FindOrDie(translation_ordering, edge.first.first);
    const double& projection_weight_of_edge =
        FindOrDieNoPrint(translation_direction_projections, edge.first);

    VLOG(3) << "Edge (" << edge.first.first << ", " << edge.first.second
            << ") has ordering diff of " << ordering_diff
            << " and a projection of " << projection_weight_of_edge << " from "
            << FindOrDieNoPrint(relative_translations, edge.first).transpose();
    // If the ordering is inconsistent, add the absolute value of the bad weight
    // to the aggregate bad weight.
    if ((ordering_diff < 0 && projection_weight_of_edge > 0) ||
        (ordering_diff > 0 && projection_weight_of_edge < 0)) {
      std::lock_guard<std::mutex> lock(*mutex);
      edge.second += std::abs(projection_weight_of_edge);
    }
  }
}

}  // namespace

void FilterViewPairsFromRelativeTranslation(
    const FilterViewPairsFromRelativeTranslationOptions& options,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    ViewGraph* view_graph) {
  const auto& view_pairs = view_graph->GetAllEdges();

  // Weights of edges that have been accumulated throughout the iterations. A
  // higher weight means the edge is more likely to be bad.
  std::unordered_map<ViewIdPair, double> bad_edge_weight;
  for (const auto& view_pair : view_pairs) {
    bad_edge_weight[view_pair.first] = 0.0;
  }

  // Compute the adjusted translations so that they are oriented in the global
  // frame.
  const std::unordered_map<ViewIdPair, Vector3d>& rotated_translations =
      RotateRelativeTranslationsToGlobalFrame(orientations, view_pairs);

  Vector3d translation_mean, translation_variance;
  ComputeMeanVariance(rotated_translations,
                      &translation_mean,
                      &translation_variance);

  std::unique_ptr<ThreadPool> pool(new ThreadPool(options.num_threads));
  std::mutex mutex;
  for (int i = 0; i < options.num_iterations; i++) {
    pool->Add(TranslationFilteringIteration,
              rotated_translations,
              translation_mean,
              translation_variance,
              options.rng,
              &mutex,
              &bad_edge_weight);
  }
  // Wait for tasks to finish.
  pool.reset(nullptr);

  // Remove all the bad edges.
  const double max_aggregated_projection_tolerance =
      options.translation_projection_tolerance * options.num_iterations;
  int num_view_pairs_removed = 0;
  for (const auto& view_pair : bad_edge_weight) {
    VLOG(3) << "View pair (" << view_pair.first.first << ", "
            << view_pair.first.second << ") projection = " << view_pair.second;
    if (view_pair.second > max_aggregated_projection_tolerance) {
      view_graph->RemoveEdge(view_pair.first.first, view_pair.first.second);
      ++num_view_pairs_removed;
    }
  }

  VLOG(1) << "Removed " << num_view_pairs_removed
          << " view pairs by relative translation filtering.";
}

}  // namespace theia


#ifndef BLACKJACK_HPP
#define BLACKJACK_HPP
#include <iostream>
#include <string>
#include <deque>
#include <vector>
#include <random>
#include <ctime>
#include <chrono>
#include "../src/relearn.hpp"
#if USING_BOOST_SERIALIZATION
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/access.hpp>
#endif
/**
 * The basic Blackjack header structures:
 *  - card
 *  - hand
 *  - player
 *  - house
 *  - client
 *
 *  All that is minimally needed in order to
 *  create the Blackjack example
 */

// a simple card structure
struct card
{
    std::string name;
    std::string label;
    std::vector<unsigned int> value;

    void print() const
    {
        std::cout << name << " "
                  << label << " ";
    }

    bool operator==(const card & rhs) const
    {
        return this->name == rhs.name &&
               this->label == rhs.label &&
               this->value == rhs.value;

    }
#if USING_BOOST_SERIALIZATION
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & name;
        ar & label;
        ar & value;
    }
#endif
};

// a 52 playing card constant vector with unicode symbols :-D
const std::deque<card> cards {
    {"Ace",  "♠", {1, 11}}, {"Ace",  "♥", {1, 11}}, {"Ace",  "♦", {1, 11}}, {"Ace",  "♣", {1, 11}},
    {"Two",  "♠", {2}},     {"Two",  "♥", {2}},     {"Two",  "♦", {2}},     {"Two",  "♣", {2}},
    {"Three","♠", {3}},     {"Three","♥", {3}},     {"Three","♦", {3}},     {"Three","♣", {3}},
    {"Four", "♠", {4}},     {"Four", "♥", {4}},     {"Four", "♦", {4}},     {"Four", "♣", {4}},
    {"Five", "♠", {5}},     {"Five", "♥", {5}},     {"Five", "♦", {5}},     {"Five", "♣", {5}},
    {"Six",  "♠", {6}},     {"Six",  "♥", {6}},     {"Six",  "♦", {6}},     {"Six",  "♣", {6}},
    {"Seven","♠", {7}},     {"Seven","♥", {7}},     {"Seven","♦", {7}},     {"Seven","♣", {7}},
    {"Eight","♠", {8}},     {"Eight","♥", {8}},     {"Eight","♦", {8}},     {"Eight","♣", {8}},
    {"Nine", "♠", {9}},     {"Nine", "♥", {9}},     {"Nine", "♦", {9}},     {"Nine", "♣", {9}},
    {"Ten",  "♠", {10}},    {"Ten",  "♥", {10}},    {"Ten",  "♦", {10}},    {"Ten",  "♣", {10}},
    {"Jack", "♠", {10}},    {"Jack", "♥", {10}},    {"Jack", "♦", {10}},    {"Jack", "♣", {10}},
    {"Queen","♠", {10}},    {"Queen","♥", {10}},    {"Queen","♦", {10}},    {"Queen","♣", {10}},
    {"King", "♠", {10}},    {"King", "♥", {10}},    {"King", "♦", {10}},    {"King", "♣", {10}}
};

bool card_compare(const card & lhs, const card & rhs)
{
    return lhs.name == rhs.name &&
           lhs.label == rhs.label &&
           lhs.value == rhs.value;
}

// hand is the currently held cards
struct hand
{
    hand() = default;
    hand(const hand &) = default;

    // calculate value of hand - use max value of hand
    unsigned int max_value() const
    {
        unsigned int result = 0;
        for (const card & k : cards) {
            result += *std::max_element(k.value.begin(), k.value.end());
        }
        return result;
    }

    // calculate value of hand - use min value (e.g., when hold an Ace)
    unsigned int min_value() const
    {
        unsigned int result = 0;
        for (const card & k : cards) {
            result += *std::min_element(k.value.begin(), k.value.end());
        }
        return result;
    }

    // print on stdout
    void print() const
    {
        for (card k : cards) {
            k.print();
        }
        std::cout << std::endl;
    }

    // add new card
    void insert(card arg)
    {
        cards.push_back(arg);
    }

    // clear hand
    void clear()
    {
        cards.clear();
    }

    // hand is blackjack
    bool blackjack() const
    {
        std::vector<card> twoblacks = {{"Ace",  "♠", {1, 11}},
                                       {"Ace",  "♣", {1, 11}}};
        return std::is_permutation(twoblacks.begin(), twoblacks.end(),
                                   cards.begin(),  card_compare);
    }

    // hash this hand for relearn
    std::size_t hash() const
    {
        std::size_t seed = 0;
        for (auto & k : cards) {
            for (auto & v : k.value) {
                relearn::hash_combine(seed, v);
            }
        }
        return seed;
    }

    bool operator==(const hand & rhs) const
    {
        return this->cards == rhs.cards;
    }

private:
    std::vector<card> cards;
#if USING_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        ar & cards;
    }
#endif
};

namespace std
{
template <> struct hash<hand>
{
    std::size_t operator()(hand const& arg) const
    {
        return arg.hash();
    }
};
}

/// compare hands (return true for lhs wins)
bool hand_compare(const hand & lhs, const hand & rhs)
{
    if (lhs.blackjack()) return true;
    else if (rhs.blackjack()) return false;

    if (lhs.min_value() > 21) return false;
    else if (rhs.min_value() > 21 && lhs.min_value() < 21) return true;

    if (lhs.max_value() > rhs.max_value()) return true;
    else return false;
}

// Base class for all players
struct player : public hand
{
    virtual bool draw()
    {
        return false;
    }
};

// House/dealer only uses simple rules to draw or stay
struct house : public player
{
    house(std::deque<card> cards, std::mt19937 & prng)
    : cards(cards), gen(prng)
    {}

    // draw a card based on current hand - house always draws until 17 is reached
    bool draw()
    {
        return (min_value() < 17 || max_value() < 17);
    }

    // deal a card using current deck - or reset and deal
    card deal()
    {
        if (deck.size() > 0) {
            auto obj = deck.front();
            deck.pop_front();
            return obj;
        }
        else {
            reset_deck();
            return deal();
        }
    }

    // shuffle cards randomly
    void reset_deck()
    {
        deck = cards;
        std::shuffle(std::begin(deck), std::end(deck), gen);
    }

private:
    std::deque<card> deck;
    const std::deque<card> cards;
    std::mt19937 gen;
};

//
// our learning adaptive player
struct client : public player
{
    // decide on drawing or staying
    bool draw(std::mt19937 & prng,
              relearn::state<hand> s_t,
              relearn::policy<relearn::state<hand>,
                              relearn::action<bool>> & map)
    {
        auto pair = map.best(s_t);
        std::uniform_real_distribution<float> dist(0, 1);
        // there exists a "best action" and it is positive
        if (pair.first && pair.second > 0) {
            policy_actions++;
            return pair.first->trait();
        }
        // there does not exist a "best action"
        else {
            random_actions++;
            return (dist(prng) > 0.5 ? true : false);
        }
    }

    // return a state by casting self to base class
    relearn::state<hand> state() const
    {
        return relearn::state<hand>(*this);
    }

    void reset()
    {
        random_actions = 0;
        policy_actions = 0;
    }

    float random_actions = 0;
    float policy_actions = 0;
};

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <iostream>
#include <sstream>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/multi/algorithms/transform.hpp>

#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/multi/io/wkt/wkt.hpp>

// This test is a little different from transform.cpp test.
// This test explicitly tests all geometries, including multi*
// while the transform.cpp tests various strategies.

template <typename Geometry>
void test_transform(std::string const& wkt, std::string const& expected)
{
    typedef typename bg::coordinate_type<Geometry>::type coordinate_type;
    const std::size_t dim = bg::dimension<Geometry>::value;

    Geometry geometry_in, geometry_out;
    bg::read_wkt(wkt, geometry_in);
    bg::transform(geometry_in, geometry_out,
        bg::strategy::transform::scale_transformer<coordinate_type, dim, dim>(2, 2));
    std::ostringstream detected;
    detected << bg::wkt(geometry_out);
    BOOST_CHECK_EQUAL(detected.str(), expected);
}

template <typename T>
void test_all()
{
    typedef bg::model::d2::point_xy<T> P;

    test_transform<P>(
            "POINT(1 1)",
            "POINT(2 2)");
    test_transform<bg::model::linestring<P> >(
            "LINESTRING(1 1,2 2)",
            "LINESTRING(2 2,4 4)");
    test_transform<bg::model::segment<P> >(
            "LINESTRING(1 1,2 2)",
            "LINESTRING(2 2,4 4)");
    test_transform<bg::model::ring<P> >(
            "POLYGON((0 0,0 1,1 0,0 0))",
            "POLYGON((0 0,0 2,2 0,0 0))");
    test_transform<bg::model::polygon<P> >(
            "POLYGON((0 0,0 1,1 0,0 0))",
            "POLYGON((0 0,0 2,2 0,0 0))");
    test_transform<bg::model::box<P> >(
            "POLYGON((0 0,0 1,1 1,1 0,0 0))",
            "POLYGON((0 0,0 2,2 2,2 0,0 0))");
    test_transform<bg::model::multi_point<P> >(
            "MULTIPOINT((1 1),(2 2))",
            "MULTIPOINT((2 2),(4 4))");
    test_transform<bg::model::multi_linestring<bg::model::linestring<P> > >(
            "MULTILINESTRING((1 1,2 2))",
            "MULTILINESTRING((2 2,4 4))");
    test_transform<bg::model::multi_polygon<bg::model::polygon<P> > >(
            "MULTIPOLYGON(((0 0,0 1,1 0,0 0)))",
            "MULTIPOLYGON(((0 0,0 2,2 0,0 0)))");
}

int test_main(int, char* [])
{
    test_all<double>();

#ifdef HAVE_TTMATH
    test_all<ttmath_big>();
#endif
    return 0;
}


// Copyright 2013 Daniel Parker
// Distributed under Boost license

#include <boost/test/unit_test.hpp>
#include <jsoncons/json.hpp>
#include <jsoncons/json_serializer.hpp>
#include <sstream>
#include <vector>
#include <utility>
#include <ctime>
#include "boost/date_time/gregorian/gregorian.hpp"
#include <boost/numeric/ublas/matrix.hpp>

namespace jsoncons
{
    template <class Json>
    struct json_type_traits<Json,boost::gregorian::date>
    {
        static bool is(const Json& val) JSONCONS_NOEXCEPT
        {
            if (!val.is_string())
            {
                return false;
            }
            std::string s = val.template as<std::string>();
            try
            {
                boost::gregorian::from_simple_string(s);
                return true;
            }
            catch (...)
            {
                return false;
            }
        }

        static boost::gregorian::date as(const Json& val)
        {
            std::string s = val.template as<std::string>();
            return boost::gregorian::from_simple_string(s);
        }

        static Json to_json(boost::gregorian::date val)
        {
            return Json(to_iso_extended_string(val));
        }
    };

    template <class Json,class T>
    struct json_type_traits<Json,boost::numeric::ublas::matrix<T>>
    {
        static bool is(const Json& val) JSONCONS_NOEXCEPT
        {
            if (!val.is_array())
            {
                return false;
            }
            if (val.size() > 0)
            {
                size_t n = val[0].size();
                for (const auto& a: val.array_range())
                {
                    if (!(a.is_array() && a.size() == n))
                    {
                        return false;
                    }
                    for (auto x: a.array_range())
                    {
                        if (!x.template is<T>())
                        {
                            return false;
                        }
                    }
                }
            }
            return true;
        }

        static boost::numeric::ublas::matrix<T> as(const Json& val)
        {
            if (val.is_array() && val.size() > 0)
            {
                size_t m = val.size();
                size_t n = 0;
                for (const auto& a : val.array_range())
                {
                    if (a.size() > n)
                    {
                        n = a.size();
                    }
                }

                boost::numeric::ublas::matrix<T> A(m,n,T());
                for (size_t i = 0; i < m; ++i)
                {
                    const auto& a = val[i];
                    for (size_t j = 0; j < a.size(); ++j)
                    {
                        A(i,j) = a[j].template as<T>();
                    }
                }
                return A;
            }
            else
            {
                boost::numeric::ublas::matrix<T> A;
                return A;
            }
        }

        static Json to_json(const boost::numeric::ublas::matrix<T>& val)
        {
            Json a = Json::template make_array<2>(val.size1(), val.size2(), T());
            for (size_t i = 0; i < val.size1(); ++i)
            {
                for (size_t j = 0; j < val.size1(); ++j)
                {
                    a[i][j] = val(i,j);
                }
            }
            return a;
        }
    };
}

using namespace jsoncons;
using boost::numeric::ublas::matrix;

BOOST_AUTO_TEST_SUITE(json_extensibility_tests)

BOOST_AUTO_TEST_CASE(test_add_extensibility)
{
    json a = json::array();
    a.push_back(boost::gregorian::date(2013,10,14));
    auto d = a[0].as<boost::gregorian::date>();
    BOOST_CHECK_EQUAL(boost::gregorian::date(2013,10,14),d);

    json o;
    o["ObservationDates"] = std::move(a);
    o["ObservationDates"].push_back(boost::gregorian::date(2013,10,21));
    d = o["ObservationDates"][0].as<boost::gregorian::date>();
    auto d2 = o["ObservationDates"][1].as<boost::gregorian::date>();

    BOOST_CHECK_EQUAL(boost::gregorian::date(2013,10,14),d);
    BOOST_CHECK_EQUAL(boost::gregorian::date(2013,10,21),d2);

    json deal;
    deal["maturity"] = boost::gregorian::date(2015,1,1);
    json observation_dates = json::array();
    observation_dates.push_back(boost::gregorian::date(2013,10,21));
    observation_dates.push_back(boost::gregorian::date(2013,10,28));
    deal["observation_dates"] = std::move(observation_dates);

    //std::cout << pretty_print(deal) << std::endl;
}

BOOST_AUTO_TEST_CASE(test_set_extensibility)
{
    json o;
    boost::gregorian::date d(boost::gregorian::day_clock::local_day());
    o.insert_or_assign("today",d);
    boost::gregorian::date val = o["today"].as<boost::gregorian::date>();
    BOOST_CHECK_EQUAL(d,val);
}

BOOST_AUTO_TEST_CASE(test_assignment_extensibility)
{
    json o;
    boost::gregorian::date d(boost::gregorian::day_clock::local_day());
    o["today"] = d;
    boost::gregorian::date val = o["today"].as<boost::gregorian::date>();
    BOOST_CHECK_EQUAL(d,val);
}

BOOST_AUTO_TEST_CASE(test_example)
{
        using jsoncons::json;
        using boost::gregorian::date;

        json deal;
        deal["Maturity"] = date(2014,10,14);

        json observation_dates = json::array();
        observation_dates.push_back(date(2014,2,14));
        observation_dates.push_back(date(2014,2,21));

        deal["ObservationDates"] = std::move(observation_dates);

        date maturity = deal["Maturity"].as<date>();

        BOOST_CHECK(deal["Maturity"].as<date>() == date(2014,10,14));
        BOOST_REQUIRE(deal["ObservationDates"].is_array());
        BOOST_REQUIRE(deal["ObservationDates"].size() == 2);
        BOOST_CHECK(deal["ObservationDates"][0].as<date>() == date(2014,2,14));
        BOOST_CHECK(deal["ObservationDates"][1].as<date>() == date(2014,2,21));

        //std::cout << pretty_print(deal) << std::endl;
}

BOOST_AUTO_TEST_CASE(test_boost_matrix)
{
    matrix<double> A(2, 2);
    A(0, 0) = 1.1;
    A(0, 1) = 2.1;
    A(1, 0) = 3.1;
    A(1, 1) = 4.1;

    json a = A;

    BOOST_CHECK(a.is<matrix<double>>());
    BOOST_CHECK(!a.is<matrix<int>>());

    BOOST_CHECK_EQUAL(a[0][0].as<double>(),A(0,0));
    BOOST_CHECK_EQUAL(a[0][1].as<double>(),A(0,1));
    BOOST_CHECK_EQUAL(a[1][0].as<double>(),A(1,0));
    BOOST_CHECK_EQUAL(a[1][1].as<double>(),A(1,1));

    matrix<double> B = a.as<matrix<double>>();

    BOOST_CHECK_EQUAL(B.size1(),a.size());
    BOOST_CHECK_EQUAL(B.size2(),a[0].size());

    BOOST_CHECK_EQUAL(a[0][0].as<double>(),B(0,0));
    BOOST_CHECK_EQUAL(a[0][1].as<double>(),B(0,1));
    BOOST_CHECK_EQUAL(a[1][0].as<double>(),B(1,0));
    BOOST_CHECK_EQUAL(a[1][1].as<double>(),B(1,1));
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright 2019,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#include <iostream>
#include <sot/core/debug.hh>

#ifndef WIN32
#include <unistd.h>
#endif

using namespace std;

#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <sot/core/control-pd.hh>
#include <sstream>

using namespace dynamicgraph;
using namespace dynamicgraph::sot;

#define BOOST_TEST_MODULE debug - control - pd

#include <boost/test/output_test_stream.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(control_pd) {
  sot::ControlPD *aControlPD = new ControlPD("acontrol_pd");
  aControlPD->init(0.001);
  std::istringstream Kpiss("[5](10.0,20.0,30.0,40.0,50.0)");
  std::istringstream Kdiss("[5](0.10,0.20,0.30,0.40,0.50)");
  aControlPD->KpSIN.set(Kpiss);
  aControlPD->KdSIN.set(Kdiss);
  std::istringstream posiss("[5](1.0,1.0,1.0,1.0,1.0)");
  aControlPD->positionSIN.set(posiss);
  std::istringstream dposiss("[5](3.0,3.1,3.2,3.3,3.4)");
  aControlPD->desiredpositionSIN.set(dposiss);
  std::istringstream veliss("[5](0.0,0.0,0.0,0.0,0.0)");
  aControlPD->velocitySIN.set(veliss);
  std::istringstream dveliss("[5](1.5,1.4,1.3,1.2,1.1)");
  aControlPD->desiredvelocitySIN.set(dveliss);

  aControlPD->controlSOUT.recompute(0);
  aControlPD->positionErrorSOUT.recompute(0);
  aControlPD->velocityErrorSOUT.recompute(0);
  {
    boost::test_tools::output_test_stream output;
    aControlPD->controlSOUT.get(output);
    BOOST_CHECK(output.is_equal(" 20.15\n 42.28\n 66.39\n 92.48\n120.55\n"));
  }
  {
    boost::test_tools::output_test_stream output;
    aControlPD->positionErrorSOUT.get(output);
    BOOST_CHECK(output.is_equal("  2\n2.1\n2.2\n2.3\n2.4\n"));
  }
  {
    boost::test_tools::output_test_stream output;
    aControlPD->velocityErrorSOUT.get(output);
    BOOST_CHECK(output.is_equal("1.5\n1.4\n1.3\n1.2\n1.1\n"));
  }
}


#include "aslam/calibration/target-base.h"

#include <cmath>
#include <utility>

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

namespace aslam {
namespace calibration {

TargetBase::TargetBase(
    size_t rows, size_t cols, const Eigen::Matrix3Xd& points_target_frame)
    : rows_(rows), cols_(cols), points_target_frame_(points_target_frame) {
  CHECK_GT(rows, 0u);
  CHECK_GT(cols, 0u);
  CHECK_EQ(static_cast<int>(size()), points_target_frame_.cols());
}

const Eigen::Matrix3Xd& TargetBase::points() const {
  return points_target_frame_;
}

Eigen::Vector3d TargetBase::point(size_t point_idx) const {
  CHECK_LT(point_idx, size());
  return points_target_frame_.col(point_idx);
}

std::pair<size_t, size_t> TargetBase::pointToGridCoordinates(
    size_t point_idx) const {
  return std::pair<size_t, size_t>(
      point_idx % cols(), static_cast<int>(point_idx / cols()));
}

size_t TargetBase::gridCoordinatesToPoint(
    size_t row_idx, size_t col_idx) const {
  CHECK_LT(row_idx, rows());
  CHECK_LT(col_idx, cols());
  const size_t point_idx = cols() * row_idx + col_idx;
  CHECK_LT(point_idx, size());
  return point_idx;
}

Eigen::Vector3d TargetBase::gridPoint(size_t row_idx, size_t col_idx) const {
  return points_target_frame_.col(gridCoordinatesToPoint(row_idx, col_idx));
}

double* TargetBase::pointMutable(size_t point_idx) {
  CHECK_LT(point_idx, size());
  return &points_target_frame_.coeffRef(0, point_idx);
}

double TargetBase::width() const {
  CHECK_GT(cols_, 0u);
  const Eigen::Vector3d vertical_difference =
      gridPoint(0u, cols_ - 1u) - gridPoint(0u, 0u);
  return std::abs(vertical_difference.x());
}

double TargetBase::height() const {
  CHECK_GT(rows_, 0u);
  const Eigen::Vector3d horizontal_difference =
      gridPoint(rows_ - 1u, 0u) - gridPoint(0u, 0u);
  return std::abs(horizontal_difference.y());
}

}  // namespace calibration
}  // namespace aslam


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_TEST_STRATEGIES_TEST_PROJECTED_POINT_HPP
#define BOOST_GEOMETRY_TEST_STRATEGIES_TEST_PROJECTED_POINT_HPP

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/strategies/cartesian/distance_projected_point.hpp>
#include <boost/geometry/strategies/cartesian/distance_projected_point_ax.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>

#include <boost/geometry/io/wkt/read.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P, typename PS, typename CalculationType>
void test_services()
{
    PS p1, p2;
    bg::assign_values(p1, 0, 0);
    bg::assign_values(p2, 0, 4);

    P p;
    bg::assign_values(p, 2, 0);

    CalculationType const sqr_expected = 4;
    CalculationType const expected = 2;

    namespace bgsd = bg::strategy::distance;
    namespace services = bg::strategy::distance::services;

    {
        // compile-check if there is a strategy for this type
        typedef typename services::default_strategy
            <
                bg::point_tag, bg::segment_tag, P, PS
            >::type projected_point_strategy_type;

        typedef typename services::default_strategy
            <
                bg::segment_tag, bg::point_tag, PS, P
            >::type reversed_tags_projected_point_strategy_type;

        boost::ignore_unused<projected_point_strategy_type,
                             reversed_tags_projected_point_strategy_type>();
    }

    // 1: normal, calculate distance:

    typedef bgsd::projected_point<CalculationType> strategy_type;

    BOOST_CONCEPT_ASSERT( (bg::concepts::PointSegmentDistanceStrategy<strategy_type, P, PS>) );

    typedef typename services::return_type<strategy_type, P, PS>::type return_type;

    strategy_type strategy;
    return_type result = strategy.apply(p, p1, p2);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 2: the strategy should return the same result if we reverse parameters
    result = strategy.apply(p, p2, p1);
    BOOST_CHECK_CLOSE(result, return_type(expected), 0.001);

    // 3: "comparable" to construct a "comparable strategy" for P1/P2
    //    a "comparable strategy" is a strategy which does not calculate the exact distance, but
    //    which returns results which can be mutually compared (e.g. avoid sqrt)

    // 3a: "comparable_type"
    typedef typename services::comparable_type<strategy_type>::type comparable_type;

    // 3b: "get_comparable"
    comparable_type comparable = bgsd::services::get_comparable<strategy_type>::apply(strategy);

    return_type c_result = comparable.apply(p, p1, p2);
    BOOST_CHECK_CLOSE(c_result, return_type(sqr_expected), 0.001);
}

template <typename T1, typename T2>
void test_check_close(T1 const& v1, T2 const& v2, double f)
{
    BOOST_CHECK_CLOSE(v1, v2, f);
}

template <typename T1, typename T2>
void test_check_close(bg::strategy::distance::detail::projected_point_ax_result<T1> const& v1,
                      bg::strategy::distance::detail::projected_point_ax_result<T2> const& v2,
                      double f)
{
    BOOST_CHECK_CLOSE(v1.atd, v2.atd, f);
    BOOST_CHECK_CLOSE(v1.xtd, v2.xtd, f);
}

template <typename P1, typename P2, typename T, typename Strategy, typename ComparableStrategy>
void test_2d(std::string const& wkt_p,
             std::string const& wkt_sp1,
             std::string const& wkt_sp2,
             T expected_distance,
             T expected_comparable_distance,
             Strategy strategy,
             ComparableStrategy comparable_strategy)
{
    P1 p;
    P2 sp1, sp2;
    bg::read_wkt(wkt_p, p);
    bg::read_wkt(wkt_sp1, sp1);
    bg::read_wkt(wkt_sp2, sp2);

    BOOST_CONCEPT_ASSERT
        (
            (bg::concepts::PointSegmentDistanceStrategy<Strategy, P1, P2>)
        );
    BOOST_CONCEPT_ASSERT
        (
            (bg::concepts::PointSegmentDistanceStrategy<ComparableStrategy, P1, P2>)
        );

    {
        typedef typename bg::strategy::distance::services::return_type<Strategy, P1, P2>::type return_type;
        return_type d = strategy.apply(p, sp1, sp2);
        test_check_close(d, expected_distance, 0.001);
    }

    // Test combination with the comparable strategy
    {
        typedef typename bg::strategy::distance::services::return_type<ComparableStrategy, P1, P2>::type return_type;
        return_type d = comparable_strategy.apply(p, sp1, sp2);
        test_check_close(d, expected_comparable_distance, 0.01);
    }

}

template <typename P1, typename P2, typename T>
void test_2d(std::string const& wkt_p,
             std::string const& wkt_sp1,
             std::string const& wkt_sp2,
             T expected_distance)
{
    typedef bg::strategy::distance::projected_point<> strategy_type;
    typedef bg::strategy::distance::projected_point
        <
            void,
            bg::strategy::distance::comparable::pythagoras<>
        > comparable_strategy_type;

    strategy_type strategy;
    comparable_strategy_type comparable_strategy;

    T expected_squared_distance = expected_distance * expected_distance;
    test_2d<P1, P2>(wkt_p, wkt_sp1, wkt_sp2, expected_distance, expected_squared_distance, strategy, comparable_strategy);
}

#endif // BOOST_GEOMETRY_TEST_STRATEGIES_TEST_PROJECTED_POINT_HPP


#pragma once
#include <Eigen/Core>
#include <memory>
#include <tuple>
#include <vector>

#include "Geometry3D.hpp"
#include "../KDTreeFlann/KDTreeSearchParam.hpp"

namespace pointcloudhandler
{

		class PointCloud : public Geometry3D
		{
		public:
			PointCloud() : Geometry3D(Geometry::GeometryType::PointCloud) {}
			PointCloud(const std::vector<Eigen::Vector3d>& points)
				: Geometry3D(Geometry::GeometryType::PointCloud), mPoints(points) {}
			~PointCloud() override {}

		public:
			PointCloud& Clear() override;
			bool IsEmpty() const override;
			Eigen::Vector3d GetMinBound() const override;
			Eigen::Vector3d GetMaxBound() const override;
			Eigen::Vector3d GetCenter() const override;

			PointCloud& Transform(const Eigen::Matrix4d & transformation) override;
			PointCloud& Translate(const Eigen::Vector3d & translation, bool relative = true) override;
			PointCloud& Scale(const double scale, bool center = true) override;
			PointCloud& Rotate(const Eigen::Matrix3d & R, bool center = true) override;

			PointCloud& operator+=(const PointCloud & cloud);
			PointCloud operator+(const PointCloud & cloud) const;

			bool HasPoints() const { return mPoints.size() > 0; }

			/// Remove all points fromt he point cloud that have a nan entries, or infinite entries.
			PointCloud& RemoveNoneFinitePoints(bool removeNan = true, bool removeInfinite = true);

			/// Function to select points from \param input pointcloud into
			/// \return output pointcloud
			/// Points with indices in \param indices are selected.
			std::shared_ptr<PointCloud> SelectReducedPts(const std::vector<size_t>& indices, bool invert = false) const;

			/// Function to remove points that have less than \param nb_points in a
			/// sphere of radius \param search_radius
			std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
				RemoveRadiusOutliers(size_t nbPoints, double searchRadius) const;

			/// Function to remove points that are further away from their
			/// \param nb_neighbor neighbors in average.
			std::tuple<std::shared_ptr<PointCloud>, std::vector<size_t>>
				RemoveStatisticalOutliers(size_t nbNeighbors, double stdRatio) const;

			/// Function to compute the point to point distances between point clouds
			/// \param source is the first point cloud.
			/// \param target is the second point cloud.
			/// \return the output distance. It has the same size as the number
			/// of points in \param source
			std::vector<double> ComputePointCloudDistance(const PointCloud & target);

			/// Function to compute the mean and covariance matrix
			/// of an \param input point cloud
			std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance() const;

			/// Function to compute the Mahalanobis distance for points
			/// in an \param input point cloud
			/// https://en.wikipedia.org/wiki/Mahalanobis_distance
			std::vector<double> ComputeMahalanobisDistance() const;

			/// Function to compute the distance from a point to its nearest neighbor in
			/// the \param input point cloud
			std::vector<double> ComputeNearestNeighborDistance() const;

			size_t CloudSize() const {
				return mPoints.size();
			}

			std::vector<Eigen::Vector3d> Points() const {
				return mPoints;
			}

		public:
			std::vector<Eigen::Vector3d> mPoints;
		};

}  // namespace pointcloudhandler


// Copyright (c) 2006 Johan Rade
// Copyright (c) 2011 Paul A. Bristow comments
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

/*!
\file
\brief Legacy (non-C99) tests of the nonfinite num facets.

\detail legacy_test outputs using nonfinite_num_put facet
with legacy flag, and reads back in using nonfinite_num_ facet,
and checks loopback OK.

Also checks that output of infinity, -infinity and NaN are as expected,
including the 'legacy' "1.#IND",  "1.#QNAN", "1.#SNAN" representations
(was used by MSVC but now all represented on output by "1.#QNAN")
and qnan snan nanq nans (used by other systems)
excluding C99 specification  "nan -nan nan -nan" and "inf -inf".
*/

#ifdef _MSC_VER
#   pragma warning(disable : 4702)
#endif

#include <iomanip>
#include <locale>
#include <sstream>

#define BOOST_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>

//#include "almost_equal.hpp"
//#include "S_.hpp"

#include <boost/math/special_functions/nonfinite_num_facets.hpp>

namespace {

// The anonymous namespace resolves ambiguities on platforms
// with fpclassify etc functions declared at global scope.

using namespace boost::math;
using boost::math::signbit;
using boost::math::changesign;
using boost::math::isnan;

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

void legacy_test_inf();
void legacy_test_nan();

BOOST_AUTO_TEST_CASE(legacy_test)
{
    legacy_test_inf();
    legacy_test_nan();
}

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

template<class CharType, class ValType> void legacy_test_inf_impl();

void legacy_test_inf()
{
    legacy_test_inf_impl<char, float>();
    legacy_test_inf_impl<char, double>();
    legacy_test_inf_impl<char, long double>();
    legacy_test_inf_impl<wchar_t, float>();
    legacy_test_inf_impl<wchar_t, double>();
    legacy_test_inf_impl<wchar_t, long double>();
}

template<class CharType, class ValType> void legacy_test_inf_impl()
{
    std::locale old_locale;
    std::locale new_locale(old_locale, new nonfinite_num_get<CharType>(legacy));

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    ValType a1 = std::numeric_limits<ValType>::infinity();
    ValType a2 = -std::numeric_limits<ValType>::infinity();
    ss << a1 << ' ' << a2;

    ss << " 1.#INF";

    ValType b1, b2, b3;
    ss >> b1 >> b2 >> b3;

    BOOST_CHECK(b1 == a1);
    BOOST_CHECK(b2 == a2);
    BOOST_CHECK(b3 == std::numeric_limits<ValType>::infinity());
    BOOST_CHECK(ss.rdstate() == std::ios_base::eofbit);
}

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

template<class CharType, class ValType> void legacy_test_nan_impl();

void legacy_test_nan()
{
    legacy_test_nan_impl<char, float>();
    legacy_test_nan_impl<char, double>();
    legacy_test_nan_impl<char, long double>();
    legacy_test_nan_impl<wchar_t, float>();
    legacy_test_nan_impl<wchar_t, double>();
    legacy_test_nan_impl<wchar_t, long double>();
}

template<class CharType, class ValType> void legacy_test_nan_impl()
{
    std::locale old_locale;
    std::locale new_locale(old_locale, new nonfinite_num_get<CharType>(legacy));

    std::basic_stringstream<CharType> ss;
    ss.imbue(new_locale);

    ValType a1 = std::numeric_limits<ValType>::quiet_NaN();
    ValType a2 = -std::numeric_limits<ValType>::quiet_NaN();
    ValType a3 = std::numeric_limits<ValType>::signaling_NaN();
    ValType a4 = -std::numeric_limits<ValType>::signaling_NaN();
    ss << a1 << ' ' << a2 << ' ' << a3 << ' ' << a4;

    ss << " qnan snan nanq nans 1.#IND 1.#QNAN 1.#SNAN";

    ValType b1, b2, b3, b4, b5, b6, b7, b8, b9, b10, b11;
    ss >> b1 >> b2 >> b3 >> b4 >> b5 >> b6 >> b7 >> b8 >> b9 >> b10 >> b11;

    // std::cout << b11 << std::endl; // Confirms that legacy
    // IND, SNAN and QNAN are considered the same,
    // and both output the legacy string "1.#QNAN".

    BOOST_CHECK((isnan)(b1));
    BOOST_CHECK((isnan)(b2));
    BOOST_CHECK((isnan)(b3));
    BOOST_CHECK((isnan)(b4));
    BOOST_CHECK((isnan)(b5));
    BOOST_CHECK((isnan)(b6));
    BOOST_CHECK((isnan)(b7));
    BOOST_CHECK((isnan)(b8));
    BOOST_CHECK((isnan)(b9));
    BOOST_CHECK((isnan)(b10));
    BOOST_CHECK((isnan)(b11));  //  Johan V3 1.#SNAN failed on MSVC 10.
    // Change in nonfinite_num_facet.hpp Paul A. Bristow 11 Apr 11 makes work OK.
/*
    // These tests fail on platforms, such as gcc,
    // that use the same representation of +nan and -nan.

    BOOST_CHECK(!(signbit)(b1));
    BOOST_CHECK((signbit)(b2));
    BOOST_CHECK(!(signbit)(b3));
    BOOST_CHECK((signbit)(b4));
*/
    BOOST_CHECK(!(signbit)(b5));
    BOOST_CHECK(!(signbit)(b6));
    BOOST_CHECK(!(signbit)(b7));
    BOOST_CHECK(!(signbit)(b8));
    BOOST_CHECK(!(signbit)(b9));
    BOOST_CHECK(!(signbit)(b10));
    BOOST_CHECK(!(signbit)(b11));  // Johan V3 1.#SNAN failed MSVC 10.

    BOOST_CHECK(ss.rdstate() == std::ios_base::eofbit); // Fails if SNAN test fails.
}

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

}   // anonymous namespace

/*

Output:

  legacy_test.vcxproj -> J:\Cpp\fp_facet\fp_facet\Debug\legacy_test.exe
  Running 1 test case...
  1.#QNAN
  1.#QNAN
  1.#QNAN
  1.#QNAN
  1.#QNAN
  1.#QNAN

  *** No errors detected

*/


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_GEOMETRIES_POINT_XY_HPP
#define BOOST_GEOMETRY_GEOMETRIES_POINT_XY_HPP

#include <cstddef>

#include <boost/mpl/int.hpp>

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

namespace boost { namespace geometry
{

namespace model { namespace d2
{

/*!
\brief 2D point in Cartesian coordinate system
\tparam CoordinateType numeric type, for example, double, float, int
\tparam CoordinateSystem coordinate system, defaults to cs::cartesian

\qbk{before.synopsis,
[heading Model of]
[link geometry.reference.concepts.concept_point Point Concept]
}
*/
template<typename CoordinateType, typename CoordinateSystem = cs::cartesian>
class point_xy : public model::point<CoordinateType, 2, CoordinateSystem>
{
public:

    /// Default constructor, does not initialize anything
    inline point_xy()
        : model::point<CoordinateType, 2, CoordinateSystem>()
    {}

    /// Constructor with x/y values
    inline point_xy(CoordinateType const& x, CoordinateType const& y)
        : model::point<CoordinateType, 2, CoordinateSystem>(x, y)
    {}

    /// Get x-value
    inline CoordinateType const& x() const
    { return this->template get<0>(); }

    /// Get y-value
    inline CoordinateType const& y() const
    { return this->template get<1>(); }

    /// Set x-value
    inline void x(CoordinateType const& v)
    { this->template set<0>(v); }

    /// Set y-value
    inline void y(CoordinateType const& v)
    { this->template set<1>(v); }
};

}} // namespace model::d2

// Adapt the point_xy to the concept
#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template <typename CoordinateType, typename CoordinateSystem>
struct tag<model::d2::point_xy<CoordinateType, CoordinateSystem> >
{
    typedef point_tag type;
};

template<typename CoordinateType, typename CoordinateSystem>
struct coordinate_type<model::d2::point_xy<CoordinateType, CoordinateSystem> >
{
    typedef CoordinateType type;
};

template<typename CoordinateType, typename CoordinateSystem>
struct coordinate_system<model::d2::point_xy<CoordinateType, CoordinateSystem> >
{
    typedef CoordinateSystem type;
};

template<typename CoordinateType, typename CoordinateSystem>
struct dimension<model::d2::point_xy<CoordinateType, CoordinateSystem> >
    : boost::mpl::int_<2>
{};

template<typename CoordinateType, typename CoordinateSystem, std::size_t Dimension>
struct access<model::d2::point_xy<CoordinateType, CoordinateSystem>, Dimension >
{
    static inline CoordinateType get(
        model::d2::point_xy<CoordinateType, CoordinateSystem> const& p)
    {
        return p.template get<Dimension>();
    }

    static inline void set(model::d2::point_xy<CoordinateType, CoordinateSystem>& p,
        CoordinateType const& value)
    {
        p.template set<Dimension>(value);
    }
};

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_GEOMETRIES_POINT_XY_HPP


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

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

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing sin capabilities

    sine of the input in radians.

    @par Semantic:

    For every parameter of floating type T

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

    As most other trigonometric function sin can be called with a second optional parameter
    which is a tag on speed and accuracy (see @ref cos for further details)

    @see sincos, sind, sinpi

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

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

#endif