#define BOOST_TEST_MODULE "FCL_SIMPLE"
#include <boost/test/unit_test.hpp>

#include "fcl/intersect.h"
#include "fcl/collision.h"
#include "fcl/BVH/BVH_model.h"
#include "fcl_resources/config.h"
#include <boost/filesystem.hpp>
#include <sstream>
#include "fcl/math/vec_nf.h"
#include "fcl/math/sampling.h"

using namespace fcl;

static FCL_REAL epsilon = 1e-6;

static bool approx(FCL_REAL x, FCL_REAL y)
{
  return std::abs(x - y) < epsilon;
}

template<std::size_t N>
double distance_Vecnf(const Vecnf<N>& a, const Vecnf<N>& b)
{
  double d = 0;
  for(std::size_t i = 0; i < N; ++i)
    d += (a[i] - b[i]) * (a[i] - b[i]);

  return d;
}

BOOST_AUTO_TEST_CASE(Vec_nf_test)
{
  Vecnf<4> a;
  Vecnf<4> b;
  for(std::size_t i = 0; i < a.dim(); ++i)
    a[i] = i;
  for(std::size_t i = 0; i < b.dim(); ++i)
    b[i] = 1;

  std::cout << a << std::endl;
  std::cout << b << std::endl;
  std::cout << a + b << std::endl;
  std::cout << a - b << std::endl;
  std::cout << (a -= b) << std::endl;
  std::cout << (a += b) << std::endl;
  std::cout << a * 2 << std::endl;
  std::cout << a / 2 << std::endl;
  std::cout << (a *= 2) << std::endl;
  std::cout << (a /= 2) << std::endl;
  std::cout << a.dot(b) << std::endl;

  Vecnf<8> c = combine(a, b);
  std::cout << c << std::endl;

  Vecnf<4> upper, lower;
  for(int i = 0; i < 4; ++i)
    upper[i] = 1;

  Vecnf<4> aa(std::vector<FCL_REAL>({1,2}));
  std::cout << aa << std::endl;

  SamplerR<4> sampler(lower, upper);
  for(std::size_t i = 0; i < 10; ++i)
    std::cout << sampler.sample() << std::endl;

  // Disabled broken test lines. Please see #25.
  // SamplerSE2 sampler2(0, 1, -1, 1);
  // for(std::size_t i = 0; i < 10; ++i)
  //   std::cout << sampler2.sample() << std::endl;

  SamplerSE3Euler sampler3(Vec3f(0, 0, 0), Vec3f(1, 1, 1));
  for(std::size_t i = 0; i < 10; ++i)
    std::cout << sampler3.sample() << std::endl;

}

BOOST_AUTO_TEST_CASE(projection_test_line)
{
  Vec3f v1(0, 0, 0);
  Vec3f v2(2, 0, 0);

  Vec3f p(1, 0, 0);
  Project::ProjectResult res = Project::projectLine(v1, v2, p);
  BOOST_CHECK(res.encode == 3);
  BOOST_CHECK(approx(res.sqr_distance, 0));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));

  p = Vec3f(-1, 0, 0);
  res = Project::projectLine(v1, v2, p);
  BOOST_CHECK(res.encode == 1);
  BOOST_CHECK(approx(res.sqr_distance, 1));
  BOOST_CHECK(approx(res.parameterization[0], 1));
  BOOST_CHECK(approx(res.parameterization[1], 0));

  p = Vec3f(3, 0, 0);
  res = Project::projectLine(v1, v2, p);
  BOOST_CHECK(res.encode == 2);
  BOOST_CHECK(approx(res.sqr_distance, 1));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1));

}

BOOST_AUTO_TEST_CASE(projection_test_triangle)
{
  Vec3f v1(0, 0, 1);
  Vec3f v2(0, 1, 0);
  Vec3f v3(1, 0, 0);

  Vec3f p(1, 1, 1);
  Project::ProjectResult res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 7);
  BOOST_CHECK(approx(res.sqr_distance, 4/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));

  p = Vec3f(0, 0, 1.5);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 1);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 1));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));

  p = Vec3f(1.5, 0, 0);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 4);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 1));

  p = Vec3f(0, 1.5, 0);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 2);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1));
  BOOST_CHECK(approx(res.parameterization[2], 0));

  p = Vec3f(1, 1, 0);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 6);
  BOOST_CHECK(approx(res.sqr_distance, 0.5));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));

  p = Vec3f(1, 0, 1);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 5);
  BOOST_CHECK(approx(res.sqr_distance, 0.5));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));

  p = Vec3f(0, 1, 1);
  res = Project::projectTriangle(v1, v2, v3, p);
  BOOST_CHECK(res.encode == 3);
  BOOST_CHECK(approx(res.sqr_distance, 0.5));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0));
}

BOOST_AUTO_TEST_CASE(projection_test_tetrahedron)
{
  Vec3f v1(0, 0, 1);
  Vec3f v2(0, 1, 0);
  Vec3f v3(1, 0, 0);
  Vec3f v4(1, 1, 1);

  Vec3f p(0.5, 0.5, 0.5);
  Project::ProjectResult res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 15);
  BOOST_CHECK(approx(res.sqr_distance, 0));
  BOOST_CHECK(approx(res.parameterization[0], 0.25));
  BOOST_CHECK(approx(res.parameterization[1], 0.25));
  BOOST_CHECK(approx(res.parameterization[2], 0.25));
  BOOST_CHECK(approx(res.parameterization[3], 0.25));

  p = Vec3f(0, 0, 0);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 7);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0, 1, 1);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 11);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 1/3.0));

  p = Vec3f(1, 1, 0);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 14);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[3], 1/3.0));

  p = Vec3f(1, 0, 1);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 13);
  BOOST_CHECK(approx(res.sqr_distance, 1/3.0));
  BOOST_CHECK(approx(res.parameterization[0], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 1/3.0));
  BOOST_CHECK(approx(res.parameterization[3], 1/3.0));

  p = Vec3f(1.5, 1.5, 1.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 8);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 1));

  p = Vec3f(1.5, -0.5, -0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 4);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 1));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(-0.5, -0.5, 1.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 1);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 1));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(-0.5, 1.5, -0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 2);
  BOOST_CHECK(approx(res.sqr_distance, 0.75));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 1));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0.5, -0.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 5);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0.5, 1.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 10);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0.5));

  p = Vec3f(1.5, 0.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 12);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));
  BOOST_CHECK(approx(res.parameterization[3], 0.5));

  p = Vec3f(-0.5, 0.5, 0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 3);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0));

  p = Vec3f(0.5, 0.5, 1.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 9);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0.5));
  BOOST_CHECK(approx(res.parameterization[1], 0));
  BOOST_CHECK(approx(res.parameterization[2], 0));
  BOOST_CHECK(approx(res.parameterization[3], 0.5));

  p = Vec3f(0.5, 0.5, -0.5);
  res = Project::projectTetrahedra(v1, v2, v3, v4, p);
  BOOST_CHECK(res.encode == 6);
  BOOST_CHECK(approx(res.sqr_distance, 0.25));
  BOOST_CHECK(approx(res.parameterization[0], 0));
  BOOST_CHECK(approx(res.parameterization[1], 0.5));
  BOOST_CHECK(approx(res.parameterization[2], 0.5));
  BOOST_CHECK(approx(res.parameterization[3], 0));

}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
// Probabilistic Reasoning Library (PRL)
// Copyright 2009 (see AUTHORS.txt for a list of contributors)
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library 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
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef TURI_STL_UTIL_HPP
#define TURI_STL_UTIL_HPP

#include <set>
#include <map>
#include <vector>
#include <algorithm>
#include <iterator>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <core/logging/assertions.hpp>
#include <boost/exception/detail/is_output_streamable.hpp>

// #include <core/storage/serialization/serialize.hpp>
// #include <core/storage/serialization/set.hpp>
// #include <core/storage/serialization/map.hpp>

namespace turi {

  /**
   * \ingroup util
   * \addtogroup set_and_map Set And Map Utilities
   * \brief Some mathematical Set and Map routines.
   * \{
   */
  // Functions on sets
  //============================================================================

  /**
   * computes the union of two sets.
   */
  template <typename T>
  std::set<T> set_union(const std::set<T>& a, const std::set<T>& b) {
    std::set<T> output;
    std::set_union(a.begin(), a.end(),
                   b.begin(), b.end(),
                   std::inserter(output, output.begin()));
    return output;
  }

  /**
   * computes the union of a set and a value.
   */
  template <typename T>
  std::set<T> set_union(const std::set<T>& a, const T& b) {
    std::set<T> output = a;
    output.insert(b);
    return output;
  }

  /**
   * computes the intersect of two sets.
   */
  template <typename T>
  std::set<T> set_intersect(const std::set<T>& a, const std::set<T>& b) {
    std::set<T> output;
    std::set_intersection(a.begin(), a.end(),
                          b.begin(), b.end(),
                          std::inserter(output, output.begin()));
    return output;
  }

  /**
   * computes the difference of two sets.
   */
  template <typename T>
  std::set<T> set_difference(const std::set<T>& a, const std::set<T>& b) {
    std::set<T> output;
    std::set_difference(a.begin(), a.end(),
                        b.begin(), b.end(),
                        std::inserter(output, output.begin()));
    return output;
  }

  /**
   * Subtract a value from a set
   */
  template <typename T>
  std::set<T> set_difference(const std::set<T>& a, const T& b) {
    std::set<T> output = a;
    output.erase(b);
    return output;
  }

  /**
   * Partitions a set with a different set.
   * Returns 2 sets: <s in partition, s not in partition>
   */
  template <typename T>
  std::pair<std::set<T>,std::set<T> >
  set_partition(const std::set<T>& s, const std::set<T>& partition) {
    std::set<T> a, b;
    a = set_intersect(s, partition);
    b = set_difference(s, partition);
    return std::make_pair(a, b);
  }

  /**
   * Returns true if the two sets are disjoint
   */
  template <typename T>
  bool set_disjoint(const std::set<T>& a, const std::set<T>& b) {
    return (intersection_size(a,b) == 0);
  }

  /**
   * Returns true if the two sets are equal
   */
  template <typename T>
  bool set_equal(const std::set<T>& a, const std::set<T>& b) {
    if (a.size() != b.size()) return false;
    return a == b; // defined in <set>
  }

  /**
   * Returns true if b is included in a
   */
  template <typename T>
  bool includes(const std::set<T>& a, const std::set<T>& b) {
    return std::includes(a.begin(), a.end(), b.begin(), b.end());
  }

  /**
   * Returns true if $a \subseteq b$
   */
  template <typename T>
  bool is_subset(const std::set<T>& a, const std::set<T>& b) {
    return includes(b, a);
  }

  /**
   * Returns true if $b \subseteq a$
   */
  template <typename T>
  bool is_superset(const std::set<T>& a,const std::set<T>& b) {
    return includes(a, b);
  }

  /*
   * \internal
   * Prints a container.
   */
  template <typename Container>
  std::ostream& print_range(std::ostream& out, const Container& c,
                   const std::string& left, const std::string& sep,const std::string& right) {

    out << left;

    for(auto it = c.begin();;) {
      if(it == c.end())
        break;

      out << *it;

      ++it;

      if(it != c.end())
        out << sep;
    }
    out << right << std::endl;

    return out;
  }

  /**
   * Writes a human representation of the set to the supplied stream.
   */
  template <typename T>
  typename boost::enable_if_c<boost::is_output_streamable<T>::value,
           std::ostream&>::type
  operator<<(std::ostream& out, const std::set<T>& s) {
    return print_range(out, s, "{", ", ", "}");
  }

  /**
   * Writes a human representation of a vector to the supplied stream.
   */
  template <typename T>
  typename boost::enable_if_c<boost::is_output_streamable<T>::value,
           std::ostream&>::type
  operator<<(std::ostream& out, const std::vector<T>& v) {
    return print_range(out, v, "[", ", ", "]");
  }

  // Functions on maps
  //============================================================================

  /**
   * constant lookup in a map. assertion failure of key not found in map
   */
  template <typename Key, typename T>
  const T& safe_get(const std::map<Key, T>& map,
                    const Key& key) {
    typedef typename std::map<Key, T>::const_iterator iterator;
    iterator iter = map.find(key);
    ASSERT_TRUE(iter != map.end());
    return iter->second;
  } // end of safe_get

  /**
   * constant lookup in a map. If key is not found in map,
   * 'default_value' is returned. Note that this can't return a reference
   * and must return a copy
   */
  template <typename Key, typename T>
  const T safe_get(const std::map<Key, T>& map,
                    const Key& key, const T default_value) {
    typedef typename std::map<Key, T>::const_iterator iterator;
    iterator iter = map.find(key);
    if (iter == map.end())   return default_value;
    else return iter->second;
  } // end of safe_get

  /**
   * Transform each key in the map using the key_map
   * transformation. The resulting map will have the form
   * output[key_map[i]] = map[i]
   */
  template <typename OldKey, typename NewKey, typename T>
  std::map<NewKey, T>
  rekey(const std::map<OldKey, T>& map,
        const std::map<OldKey, NewKey>& key_map) {
    std::map<NewKey, T> output;
    typedef std::pair<OldKey, T> pair_type;
    for(const pair_type& pair: map) {
      output[safe_get(key_map, pair.first)] = pair.second;
    }
    return output;
  }

  /**
   * Transform each key in the map using the key_map
   * transformation. The resulting map will have the form
   output[i] = remap[map[i]]
  */
  template <typename Key, typename OldT, typename NewT>
  std::map<Key, NewT>
  remap(const std::map<Key, OldT>& map,
        const std::map<OldT, NewT>& val_map) {
    std::map<Key, NewT> output;
    typedef std::pair<Key, OldT> pair_type;
    for(const pair_type& pair: map) {
      output[pair.first] = safe_get(val_map, pair.second);
    }
    return output;
  }

  /**
   * Inplace version of remap
   */
  template <typename Key, typename T>
  void remap(std::map<Key, T>& map,
             const std::map<T, T>& val_map) {
    typedef std::pair<Key, T> pair_type;
    for(pair_type& pair: map) {
      pair.second = safe_get(val_map, pair.second);
    }
  }

  /**
   * Computes the union of two maps
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_union(const std::map<Key, T>& a,
            const std::map<Key, T>& b) {
    // Initialize the output map
    std::map<Key, T> output;
    std::set_union(a.begin(), a.end(),
                   b.begin(), b.end(),
                   std::inserter(output, output.begin()),
                   output.value_comp());
    return output;
  }

  /**
   * Computes the intersection of two maps
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_intersect(const std::map<Key, T>& a,
                const std::map<Key, T>& b) {
    // Initialize the output map
    std::map<Key, T> output;
    // compute the intersection
    std::set_intersection(a.begin(), a.end(),
                          b.begin(), b.end(),
                          std::inserter(output, output.begin()),
                          output.value_comp());
    return output;
  }

  /**
   * Returns the entries of a map whose keys show up in the set keys
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_intersect(const std::map<Key, T>& m,
                const std::set<Key>& keys) {
    std::map<Key, T> output;
    for(const Key& key: keys) {
      typename std::map<Key,T>::const_iterator it = m.find(key);
      if (it != m.end())
        output[key] = it->second;
    }
    return output;
  }

  /**
   * Computes the difference between two maps
   */
  template <typename Key, typename T>
  std::map<Key, T>
  map_difference(const std::map<Key, T>& a,
                 const std::map<Key, T>& b) {
    // Initialize the output map
    std::map<Key, T> output;
    // compute the intersection
    std::set_difference(a.begin(), a.end(),
                        b.begin(), b.end(),
                        std::inserter(output, output.begin()),
                        output.value_comp());
    return output;
  }

  /**
   * Returns the set of keys in a map
   */
  template <typename Key, typename T>
  std::set<Key> keys(const std::map<Key, T>& map) {
    std::set<Key> output;
    typedef std::pair<Key, T> pair_type;
    for(const pair_type& pair: map) {
      output.insert(pair.first);
    }
    return output;
  }

  /**
   * Get the set of keys in a map as a vector
   */
  template <typename Key, typename T>
  std::vector<Key> keys_as_vector(const std::map<Key, T>& map) {
    std::vector<Key> output(map.size());
    typedef std::pair<Key, T> pair_type;
    size_t i = 0;
    for(const pair_type& pair: map) {
      output[i++] = pair.first;
    }
    return output;
  }

  /**
   * Gets the values from a map
   */
  template <typename Key, typename T>
  std::set<T> values(const std::map<Key, T>& map) {
    std::set<T> output;
    typedef std::pair<Key, T> pair_type;
    for(const pair_type& pair: map) {
      output.insert(pair.second);
    }
    return output;
  }

  /**
   * Gets a subset of values from a map
   */
  template <typename Key, typename T>
  std::vector<T> values(const std::map<Key, T>& m,
                        const std::set<Key>& keys) {
    std::vector<T> output;

    for(const Key &i: keys) {
      output.push_back(safe_get(m, i));
    }
    return output;
  }

  /**
   * Gets a subset of values from a map
   */
  template <typename Key, typename T>
  std::vector<T> values(const std::map<Key, T>& m,
                        const std::vector<Key>& keys) {
    std::vector<T> output;
    for(const Key &i: keys) {
      output.push_back(safe_get(m, i));
    }
    return output;
  }

  /** Creates an identity map (a map from elements to themselves)
   */
  template <typename Key>
  std::map<Key, Key> make_identity_map(const std::set<Key>& keys) {
    std::map<Key, Key> m;
    for(const Key& key: keys)
      m[key] = key;
    return m;
  }

  //! Writes a map to the supplied stream.
  template <typename Key, typename T>
  std::ostream& operator<<(std::ostream& out, const std::map<Key, T>& m) {
    out << "{";
    for (typename std::map<Key, T>::const_iterator it = m.begin();
         it != m.end();) {
      out << it->first << "-->" << it->second;
      if (++it != m.end()) out << " ";
    }
    out << "}";
    return out;
  }

  /** Removes white space (space and tabs) from the beginning and end of str,
      returning the resultant string
  */
  inline std::string trim(const std::string& str) {
    std::string::size_type pos1 = str.find_first_not_of(" \t");
    std::string::size_type pos2 = str.find_last_not_of(" \t");
    return str.substr(pos1 == std::string::npos ? 0 : pos1,
                      pos2 == std::string::npos ? str.size()-1 : pos2-pos1+1);
  }

  /**
  * Convenience function for using std streams to convert anything to a string
  */
  template<typename T>
  std::string tostr(const T& t) {
    std::stringstream strm;
    strm << t;
    return strm.str();
  }

  /**
  * Convenience function for using std streams to convert a string to anything
  */
  template<typename T>
  T fromstr(const std::string& str) {
    std::stringstream strm(str);
    T elem;
    strm >> elem;
    ASSERT_FALSE(strm.fail());
    return elem;
  }

  /**
  Returns a string representation of the number,
  padded to 'npad' characters using the pad_value character
  */
  inline std::string pad_number(const size_t number,
                                const size_t npad,
                                const char pad_value = '0') {
    std::stringstream strm;
    strm << std::setw((int)npad) << std::setfill(pad_value)
         << number;
    return strm.str();
  }

  // inline std::string change_suffix(const std::string& fname,
  //                                  const std::string& new_suffix) {
  //   size_t pos = fname.rfind('.');
  //   assert(pos != std::string::npos);
  //   const std::string new_base(fname.substr(0, pos));
  //   return new_base + new_suffix;
  // } // end of change_suffix

  /**
  Using splitchars as delimiters, splits the string into a vector of strings.
  if auto_trim is true, trim() is called on all the extracted strings
  before returning.
  */
  inline std::vector<std::string> strsplit(const std::string& str,
                                           const std::string& splitchars,
                                           const bool auto_trim = false) {
    std::vector<std::string> tokens;
    for(size_t beg = 0, end = 0; end != std::string::npos; beg = end+1) {
      end = str.find_first_of(splitchars, beg);
      if(auto_trim) {
        if(end - beg > 0) {
          std::string tmp = trim(str.substr(beg, end - beg));
          if(!tmp.empty()) tokens.push_back(tmp);
        }
      } else tokens.push_back(str.substr(beg, end - beg));
    }
    return tokens;
    // size_t pos = 0;
    // while(1) {
    //   size_t nextpos = s.find_first_of(splitchars, pos);
    //   if (nextpos != std::string::npos) {
    //     ret.push_back(s.substr(pos, nextpos - pos));
    //     pos = nextpos + 1;
    //   } else {
    //     ret.push_back(s.substr(pos));
    //     break;
    //   }
    // }
    // return ret;
  }

  /**
   * \}
   */
}; // end of namespace turi

#endif


#include "include/core/Ellipsoid.h"

#include "src/Polygon/Polygon.hpp"

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Dense>

namespace g2o
{
    ellipsoid::ellipsoid()
    {
    }

    // xyz roll pitch yaw half_scale
    void ellipsoid::fromMinimalVector(const Vector9d& v){
        Eigen::Quaterniond posequat = zyx_euler_to_quat(v(3),v(4),v(5));
        pose = SE3Quat(posequat, v.head<3>());
        scale = v.tail<3>();

        vec_minimal = v;
    }

    // xyz quaternion, half_scale
    void ellipsoid::fromVector(const Vector10d& v){
        pose.fromVector(v.head<7>());
        scale = v.tail<3>();
        vec_minimal = toMinimalVector();
    }

    const Vector3d& ellipsoid::translation() const {return pose.translation();}
    void ellipsoid::setTranslation(const Vector3d& t_) {pose.setTranslation(t_);}
    void ellipsoid::setRotation(const Quaterniond& r_) {pose.setRotation(r_);}
    void ellipsoid::setRotation(const Matrix3d& R) {pose.setRotation(Quaterniond(R));}
    void ellipsoid::setScale(const Vector3d &scale_) {scale=scale_;}

    // apply update to current ellipsoid. exponential map
    ellipsoid ellipsoid::exp_update(const Vector9d& update)
    {
        ellipsoid res;
        res.pose = this->pose*SE3Quat::exp(update.head<6>());
        res.scale = this->scale + update.tail<3>();

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();
        return res;
    }

    // TOBE DELETED.
    ellipsoid ellipsoid::exp_update_XYZABC(const Vector6d& update)
    {
        ellipsoid res;

        Vector6d pose_vec; pose_vec << 0, 0, 0, update[0], update[1], update[2];
        res.pose = this->pose*SE3Quat::exp(pose_vec);
        res.scale = this->scale + update.tail<3>();

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();
        return res;
    }

    Vector9d ellipsoid::ellipsoid_log_error_9dof(const ellipsoid& newone) const
    {
        Vector9d res;
        SE3Quat pose_diff = newone.pose.inverse()*this->pose;

        res.head<6>() = pose_diff.log();
        res.tail<3>() = this->scale - newone.scale;
        return res;
    }

    // change front face by rotate along current body z axis.
    // another way of representing cuboid. representing same cuboid (IOU always 1)
    ellipsoid ellipsoid::rotate_ellipsoid(double yaw_angle) const // to deal with different front surface of cuboids
    {
        ellipsoid res;
        SE3Quat rot(Eigen::Quaterniond(cos(yaw_angle*0.5),0,0,sin(yaw_angle*0.5)),Vector3d(0,0,0));   // change yaw to rotation.
        res.pose = this->pose*rot;
        res.scale = this->scale;

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();

        const double eps = 1e-6;
        if ( (std::abs(yaw_angle-M_PI/2.0) < eps) || (std::abs(yaw_angle+M_PI/2.0) < eps) || (std::abs(yaw_angle-3*M_PI/2.0) < eps))
            std::swap(res.scale(0),res.scale(1));

        return res;
    }

    Vector9d ellipsoid::min_log_error_9dof(const ellipsoid& newone, bool print_details) const
    {
        bool whether_rotate_ellipsoid=true;  // whether rotate cube to find smallest error
        if (!whether_rotate_ellipsoid)
            return ellipsoid_log_error_9dof(newone);

        // NOTE rotating ellipsoid... since we cannot determine the front face consistenly, different front faces indicate different yaw, scale representation.
        // need to rotate all 360 degrees (global cube might be quite different from local cube)
        // this requires the sequential object insertion. In this case, object yaw practically should not change much. If we observe a jump, we can use code
        // here to adjust the yaw.
        Vector4d rotate_errors_norm; Vector4d rotate_angles(-1,0,1,2); // rotate -90 0 90 180
        Eigen::Matrix<double, 9, 4> rotate_errors;
        for (int i=0;i<rotate_errors_norm.rows();i++)
        {
            ellipsoid rotated_cuboid = newone.rotate_ellipsoid(rotate_angles(i)*M_PI/2.0);  // rotate new cuboids
            Vector9d cuboid_error = this->ellipsoid_log_error_9dof(rotated_cuboid);
            rotate_errors_norm(i) = cuboid_error.norm();
            rotate_errors.col(i) = cuboid_error;
        }
        int min_label;
        rotate_errors_norm.minCoeff(&min_label);
        if (print_details)
            if (min_label!=1)
                std::cout<<"Rotate ellipsoid   "<<min_label<<std::endl;
        return rotate_errors.col(min_label);
    }

    // transform a local cuboid to global cuboid  Twc is camera pose. from camera to world
    ellipsoid ellipsoid::transform_from(const SE3Quat& Twc) const{
        ellipsoid res;
        res.pose = Twc*this->pose;
        res.scale = this->scale;

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();

        return res;
    }

    // transform a global cuboid to local cuboid  Twc is camera pose. from camera to world
    ellipsoid ellipsoid::transform_to(const SE3Quat& Twc) const{
        ellipsoid res;
        res.pose = Twc.inverse()*this->pose;
        res.scale = this->scale;

        res.UpdateValueFrom(*this);
        res.vec_minimal = res.toMinimalVector();

        return res;
    }

    // xyz roll pitch yaw half_scale
    Vector9d ellipsoid::toMinimalVector() const{
        Vector9d v;
        v.head<6>() = pose.toXYZPRYVector();
        v.tail<3>() = scale;
        return v;
    }

    // xyz quaternion, half_scale
    Vector10d ellipsoid::toVector() const{
        Vector10d v;
        v.head<7>() = pose.toVector();
        v.tail<3>() = scale;
        return v;
    }

    Matrix4d ellipsoid::similarityTransform() const
    {
        Matrix4d res = pose.to_homogeneous_matrix();    // 4x4 transform matrix
        Matrix3d scale_mat = scale.asDiagonal();
        res.topLeftCorner<3,3>() = res.topLeftCorner<3,3>()*scale_mat;
        return res;
    }

    void ellipsoid::UpdateValueFrom(const g2o::ellipsoid& e){
        this->miLabel = e.miLabel;
        this->mbColor = e.mbColor;
        this->mvColor = e.mvColor;
        this->miInstanceID = e.miInstanceID;

        this->prob = e.prob;
    }

    ellipsoid::ellipsoid(const g2o::ellipsoid &e) {
        pose = e.pose;
        scale = e.scale;
        vec_minimal = e.vec_minimal;

        UpdateValueFrom(e);
    }

    const ellipsoid& ellipsoid::operator=(const g2o::ellipsoid &e) {
        pose = e.pose;
        scale = e.scale;
        vec_minimal = e.vec_minimal;

        UpdateValueFrom(e);
        return e;
    }

    // ************* Functions As Ellipsoids ***************
    Vector2d ellipsoid::projectCenterIntoImagePoint(const SE3Quat& campose_cw, const Matrix3d& Kalib)
    {
        Matrix3Xd  P = generateProjectionMatrix(campose_cw, Kalib);

        Vector3d center_pos = pose.translation();
        Vector4d center_homo = real_to_homo_coord<double>(center_pos);
        Vector3d u_homo = P * center_homo;
        Vector2d u = homo_to_real_coord_vec<double>(u_homo);

        return u;
    }

    // project the ellipsoid into the image plane, and get an ellipse represented by a Vector5d.
    // Ellipse: x_c, y_c, theta, axis1, axis2
    Vector5d ellipsoid::projectOntoImageEllipse(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix4d Q_star = generateQuadric();
        Matrix3Xd  P = generateProjectionMatrix(campose_cw, Kalib);
        Matrix3d C_star = P * Q_star * P.transpose();
        Matrix3d C = C_star.inverse();
        C = C / C(2,2); // normalize

        SelfAdjointEigenSolver<Matrix3d> es(C);    // ascending sort by default
        VectorXd eigens = es.eigenvalues();

        // If it is an ellipse, the sign of eigen values must be :  1 1 -1
        // Ref book : Multiple View Geometry in Computer Vision
        int num_pos = int(eigens(0)>0) +int(eigens(1)>0) +int(eigens(2)>0);
        int num_neg = int(eigens(0)<0) +int(eigens(1)<0) +int(eigens(2)<0);

        // matrix to equation coefficients: ax^2+bxy+cy^2+dx+ey+f=0
        double a = C(0,0);
        double b = C(0,1)*2;
        double c = C(1,1);
        double d = C(0,2)*2;
        double e = C(2,1)*2;
        double f = C(2,2);

        // get x_c, y_c, theta, axis1, axis2 from coefficients
        double delta = c*c - 4.0*a*b;
        double k = (a*f-e*e/4.0) - pow((2*a*e-c*d),2)/(4*(4*a*b-c*c));
        double theta = 1/2.0*atan2(b,(a-c));
        double x_c = (b*e-2*c*d)/(4*a*c-b*b);
        double y_c = (b*d-2*a*e)/(4*a*c-b*b);
        double a_2 =  2*(a* x_c*x_c+ c * y_c*y_c+ b *x_c*y_c -1) /(a + c + sqrt((a-c)*(a-c)+b*b));
        double b_2 =  2*(a*x_c*x_c+c*y_c*y_c+b*x_c*y_c -1) /( a + c - sqrt((a-c)*(a-c)+b*b));

        double axis1= sqrt(a_2);
        double axis2= sqrt(b_2);

        Vector5d output;
        output << x_c, y_c, theta, axis1, axis2;

        return output;
    }

    // Get the bounding box from ellipse in image plane
    Vector4d ellipsoid::getBoundingBoxFromEllipse(Vector5d &ellipse) const
    {
        double a = ellipse[3];
        double b = ellipse[4];
        double theta = ellipse[2];
        double x = ellipse[0];
        double y = ellipse[1];

        double cos_theta_2 = cos(theta)*cos(theta);
        double sin_theta_2 = 1- cos_theta_2;

        double x_limit = sqrt(a*a*cos_theta_2+b*b*sin_theta_2);
        double y_limit = sqrt(a*a*sin_theta_2+b*b*cos_theta_2);

        Vector4d output;
        output[0] = x-x_limit; // left up
        output[1] = y-y_limit;
        output[2] = x+x_limit; // right down
        output[3] = y+y_limit;

        return output;
    }

    // Get projection matrix P = K [ R | t ]
    Matrix3Xd ellipsoid::generateProjectionMatrix(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix3Xd identity_lefttop;
        identity_lefttop.resize(3, 4);
        identity_lefttop.col(3)=Vector3d(0,0,0);
        identity_lefttop.topLeftCorner<3,3>() = Matrix3d::Identity(3,3);

        Matrix3Xd proj_mat = Kalib * identity_lefttop;
        proj_mat = proj_mat * campose_cw.to_homogeneous_matrix();

        return proj_mat;
    }

    // Get Q^*
    Matrix4d ellipsoid::generateQuadric() const
    {
        Vector4d axisVec;
        axisVec << 1/(scale[0]*scale[0]), 1/(scale[1]*scale[1]), 1/(scale[2]*scale[2]), -1;
        Matrix4d Q_c = axisVec.asDiagonal();
        Matrix4d Q_c_star = Q_c.inverse();
        Matrix4d Q_pose_matrix = pose.to_homogeneous_matrix();   // Twm  model in world,  world to model
        Matrix4d Q_c_star_trans = Q_pose_matrix * Q_c_star * Q_pose_matrix.transpose();

        return Q_c_star_trans;
    }

    // Get the projected bounding box in the image plane of the ellipsoid using a camera pose and a calibration matrix.
    Vector4d ellipsoid::getBoundingBoxFromProjection(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Vector5d ellipse = projectOntoImageEllipse(campose_cw, Kalib);
        return getBoundingBoxFromEllipse(ellipse);
    }

    Vector3d ellipsoid::getColor(){
        return mvColor.head(3);
    }

    Vector4d ellipsoid::getColorWithAlpha(){
        return mvColor;
    }

    void ellipsoid::setColor(const Vector3d &color_, double alpha){
        mbColor = true;
        mvColor.head<3>() = color_;
        mvColor[3] = alpha;

    }

    bool ellipsoid::isColorSet(){
        return mbColor;
    }

    bool ellipsoid::CheckObservability(const SE3Quat& campose_cw)
    {
        Vector3d ellipsoid_center = toMinimalVector().head(3);    // Pwo
        Vector4d center_homo = real_to_homo_coord_vec<double>(ellipsoid_center);

        Eigen::Matrix4d projMat = campose_cw.to_homogeneous_matrix(); // Tcw
        Vector4d center_inCameraAxis_homo = projMat * center_homo;   // Pco =  Tcw * Pwo
        Vector3d center_inCameraAxis = homo_to_real_coord_vec<double>(center_inCameraAxis_homo);

        if( center_inCameraAxis_homo(2) < 0)    // if the center is behind the camera ; z<0
        {
            return false;
        }
        else
            return true;
    }

    // calculate the IoU Error between two axis-aligned ellipsoid
    double ellipsoid::calculateMIoU(const g2o::ellipsoid& e) const
    {
        return calculateIntersectionError(*this, e);
    }

    double ellipsoid::calculateIntersectionOnZ(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const
    {
        g2o::SE3Quat pose_diff = e1.pose.inverse() * e2.pose;
        double z1 = 0; double z2 = pose_diff.translation()[2];

        bool flag_oneBigger = false;
        if( z1 > z2 )
            flag_oneBigger = true;

        double length;
        if( flag_oneBigger )
        {
            length = (z2 + e2.scale[2]) - (z1 - e1.scale[2]);
        }
        else
            length = (z1 + e1.scale[2]) - (z2 - e2.scale[2]);

        if( length < 0 )
            length = 0;     // if they are not intersected

        return length;
    }

    double ellipsoid::calculateArea(const g2o::ellipsoid& e) const
    {
        return e.scale[0]*e.scale[1]*e.scale[2]*8;
    }

    void OutputPolygon(EllipsoidSLAM::Polygon& polygon, double resolution)
    {
        int num = polygon.n;
        for( int i=0;i<num;i++)
            std::cout << i << ":" << polygon[i].x << ", " << polygon[i].y << std::endl;
        std::cout << std::endl;
    }

    // Calculate the intersection area after projected the external cubes of two axis-aligned ellipsoids into XY-Plane.
    double ellipsoid::calculateIntersectionOnXY(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const
    {
        // First, get the axis-aligned pose error
        g2o::SE3Quat pose_diff = e1.pose.inverse() * e2.pose;
        double x_center1 = 0; double y_center1 = 0;

        double x_center2 = pose_diff.translation()[0];
        double y_center2 = pose_diff.translation()[1];

        double roll,pitch,yaw;
        quat_to_euler_zyx(pose_diff.rotation(),roll,pitch,yaw);

        double a1 = std::abs(e1.scale[0]);
        double b1 = std::abs(e1.scale[1]);

        double a2 = std::abs(e2.scale[0]);
        double b2 = std::abs(e2.scale[1]);

        // Use polygon to calculate the intersection
        EllipsoidSLAM::Polygon polygon1, polygon2;
        double resolution = 0.001;  // m / resolution = pixel
        polygon1.add(cv::Point(a1/resolution, b1/resolution));    // cvPoint only accepts integer, so use resolution to map meter to pixel ( 0.01 resolution means: 1pixel = 0.01m )
        polygon1.add(cv::Point(-a1/resolution, b1/resolution));
        polygon1.add(cv::Point(-a1/resolution, -b1/resolution));
        polygon1.add(cv::Point(a1/resolution, -b1/resolution));

        double c_length = sqrt(a2*a2+b2*b2);

        double init_theta = CV_PI/2.0 - atan2(a2,b2);
        Vector4d angle_plus_vec;
        angle_plus_vec << 0, atan2(a2,b2)*2, CV_PI, CV_PI+atan2(a2,b2)*2;
        for( int n=0;n<4;n++){
            double angle_plus = angle_plus_vec[n];  // rotate 90deg for four times
            double point_x = c_length * cos( init_theta - yaw + angle_plus ) + x_center2;
            double point_y = c_length * sin( init_theta - yaw + angle_plus ) + y_center2;
            polygon2.add(cv::Point(point_x/resolution, point_y/resolution));
        }

        // calculate the intersection
        EllipsoidSLAM::Polygon interPolygon;
        EllipsoidSLAM::intersectPolygon(polygon1, polygon2, interPolygon);

        // eliminate resolution.
        double inter_area = interPolygon.area();
        double inter_area_in_m = inter_area * resolution * resolution;

        return inter_area_in_m;
    }

    double ellipsoid::calculateIntersectionError(const g2o::ellipsoid& e1, const g2o::ellipsoid& e2) const
    {
        //          AXB
        // IoU = ----------
        //          AUB
        //   AXB  =  intersection
        //   AUB  =  A+B-intersection

        // Error of IoU : 1 - IoU
        double areaA = std::abs(calculateArea(e1));
        std::cout << "areaA : " << areaA << std::endl;

        double areaB = std::abs(calculateArea(e2));
        std::cout << "areaB : " << areaB << std::endl;

        double proj_inter = calculateIntersectionOnXY(e1,e2);
        double z_inter = calculateIntersectionOnZ(e1,e2);
        std::cout << "projInter : " << proj_inter << std::endl;
        std::cout << "z_inter : " << z_inter << std::endl;

        double areaIntersection = proj_inter * z_inter;
        std::cout << "areaIntersection : " << areaIntersection << std::endl;

        double MIoU = 1 - ((areaIntersection) / (areaA + areaB - areaIntersection));
        std::cout << "MIoU : " << MIoU << std::endl;
        std::cout << "e1 : " << e1.toMinimalVector().transpose() << std::endl;
        std::cout << "e2 : " << e2.toMinimalVector().transpose() << std::endl;

        return MIoU;
    }

    // ***************** Functions as Cubes ******************

    // calculate the external cube of the ellipsoid
    // 8 corners 3*8 matrix, each row is x y z
    Matrix3Xd ellipsoid::compute3D_BoxCorner() const
    {
        Matrix3Xd corners_body;corners_body.resize(3,8);
        corners_body<< 1, 1, -1, -1, 1, 1, -1, -1,
                1, -1, -1, 1, 1, -1, -1, 1,
                -1, -1, -1, -1, 1, 1, 1, 1;
        Matrix3Xd corners_world = homo_to_real_coord<double>(similarityTransform()*real_to_homo_coord<double>(corners_body));
        return corners_world;
    }

    Matrix2Xd ellipsoid::projectOntoImageBoxCorner(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix3Xd corners_3d_world = compute3D_BoxCorner();
        Matrix2Xd corner_2d = homo_to_real_coord<double>(Kalib*homo_to_real_coord<double>(campose_cw.to_homogeneous_matrix()*real_to_homo_coord<double>(corners_3d_world)));

        return corner_2d;
    }

    // get rectangles after projection  [topleft, bottomright]
    Vector4d ellipsoid::projectOntoImageRect(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Matrix2Xd corner_2d = projectOntoImageBoxCorner(campose_cw, Kalib);
        Vector2d bottomright = corner_2d.rowwise().maxCoeff(); // x y
        Vector2d topleft = corner_2d.rowwise().minCoeff();
        return Vector4d(topleft(0),topleft(1),bottomright(0),bottomright(1));
    }

    // get rectangles after projection  [center, width, height]
    Vector4d ellipsoid::projectOntoImageBbox(const SE3Quat& campose_cw, const Matrix3d& Kalib) const
    {
        Vector4d rect_project = projectOntoImageRect(campose_cw, Kalib);  // top_left, bottom_right  x1 y1 x2 y2
        Vector2d rect_center = (rect_project.tail<2>()+rect_project.head<2>())/2;
        Vector2d widthheight = rect_project.tail<2>()-rect_project.head<2>();
        return Vector4d(rect_center(0),rect_center(1),widthheight(0),widthheight(1));
    }

} // g2o


//==================================================================================================
/*!
  @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_EXPOCVT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_EXPOCVT_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing expocvt capabilities

    Computes the integer conversion of of the exponent bits
    for a given exponent in a floating type.

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

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

    is equivalent to:

    @code
    as_integer_t<T> r = toint(x+Maxexponent<T>();
    @endcode

  **/
  as_integer_T<Value> expocvt(Value const & v0);
} }
#endif

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

#endif


// The MIT License (MIT)
//
// Copyright (c) 2015 Jonathan McCluskey and William Harding
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//

#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <gmpxx.h>
#include <iostream>

#include "Utilities.h"

BOOST_AUTO_TEST_CASE(utilties_test_1)
{
    std::string str = "Bob";

    mpz_class num = Utilities::StringToNumber(str);

    // there is an additional null terminator (that is why 0x00 on end)
    mpz_class expected_num("426F62", 16);
    BOOST_CHECK(expected_num == num);
}

BOOST_AUTO_TEST_CASE(utilties_test_2)
{
    std::string expected_str = "Bob";
    mpz_class num("426F62", 16);

    std::string str = Utilities::NumberToString(num);
    BOOST_CHECK_EQUAL(str, expected_str);
}

BOOST_AUTO_TEST_CASE(utilties_test_3)
{
    mpz_class ans = Utilities::FastExp(11, 13, 53);
    BOOST_CHECK(ans == 52);
}

BOOST_AUTO_TEST_CASE(utilties_test_4)
{
    mpz_class gcd;
    mpz_class x;
    mpz_class y;

    std::tie(gcd, x, y) = Utilities::ExtendedGcd(65, 40);
    BOOST_CHECK(gcd == 5);
    BOOST_CHECK(x == -3);
    BOOST_CHECK(y == 5);

    std::tie(gcd, x, y) = Utilities::ExtendedGcd(1239, 735);
    BOOST_CHECK(gcd == 21);
    BOOST_CHECK(x == -16);
    BOOST_CHECK(y == 27);
}


#pragma once

#include <boost/multi_array.hpp>
#include <cmath>
#include <iostream>
#include <vector>

// ------------------------------------------------------------
#include "aux/hash_specializations.hpp"
#include "laguerren_impl.hpp"

namespace boltzmann {

template <typename NUMERIC>
class LaguerreN
{
 public:
  typedef NUMERIC numeric_t;

 public:
  LaguerreN(int K)
      : Y_(K + 1)
      , K_(K)
      , is_initialized_(false)
  {
  }

  void compute(const std::vector<numeric_t> &x);
  void compute(const numeric_t *x, unsigned int n);

  unsigned int get_npoints() const { return Y_[0].shape()[1]; }

 public:
  typedef boost::multi_array<numeric_t, 2> array_t;

 public:
  const NUMERIC *get(unsigned int k, unsigned int alpha) const;
  void info() const;

 private:
  std::vector<array_t> Y_;
  unsigned int K_;
  bool is_initialized_;
};

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreN<NUMERIC>::compute(const std::vector<numeric_t> &x)
{
  compute(x.data(), x.size());
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreN<NUMERIC>::compute(const numeric_t *x, unsigned int n)
{
  // L_n-1
  std::vector<numeric_t> Lnm1(n);
  // L_n-2
  std::vector<numeric_t> Lnm2(n);

  for (unsigned int alpha = 0; alpha <= K_; ++alpha) {
    Y_[alpha].resize(boost::extents[K_ / 2 + 1][n]);
    // init
    for (size_t xi = 0; xi < n; ++xi) {
      Y_[alpha][0][xi] = boost::math::laguerren(0, alpha, x[xi]);
      Y_[alpha][1][xi] = boost::math::laguerren(1, alpha, x[xi]);
    }

    for (unsigned int k = 2; k <= K_ / 2; ++k) {
      for (size_t xi = 0; xi < n; ++xi) {
        Y_[alpha][k][xi] = boost::math::laguerren_next(
            k - 1, alpha, x[xi], Y_[alpha][k - 1][xi], Y_[alpha][k - 2][xi]);
      }
    }
  }
  is_initialized_ = true;
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
const NUMERIC *
LaguerreN<NUMERIC>::get(unsigned int k, unsigned int alpha) const
{
  assert(is_initialized_);
  assert(alpha < Y_.size());
  assert(k < Y_[alpha].shape()[0]);
  return Y_[alpha][k].origin();
}

// ----------------------------------------------------------------------
template <typename NUMERIC>
void
LaguerreN<NUMERIC>::info() const
{
  unsigned long long int nentries = 0;
  for (unsigned int alpha = 0; alpha < Y_.size(); ++alpha) {
    nentries += Y_[alpha].shape()[0] * Y_[alpha].shape()[1];
  }

  std::cout << " LaguerreN uses " << nentries * sizeof(NUMERIC) / 1e6 << " MB" << std::endl;
}

}  // end namespace boltzmann


//
// Created by lejonmcgowan on 5/22/16.
//

#include <Eigen/Dense>
#include <include/gtest/gtest.h>
#include <partitioning/Octree.h>
#include <geometry/Box.h>
using namespace std;

std::ostream& operator<<(std::ostream& ostream, const Eigen::Vector3f vec3)
{
    return ostream << "(" << vec3[0] << "," << vec3[1] << "," << vec3[2] << ")";
}

TEST(OctreeTest, splitTest)
{
    Octree octree(0, Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(1, 1, 1));
    octree.split();
    EXPECT_EQ(octree.nodes[0]->minBounds, Eigen::Vector3f(0, 0, 0));
    EXPECT_EQ(octree.nodes[0]->maxBounds, Eigen::Vector3f(0.5f, 0.5f, 0.5f));

    EXPECT_EQ(octree.nodes[1]->minBounds, Eigen::Vector3f(0, 0, 0.5f));
    EXPECT_EQ(octree.nodes[1]->maxBounds, Eigen::Vector3f(0.5f, 0.5f, 1.0f));

    EXPECT_EQ(octree.nodes[2]->minBounds, Eigen::Vector3f(0, 0.5f, 0));
    EXPECT_EQ(octree.nodes[2]->maxBounds, Eigen::Vector3f(0.5f, 1.0f, 0.5f));

    EXPECT_EQ(octree.nodes[3]->minBounds, Eigen::Vector3f(0, 0.5f, 0.5f));
    EXPECT_EQ(octree.nodes[3]->maxBounds, Eigen::Vector3f(0.5f, 1.0f, 1.0f));

    EXPECT_EQ(octree.nodes[4]->minBounds, Eigen::Vector3f(0.5f, 0, 0));
    EXPECT_EQ(octree.nodes[4]->maxBounds, Eigen::Vector3f(1.0f, 0.5f, 0.5f));

    EXPECT_EQ(octree.nodes[5]->minBounds, Eigen::Vector3f(0.5f, 0, 0.5f));
    EXPECT_EQ(octree.nodes[5]->maxBounds, Eigen::Vector3f(1.0f, 0.5f, 1.0f));

    EXPECT_EQ(octree.nodes[6]->minBounds, Eigen::Vector3f(0.5f, 0.5f, 0));
    EXPECT_EQ(octree.nodes[6]->maxBounds, Eigen::Vector3f(1.0f, 1.0f, 0.5f));

    EXPECT_EQ(octree.nodes[7]->minBounds, Eigen::Vector3f(0.5f, 0.5f, 0.5f));
    EXPECT_EQ(octree.nodes[7]->maxBounds, Eigen::Vector3f(1.0f, 1.0f, 1.0f));
}

TEST(OctreeTest, selfSplitTest)
{
    Octree::MAX_SHAPES = 4;
    Octree::MAX_LEVEL = 5;

    Octree octree(0, Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(1, 1, 1));
    //because I'm lazy and don't want to calulate the bounds again myself
    Octree octree2(0, Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(1, 1, 1));
    octree2.split();
    std::vector<std::shared_ptr<Box>> boxes;
    for (int i = 0; i < 8; i++)
    {
        Eigen::Vector3f origMin = octree2.nodes[i]->minBounds;
        Eigen::Vector3f origMax = octree2.nodes[i]->maxBounds;

        Eigen::Vector3f min = origMin + 0.25f * (origMax - origMin);
        Eigen::Vector3f max = origMax - 0.25f * (origMax - origMin);
        //  cout << min << " " << max << endl;
        auto box = std::make_shared<Box>(min, max);
        boxes.push_back(box);
        octree.addShape(box);
    }

    EXPECT_NE(octree.nodes[0], nullptr);

    EXPECT_EQ(octree.objects.size(), 0);

    for (int i = 0; i < 8; i++)
    {
        EXPECT_TRUE(octree.inTree(boxes[i]));
        EXPECT_EQ(1, octree.nodes[i]->objects.size());
    }
}

TEST(OctreeTest, inTreeTest)
{
    Octree::MAX_SHAPES = 10;
    Octree::MAX_LEVEL = 5;

    Octree octree(0, Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(1, 1, 1));
    octree.split();
    std::vector<std::shared_ptr<Box>> boxes;
    for (int i = 0; i < 8; i++)
    {
        Eigen::Vector3f origMin = octree.nodes[i]->minBounds;
        Eigen::Vector3f origMax = octree.nodes[i]->maxBounds;
        Eigen::Vector3f length = origMax - origMin;

        Eigen::Vector3f min = origMin + 0.25f * (origMax - origMin);
        Eigen::Vector3f max = origMax - 0.25f * (origMax - origMin);
        //  cout << min << " " << max << endl;
        auto box = std::make_shared<Box>(min, max);
        boxes.push_back(box);
        octree.addShape(box);
    }

    EXPECT_EQ(octree.objects.size(), 0);

    for (int i = 0; i < 8; i++)
    {
        EXPECT_TRUE(octree.inTree(boxes[i]));
        EXPECT_EQ(1, octree.nodes[i]->objects.size());
    }

    //add a box which misses the tree entirely
    auto box = std::make_shared<Box>(Eigen::Vector3f(1, 1, 1), Eigen::Vector3f(1.5f, 1.5f, 1.5f));
    octree.addShape(box);

    EXPECT_EQ(octree.objects.size(), 0);

    EXPECT_FALSE(octree.inTree(box));

    //add a box which patrially is in root tree
    box = std::make_shared<Box>(Eigen::Vector3f(0.75, 0.75, 0.75), Eigen::Vector3f(1.5f, 1.5f, 1.5f));
    octree.addShape(box);

    EXPECT_EQ(octree.objects.size(), 1);

    EXPECT_TRUE(octree.inTree(box));

    for (int i = 0; i < 8; i++)
        EXPECT_EQ(1, octree.nodes[i]->objects.size());

}

TEST(OctreeTest, indexTest)
{
    Octree::MAX_SHAPES = 10;
    Octree::MAX_LEVEL = 5;

    Octree octree(0, Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(1, 1, 1));
    octree.split();
    std::vector<std::shared_ptr<Box>> boxes;
    for (int i = 0; i < 8; i++)
    {
        Eigen::Vector3f origMin = octree.nodes[i]->minBounds;
        Eigen::Vector3f origMax = octree.nodes[i]->maxBounds;

        Eigen::Vector3f min;
        Eigen::Vector3f max;
        //cout << origMin << " " << origMax << " -> ";
        min = origMin + 0.25f * (origMax - origMin);
        max = origMax - 0.25f * (origMax - origMin);
        //  cout << min << " " << max << endl;
        auto box = std::make_shared<Box>(min, max);
        boxes.push_back(box);
        octree.addShape(box);
    }

    EXPECT_EQ(octree.objects.size(), 0);

    for (int i = 0; i < 8; i++)
    {
        EXPECT_EQ(i, octree.getIndices(boxes[i]));
    }
}

TEST(OctreeTest, simpleRay)
{
    Octree octree(0, Eigen::Vector3f(0, 0, 0), Eigen::Vector3f(1, 1, 1));
    octree.split();
    octree.nodes[0]->split();
    Ray ray(Eigen::Vector3f(0.25, 0.25, 0.25), Eigen::Vector3f(0, 1, 0));
    octree.getShapes(ray);
}

std::ostream& ::MathHelper::operator<<(std::ostream& ostream, const Eigen::Vector3f vec3)
{
    return ostream << "(" << vec3[0] << "," << vec3[1] << "," << vec3[2] << ")";
}


#include <boost/test/auto_unit_test.hpp>

#include <bq.h>

BOOST_AUTO_TEST_CASE(v2f_construction) {
	bq::v2f v2(4.f,2.f);

	BOOST_CHECK(v2.x == 4.f);
	BOOST_CHECK(v2.y == 2.f);
}
BOOST_AUTO_TEST_CASE(v2f_add) {
	bq::v2f v1(4.f,4.f);
	bq::v2f v2(4.f, 5.f);
	v1 += v2;
	BOOST_CHECK(v1.x == 8.f);
	BOOST_CHECK(v1.y == 9.f);
}
BOOST_AUTO_TEST_CASE(v2i_construction) {
	bq::v2i v2(4, 2);

	BOOST_CHECK(v2.x == 4);
	BOOST_CHECK(v2.y == 2);
}
BOOST_AUTO_TEST_CASE(v2i_add) {
	bq::v2i v1(4, 4);
	bq::v2i v2(4, 5);
	v1 += v2;
	BOOST_CHECK(v1.x == 8);
	BOOST_CHECK(v1.y == 9);
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
Copyright (C) 2008 Yee Man Chan
Copyright (C) 2015 CompatibL

This file is part of QuantLib, a free-software/open-source library
for financial quantitative analysts and developers - http://quantlib.org/

QuantLib is free software: you can redistribute it and/or modify it
under the terms of the QuantLib license.  You should have received a
copy of the license along with this program; if not, please email
<quantlib-dev@lists.sf.net>. The license is also available online at
<http://quantlib.org/license.shtml>.

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 license for more details.
*/

// based on gjrgarchmodel.cpp file from test-suite

#ifndef cl_adjoint_gjrgarch_model_impl_hpp
#define cl_adjoint_gjrgarch_model_impl_hpp
#pragma once

#include "utilities.hpp"
#include "adjointtestutilities.hpp"
#include "adjointgjrgarchmodeltest.hpp"
#include "adjointtestbase.hpp"

#include <boost/make_shared.hpp>
#include <ql/quantlib.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

#define OUTPUT_FOLDER_NAME "AdjointGjrgarchmodel"

namespace
{
    enum
    {
#if defined CL_GRAPH_GEN
        // Number of points for dependency plots.
        pointNo = 15,
        // Number of points for performance plot.
        iterNo = 15,
        // Step for portfolio size for performance testing .
        step = 1,
#else
        // Number of points for dependency plots.
        pointNo = 1,
        // Number of points for performance plot.
        iterNo = 1,
        // Step for portfolio size for performance testing .
        step = 1,
#endif
        // Defines performance accuracy. Its value is a minimum number
        // of calling of O(1) complexity methods per one performance test.
        iterNumFactor = 0,
    };

    struct Variation
    {
        static std::deque<std::string > get_columns()
        {
            static std::deque<std::string > columns =
            {
                "Volatility", ""
            };

            return columns;
        }

        template <typename stream_type>
        friend inline stream_type&
            operator << (stream_type& stm, Variation& v)
        {
                stm << v.volatility_
                    << ";" << v.calibrationError_
                    << std::endl;
                return stm;
            }

        Real volatility_;
        Real calibrationError_;
    };

    struct GJRGARCHmodelData
    {

        GJRGARCHmodelData()
            : settlement_(Date(5, July, 2002))
            , dayCounter_(Actual365Fixed())
            , calendar_(TARGET())
            , strike_(3400)
        {
            Settings::instance().evaluationDate() = settlement_;

            t_ = { 7, 14, 21, 28, 35, 42, 49, 56, 63, 70, 77, 84, 91, 98, 105 };

            r_ = { 0.0341
                , 0.0334
                , 0.0323
                , 0.0378
                , 0.0362
                , 0.0305
                , 0.0381
                , 0.0361
                , 0.0395
                , 0.0327
                , 0.0391
                , 0.0302
                , 0.0392
                , 0.0321
                , 0.0318 };

            v_ = { 0.3145
                , 0.3153
                , 0.3382
                , 0.3464
                , 0.3491
                , 0.3827
                , 0.4358
                , 0.4436
                , 0.5467
                , 0.55
                , 0.5604
                , 0.5716
                , 0.6724
                , 0.6895
                , 0.6942};
        }

        // Calculate model calibration error with given volatilities.
        Real calculateGJRGARCHmodelCalibrationError(std::vector<Real> vol)
        {
            std::vector<Date> dates;
            std::vector<Rate> rates;
            dates.push_back(settlement_);
            rates.push_back(0.0357);
            for (Size i = 0; i < vol.size(); i++)
            {
                dates.push_back(settlement_ + t_[i]);
                rates.push_back(r_[i]);
            }

            // Create handle for YieldTermStructure.
            Handle<YieldTermStructure> riskFreeTS(boost::shared_ptr<YieldTermStructure>(new ZeroCurve(dates, rates, dayCounter_)));
            Handle<YieldTermStructure> dividendTS(boost::shared_ptr<YieldTermStructure>(new FlatForward(settlement_, Handle<Quote>(boost::shared_ptr<Quote>(new SimpleQuote(0.0))), dayCounter_)));

            Handle<Quote> s0(boost::shared_ptr<Quote>(new SimpleQuote(4468.17)));

            // Calculate coef.
            const Real m1 = beta_ + (alpha_ + gamma_*CumulativeNormalDistribution()(lambda_))
                *(1.0 + lambda_*lambda_) + gamma_*lambda_*std::exp(-lambda_*lambda_ / 2.0)
                / std::sqrt(2.0*M_PI);
            const Real v0 = omega_ / (1.0 - m1);

            // Create gjrgarch process for model.
            boost::shared_ptr<GJRGARCHProcess> process(new GJRGARCHProcess(
                riskFreeTS, dividendTS, s0, v0,
                omega_, alpha_, beta_, gamma_, lambda_, daysPerYear_));

            // Create gjrgarch model using gjrgarch process.
            boost::shared_ptr<GJRGARCHModel> model(new GJRGARCHModel(process));

            // Create engine.
            boost::shared_ptr<PricingEngine> engine(new AnalyticGJRGARCHEngine(boost::shared_ptr<GJRGARCHModel>(model)));

            std::vector<boost::shared_ptr<CalibrationHelper>> options;

            for (Size i = 0; i < vol.size(); i++)
            {
                Handle<Quote> vol(boost::shared_ptr<Quote>(new SimpleQuote(vol[i])));
                Period maturity((int)(t_[i] / 7.), Weeks);

                // Create calibration helper.
                boost::shared_ptr<CalibrationHelper> helper(
                    new HestonModelHelper(maturity, calendar_,
                    s0->value(), strike_, vol,
                    riskFreeTS, dividendTS,
                    CalibrationHelper::ImpliedVolError));

                // Set engine in helper.
                helper->setPricingEngine(engine);
                options.push_back(helper);
            }

            Real error = 0;
            // Calculate calibration error before calibration.
            for (Size i = 0; i < vol.size(); ++i)
            {
                const Real diff = options[i]->calibrationError()*100.0;
                error += diff*diff;
            }

            // Set optimization method.
            Simplex om(0.01);

            // Calibrate model.
            model->calibrate(options, om, EndCriteria(20, 10, 1.0e-2, 1.0e-2, 1.0e-2));

            error = 0;
            // Calculate calibration error after calibration.
            for (Size i = 0; i < options.size(); ++i)
            {
                const Real diff = options[i]->calibrationError()*100.0;
                error += diff*diff;
            }

            return error;
        }

        void calculateFinDiff(std::vector<Real>& vol, double h, std::vector<Real>& sf_Finite)
        {
            Size size = vol.size();

            Real totalCalibrationErrorValue = calculateGJRGARCHmodelCalibrationError(vol);

            for (Size i = 0; i < size; i++)
            {
                vol[i] += h;
                sf_Finite[i] = (calculateGJRGARCHmodelCalibrationError(vol) - totalCalibrationErrorValue) / h;
                vol[i] -= h;
            }
        }

        // Cleanup.
        SavedSettings backup_;

        // Global data.
        Date settlement_;

        DayCounter dayCounter_;
        Calendar calendar_;

        std::vector<Integer> t_;
        std::vector<Real> r_;
        std::vector<Real> v_;

        Real strike_;

        const Real omega_ = 2.0e-6;
        const Real alpha_ = 0.024;
        const Real beta_ = 0.93;
        const Real gamma_ = 0.059;
        const Real lambda_ = 0.1;
        const Real daysPerYear_ = 365.0;
    };

    struct TestData
        : public GJRGARCHmodelData
    {
        struct Test
        : public cl::AdjointTest<Test>
        {
            Test(Size size, TestData* data)
            : size_(size)
            , data_(data)
            , volatility_(size)
            , totalCalibrationErrorValue_()
            {
                setLogger(&data_->outPerform_);

                for (Size i = 0; i < size_; i++)
                {
                    volatility_[i] = data->v_[i];
                }

            }

            Size indepVarNumber() { return size_; }

            Size depVarNumber() { return 1; }

            Size minPerfIteration() { return iterNumFactor; }

            void recordTape()
            {
                cl::Independent(volatility_);
                calculateTotalError();
                f_ = std::make_unique<cl::tape_function<double>>(volatility_, totalCalibrationErrorValue_);
            }

            // Calculates total calibration error.
            void calculateTotalError()
            {
                totalCalibrationErrorValue_.push_back(data_->calculateGJRGARCHmodelCalibrationError(volatility_));
            }

            // Calculates derivatives using finite difference method.
            void calcAnalytical()
            {
                double h = 1.0e-10;  // shift for finite diff. method
                analyticalResults_.resize(size_);
                data_->calculateFinDiff(volatility_, h, analyticalResults_);
            }

            double relativeTol() const { return 1e-2; }

            double absTol() const { return 1e-10; }

            Size size_;
            TestData* data_;
            std::vector<cl::tape_double> volatility_;
            std::vector<cl::tape_double> totalCalibrationErrorValue_;
        };

        TestData()
            : GJRGARCHmodelData()

            , outPerform_(OUTPUT_FOLDER_NAME "//"
              , { { "filename", "AdjointPerformance" }
                , { "not_clear", "Not" }
                , { "line_box_width", "-5" }
                , { "title", "Calibration error differentiation performance with respect to volatility" }
                , { "ylabel", "Time (s)" }
                , { "xlabel", "Number of volatilities" }
                , { "smooth", "default" }
                , { "cleanlog", "true" } })

            , outAdjoint_(OUTPUT_FOLDER_NAME "//"
              , { { "filename", "Adjoint" }
                , { "not_clear", "Not" }
                , { "smooth", "2" }
                , { "title", "Calibration error adjoint differentiation performance with respect to volatility" }
                , { "cleanlog", "false" }
                , { "ylabel", "Time (s)" }
                , { "xlabel", "Number of volatilities" } })

            , outSize_(OUTPUT_FOLDER_NAME "//"
              , { { "filename", "TapeSize" }
                , { "not_clear", "Not" }
                , { "title", "Tape size dependence on number of volatilities" }
                , { "cleanlog", "false" }
                , { "smooth", "default" }
                , { "ylabel", "Size (MB)" }
                , { "xlabel", "Number of volatilities" } })

            , out_(OUTPUT_FOLDER_NAME "//output"
              , { { "filename", "CalibrErronVol" }
                , { "ylabel", "Calibration Error" }
                , { "not_clear", "Not" }
                , { "title", "Calibration error on volatility" }
                , { "cleanlog", "false" }
                , { "smooth", "10" }
                , { "xlabel", "Volatility" } })
        {
        }

        bool makeOutput()
        {
            bool ok = true;
            if (pointNo > 0)
            {
                ok &= recordDependencePlot();
            }
            ok &= cl::recordPerformance(*this, iterNo, step);
            return ok;
        }

        std::shared_ptr<Test> getTest(size_t size)
        {
            return std::make_shared<Test>(size, this);
        }

        // Makes plots for strike sensitivity dependence.
        bool recordDependencePlot()
        {
            std::vector<Variation> outData(pointNo);
            auto test = getTest(pointNo);
            for (Size i = 0; i < pointNo; i++)
            {
                outData[i] = { test->volatility_[i], test->data_->calculateGJRGARCHmodelCalibrationError(std::vector<Real>(1, test->volatility_[i])) };
            }
            out_ << outData;
            return true;
        }

        cl::tape_empty_test_output outPerform_;
        cl::tape_empty_test_output outAdjoint_;
        cl::tape_empty_test_output outSize_;
        cl::tape_empty_test_output out_;
    };

    typedef TestData::Test GJRGARCHmodelTest;
}

#endif


/*
 * ex5.cpp
 *
 * 	\brief     Fifth exercixe
 *  \details   This class reads graph-data. It computes the longest shortest path for starting point 1.
 *  \author    Julia Baumbach
 *  \date      03.06.2017
 */

#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/timer/timer.hpp>
#include "dijkstra.h"

using namespace std;

/**
 * \param SOURCE_FILE_PATH default file path if none is hand over while starting the programm
 */
const char* SOURCE_FILE_PATH = "testgraph.gph";

/**
 *\typedef defines a graph by undirected adjacency-list with weighted edges
 */
using graph =  boost::adjacency_list<boost::listS, boost::vecS,
	boost::undirectedS, boost::no_property,
	boost::property<boost::edge_weight_t, int>>;
/*
 * \typedef short version for vertex descriptor from boost::graph_traits
 */
using vertex_descriptor = boost::graph_traits < graph >::vertex_descriptor;

/*
 * Main function. Reads some graph data from a given file and computes the longest
 * shortest path to vertex with number 1.
 */
int main(int argc, char* argv[]){
	//Initialize timer for time measurement
	boost::timer::cpu_timer timer;

	if(argc != 3){
		cerr << "Invalid method call. Please call with ./ex5 -m1/-m2 FILENAME" << endl;
		return EXIT_FAILURE;
	}

	string method = argv[1];
	bool boostMethod = false;
	if (method.compare("-m1") == 0){
		cout << "Using dijkstra from boost-library" << endl;
		boostMethod = true;
	} else if (method.compare("-m2") == 0){
		cout << "Using my dijkstra" << endl;
	} else {
		cerr << "Invalid argument. Please type \"-m1\" for boost-dijkstra or \"-m2\" for manual dijkstra." << endl;
	}

	ifstream infile;
	infile.open(argv[2], ios::in);
	if (!infile){
		cout << "File could not be opened." << endl;
		return 1;
	}

	string line;
	stringstream s;
	unsigned int numberVertices;
	unsigned int numberEdges;

	//Read first line
	if (getline(infile, line)){
		s.str(line);
		s >> numberVertices >> numberEdges;
		//Number of vertices is one more than the real number of vertices for storing all the vertices correctly (it's 1-based)
		numberVertices++;
	} else {
		cerr << "Empty file. Exit program" << endl;
		return 1;
	}

	//Reading the edge data
	Edges edges;
	WeightMap weights;
	int startEdge;
	int endEdge;
	int weight;

	while (getline(infile, line)){
		if (!line.empty()){
			s.clear();
			s.str(line);
			s >> startEdge >> endEdge >> weight;
			edges.push_back(make_pair(startEdge, endEdge));
			weights.push_back(weight);
		}
	}
	infile.close();

	WeightMap weightMap(numberVertices);

	if (boostMethod){
		//Creating a graph g
		graph g{edges.begin(), edges.end(), weights.begin(), numberVertices};

		//storing the shortest paths and its weights
		vector<vertex_descriptor> directions(numberVertices);

		boost::dijkstra_shortest_paths(g, 1,//
				boost::predecessor_map(//
						boost::make_iterator_property_map(directions.begin(), get(boost::vertex_index, g)))//
				.distance_map(//
						boost::make_iterator_property_map(weightMap.begin(), get(boost::vertex_index, g))));
	}
	else {
		dijkstra myDijkstra(weights, edges, numberVertices);
		weightMap = myDijkstra.computeShortestPath(1);
	}

	//Compute the longest shortest path
	int weightOfLongestShortestPath = -1;
	int indexOfVertex = -1;
	int totalWeight;
	for(unsigned int i = 2; i < numberVertices; i++){
		totalWeight = weightMap[i];
		if (totalWeight > weightOfLongestShortestPath){
			weightOfLongestShortestPath = totalWeight;
			indexOfVertex = i;
		}
	}

	cout << "RESULT VERTEX " << indexOfVertex << endl;
	cout << "RESULT DIST " << weightOfLongestShortestPath << endl;

	//Print measured time
	boost::timer::cpu_times times = timer.elapsed();
	cout << "Wall-clock time: " << times.wall * 1e-9 << " seconds" << endl;
	cout << "User-time: " << times.user * 1e-9 <<  " seconds" << endl;

	return 0;
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are 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 "test_gamma.hpp"
#ifdef TEST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#else
#include <boost/multiprecision/cpp_bin_float.hpp>
#endif

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double";
   }
   else
   {
      largest_type = "long double";
   }
#else
   largest_type = "(long\\s+)?double";
#endif
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*near 1.*",                  // test data group
      ".*lgamma.*", 100000000000LL, 100000000000LL);    // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*near 0.*",                  // test data group
      ".*lgamma.*", 300000, 100000);    // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      ".*",                          // test type(s)
      ".*",                          // test data group
      ".*", 110000, 50000);          // test function

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

BOOST_AUTO_TEST_CASE( test_main )
{
   expected_results();
   BOOST_MATH_CONTROL_FP;

#ifdef TEST_MPFR
   typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<450> > mp_type;
   const char* name = "number<mpfr_float_backend<450> >";
#else
   typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<450> > mp_type;
   const char* name = "number<cpp_bin_float<450> >";
#endif

   test_gamma(mp_type(0), name);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/lognstat.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( lognstat, NT2_REAL_TYPES)
{
  using nt2::lognstat;
  using nt2::tag::lognstat_;
  using nt2::sqr;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<lognstat_(T, T)>::type)
                  , (std::pair<T,T>)
                  );

  T mean;
  T var;
  T m = T(1.5);
  T s = T(2.5);
  T rm = T(1.020027730826997e+02);
  T rs = T(5.379293910566451e+06);
 {
    lognstat(m, s, mean, var);
    NT2_TEST_ULP_EQUAL(var, rs, 0.5);
    NT2_TEST_ULP_EQUAL(mean, rm, 0.5);
  }

  {
    mean = lognstat(m, s, var);
    NT2_TEST_ULP_EQUAL(var, rs, 0.5);
    NT2_TEST_ULP_EQUAL(mean, rm, 0.5);
  }

  {
    boost::fusion::vector_tie(mean,var) = lognstat(m, s);
    NT2_TEST_ULP_EQUAL(var, rs, 0.5);
    NT2_TEST_ULP_EQUAL(mean, rm, 0.5);
  }

  {
    std::pair<T,T> p;

    p = lognstat(m, s);
    NT2_TEST_ULP_EQUAL(p.first, rm, 0.5);
    NT2_TEST_ULP_EQUAL(p.second, rs, 0.5);
  }
}


/*
 * Copyright 2015 David A. Boyuka II
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*
 * interval-encoding.cpp
 *
 *  Created on: Dec 2, 2014
 *      Author: David A. Boyuka II
 */

#include <boost/iterator/counting_iterator.hpp>

#include "pique/encoding/interval/interval-encoding.hpp"

auto IntervalIndexEncoding::get_region_math_impl(bin_count_t nbins, bin_id_t lb, bin_id_t ub, bool prefer_complement) const -> RMath {
	using regid_t = region_id_t;

	const regid_t nregions = (nbins + 1) / 2; // ceil(nbins / 2)
	const regid_t interval_width = nbins / 2; // floor(nbins / 2)
	// Examples:
	// nbins == 7: [0, 3), [1, 4), [2, 5), [3, 6)   (interval_width = 3, nregions = 4)
	// nbins == 8: [0, 4), [1, 5), [2, 6), [3, 7)   (interval_width = 4, nregions = 4)

	// If the request interval covers the last bin, we have to use complement
	bool complement = false;
	if (ub == nbins) {
		ub = lb;
		lb = 0;
		complement = true;
	}

	// There are five cases, based (with modifications) on this seminal paper, page 6, eqn 6: http://dl.acm.org/citation.cfm?id=304201
	RMath rmath;
	if (ub < nregions) {
		// lb < ub < nregions from here
		rmath.push_region((regid_t)lb)
		     .push_region((regid_t)ub)
		     .push_op(NArySetOperation::DIFFERENCE);
	} else if (lb >= nregions) {
		// ub >= nregions >= interval_width from previous
		// lb >= nregions >= interval_width from here
		rmath.push_region((regid_t)ub - interval_width)
		     .push_region((regid_t)lb - interval_width)
		     .push_op(NArySetOperation::DIFFERENCE);
	} else if (ub - lb < interval_width) {
		// ub >= nregions >= interval_width from previous
		// lb < nregions from previous
		// ub - lb < interval_width from here
		rmath.push_region((regid_t)lb)
		     .push_region((regid_t)ub - interval_width)
		     .push_op(NArySetOperation::INTERSECTION);
	} else if (ub - lb > interval_width) {
		// ub >= nregions >= interval_width from previous
		// lb < nregions from previous
		// ub - lb >= interval_width from previous
		// ub - lb > interval_width from here
		rmath.push_region((regid_t)lb)
		     .push_region((regid_t)ub - interval_width)
		     .push_op(NArySetOperation::UNION);
	} else {
		// ub >= nregions >= interval_width from previous
		// lb < nregions from previous
		// ub - lb == interval_width from previous
		rmath.push_region((regid_t)lb);
	}

	// If we are computing the complement, invert the whole calculation at the end
	if (complement)
		rmath.push_op(UnarySetOperation::COMPLEMENT);

	return rmath;
}

auto IntervalIndexEncoding::get_encoded_region_definitions_impl(bin_count_t nbins) const -> std::vector< bin_id_vector_t > {
	const region_id_t nregions = (nbins + 1) / 2; // = ceil(nbins/2)
	const region_id_t interval_width = nbins / 2; //  = floor(nbins/2); intervals are of the form [x, x + interval_width)

	std::vector< bin_id_vector_t > enc_region_defs;

	for (region_id_t enc_region = 0; enc_region < nregions; ++enc_region) {
		bin_id_vector_t enc_region_def;
		// ith encoded region = [ith bin, (i+width) bin)
		enc_region_def.insert(
			enc_region_def.end(),
			boost::counting_iterator< bin_id_t >(enc_region),
			boost::counting_iterator< bin_id_t >(enc_region + interval_width));
		enc_region_defs.push_back(enc_region_def);
	}

	return enc_region_defs;
}

auto IntervalIndexEncoding::get_encoded_regions_impl(region_vector_t bins, const AbstractSetOperations& setops) const -> region_vector_t {
	region_vector_t enc_regions;
	const region_id_t nregions = (bins.size() + 1) / 2; // = ceil(nbins/2)
	const region_id_t interval_width = bins.size() / 2; //  = floor(nbins/2); intervals are of the form [x, x + interval_width)

	if (nregions > 0) {
		boost::shared_ptr< RegionEncoding > first_enc_region =
			setops.dynamic_nary_set_op(bins.begin(), bins.begin() + interval_width, NArySetOperation::UNION);
		enc_regions.push_back(first_enc_region);
	}

	for (region_id_t i = 1; i < nregions; ++i) {
		boost::shared_ptr< RegionEncoding > enc_region;

		// ith encoded region = (i-1)th encoded region - (i-1)th bin + (i-1+w)th bin
		enc_region = setops.dynamic_binary_set_op(enc_regions[i - 1], bins[i - 1], NArySetOperation::DIFFERENCE);
		enc_region = setops.dynamic_inplace_binary_set_op(enc_region, bins[i - 1 + interval_width], NArySetOperation::UNION);
		enc_regions.push_back(enc_region);
	}

	return enc_regions;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2007-2008 Steven Watanabe
//
// 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 quaternion.cpp

\details
Demonstrate interoperability with Boost.Quaternion.

Output:
@verbatim

//[quaternion_output_1
+L      = (4,3,2,1) m
-L      = (-4,-3,-2,-1) m
L+L     = (8,6,4,2) m
L-L     = (0,0,0,0) m
L*L     = (2,24,16,8) m^2
L/L     = (1,0,0,0) dimensionless
L^3     = (-104,102,68,34) m^3
//]

//[quaternion_output_2
+L      = (4 m,3 m,2 m,1 m)
-L      = (-4 m,-3 m,-2 m,-1 m)
L+L     = (8 m,6 m,4 m,2 m)
L-L     = (0 m,0 m,0 m,0 m)
L^3     = (-104 m^3,102 m^3,68 m^3,34 m^3)
//]

@endverbatim
**/

#include <iostream>

#include <boost/math/quaternion.hpp>
#include <boost/mpl/list.hpp>

#include <boost/units/pow.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>

#include "test_system.hpp"

#if BOOST_UNITS_HAS_BOOST_TYPEOF

#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()

BOOST_TYPEOF_REGISTER_TEMPLATE(boost::math::quaternion, 1)

#endif

namespace boost {

namespace units {

//[quaternion_class_snippet_1a
/// specialize power typeof helper
template<class Y,long N,long D>
struct power_typeof_helper<boost::math::quaternion<Y>,static_rational<N,D> >
{
    // boost::math::quaternion only supports integer powers
    BOOST_STATIC_ASSERT(D==1);

    typedef boost::math::quaternion<
        typename power_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const boost::math::quaternion<Y>& x)
    {
        return boost::math::pow(x,static_cast<int>(N));
    }
};
//]

//[quaternion_class_snippet_1b
/// specialize root typeof helper
template<class Y,long N,long D>
struct root_typeof_helper<boost::math::quaternion<Y>,static_rational<N,D> >
{
    // boost::math::quaternion only supports integer powers
    BOOST_STATIC_ASSERT(N==1);

    typedef boost::math::quaternion<
        typename root_typeof_helper<Y,static_rational<N,D> >::type
    > type;

    static type value(const boost::math::quaternion<Y>& x)
    {
        return boost::math::pow(x,static_cast<int>(D));
    }
};
//]

//[quaternion_class_snippet_2a
/// specialize power typeof helper for quaternion<quantity<Unit,Y> >
template<class Unit,long N,long D,class Y>
struct power_typeof_helper<
    boost::math::quaternion<quantity<Unit,Y> >,
    static_rational<N,D> >
{
    typedef typename power_typeof_helper<
        Y,
        static_rational<N,D>
    >::type     value_type;

    typedef typename power_typeof_helper<
        Unit,
        static_rational<N,D>
    >::type  unit_type;

    typedef quantity<unit_type,value_type>         quantity_type;
    typedef boost::math::quaternion<quantity_type> type;

    static type value(const boost::math::quaternion<quantity<Unit,Y> >& x)
    {
        const boost::math::quaternion<value_type>   tmp =
            pow<static_rational<N,D> >(boost::math::quaternion<Y>(
                x.R_component_1().value(),
                x.R_component_2().value(),
                x.R_component_3().value(),
                x.R_component_4().value()));

        return type(quantity_type::from_value(tmp.R_component_1()),
                    quantity_type::from_value(tmp.R_component_2()),
                    quantity_type::from_value(tmp.R_component_3()),
                    quantity_type::from_value(tmp.R_component_4()));
    }
};
//]

//[quaternion_class_snippet_2b
/// specialize root typeof helper for quaternion<quantity<Unit,Y> >
template<class Unit,long N,long D,class Y>
struct root_typeof_helper<
    boost::math::quaternion<quantity<Unit,Y> >,
    static_rational<N,D> >
{
    typedef typename root_typeof_helper<
        Y,
        static_rational<N,D>
    >::type      value_type;

    typedef typename root_typeof_helper<
        Unit,
        static_rational<N,D>
    >::type   unit_type;

    typedef quantity<unit_type,value_type>         quantity_type;
    typedef boost::math::quaternion<quantity_type> type;

    static type value(const boost::math::quaternion<quantity<Unit,Y> >& x)
    {
        const boost::math::quaternion<value_type>   tmp =
            root<static_rational<N,D> >(boost::math::quaternion<Y>(
                x.R_component_1().value(),
                x.R_component_2().value(),
                x.R_component_3().value(),
                x.R_component_4().value()));

        return type(quantity_type::from_value(tmp.R_component_1()),
                    quantity_type::from_value(tmp.R_component_2()),
                    quantity_type::from_value(tmp.R_component_3()),
                    quantity_type::from_value(tmp.R_component_4()));
    }
};
//]

} // namespace units

} // namespace boost

int main(void)
{
    using boost::math::quaternion;
    using namespace boost::units;
    using namespace boost::units::test;
    using boost::units::pow;

    {
    //[quaternion_snippet_1
    typedef quantity<length,quaternion<double> >     length_dimension;

    length_dimension    L(quaternion<double>(4.0,3.0,2.0,1.0)*meters);
    //]

    std::cout << "+L      = " << +L << std::endl
              << "-L      = " << -L << std::endl
              << "L+L     = " << L+L << std::endl
              << "L-L     = " << L-L << std::endl
              << "L*L     = " << L*L << std::endl
              << "L/L     = " << L/L << std::endl
              // unfortunately, without qualification msvc still
              // finds boost::math::pow by ADL.
              << "L^3     = " << boost::units::pow<3>(L) << std::endl
//              << "L^(3/2) = " << pow< static_rational<3,2> >(L) << std::endl
//              << "3vL     = " << root<3>(L) << std::endl
//              << "(3/2)vL = " << root< static_rational<3,2> >(L) << std::endl
              << std::endl;
    }

    {
    //[quaternion_snippet_2
    typedef quaternion<quantity<length> >     length_dimension;

    length_dimension    L(4.0*meters,3.0*meters,2.0*meters,1.0*meters);
    //]

    std::cout << "+L      = " << +L << std::endl
              << "-L      = " << -L << std::endl
              << "L+L     = " << L+L << std::endl
              << "L-L     = " << L-L << std::endl
//              << "L*L     = " << L*L << std::endl
//              << "L/L     = " << L/L << std::endl
              << "L^3     = " << boost::units::pow<3>(L) << std::endl
//              << "L^(3/2) = " << pow< static_rational<3,2> >(L) << std::endl
//              << "3vL     = " << root<3>(L) << std::endl
//              << "(3/2)vL = " << root< static_rational<3,2> >(L) << std::endl
              << std::endl;
    }

    return 0;
}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/equal.hpp>
#include <boost/hana/ext/std/tuple.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>
#include <boost/hana/unpack.hpp>
#include <boost/hana/zip_with.hpp>

#include <tuple>
#include <type_traits>
#include <utility>
namespace hana = boost::hana;

// Basic usage:
static_assert(
    hana::zip_with(hana::mult, hana::make_tuple(1, 2, 3, 4), hana::make_tuple(5, 6, 7, 8))
    ==
    hana::make_tuple(5, 12, 21, 32)
, "");

// Example of computing a tuple of all the common types of several tuples:
template<typename... Ts>
using common_tuple_t = typename decltype(
    hana::unpack(
        hana::zip_with(
            hana::metafunction<std::common_type>,
            hana::transform(std::declval<Ts>(), hana::decltype_)...
        ),
        hana::template_<std::tuple>
    )
)::type;

static_assert(std::is_same<
    common_tuple_t<
        std::tuple<bool, int, unsigned>,
        std::tuple<char, long, long>,
        std::tuple<int, long long, double>
    >,
    std::tuple<int, long long, double>
>::value, "");

int main() { }


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#include "Constraint.hpp"
#include "Scheduler.hpp"

#include <popsolver/Model.hpp>
#define BOOST_TEST_MODULE Min
#include <boost/test/unit_test.hpp>

using namespace popsolver;

const Variable a(0);
const Variable b(1);
const Variable c(2);

BOOST_AUTO_TEST_CASE(PropagateConstrainResult) {
  Min min(a, {b, c});

  Domains domains;
  domains.push_back({DataType{15}, DataType{40}}); // a
  domains.push_back({DataType{20}, DataType{30}}); // b
  domains.push_back({DataType{25}, DataType{35}}); // c

  Scheduler scheduler(domains, {&min});
  BOOST_CHECK(min.propagate(scheduler));

  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{20});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{30});

  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{20});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{30});

  BOOST_CHECK_EQUAL(scheduler.getDomains()[c].min(), DataType{25});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[c].max(), DataType{35});
}

BOOST_AUTO_TEST_CASE(PropagateConstrainValues) {
  Min min(a, {b, c});

  Domains domains;
  domains.push_back({DataType{15}, DataType{20}}); // a
  domains.push_back({DataType{0}, DataType{30}});  // b
  domains.push_back({DataType{10}, DataType{35}}); // c

  Scheduler scheduler(domains, {&min});
  BOOST_CHECK(min.propagate(scheduler));

  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].min(), DataType{15});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[a].max(), DataType{20});

  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].min(), DataType{15});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[b].max(), DataType{30});

  BOOST_CHECK_EQUAL(scheduler.getDomains()[c].min(), DataType{15});
  BOOST_CHECK_EQUAL(scheduler.getDomains()[c].max(), DataType{35});
}

BOOST_AUTO_TEST_CASE(PropagateFailsResultUpperBound) {
  Min min(a, {b, c});

  Domains domains;
  domains.push_back({DataType{35}, DataType{40}}); // a
  domains.push_back({DataType{25}, DataType{30}}); // b
  domains.push_back({DataType{20}, DataType{35}}); // c

  Scheduler scheduler(domains, {&min});
  BOOST_CHECK(!min.propagate(scheduler));
}

BOOST_AUTO_TEST_CASE(PropagateFailsResultLowerBound) {
  Min min(a, {b, c});

  Domains domains;
  domains.push_back({DataType{0}, DataType{10}});  // a
  domains.push_back({DataType{15}, DataType{20}}); // b
  domains.push_back({DataType{25}, DataType{35}}); // c

  Scheduler scheduler(domains, {&min});
  BOOST_CHECK(!min.propagate(scheduler));
}

BOOST_AUTO_TEST_CASE(MinimizeBelowVariable) {
  Model m;

  const auto b = m.addConstant(5);
  const auto c = m.addVariable(15, 30);

  const auto a = m.min({b, c});
  auto s = m.minimize(a);
  BOOST_CHECK_EQUAL(s[a], DataType{5});
}

BOOST_AUTO_TEST_CASE(MinimizeInsideVariable) {
  Model m;

  const auto b = m.addConstant(20);
  const auto c = m.addVariable(15, 30);

  const auto a = m.min({b, c});
  auto s = m.minimize(a);
  BOOST_CHECK_EQUAL(s[a], DataType{15});
}

BOOST_AUTO_TEST_CASE(MinimizeAboveVariable) {
  Model m;

  const auto b = m.addConstant(35);
  const auto c = m.addVariable(15, 30);

  const auto a = m.min({b, c});
  auto s = m.minimize(a);
  BOOST_CHECK_EQUAL(s[a], DataType{15});
}