#ifndef MANIFOLDS_FUNCTIONS_TUPLE_UTILITIES_HH
#define MANIFOLDS_FUNCTIONS_TUPLE_UTILITIES_HH

#include <utility>
#include "tmp_utils.hh"
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/sort.hpp>
#include <boost/mpl/at.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/size.hpp>
#include <array>
#include <tuple>

namespace manifolds {
using std::tuple;

template <class... Args> tuple<Args...> make_my_tuple(Args... args) {
  return std::make_tuple(args...);
}

template <class... Ts, class... Args>
auto make_same_as(const tuple<Ts...> &, Args... args) {
  return make_my_tuple(args...);
}

inline auto tuple_cat() { return tuple<>(); }

template <class T> auto tuple_cat(const T &t) { return t; }

template <class T1, class T2> auto tuple_cat(const T1 &t1, const T2 &t2) {
  return tuple_cat(t1, t2,
                   std::make_index_sequence<std::tuple_size<T1>::value>(),
                   std::make_index_sequence<std::tuple_size<T2>::value>());
}

template <class Tuple1, class Tuple2, class... Tuples>
auto tuple_cat(const Tuple1 &t1, const Tuple2 &t2, const Tuples &... tuples) {
  return tuple_cat(tuple_cat(t1, t2), tuples...);
}

template <class Tuple, std::size_t... befores, std::size_t... afters>
auto remove_element_helper(const Tuple &t,
                           std::integer_sequence<std::size_t, befores...>,
                           std::integer_sequence<std::size_t, afters...>) {
  using namespace std;
  static const std::size_t offset = sizeof...(befores) + 1;
  auto r =
      make_same_as(t, std::get<befores>(t)..., std::get<afters + offset>(t)...);
  return r;
}

template <int index, class Tuple> auto remove_element(const Tuple &t) {
  typedef std::make_index_sequence<index> befores;
  typedef std::make_integer_sequence<
      std::size_t, std::tuple_size<Tuple>::value - index - 1> afters;
  return remove_element_helper(t, befores(), afters());
}

template <class Tuple>
auto remove_element(const Tuple &t, boost::mpl::vector<>) {
  return t;
}

template <class A, class... As, class B, class... Bs>
bool operator==(const tuple<A, As...> &a, const tuple<B, Bs...> &b) {
  return std::get<0>(a) == std::get<0>(b) &&
         remove_element<0>(a) == remove_element<0>(b);
}

template <
    class Tuple, class Vector,
    class = typename std::enable_if<boost::mpl::size<Vector>::value == 0>::type>
auto remove_elements(const Tuple &t, Vector, long) {
  return t;
}

template <
    class Tuple, class Vector,
    class = typename std::enable_if<boost::mpl::size<Vector>::value != 0>::type>
auto remove_elements(const Tuple &t, Vector, int) {
  return remove_element<boost::mpl::at_c<Vector, 0>::type::value>(
      remove_elements(t, typename boost::mpl::pop_front<Vector>::type(), 0));
}

template <int... indices, class Tuple> auto remove_elements(const Tuple &t) {
  typedef typename boost::mpl::sort<
      boost::mpl::vector_c<int, indices...> >::type sorted_list;
  return remove_elements(t, sorted_list(), 0);
}

template <int index, class Tuple, class E, std::size_t... befores,
          std::size_t... afters>
auto insert_element_helper(const Tuple &t, const E &e,
                           std::integer_sequence<std::size_t, befores...>,
                           std::integer_sequence<std::size_t, afters...>) {
  using namespace std;
  auto before = make_same_as(t, std::get<befores>(t)...);
  auto middle = make_same_as(t, e);
  auto after = make_same_as(t, std::get<afters + index>(t)...);
  return tuple_cat(before, middle, after);
}

template <int index, class Tuple, class E>
auto insert_element(const Tuple &t, const E &e) {
  return insert_element_helper<index>(
      t, e, std::make_index_sequence<index>(),
      std::make_index_sequence<std::tuple_size<Tuple>::value - index>());
}

template <class Tuple, class E> auto push_back(const Tuple &t, const E &e) {
  return insert_element<std::tuple_size<Tuple>::value>(t, e);
}

template <class T> struct array_size;

template <class T, std::size_t n>
struct array_size<std::array<T, n> > : int_<n> {};

template <class T, std::size_t n>
struct array_size<const std::array<T, n> &> : int_<n> {};

template <class T, std::size_t n>
struct array_size<std::array<T, n> &> : int_<n> {};

template <class T, std::size_t n> struct array_size<T[n]> : int_<n> {};

template <class T, std::size_t n> struct array_size<T (&)[n]> : int_<n> {};

template <class T, std::size_t n>
struct array_size<const T (&)[n]> : int_<n> {};

FUNC_CHECK_DEF(is_gettable, decltype(std::get<0>(std::declval<T>())))

template <int start, int size, class Tuple>
auto subset_helper(const Tuple &t, int_<start>, int_<size>) {
  typedef boost::mpl::range_c<int, 0, start> first_r;
  typedef typename boost::mpl::copy<
      first_r, boost::mpl::back_inserter<boost::mpl::vector<> > >::type first_v;
  typedef typename boost::mpl::if_c<
      (start + size > std::tuple_size<Tuple>::value),
      boost::mpl::range_c<int, 0, 0>,
      boost::mpl::range_c<int, start + size,
                          std::tuple_size<Tuple>::value> >::type second_r;
  typedef typename boost::mpl::copy<
      second_r, boost::mpl::back_inserter<boost::mpl::vector<> > >::type
  second_v;
  return remove_elements(remove_elements(t, second_v(), 0), first_v(), 0);
}

template <int start, class Tuple>
auto subset_helper(const Tuple &, int_<start>, int_<0>) {
  return tuple<>();
}

template <int start, int size, class Tuple> auto subset(const Tuple &t) {
  return subset_helper(t, int_<start>(), int_<size>());
}

template <template <class...> class Variadic, class Tuple,
          std::size_t... indices>
auto MakeVariadic(const Tuple &t,
                  std::integer_sequence<std::size_t, indices...>) {
  return Variadic<typename std::tuple_element<indices, Tuple>::type...>(
      std::get<indices>(t)...);
}

template <template <class...> class Variadic, class Tuple>
auto MakeVariadic(Tuple t) {
  return MakeVariadic<Variadic>(
      t, std::make_index_sequence<std::tuple_size<Tuple>::value>());
}

template <class... Types, std::size_t... is>
auto ToArray(const tuple<Types...> &t,
             std::integer_sequence<std::size_t, is...>) {
  return std::array<typename std::common_type<Types...>::type,
                    sizeof...(Types)>{ { std::get<is>(t)... } };
}

template <class... Types> auto ToArray(const tuple<Types...> &t) {
  return ToArray(t, std::index_sequence_for<Types...>());
}

template <int i, class... Args, class A>
auto replace_element(const tuple<Args...> &t, const A &a) {
  return insert_element<i>(remove_element<i>(t), a);
}
}

#endif


#ifndef BINARY_IO_BITFIELD_HPP_
#define BINARY_IO_BITFIELD_HPP_

#include <cassert>
#include <type_traits>
#include <boost/optional.hpp>
#include "binary_io/element.hpp"

namespace binary_io {
template <
        typename Enum,
        Enum key,
        std::size_t bit_size,
        typename Type,
        Type default_value_ = Type(0),
        Type min_value_ = Type(0),
        Type max_value_ = Type(-1)>
class Bitfield: public Element<Enum, key, bit_size, Type> {
  static_assert(std::is_unsigned<Type>::value, "invalid type");
  static_assert(min_value_ <= max_value_, "invalid range");
  static_assert(bit_size <= sizeof(Type) * 8, "Type is short of bits");

  static constexpr Type default_value = default_value_;
  static constexpr Type min_value = min_value_;
  static constexpr Type max_value = max_value_;

 public:
  // default value
  static constexpr Type DefaultValue() {
    return default_value;
  }
  // read
  static boost::optional<Type> Read(
          const void* buffer_head,
          const std::size_t& bit_offset) {
    auto result = Type(0);
    for (int i = static_cast<int>(bit_size) - 1; i >= 0; --i) {
      // shift
      result <<= 1;
      // bit check
      const auto byte = *(reinterpret_cast<const uint8_t*>(buffer_head)
                          + (bit_offset + i) / 8);
      const uint8_t mask = 0x1 << (bit_offset + i) % 8;
      if ((byte & mask) != 0) {
        result += 0x1;
      }
    }
    if (IsValid(result)) {
      return result;
    } else {
      return boost::none;
    }
  }
  // write
  static bool Write(
          void* buffer_head,
          const std::size_t& bit_offset,
          const Type& value) {
    if (IsValid(value)) {
      for (std::size_t i = 0; i < bit_size; ++i) {
        // bit mask
        const auto value_byte =
                *(reinterpret_cast<const uint8_t*>(&value) + i / 8);
        const uint8_t value_mask = 0x01 << i % 8;
        // write
        if ((value_byte & value_mask) != 0) {
          auto* const write_byte_ptr =
                  static_cast<uint8_t*>(buffer_head) + (bit_offset + i) / 8;
          const uint8_t write_mask = 0x1 << (bit_offset + i) % 8;
          *write_byte_ptr |= write_mask;
        }
      }
      return true;
    }
    return false;
  }
  // write default value
  static bool WriteDefaultValue(
          void* buffer_head,
          const std::size_t& bit_offset) {
    return Write(buffer_head, bit_offset, DefaultValue());
  }

 private:
  // check if the value is valid
  static constexpr bool IsValid(
          const Type& value) {
    return (min_value <= value && value <= max_value)
           || value == DefaultValue();
  }
};
}  // namespace binary_io
#endif  // BINARY_IO_BITFIELD_HPP_


#include <stdexcept>
#include <boost/test/unit_test.hpp>
#include "matrix.hpp"
#include "circle.hpp"
#include "rectangle.hpp"
#include "composite-shape.hpp"

const double EPSILON = 0.00000001;

BOOST_AUTO_TEST_SUITE(Matrix)

  BOOST_AUTO_TEST_SUITE(Matrix_Constructor)

    BOOST_AUTO_TEST_CASE(Matrix_NoParameters_NoException)
    {
      BOOST_CHECK_NO_THROW(samokhin::Matrix{});
    }

    BOOST_AUTO_TEST_CASE(Matrix_EmptyMatrixAsParameter_ExceptionThrown)
    {
      samokhin::Matrix matrix{};
      BOOST_CHECK_THROW(samokhin::Matrix{matrix}, std::invalid_argument);
    }

    BOOST_AUTO_TEST_CASE(Matrix_ValidMatrixAsParameter_NoException)
    {
      samokhin::Matrix matrix{};
      double radius = 1.1;
      double x = 4.1;
      double y = 6.1;
      matrix.add(std::make_shared<samokhin::Circle>(samokhin::Circle{radius, {x, y}}));
      BOOST_CHECK_NO_THROW(samokhin::Matrix{matrix});
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Matrix_Add)

    BOOST_AUTO_TEST_CASE(Add_ValidShapeAsParameter_NoException)
    {
      double radius = 1.0;
      double x = -1.0;
      double y = 2.1;
      std::shared_ptr<samokhin::Circle> circle = std::make_shared<samokhin::Circle>(samokhin::Circle{radius, {x, y}});

      samokhin::Matrix matrix{};
      BOOST_CHECK_NO_THROW(matrix.add(circle));
    }

    BOOST_AUTO_TEST_CASE(Add_EmptyShapeAsParameter_ExceptionThrown)
    {
      std::shared_ptr<samokhin::CompositeShape> emptyCompositeShape{};
      samokhin::Matrix matrix{};

      BOOST_CHECK_THROW(matrix.add(emptyCompositeShape), std::invalid_argument);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Matrix_IndexOperator)

    BOOST_AUTO_TEST_CASE(IndexOperator_ValidIndexesAsParameters_NoException)
    {
      double radius = 2.1;
      double x = 9.1;
      double y = 2.1;
      std::shared_ptr<samokhin::Circle> circle = std::make_shared<samokhin::Circle>(samokhin::Circle{radius, {x, y}});

      samokhin::Matrix matrix{};
      matrix.add(circle);

      BOOST_CHECK_NO_THROW(matrix[0][0]);
    }

    BOOST_AUTO_TEST_CASE(IndexOperator_ValidIndexesAsParameters_RightShape)
    {
      double radius = 2.1;
      double x = 9.1;
      double y = 2.1;
      std::shared_ptr<samokhin::Circle> circle = std::make_shared<samokhin::Circle>(samokhin::Circle{radius, {x, y}});

      double width = 4.0;
      double height = 3.1;

      std::shared_ptr<samokhin::Rectangle> rectangle = std::make_shared<samokhin::Rectangle>(
        samokhin::Rectangle{width, height, {x, y}});

      samokhin::Matrix matrix{};
      matrix.add(circle);
      matrix.add(rectangle);

      std::shared_ptr<samokhin::Shape> shape = matrix[1][0];
      samokhin::rectangle_t frame = shape->getFrameRect();

      BOOST_CHECK_CLOSE(frame.height, height, EPSILON);
      BOOST_CHECK_CLOSE(frame.width, width, EPSILON);
      BOOST_CHECK_CLOSE(frame.pos.x, x, EPSILON);
      BOOST_CHECK_CLOSE(frame.pos.y, y, EPSILON);
    }

    BOOST_AUTO_TEST_CASE(IndexOperator_InvalidIndexesAsParameters_ExceptionThrown)
    {
      double radius = 6.1;
      double x = -2.1;
      double y = 2.3;
      std::shared_ptr<samokhin::Circle> circle = std::make_shared<samokhin::Circle>(samokhin::Circle{radius, {x, y}});

      samokhin::Matrix matrix{};
      matrix.add(circle);

      BOOST_CHECK_THROW(matrix[1][0], std::out_of_range);
      BOOST_CHECK_THROW(matrix[0][1], std::out_of_range);
    }

    BOOST_AUTO_TEST_CASE(IndexOperator_GetFromEmptyMatrix_ExceptionThrown)
    {
      samokhin::Matrix matrix{};
      BOOST_CHECK_THROW(matrix[0][0], std::logic_error);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Matrix_GetRowSize)

    BOOST_AUTO_TEST_CASE(GetRowSize_RightRowSize)
    {
      double radius = 1.5;
      double x = 0.0;
      double y = 0.0;
      std::shared_ptr<samokhin::Circle> circle = std::make_shared<samokhin::Circle>(samokhin::Circle{radius, {x, y}});

      double width = 1.74;
      double height = 3.2;

      std::shared_ptr<samokhin::Rectangle> rectangle = std::make_shared<samokhin::Rectangle>(
        samokhin::Rectangle{width, height, {x, y}});

      samokhin::Matrix matrix{};
      matrix.add(circle);
      matrix.add(rectangle);

      BOOST_CHECK_EQUAL(matrix.getRowSize(), 2);
    }

  BOOST_AUTO_TEST_SUITE_END()
  BOOST_AUTO_TEST_SUITE(Matrix_GetColumnSize)

    BOOST_AUTO_TEST_CASE(Matrix_RightColumnSize)
    {
      double radius = 1.5;
      double x = 7.4;
      double y = 9.2;
      std::shared_ptr<samokhin::Circle> circle = std::make_shared<samokhin::Circle>(samokhin::Circle{radius, {x, y}});

      double width = 1.74;
      double height = 3.2;
      x = 0.0;
      y = 0.0;
      std::shared_ptr<samokhin::Rectangle> rectangle = std::make_shared<samokhin::Rectangle>(
        samokhin::Rectangle{width, height, {x, y}});

      samokhin::Matrix matrix{};
      matrix.add(circle);
      matrix.add(rectangle);

      BOOST_CHECK_EQUAL(matrix.getColumnSize(), 2);
    }

  BOOST_AUTO_TEST_SUITE_END()

  BOOST_AUTO_TEST_SUITE(Matrix_Remove)

    BOOST_AUTO_TEST_CASE(Remove_TooBigIndexAsParameter_ExceptionThrown)
    {
      double radius = 9.3;
      double x = 5.2;
      double y = 1.2;
      std::shared_ptr<samokhin::Circle> circle(new samokhin::Circle(radius, {x, y}));

      samokhin::Matrix matrix{};
      matrix.add(circle);

      BOOST_CHECK_THROW(matrix.remove(0, 1), std::out_of_range);
      BOOST_CHECK_THROW(matrix.remove(1, 0), std::out_of_range);
    }

    BOOST_AUTO_TEST_CASE(Remove_RemoveFromEmptyMatrix_ExceptionThrown)
    {
      samokhin::Matrix matrix{};
      BOOST_CHECK_THROW(matrix.remove(0, 0), std::out_of_range);
    }

    BOOST_AUTO_TEST_CASE(Remove_ValidIndexAsParameter_NoException)
    {
      double radius = 9.3;
      double x = 5.2;
      double y = 1.2;
      std::shared_ptr<samokhin::Circle> circle(new samokhin::Circle(radius, {x, y}));

      samokhin::Matrix matrix{};
      matrix.add(circle);

      BOOST_CHECK_NO_THROW(matrix.remove(0, 0));
    }

    BOOST_AUTO_TEST_CASE(Remove_ValidIndexAsParameter_CorrectRemove)
    {
      double radius = 9.3;
      double x = 5.2;
      double y = 1.2;
      std::shared_ptr<samokhin::Circle> circle(new samokhin::Circle(radius, {x, y}));

      samokhin::Matrix matrix{};
      matrix.add(circle);
      matrix.remove(0, 0);

      BOOST_CHECK_EQUAL(matrix.getRowSize(), 0);
      BOOST_CHECK_EQUAL(matrix.getColumnSize(), 0);
    }

    BOOST_AUTO_TEST_CASE(Remove_DeletingNonexistElement_ExceptionThrown)
    {
      double radius = 10.9;
      double x = 5.2;
      double y = 1.2;
      std::shared_ptr<samokhin::Circle> circle(new samokhin::Circle(radius, {x, y}));

      samokhin::Matrix matrix{};
      matrix.add(circle);

      radius = 1.0;
      std::shared_ptr<samokhin::Circle> circle2(new samokhin::Circle(radius, {x, y}));
      matrix.add(circle2);

      x = 100.0;
      std::shared_ptr<samokhin::Circle> circle3(new samokhin::Circle(radius, {x, y}));
      matrix.add(circle3);

      BOOST_CHECK_THROW(matrix.remove(1, 1), std::domain_error);
    }

  BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


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

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

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_MANTISSA_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/mantissamask.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/bitwise_or.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/simd/is_invalid.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(mantissa_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::integer_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return a0;
      }
   };

   BOOST_DISPATCH_OVERLOAD(mantissa_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        A0 r = bitwise_or(bitwise_and(a0,Mantissamask<A0>()),One<A0>());
        #ifndef BOOST_SIMD_NO_INVALIDS
        return if_else(logical_or(is_invalid(a0),is_eqz(a0)),a0,r);
        #else
        return if_else(is_eqz(a0),a0,r);
        #endif
      }
   };

} } }

#endif


/**
 * @file
 * implementation of methods described in ClipsExtensions.h
 * @copyright
 * syn
 * Copyright (c) 2013-2017, Joshua Scoggins and Contributors
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "BaseTypes.h"
#include "Base.h"
#include "BaseArithmetic.h"
#include "ClipsExtensions.h"
#include "functional.h"
#include "ExternalAddressWrapper.h"

#include <cstdint>
#include <climits>
#include <sstream>
#include <memory>
#include <map>
#include <iostream>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/algorithm/string/trim.hpp>

extern "C" {
#include "clips.h"
}

namespace syn {

void
BinaryNot(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue val;
	if (!UDFFirstArgument(context, INTEGER_BIT, &val)) {
		setBoolean(env, ret, false);
		return;
	}
	ret->integerValue = CreateInteger(env, ~(CVCoerceToInteger(&val)));
}

void
BinaryOr(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue a, b;
	if (!UDFFirstArgument(context, INTEGER_BIT, &a)) {
		setBoolean(env, ret, false);
		return;
	} else if (!UDFNextArgument(context, INTEGER_BIT, &b)) {
		setBoolean(env, ret, false);
		return;
	}

	ret->integerValue = CreateInteger(env, CVCoerceToInteger(&a) | CVCoerceToInteger(&b));
}

void
BinaryAnd(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue a, b;
	if (!UDFFirstArgument(context, INTEGER_BIT, &a)) {
		setBoolean(env, ret, false);
		return;
	} else if (!UDFNextArgument(context, INTEGER_BIT, &b)) {
		setBoolean(env, ret, false);
		return;
	}
	ret->integerValue = CreateInteger(env, CVCoerceToInteger(&a) & CVCoerceToInteger(&b));
}

void
BinaryXor(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue a, b;
	if (!UDFFirstArgument(context, INTEGER_BIT, &a)) {
		setBoolean(env, ret, false);
		return;
	} else if (!UDFNextArgument(context, INTEGER_BIT, &b)) {
		setBoolean(env, ret, false);
		return;
	}
	ret->integerValue = CreateInteger(env, CVCoerceToInteger(&a) ^ CVCoerceToInteger(&b));
}

void
BinaryNor(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue a, b;
	if (!UDFFirstArgument(context, INTEGER_BIT, &a)) {
		setBoolean(env, ret, false);
		return;
	} else if (!UDFNextArgument(context, INTEGER_BIT, &b)) {
		setBoolean(env, ret, false);
		return;
	}

	ret->integerValue = CreateInteger(env,  ~(CVCoerceToInteger(&a) | CVCoerceToInteger(&b)));
}

void
BinaryNand(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue a, b;
	if (!UDFFirstArgument(context, INTEGER_BIT, &a)) {
		setBoolean(env, ret, false);
		return;
	} else if (!UDFNextArgument(context, INTEGER_BIT, &b)) {
		setBoolean(env, ret, false);
		return;
	}

	ret->integerValue = CreateInteger(env,  ~(CVCoerceToInteger(&a) & CVCoerceToInteger(&b)));
}

void
ShiftLeft(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue value, by;
	if (!UDFFirstArgument(context, INTEGER_BIT, &value)) {
		setBoolean(env, ret, false);
		return;
	} else if (!UDFNextArgument(context, INTEGER_BIT, &by)) {
		setBoolean(env, ret, false);
		return;
	}

	ret->integerValue = CreateInteger(env,  (CVCoerceToInteger(&value) << CVCoerceToInteger(&by)));
}

void
ShiftRight(Environment* env, UDFContext* context, UDFValue* ret) {
	UDFValue value, by;
	if (!UDFFirstArgument(context, INTEGER_BIT, &value)) {
		setBoolean(env, ret, false);
		return;
	} else if (!UDFNextArgument(context, INTEGER_BIT, &by)) {
		setBoolean(env, ret, false);
		return;
	}

	ret->integerValue = CreateInteger(env,  (CVCoerceToInteger(&value) >> CVCoerceToInteger(&by)));
}
	void CLIPS_errorMessageGeneric(Environment* env, UDFContext* context, UDFValue* ret, const char* msg) noexcept {
		UDFInvalidArgumentMessage(context, msg);
		setBoolean(env, ret, false);
	}
	void CLIPS_errorMessageGeneric(Environment* env, UDFContext* context, UDFValue* ret, const std::string& msg) noexcept {
		CLIPS_errorMessageGeneric(env, context, ret, msg.c_str());
	}
	void CLIPS_errorOverflowedNumber(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
		CLIPS_errorMessageGeneric(env, context, ret, "number is too large and overflowed");
	}
	void CLIPS_translateBitmask(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
		UDFValue value;
		if (!UDFFirstArgument(context, LEXEME_BITS, &value)) {
			setBoolean(env, ret, false);
		} else {
			std::string str(value.lexemeValue->contents);
			if (boost::starts_with(str, "0m")) {
				str.at(1) = '0';
				auto tmp = strtoul(str.c_str(), NULL, 2);
				if (tmp == ULONG_MAX && errno == ERANGE) {
					CLIPS_errorOverflowedNumber(env, context, ret);
				} else {
					if (tmp > 0xFF) {
						CLIPS_errorMessageGeneric(env, context, ret, "provided number is larger than 8-bits!");
					} else {
						ret->integerValue = CreateInteger(env,  static_cast<int64_t>(static_cast<byte>(tmp)));
					}
				}
			} else {
				CLIPS_errorMessageGeneric(env, context, ret, "Bitmask must start with 0m");
			}
		}
	}
	void CLIPS_errorNumberLargerThan64Bits(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
		CLIPS_errorMessageGeneric(env, context, ret, "provided number is larger than 64-bits!");
	}

	void CLIPS_expandBit(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
		UDFValue number;
		if (!UDFFirstArgument(context, NUMBER_BITS, &number)) {
			setBoolean(env, ret, false);
			return;
		}
		auto value = CVCoerceToInteger(&number);
		ret->integerValue = CreateInteger(env,  int64_t(expandBit(value != 0)));
	}

	void CLIPS_basePrintAddress(Environment* env, const char* logicalName, void* theValue, const char* func, const char* majorType) {
		std::stringstream ss;
		auto* ptr = static_cast<CLIPSExternalAddress*>(theValue);
		ss << "<" << majorType << "-" << func << "-" << std::hex << ((ptr) ? ptr->contents : theValue) << ">";
		auto str = ss.str();
        clips::printRouter(env, logicalName, str);
	}
	//void CLIPS_basePrintAddress_Pointer(Environment* env, const char* logicalName, void* theValue, const char* func) noexcept {
	//	CLIPS_basePrintAddress(env, logicalName, theValue, func, "Pointer");
	//}
	void CLIPS_decodeBits(Environment* env, UDFContext* context, UDFValue* ret) {
		UDFValue value, mask, shift;
		if (!UDFFirstArgument(context, NUMBER_BITS, &value)) {
			setBoolean(env, ret, false);
		} else if (!UDFNextArgument(context, NUMBER_BITS, &mask)) {
			setBoolean(env, ret, false);
		} else if (!UDFNextArgument(context, NUMBER_BITS, &shift)) {
			setBoolean(env, ret, false);
		} else {
			ret->integerValue = CreateInteger(env,  decodeBits<int64_t, int64_t>(CVCoerceToInteger(&value), CVCoerceToInteger(&mask), CVCoerceToInteger(&shift)));
		}
	}
	void CLIPS_encodeBits(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
		UDFValue input, value, mask, shift;
		if (!UDFFirstArgument(context, NUMBER_BITS, &input)) {
			setBoolean(env, ret, false);
		} else if (!UDFNextArgument(context, NUMBER_BITS, &value)) {
			setBoolean(env, ret, false);
		} else if (!UDFNextArgument(context, NUMBER_BITS, &mask)) {
			setBoolean(env, ret, false);
		} else if (!UDFNextArgument(context, NUMBER_BITS, &shift)) {
			setBoolean(env, ret, false);
		} else {
			auto i = CVCoerceToInteger(&input);
			auto v = CVCoerceToInteger(&value);
			auto m = CVCoerceToInteger(&mask);
			auto s = CVCoerceToInteger(&shift);
			ret->integerValue = CreateInteger(env,  encodeBits<int64_t, int64_t>(i, v, m, s));
		}
	}
    template<typename I, typename O, I mask, int index>
    O performDecode(I input) noexcept {
        return syn::decodeBits<I, O, mask << (8 * index), (8 * index)>(input);
    }
	void CLIPS_breakApartNumber(Environment* env, UDFContext* context, UDFValue* ret) {
		UDFValue number;
		if (!UDFFirstArgument(context, NUMBER_BITS, &number)) {
			setBoolean(env, ret, false);
            return;
		}
        auto integer = CVCoerceToInteger(&number);
        constexpr int integerWidth = byteCount<decltype(integer)>;
        constexpr auto baseMask = static_cast<decltype(integer)>(0xFF);
		maya::MultifieldBuilder mb(env);
        if (integerWidth == 8) {
			mb.append(performDecode<int64_t, int64_t, baseMask, 0>(integer));
			mb.append(performDecode<int64_t, int64_t, baseMask, 1>(integer));
			mb.append(performDecode<int64_t, int64_t, baseMask, 2>(integer));
			mb.append(performDecode<int64_t, int64_t, baseMask, 3>(integer));
			mb.append(performDecode<int64_t, int64_t, baseMask, 4>(integer));
			mb.append(performDecode<int64_t, int64_t, baseMask, 5>(integer));
			mb.append(performDecode<int64_t, int64_t, baseMask, 6>(integer));
			mb.append(performDecode<int64_t, int64_t, baseMask, 7>(integer));
        } else {
            for (int i = 0; i < integerWidth; ++i) {
				mb.append(int64_t(syn::decodeBits<int64_t, int64_t>(integer, baseMask << (8 * i), (8 * i))));
            }
        }
		ret->multifieldValue = mb.create();
	}

	bool errorMessage(Environment* env, const std::string& idClass, int idIndex, const std::string& msgPrefix, const std::string& msg) noexcept {
		PrintErrorID(env, idClass.c_str(), idIndex, false);
        clips::printRouter(env, STDERR, msgPrefix);
        clips::printRouter(env, STDERR, msg);
        clips::printLine(env, STDERR);
		SetEvaluationError(env, true);
		return false;
	}
    template<bool shiftLeft>
    void CLIPS_circularShiftBase(Environment* env, UDFContext* context, UDFValue* ret) {
        UDFValue a, b;
        if (!UDFFirstArgument(context, INTEGER_BIT, &a)) {
            setBoolean(env, ret, false);
        } else if (!UDFNextArgument(context, INTEGER_BIT, &b)) {
            setBoolean(env, ret, false);
        } else {
            auto firstValue = CVCoerceToInteger(&a);
            auto secondValue = CVCoerceToInteger(&b);
            auto result = shiftLeft ?
                circularShiftLeft<int64_t>(firstValue, secondValue) :
                circularShiftRight<int64_t>(firstValue, secondValue);
            ret->integerValue = CreateInteger(env,  result);
        }
    }
    void CLIPS_circularShiftLeft(Environment* env, UDFContext* context, UDFValue* ret) {
        CLIPS_circularShiftBase<true>(env, context, ret);
    }
    void CLIPS_circularShiftRight(Environment* env, UDFContext* context, UDFValue* ret) {
        CLIPS_circularShiftBase<false>(env, context, ret);
    }
    void CLIPS_onesComplement(Environment* env, UDFContext* context, UDFValue* ret) {
        UDFValue val;
        if (!UDFFirstArgument(context, INTEGER_BIT, &val)) {
            setBoolean(env, ret, false);
        } else {
            ret->integerValue = CreateInteger(env,  onesComplement(CVCoerceToInteger(&val)));
        }
    }
    void CLIPS_twosComplement(Environment* env, UDFContext* context, UDFValue* ret) {
        UDFValue val;
        if (!UDFFirstArgument(context, INTEGER_BIT, &val)) {
            setBoolean(env, ret, false);
        } else {
            ret->integerValue = CreateInteger(env,  twosComplement(CVCoerceToInteger(&val)));
        }
    }
    void CLIPS_multiplyAdd(Environment* env, UDFContext* context, UDFValue* ret) {
        UDFValue a, b, c;
        if (!UDFFirstArgument(context, INTEGER_BIT, &a)) {
            setBoolean(env, ret, false);
        } else if (!UDFNextArgument(context, INTEGER_BIT, &b)) {
            setBoolean(env, ret, false);
        } else if (!UDFNextArgument(context, INTEGER_BIT, &c)) {
            setBoolean(env, ret, false);
        } else {
            ret->integerValue = CreateInteger(env,  multiplyAdd(CVCoerceToInteger(&a), CVCoerceToInteger(&b), CVCoerceToInteger(&c)));
        }
    }
    void CLIPS_getEndianness(Environment* env, UDFContext* context, UDFValue* ret) {
        static bool init = true;
        static std::string storage;
        if (init) {
            init = false;
            if (syn::isBigEndian()) {
                storage = "big";
            } else if (syn::isLittleEndian()) {
                storage = "little";
            } else {
                storage = "unknown";
            }
        }
        // only compute this once!
		ret->lexemeValue = CreateSymbol(env, storage.c_str());
    }

    template<bool upperHalf>
    void upperLowerHalfManip(Environment* env, UDFContext* context, UDFValue* ret) {
        UDFValue num;
        if (!UDFFirstArgument(context, INTEGER_BIT, &num)) {
            setBoolean(env, ret, false);
        } else {
            int64_t n = num.integerValue->contents;
            if (upperHalf) {
				ret->integerValue = CreateInteger(env, syn::decodeBits<int64_t, int64_t, static_cast<int64_t>(0xFFFFFFFF00000000), getShiftCount<int64_t>()>(n));
            } else {
				ret->integerValue = CreateInteger(env, decodeBits<int64_t, int64_t, 0x00000000FFFFFFFF, 0>(n));
            }
        }
    }

    void CLIPS_getUpperHalf(Environment* env, UDFContext* c, UDFValue* ret) { upperLowerHalfManip<true>(env, c, ret); }
    void CLIPS_getLowerHalf(Environment* env, UDFContext* c, UDFValue* ret) { upperLowerHalfManip<false>(env, c, ret); }

	void CLIPS_translateBinary(Environment* env, UDFContext* context, UDFValue* ret) noexcept;
	void CLIPS_translateHex(Environment* env, UDFContext* context, UDFValue* ret) noexcept;
	void CLIPS_translateOctal(Environment* env, UDFContext* context, UDFValue* ret) noexcept;

	void installExtensions(Environment* theEnv) {

		AddUDF(theEnv, "bitmask->int", "l", 1, 1, "sy", CLIPS_translateBitmask, "CLIPS_translateBitmask", nullptr);
		AddUDF(theEnv, "expand-bit", "l", 1, 1,  nullptr,  CLIPS_expandBit, "CLIPS_expandBit",  nullptr);
		AddUDF(theEnv, "decode-bits", "l", 3, 3, "l;l;l",   CLIPS_decodeBits, "CLIPS_decodeBits",nullptr);
		AddUDF(theEnv, "encode-bits", "l", 4, 4, "l;l;l;l", CLIPS_encodeBits, "CLIPS_encodeBits",nullptr);
		AddUDF(theEnv, "break-apart-number", "m",    1, 1, "l",   CLIPS_breakApartNumber, "CLIPS_breakApartNumber",    nullptr);
        AddUDF(theEnv, "circular-shift-right", "l",  2, 2, "l;l", CLIPS_circularShiftRight, "CLIPS_circularShiftRight",nullptr);
        AddUDF(theEnv, "circular-shift-left", "l",   2, 2, "l;l", CLIPS_circularShiftLeft, "CLIPS_circularShiftLeft",  nullptr);
        AddUDF(theEnv, "ones-complement", "l",  1, 1, "l",     CLIPS_onesComplement, "CLIPS_onesComplement",nullptr);
        AddUDF(theEnv, "twos-complement", "l",  1, 1, "l",     CLIPS_twosComplement, "CLIPS_twosComplement",nullptr);
        AddUDF(theEnv, "multiply-add", "l",     3, 3, "l;l;l", CLIPS_multiplyAdd, "CLIPS_multiplyAdd",      nullptr);
        AddUDF(theEnv, "get-endian", "sy",      0, 0, nullptr, CLIPS_getEndianness, "CLIPS_getEndianness",  nullptr);
        AddUDF(theEnv, "upper-half", "l",  1, 1, "l", CLIPS_getUpperHalf, "CLIPS_getUpperHalf",nullptr);
        AddUDF(theEnv, "lower-half", "l",  1, 1, "l", CLIPS_getLowerHalf, "CLIPS_getLowerHalf",nullptr);
		AddUDF(theEnv, "binary-not", "l",  1, 1, "l",   BinaryNot, "BinaryNot",   nullptr);
		AddUDF(theEnv, "binary-and", "l",  2, 2, "l;l", BinaryAnd, "BinaryAnd",   nullptr);
		AddUDF(theEnv, "binary-or", "l",   2, 2, "l;l", BinaryOr, "BinaryOr",     nullptr);
		AddUDF(theEnv, "binary-xor", "l",  2, 2, "l;l", BinaryXor, "BinaryXor",   nullptr);
		AddUDF(theEnv, "binary-nand", "l", 2, 2, "l;l", BinaryNand, "BinaryNand", nullptr);
		AddUDF(theEnv, "binary-nor", "l",  2, 2, "l;l", BinaryNor, "BinaryNor",   nullptr);
		AddUDF(theEnv, "left-shift", "l",   2, 2, "l;l",ShiftLeft, "ShiftLeft",    nullptr);
		AddUDF(theEnv, "right-shift", "l", 2, 2, "l;l", ShiftRight, "ShiftRight", nullptr);
		AddUDF(theEnv, "binary->int", "l", 1, 1, "sy", CLIPS_translateBinary, "CLIPS_translateBinary", nullptr);
		AddUDF(theEnv, "hex->int", "l", 1, 1, "sy", CLIPS_translateHex, "CLIPS_translateHex", nullptr);
		AddUDF(theEnv, "oct->int", "l", 1, 1, "sy", CLIPS_translateOctal, "CLIPS_translateOctal", nullptr);
	}

    void buildFunctionString(std::ostream& stream, const std::string& action, const std::string& name) noexcept {
        stream << "Function " << action << " (" << name << ")";
    }
    void buildFunctionErrorString(std::ostream& stream, const std::string& action, const std::string& name) noexcept {
        stream << "Function ";
        buildFunctionString(stream, action, name);
	}

void CLIPS_translateNumberBase(Environment* env, UDFContext* context, UDFValue* ret, const std::string& prefix, int base, const std::string& badPrefix, bool zeroPositionOne = false) noexcept {
	constexpr uint64_t maximumIntegerValue = 0xFFFFFFFFFFFFFFFF;
	UDFValue value;
	if (!UDFFirstArgument(context, LEXEME_BITS, &value)) {
		setBoolean(context, ret, false);
	}
	std::string str(getLexeme(value));
	if (boost::starts_with(str, prefix)) {
		if (zeroPositionOne) {
			str.at(1) = '0';
		}
		auto tmp = strtoull(str.c_str(), nullptr, base);
		if (tmp == ULLONG_MAX && errno == ERANGE) {
			CLIPS_errorOverflowedNumber(env, context, ret);
		} else {
			if (tmp > maximumIntegerValue) {
				CLIPS_errorNumberLargerThan64Bits(env, context, ret);
			} else {
				setInteger(env, ret, tmp);
			}
		}
	} else {
		CLIPS_errorMessageGeneric(env, context, ret, badPrefix);
	}
}
void CLIPS_translateBinary(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
	CLIPS_translateNumberBase(env, context, ret, "0b", 2, "Binary must start with 0b", true);
}

void CLIPS_translateHex(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
	CLIPS_translateNumberBase(env, context, ret, "0x", 16, "Hex must start with 0x");
}

void CLIPS_translateOctal(Environment* env, UDFContext* context, UDFValue* ret) noexcept {
	CLIPS_translateNumberBase(env, context, ret, "0q", 8, "Octal must start with 0q", true);
}

}


// Shasta.
#include "Assembler.hpp"
#include "MarkerConnectivityGraph.hpp"
using namespace shasta;

// Boost libraries.
#include <boost/graph/iteration_macros.hpp>

// Standard library.
#include <map>
#include <queue>

// Create the marker connectivity graph starting with a given marker.
void Assembler::createMarkerConnectivityGraph(
    OrientedReadId orientedReadId,
    uint32_t ordinal,
    bool useReadGraphAlignmentsOnly,
    MarkerConnectivityGraph& graph) const
{
    MarkerConnectivityGraphVertexMap vertexMap;
    createMarkerConnectivityGraph(orientedReadId, ordinal, useReadGraphAlignmentsOnly,
        graph, vertexMap);

}

void Assembler::createMarkerConnectivityGraph(
    OrientedReadId orientedReadId,
    uint32_t ordinal,
    bool useReadGraphAlignmentsOnly,
    MarkerConnectivityGraph& graph,
    MarkerConnectivityGraphVertexMap& vertexMap) const
{

    using vertex_descriptor = MarkerConnectivityGraph::vertex_descriptor;
    vertexMap.clear();

    // Initialize a BFS in the space of aligned markers.
    const vertex_descriptor v = add_vertex(MarkerDescriptor(orientedReadId, ordinal), graph);
    vertexMap[MarkerDescriptor(orientedReadId, ordinal)] = v;
    std::queue<vertex_descriptor> q;
    q.push(v);

    // BFS loop.
    vector<MarkerDescriptor> alignedMarkers;
    while(not q.empty()) {

        // Dequeue a vertex.
        const vertex_descriptor v0 = q.front();
        q.pop();
        const MarkerDescriptor markerDescriptor0 =graph[v0];
        SHASTA_ASSERT(vertexMap[markerDescriptor0] == v0);

        // Find aligned markers for this vertex.
        findAlignedMarkers(markerDescriptor0.first, markerDescriptor0.second,
            useReadGraphAlignmentsOnly, alignedMarkers);

        for(const MarkerDescriptor& markerDescriptor1: alignedMarkers) {

            // If there is no vertex for markerDescriptor1, add it and enqueue it.
            auto it1 = vertexMap.find(markerDescriptor1);
            if(it1 == vertexMap.end()) {
                const vertex_descriptor v1 = add_vertex(markerDescriptor1, graph);
                tie(it1, ignore) = vertexMap.insert(make_pair(markerDescriptor1, v1));
                q.push(v1);
            }
            const vertex_descriptor v1 = it1->second;

            // If there is no edge between v0 and v1, create one.
            bool edgeExists;
            tie(ignore, edgeExists) = edge(v0, v1, graph);
            if(not edgeExists) {
                add_edge(v0, v1, graph);
            }
        }
    }

}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#include <deque>
#include <vector>

#include <geometry_test_common.hpp>

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

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

#include <test_common/test_point.hpp>

#define BOOST_GEOMETRY_TEST_RING

#if defined(BOOST_GEOMETRY_TEST_RING)

    #include <boost/geometry/geometries/register/ring.hpp>
    #include <boost/geometry/geometries/concepts/ring_concept.hpp>

    BOOST_GEOMETRY_REGISTER_RING_TEMPLATED(std::vector)
    BOOST_GEOMETRY_REGISTER_RING_TEMPLATED(std::deque)

#elif defined(BOOST_GEOMETRY_TEST_MULTI_POINT)

    #include <boost/geometry/multi/geometries/register/multi_point.hpp>
    #include <boost/geometry/multi/geometries/concepts/multi_point_concept.hpp>

    BOOST_GEOMETRY_REGISTER_MULTI_POINT_TEMPLATED(std::vector)
    BOOST_GEOMETRY_REGISTER_MULTI_POINT_TEMPLATED(std::deque)

#else

    #include <boost/geometry/geometries/register/linestring.hpp>
    #include <boost/geometry/geometries/concepts/linestring_concept.hpp>

    BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::vector)
    BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::deque)

#endif

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

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

template <typename G>
void test_geometry(G const& geometry, std::size_t expected_size = 0)
{
#if defined(BOOST_GEOMETRY_TEST_RING)
    BOOST_CONCEPT_ASSERT( (bg::concept::ConstRing<G>) );
#elif defined(BOOST_GEOMETRY_TEST_MULTI_POINT)
    BOOST_CONCEPT_ASSERT( (bg::concept::ConstMultiPoint<G>) );
#else
    BOOST_CONCEPT_ASSERT( (bg::concept::ConstLinestring<G>) );
#endif

    typedef typename bg::point_type<G>::type P;
    typedef typename bg::coordinate_type<P>::type C;

    // Check range-like behaviour
    BOOST_CHECK_EQUAL(boost::size(geometry), expected_size);

}

template <typename P>
void test_all()
{
    test_geometry(std::vector<P>());
    test_geometry(std::deque<P>());
    //test_geometry(std::list<P>());

    /***
    double vertices[][3] = {
        {-1, -1,  1}, {1, -1,  1}, {1, 1,  1}, {-1, 1,  1},
        {-1, -1, -1}, {1, -1, -1}, {1, 1, -1}, {-1, 1, -1}
        };

    test_geometry(vertices, 8);
    ***/
}

int test_main(int, char* [])
{
    test_all<test::test_point>();
    test_all<boost::tuple<float, float> >();
    test_all<bg::model::point<int, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 2, bg::cs::cartesian> >();
    test_all<bg::model::point<long double, 2, bg::cs::cartesian> >();

    test_all<boost::tuple<float, float, float> >();
    test_all<bg::model::point<double, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<long double, 3, bg::cs::cartesian> >();

    test_all<boost::tuple<float, float, float, float, float> >();

    return 0;
}


// -----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax make_matrix
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mitrax/make_matrix.hpp>

#include <complex>
#include <iostream>

using namespace mitrax;
using namespace mitrax::literals;

BOOST_AUTO_TEST_SUITE(suite_matrix_access)

using types = boost::mpl::list< int, double, std::complex< float > >;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_1x1, T, types){
	constexpr auto m1 = make_matrix(1_CS, 1_RS, {{T(5)}});
	auto m2 = make_matrix(1_CS, 1_RS, {{T(5)}});

	T a = m1;
	T b = m2;

	BOOST_TEST(a == T(5));
	BOOST_TEST(b == T(5));

	static_cast< T& >(m2) = T(7);
	T c = m2;

	BOOST_TEST(c == T(7));
}

BOOST_AUTO_TEST_SUITE_END()


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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.
 */

/**
 * @file aig_from_bdd.hpp
 *
 * @brief Create AIGs from BDDs
 *
 * @author Mathias Soeken
 * @since  2.0
 */

#include "aig_from_bdd.hpp"

#include <boost/format.hpp>

#include <cuddInt.h>

#include <classical/utils/aig_utils.hpp>

namespace cirkit
{

aig_function aig_from_bdd_rec( aig_graph& aig, DdManager* dd, DdNode* node )
{
  const auto& info = aig_info( aig );

  auto   is_complement = Cudd_IsComplement( node );
  auto * r = Cudd_Regular( node );

  aig_function f;

  if ( Cudd_IsConstant( r ) )
  {
    f = aig_get_constant( aig, r == DD_ONE( dd ) );
  }
  else
  {
    auto index = Cudd_NodeReadIndex( r );

    auto f_true  = aig_from_bdd_rec( aig, dd, cuddT( r ) );
    auto f_false = aig_from_bdd_rec( aig, dd, cuddE( r ) );

    f = aig_create_ite( aig, {info.inputs[index], false}, f_true, f_false );
  }

  return is_complement ? !f : f;
}

aig_function aig_from_bdd( aig_graph& aig, DdManager* dd, DdNode* node )
{
  const auto& info = aig_info( aig );

  auto n = Cudd_ReadSize( dd );
  auto num_pis = info.inputs.size();

  for ( auto i = num_pis; static_cast<int>( i ) < n; ++i )
  {
    aig_create_pi( aig, boost::str( boost::format( "x%d" ) % i ) );
  }

  return aig_from_bdd_rec( aig, dd, node );
}

aig_function aig_from_bdd( aig_graph& aig, const BDD& node )
{
  return aig_from_bdd( aig, node.manager(), node.getNode() );
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include <boost/mpl/assert.hpp>
#include <boost/dispatch/meta/make_floating.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::is_same;
using boost::mpl::_;
using boost::dispatch::meta::make_floating;

template<typename T> struct test {};

int main()
{
  BOOST_MPL_ASSERT(( is_same<float,       make_floating<4>::type>           ));
  BOOST_MPL_ASSERT(( is_same<double,      make_floating<8>::type>           ));
  BOOST_MPL_ASSERT(( is_same<test<float>, make_floating<4, test<_> >::type> ));
}


/** This is free and unencumbered software released into the public domain.

The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */

#include <cmath>

#include <sstream>
#include <iomanip>
#include <boost/foreach.hpp>

#include "IString.h"
#include "LroWideAngleCameraDistortionMap.h"

using namespace std;

namespace Isis {
  /** Camera distortion map constructor
   *
   * Create a camera distortion map.  This class maps between distorted
   * and undistorted focal plane x/y's.  The default mapping is the
   * identity, that is, the focal plane x/y and undistorted focal plane
   * x/y will be identical.
   *
   * @param parent        the parent camera that will use this distortion map
   * @param zDirection    the direction of the focal plane Z-axis
   *                      (either 1 or -1)
   *
   */
  LroWideAngleCameraDistortionMap::LroWideAngleCameraDistortionMap(Camera *parent,
                                                                   int naifIkCode) :
                                                                   CameraDistortionMap(parent) {
    SetDistortion(naifIkCode);
  }

/**
 * @brief Add an additional set of parameters for a given LROC/WAC filter
 *
 * This method will read the parameters for LROC/WAC filter as indicated by the
 * IK code provided. It will create a vector of these parameters and append them
 * to the band list.
 *
 * The filters added should correspond directly to the order in which the
 * filters are physically stored in the ISIS cube (or the virtually selected
 * bands).
 *
 * @author 2013-03-07 Kris Becker
 *
 * @param naifIkCode NAIF IK code for the desired filter to add.
 */
  void LroWideAngleCameraDistortionMap::addFilter(int naifIkCode) {
    QString odkkey = "INS" + QString::number(naifIkCode) + "_OD_K";

    std::vector<double> v_odk;
    for(int i = 0; i < 3; i++) {
      v_odk.push_back(p_camera->getDouble(odkkey, i));
    }

    m_odkFilters.push_back(v_odk);
  }

/**
 * @brief Implements band-dependant distortion parameters
 *
 * This method should be used to switch to another band's set of distortion
 * parameters.  See the addFilter() method to add additional band parameters to
 * this object. Note that the band number should correspond with the same order
 * as they were added in the addFilter() method.
 *
 * @author 2013-03-07 Kris Becker
 *
 * @param vband  Band number to select.  Range is 1 to Bands.
 */
  void LroWideAngleCameraDistortionMap::setBand(int vband) {
    if ( (vband <= 0) || (vband > m_odkFilters.size()) ) {
      QString mess = "Invalid band (" + QString::number(vband) + " requested " +
                     " Must be <= " + QString::number(m_odkFilters.size());
      throw IException(IException::Programmer, mess, _FILEINFO_);
    }

    //  Install new parameters
    p_odk = m_odkFilters[vband-1];

    return;

  }

  /** Compute undistorted focal plane x/y
   *
   * Compute undistorted focal plane x/y given a distorted focal plane x/y.
   *
   * @param dx distorted focal plane x in millimeters
   * @param dy distorted focal plane y in millimeters
   *
   * @return if the conversion was successful
   * @see SetDistortion
   * @todo Generalize polynomial equation
   */

  bool LroWideAngleCameraDistortionMap::SetFocalPlane(const double dx, const double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;

    double dk1 = p_odk[0];
    double dk2 = p_odk[1];
    double dk3 = p_odk[2];

    double rr = dx * dx + dy * dy;

    double dr = 1.0 + dk1 * rr + dk2 * rr * rr + dk3 * rr * rr * rr;

    // This was in here when the model used to be dx/dr so that the model
    // would never divide by zero.
    if ( dr == 0.0 ) return false;

    // Compute the undistorted positions
    p_undistortedFocalPlaneX = dx * dr;
    p_undistortedFocalPlaneY = dy * dr;

    return true;
  }

  /** Compute distorted focal plane x/y
   *
   * Compute distorted focal plane x/y given an undistorted focal plane x/y.
   *
   * @param ux undistorted focal plane x in millimeters
   * @param uy undistorted focal plane y in millimeters
   *
   * @return if the conversion was successful
   * @see SetDistortion
   * @todo Generalize polynomial equation
   * @todo Figure out a better solution for divergence condition
   */

  bool LroWideAngleCameraDistortionMap::SetUndistortedFocalPlane(const double ux, const double uy) {
    // image coordinates prior to introducing distortion
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;

    double xt = ux;
    double yt = uy;

    double rr, dr;
    double xdistorted, ydistorted;
    double xprevious, yprevious;

    xprevious = 1000000.0;
    yprevious = 1000000.0;
    // Changed this line to 10^-6... it allows the outer pixels to be found
    // when mapping back to the sensor
    double tolerance = 1.0e-6;

    bool bConverged = false;

    double dk1 = p_odk[0];
    double dk2 = p_odk[1];
    double dk3 = p_odk[2];

    // iterating to introduce distortion...
    // we stop when the difference between distorted coordinates
    // in successive iterations is at or below the given tolerance
    for(int i = 0; i < 50; i++) {
      rr = xt * xt + yt * yt;
      // dr is the radial distortion contribution
      dr = 1.0 + dk1 * rr + dk2 * rr * rr + dk3 * rr * rr * rr;

      // introducing distortion
      xt = ux / dr;
      yt = uy / dr;

      // distorted point
      xdistorted = xt;
      ydistorted = yt;

      // check for convergence
      if((fabs(xt - xprevious) <= tolerance) && (fabs(yt - yprevious) <= tolerance)) {
        bConverged = true;
        break;
      }

      xprevious = xt;
      yprevious = yt;
    }

    if(bConverged) {
      p_focalPlaneX = xdistorted;
      p_focalPlaneY = ydistorted;
    }

    return bConverged;
  }
}


//
// Created by pengpeng on 11/4/20.
//

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <geometry_msgs/PoseStamped.h>
#include <math.h>
#include <Eigen/Eigen>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>

using namespace Eigen;

Vector3d current_p;
Quaterniond current_att;
mavros_msgs::State current_state;
ros::Publisher pva_pub;

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{

    current_p << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;

    //ROS_INFO("X:%f Y:%f  Z:%f",-current_p(0),current_p(1),current_p(2));
}

void stateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;

}

void setPVA(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw=0.0)
{
    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    pva_setpoint.positions.push_back(p(0)); //x
    pva_setpoint.positions.push_back(p(1)); //y
    pva_setpoint.positions.push_back(p(2)); //z
    pva_setpoint.positions.push_back(yaw);

    pva_setpoint.velocities.push_back(v(0));
    pva_setpoint.velocities.push_back(v(1));
    pva_setpoint.velocities.push_back(v(2));

    pva_setpoint.accelerations.push_back(a(0));
    pva_setpoint.accelerations.push_back(a(1));
    pva_setpoint.accelerations.push_back(a(2));

    pva_pub.publish(pva_setpoint);

//    ROS_INFO_THROTTLE(1.0, "P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
//    ROS_INFO_THROTTLE(1.0, "V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
//    ROS_INFO_THROTTLE(1.0, "A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);
//
//    ROS_INFO("P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
//    ROS_INFO("V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
//    ROS_INFO("A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);
}

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

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 1, stateCallback);
    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 1, positionCallback);

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1);

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 1);

    const int LOOPRATE = 40;
    ros::Rate loop_rate(LOOPRATE);

    double take_off_height = 1.5;
    double take_off_acc = 0.1;
    double take_off_time=sqrt(take_off_height/take_off_acc)*2.0;
    double delt_t = 1.0 / LOOPRATE;
    double take_off_send_times = take_off_time / delt_t;
    int counter = 0;
    int take_off_init=1;
    double into_offb=0;
    Vector3d take_off_position;
    ros::Time last_time=ros::Time::now();
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        loop_rate.sleep();
    }
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();
    geometry_msgs::PoseStamped pose;

    double yaw_set = 3.1415926;
    int mode=0;
    double z_sp=0;
    double vz_sp=0;
    ROS_INFO("Arm and ta1111keoff");

    while(ros::ok())
    {

        if(current_state.mode != "OFFBOARD" || !current_state.armed)
        {
            ros::spinOnce();
            loop_rate.sleep();
            take_off_position=current_p;
            into_offb=0;
/*            pose.header.stamp = ros::Time::now();
            pose.pose.position.x = 0;
            pose.pose.position.y = 0;
            pose.pose.position.z =0;
            double theta=0/180.0*3.14;
            pose.pose.orientation.w=cos(theta/2);
            pose.pose.orientation.x=0.0;
            pose.pose.orientation.y=0;
            pose.pose.orientation.z=sin(theta/2);
            local_pos_pub.publish(pose);*/
            setPVA(current_p,Vector3d::Zero(),Vector3d::Zero(),yaw_set);
            continue;
        }
        Vector3d p_sp(take_off_position(0), take_off_position(1), take_off_position(2));
        Vector3d v_sp(0, 0, 0);
        setPVA(p_sp, v_sp, Vector3d::Zero(), yaw_set);
        ros::spinOnce();
        loop_rate.sleep();

/*         Vector3d recorded_position = current_p;
         while(ros::ok()){
             if(current_state.mode != "OFFBOARD" || !current_state.armed){
                 setPVA(current_p, Vector3d::Zero(), Vector3d::Zero(), yaw_set);
                 recorded_position = current_p;
             }else{
                 setPVA(recorded_position, Vector3d::Zero(), Vector3d::Zero(), yaw_set);
             }

             loop_rate.sleep();
             ros::spinOnce();
         }*/

    }
    return 0;
}


/*
 * EigenvalueCovariance.cpp
 *
 *  Created on: May 6, 2015
 *      Author: dbazazian
 */

     #include <iostream>
     #include <Eigen/Eigenvalues>
     #include <Eigen/Dense>
     #include <vector>
     #include <math.h>
     #include <cmath>
     #include <fstream>
     #include <string>
     #include <vector>
     #include <pcl/io/io.h>
     #include <pcl/io/pcd_io.h>
     #include <pcl/point_types.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/common/common_headers.h>
     #include <pcl/features/integral_image_normal.h>
     #include <pcl/features/normal_3d.h>
     #include <pcl/visualization/cloud_viewer.h>
     #include <pcl/filters/passthrough.h>
     #include <pcl/ModelCoefficients.h>
     #include <pcl/filters/project_inliers.h>
     #include <pcl/features/shot_omp.h>
     #include "pcl/features/fpfh.h"
     #include <pcl/io/ply_io.h>

using namespace std;
using namespace Eigen;

int
main (int argc, char*argv[])
{
	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
		pcl::PointCloud<pcl::PointXYZRGBA>::Ptr RelevantElements(new pcl::PointCloud<pcl::PointXYZRGBA>);
		pcl::PointCloud<pcl::PointXYZRGBA>::Ptr SelectedElements(new pcl::PointCloud<pcl::PointXYZRGBA>);

	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TwoPlane45.pcd", *cloud);
	 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/CubeSharpEdge.pcd", *cloud);
	  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/OnePlane.pcd", *cloud);
	      // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/bunny.pcd",*cloud);
	     //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Statue.pcd", *cloud);
	  //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/dragon.pcd", *cloud);

	  //   pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/trim-starC.pcd", *cloud);
	      //	pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/VaseC.pcd", *cloud);
	       //  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/twirlC.pcd", *cloud);
	      	//  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/fandiskC.pcd", *cloud);
	      // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AimAtShape/sharp_sphereC.pcd", *cloud);
		// pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/CubeFractal2.pcd", *cloud);
		//  pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AddingNoise/Frac2Guass12Noise.pcd", *cloud);
		  // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/IntersectionThreePlanes.pcd", *cloud);
		// pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/SpherMultiple.pcd", *cloud);
		 // pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/Tetrahedron.pcd", *cloud);
		 //pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/AddingNoise/TetrahedronNoise35.pcd", *cloud);
		   pcl::io::loadPCDFile ("/Path/TO/ArtificialPointClouds/TetrahedronMultiple.pcd", *cloud);

	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr edge (new pcl::PointCloud<pcl::PointXYZRGBA>);
	 // pcl::io::loadPCDFile ("/Path/TO/GroundTruth/GroundTruthTwoPlaneX.pcd", *edge);
	  	  // pcl::io::loadPCDFile ("/Path/TO/GroundTruth/GroundTruthTwoPlaneY.pcd", *edge);
	  	// pcl::io::loadPCDFile ("/Path/TO/GroundTruth/ExactEdge.pcd", *edge);
	  	// pcl::io::loadPCDFile (	"/Path/TO/GroundTruth/GroundtruthFractal2.pcd", *edge);
	 // pcl::io::loadPCDFile (	"/Path/TO/GroundTruth/GroundTruthThreeoPlanes.pcd", *edge);
	 // pcl::io::loadPCDFile (	"/Path/TO/GroundTruth/GroundTruthTetrahedron.pcd", *edge);
	  pcl::io::loadPCDFile ("/Path/TO/GroundTruth/GroundTruthTetrahedronMultiple.pcd", *edge);

	   std::cout << "Number of points in the Cube Input cloud is:"<< cloud->points.size() << std::endl;
	   std::cout << "Number of points in the Edge Ground Truth cloud is:"<< edge->points.size() << std::endl;

	   // Compute distance between all the points of the flat cloud and points of the Edge cloud
	   	std::vector<std::vector <double> > Distances ; // 2D vector(Matrix) for each point that has the Angle between lines of normals around barycenter
	   		 int heightD;
	   		 int widthD;
	    // defining an empty 2D vector (matrix)
	   	 heightD= cloud ->points.size ();
	   		 widthD = edge ->points.size ();
	   			 Distances.resize(heightD); // to defining numbers of row in 2D vector
	   				   for(int jj=0; jj< heightD; ++jj)
	   				   	{Distances[jj].resize(widthD);}   // to defining numbers of column for each row in 2d vector

	   for (size_t ii = 0; ii < cloud ->points.size (); ++ii) {
	    for (size_t jj = 0; jj< edge ->points.size (); ++jj) {
	   double distance = sqrt (   	( ( (cloud->points[ii].x)	-  (edge->points[jj].x) ) *  ( (cloud->points[ii].x)	-  (edge->points[jj].x) ) ) +   ( ( (cloud->points[ii].y)	-  (edge->points[jj].y) ) *  ( (cloud->points[ii].y)	-  (edge->points[jj].y) ) ) + ( ( (cloud->points[ii].z)	-  (edge->points[jj].z) ) *  ( (cloud->points[ii].z)	-  (edge->points[jj].z) ) )   );
	   		Distances [ii][jj] = distance;
	   			 }
	   		 }
	   	 // Define a cloud for the relevant elements
	   	 double relevantsize = 0.00 ;
	   		 // find minimum distance for each line
	   		std::vector<double> MinDistances;
	   		 for (size_t ii = 0; ii < cloud ->points.size (); ++ii) {
	   		 double min = 100.00;
	   		for (size_t jj = 0; jj< edge ->points.size (); ++jj) {
	   		 if (Distances [ii][jj] < min ){
	   			 min = Distances [ii][jj] ;}
	   				   }
	   	MinDistances .push_back (min );
	   	 if (min< 0.0075){
	   // Copy the 	coordinates on to  the relevant cloud
	   				pcl::PointXYZRGBA basic_point;
	   				basic_point.x = cloud->points[ii].x;
	   				basic_point.y = cloud->points[ii].y;
	   				basic_point.z = cloud->points[ii].z;
	   				basic_point.r = 255;
	   				basic_point.g = 255;
	   				basic_point.b= 255;
	   				RelevantElements->points.push_back(basic_point);
	   				relevantsize += 1.00 ;
	   				         }// change the color in main cloud
	   		 	 }// if min <0.03
	   // Size of the RelevantElements Cloud
	   		 RelevantElements->width = (int) RelevantElements->points.size ();
	   		 RelevantElements->height = 1;
	   		 std::cout << "Number of points in the relevant location is:"<< relevantsize << std::endl;
	   		 std::cout << "size of the relevant cloud is:"<< RelevantElements->points.size () << std::endl;

// to visualize the relevant points
	   	//  pcl::visualization::CloudViewer viewer1("Relevant points");
	   	//  viewer1.showCloud(RelevantElements);
	   	 //   while (!viewer1.wasStopped ())
	   	  // {}

	  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Normals (new pcl::PointCloud<pcl::PointXYZRGBA>);
	  Normals->resize(cloud->size());

	  // K nearest neighbor search
	  int KNumbersNeighbor = 10; // numbers of neighbors 7
	  std::vector<int> NeighborsKNSearch(KNumbersNeighbor);
	  std::vector<float> NeighborsKNSquaredDistance(KNumbersNeighbor);

	  int* NumbersNeighbor = new  int [cloud ->points.size ()];
	  pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
	  kdtree.setInputCloud (cloud);
	  pcl::PointXYZRGBA searchPoint;

	  double* SmallestEigen = new  double [cloud->points.size() ];
	  double* MiddleEigen = new  double [cloud->points.size() ];
	  double* LargestEigen = new  double [cloud->points.size() ];

	  double* DLS = new  double [cloud->points.size() ];
	  double* DLM = new  double [cloud->points.size() ];
	  double* DMS = new  double [cloud->points.size() ];
	  double* Sigma = new  double [cloud->points.size() ];

//		std::vector<double> SmallestEigen;
//		std::vector<double> MiddleEigen;
//		std::vector<double> LargestEigen;
//
//		std::vector<double> DLS;
//		std::vector<double> DML;
//		std::vector<double> DMS;

	    //  ************ All the Points of the cloud *******************
	for (size_t i = 0; i < cloud ->points.size (); ++i) {

	searchPoint.x =   cloud->points[i].x;
	searchPoint.y =   cloud->points[i].y;
	searchPoint.z =   cloud->points[i].z;

	if ( kdtree.nearestKSearch (searchPoint, KNumbersNeighbor, NeighborsKNSearch, NeighborsKNSquaredDistance) > 0 ) {
		 NumbersNeighbor[i]= NeighborsKNSearch.size (); }
	    else { NumbersNeighbor[i] = 0; }

	float Xmean; float Ymean; float Zmean;
	float sum= 0.00;
// Computing Covariance Matrix
	for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
      sum += cloud->points[ NeighborsKNSearch[ii] ].x; }
	Xmean = sum / NumbersNeighbor[i] ;
	sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].y;}
			Ymean = sum / NumbersNeighbor[i] ;
		 sum= 0.00;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += cloud->points[NeighborsKNSearch[ii] ].z;}
			Zmean = sum / NumbersNeighbor[i] ;

			float	CovXX;  float CovXY; float CovXZ; float CovYX; float CovYY; float CovYZ; float CovZX; float CovZY; float CovZZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].x - Xmean )  );}
			CovXX = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovXY = sum / ( NumbersNeighbor[i]-1) ;

			CovYX = CovXY ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].x - Xmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovXZ= sum / ( NumbersNeighbor[i]-1) ;

			CovZX = CovXZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].y - Ymean )  );}
			CovYY = sum / ( NumbersNeighbor[i]-1) ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].y - Ymean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovYZ = sum / ( NumbersNeighbor[i]-1) ;

			CovZY = CovYZ;

			sum = 0.00 ;
			for (size_t ii = 0; ii < NeighborsKNSearch.size (); ++ii){
			sum += ( (cloud->points[NeighborsKNSearch[ii] ].z - Zmean ) * ( cloud->points[NeighborsKNSearch[ii] ].z - Zmean )  );}
			CovZZ = sum / ( NumbersNeighbor[i]-1) ;

// Computing Eigenvalue and EigenVector
   Matrix3f Cov;
   Cov << CovXX, CovXY, CovXZ, CovYX, CovYY, CovYZ, CovZX, CovZY, CovZZ;

  SelfAdjointEigenSolver<Matrix3f> eigensolver(Cov);
  if (eigensolver.info() != Success) abort();

  double EigenValue1 = eigensolver.eigenvalues()[0];
  double EigenValue2 = eigensolver.eigenvalues()[1];
  double EigenValue3 = eigensolver.eigenvalues()[2];

  double Smallest = 0.00; double Middle = 0.00; double Largest= 0.00;
  if (EigenValue1<  EigenValue2 ) { Smallest =  EigenValue1 ; } else { Smallest = EigenValue2 ; }
  if (EigenValue3<  Smallest ) { Smallest =  EigenValue3 ; }

  if(EigenValue1 <= EigenValue2 && EigenValue1 <= EigenValue3) {
	  Smallest = EigenValue1;
  if(EigenValue2 <= EigenValue3) {Middle = EigenValue2; Largest = EigenValue3;}
  else {Middle = EigenValue3; Largest = EigenValue2;}
  }

  if(EigenValue1 >= EigenValue2 && EigenValue1 >= EigenValue3)
  {
	  Largest = EigenValue1;
  if(EigenValue2 <= EigenValue3) { Smallest = EigenValue2; Middle = EigenValue3; }
  else {Smallest = EigenValue3; Middle = EigenValue2;}
  }

  if ((EigenValue1 >= EigenValue2 && EigenValue1 <= EigenValue3) || (EigenValue1 <= EigenValue2 && EigenValue1 >= EigenValue3))
  {
	  Middle = EigenValue1;
  if(EigenValue2 >= EigenValue3){Largest = EigenValue2; Smallest = EigenValue3;}
  else{Largest = EigenValue3; Smallest = EigenValue2;}
  }

SmallestEigen[i]= Smallest ;
MiddleEigen[i]= Middle;
LargestEigen[i]= Largest;

DLS[i] = std::abs ( LargestEigen[i] -  SmallestEigen[i] ) ;
DLM[i] = std::abs (  LargestEigen[i] - MiddleEigen[i] ) ;
DMS[i] = std::abs ( MiddleEigen[i] -  SmallestEigen[i] ) ;
Sigma[i] = (SmallestEigen[i] ) / ( SmallestEigen[i] + MiddleEigen[i] + LargestEigen[i] ) ;
	} // For each point of the cloud

// Color Map For the difference of the eigen values

	  double MaxD=0.00 ;
	  double MinD= cloud ->points.size ();
	  int Ncolors=256;

	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  	if (  Sigma [i] < MinD) MinD= Sigma [i];
	  	if (  Sigma[i] > MaxD) MaxD = Sigma [i];
	  }

	  std::cout<< " Minimum is :" << MinD<< std::endl;
	  std::cout<< " Maximum  is :" << MaxD << std::endl;

	 double ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += Sigma [i] ;}
	  double avg = ss / cloud ->points.size () ;
	  ss = 0.00 ;
	  for (size_t i = 0; i < cloud ->points.size (); ++i) {
		  ss += (Sigma [i] -  avg ) * (  Sigma [i] -  avg ) ;}
	  double stddvtion =   sqrt (  ss  /  ( cloud ->points.size () - 1 )  ) ;

	  std::cout<< " Standard Deviation is :" << stddvtion << std::endl;

	  MaxD = ( 2 )* stddvtion;

/*
	  // Color table
		double line;
		double code[Ncolors][3];
	   ifstream colorcode ( "/Path/TO/ArtificialPointCloud/JetColorDensity/ColorCodes256.txt" );
	   //store color codes in array
	    int i=0,j=0;
	    while( colorcode>> line ) {
	    code[i][j]=line;
	    j++;
	    if (j == 3)
	    i++;
	}
	    code[1][0] = 0;
	    code[1][1] = 0;
	    code[1][2] = 135.468;

	int level = 0;
	float step = ( ( MaxD -  MinD) / Ncolors ) ;
	    for (size_t i = 0; i < cloud ->points.size (); ++i) {
if (  Sigma [i] < MaxD ) {
	    level = floor( (Sigma [i] - MinD ) /  step ) ;

	    cloud->points[i].r = code[ level ][0];
	    cloud->points[i].g =  code[ level ][1];
	    cloud->points[i].b =  code[ level ][2];
} // if sigma less than MAx
	    }
*/

	  int level = 0;
	  float step = ( ( MaxD -  MinD) / Ncolors ) ;
	 //  level = floor( (Sigma [i] - MinD ) /  step ) ;
for (size_t i = 0; i < cloud ->points.size (); ++i) {
	  if ( Sigma [i] > ( MinD + ( 0.003* step) ) ) {   // for the 3 plane ( Sigma [i] > ( MinD + ( 0.003* step) ) )
	    cloud->points[i].r = 0;
	    cloud->points[i].g =  0 ;
	    cloud->points[i].b =  128;
		pcl::PointXYZRGBA basic_point2;
		basic_point2.x = cloud->points[i].x;
		basic_point2.y = cloud->points[i].y;
		basic_point2.z = cloud->points[i].z;
		basic_point2.r = 255;
		basic_point2.g = 255;
		basic_point2.b= 255;
		SelectedElements->points.push_back(basic_point2);
        }
     }

// Computing

SelectedElements->width = (int) SelectedElements->points.size ();
SelectedElements->height = 1;
std::cout <<"Size of the Edge  cloud by clustering method is:  " << SelectedElements->points.size ()<< std::endl;

// Compute the  True Positive by Comparing the two Clouds of Relevant and Selective
double TruePositive = 0.00 ;

	 for (size_t ii = 0; ii< RelevantElements ->points.size (); ++ii) {
 	 for (size_t jj = 0; jj< SelectedElements ->points.size (); ++jj) {
 		 if ( ((RelevantElements->points[ii].x) == (SelectedElements->points[jj].x)) &&   ((RelevantElements->points[ii].y) == (SelectedElements->points[jj].y)) &&  ((RelevantElements->points[ii].z) == (SelectedElements->points[jj].z)) ) {
			TruePositive += 1.00 ; 	 } // if
		     } // jj
 	 } // ii

std::cout <<"Numbers of True Positive  is:  " << TruePositive << std::endl;
double FalseNegative = (RelevantElements ->points.size () ) - TruePositive ;
std::cout <<"Numbers of False Negative  is:  " << FalseNegative << std::endl;
double FalsePositive = (SelectedElements ->points.size ()) - TruePositive;
std::cout <<"Numbers of False Positive  is:  " << FalsePositive << std::endl;
double TrueNegative = ((cloud ->points.size ()) -  (RelevantElements ->points.size () ) ) -  FalsePositive ;
std::cout <<"Numbers of True Negative  is:  " << TrueNegative << std::endl;

double Precision = (TruePositive / ( SelectedElements ->points.size () ) ) ;
std::cout <<"Numbers of Precision  is:  " << Precision << std::endl;
double Recall = (TruePositive / (RelevantElements ->points.size () ) ) ;
std::cout <<"Numbers of Recall   is:  " << Recall << std::endl;
double FScore = 2* (   ( Precision * Recall  ) / (Precision +  Recall )) ;
std::cout <<"F1 Score of True Positive  is:  " << FScore << std::endl;

  	pcl::PLYWriter writePLY;
  //	writePLY.write ("/Path/TO/CUbeEigenJetColor.ply", *cloud,  false);
	 //   writePLY.write ("/Path/TO/CloudEigeJnetTwirl.ply", *cloud,  false);
	// writePLY.write ("/Path/TO/CloudEigeJnetDragon.ply", *cloud,  false);
  	//  writePLY.write ("/Path/TO/CloudEigeJnetTwoPlane22.ply", *cloud,  false);

  pcl::visualization::CloudViewer viewer("Cloud Viewer");
  viewer.showCloud(cloud);
  while (!viewer.wasStopped ())
  {}

  return 0;
  }


#pragma once
#include <Eigen/Core>
#include <memory>
#include <numeric>
#include <set>
#include <string>
#include "ear/layout.hpp"
#include "ear/metadata.hpp"

namespace ear {

  /** @brief An interface for objects that can calculate gains for some
   * positions, e.g. a triangle of loudspeakers.
   */
  class RegionHandler {
   public:
    RegionHandler(Eigen::VectorXi outputChannels, Eigen::MatrixXd positions);
    virtual boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const = 0;
    virtual ~RegionHandler() = default;

    boost::optional<Eigen::VectorXd> handleRemap(Eigen::Vector3d position,
                                                 int numberOfChannels) const;
    Eigen::VectorXi outputChannels();

   protected:
    Eigen::VectorXi _outputChannels;
    Eigen::MatrixXd _positions;
  };

  /** @brief Region handler representing a triplet of loudspeakers, implementing
   * VBAP.
   *
   * This is implemented such that if handle(pos) returns array x:
   *
   *  - dot(x, positions) is collinear with pos
   *  - x[i] >= 0 for all i
   *  - norm(x) == 1
   *
   * Note that the positions are *not* normalised, as this is not always
   * desirable.
   */
  class Triplet : public RegionHandler {
   public:
    /** @brief Ctor
     *
     * @param  output_channels The channel numbers of the values returned by
     *   handle.
     * @param  positions  Cartesian positions of the three speakers; index order
     *   is speaker, axis.
     */
    Triplet(Eigen::Vector3i outputChannels, Eigen::Matrix3d positions);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    Eigen::Matrix3d _basis;
  };

  /** @brief Region handler representing n real loudspeakers and a central
   * virtual loudspeaker, whose gain is distributed to the real loudspeakers.
   *
   * Triplet regions are formed between the virtual speaker and pairs of real
   * speakers on the edge of the ngon. Any gain sent to the virtual speaker is
   * multiplied by centre_downmix and summed into the gains for the real
   * loudspeakers,which are then normalised.
   */
  class VirtualNgon : public RegionHandler {
   public:
    /** @brief Ctor
     *
     * @param  output_channels The channel numbers of the values returned by
     *   handle.
     * @param  positions Cartesian positions of the n loudspeakers.
     * @param  centrePosition Cartesian position of the central virtual
     *   loudspeaker
     * @param  centreDownmix Downmix coefficients for distributing gains from
     * the centre virtual loudspeaker to the loudspeakers defined by
     * positions.
     */
    VirtualNgon(Eigen::VectorXi outputChannels, Eigen::MatrixXd positions,
                Eigen::Vector3d centrePosition, Eigen::VectorXd centreDownmix);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    Eigen::Vector3d _centrePosition;
    Eigen::VectorXd _centreDownmix;
    std::vector<std::unique_ptr<RegionHandler>> _regions;
  };

  class QuadRegion : public RegionHandler {
   public:
    QuadRegion(Eigen::VectorXi outputChannels, Eigen::MatrixXd positions);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    Eigen::Matrix3d _calcPolyBasis(Eigen::MatrixXd positions);

    boost::optional<double> _pan(Eigen::Vector3d position,
                                 Eigen::Matrix3d polyBasis) const;

    Eigen::VectorXi _order;
    Eigen::Matrix3d _polyBasisX;
    Eigen::Matrix3d _polyBasisY;
  };

  /** @brief Base class for all PointSourcePanner like classes
   */
  class PointSourcePanner {
   public:
    virtual boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) = 0;
    virtual int numberOfOutputChannels() const = 0;
  };

  /** @brief Wrapper around multiple regions.
   */
  class PolarPointSourcePanner : public PointSourcePanner {
   public:
    /** @brief Ctor
     *
     * @param  regions Regions used to handle a position.
     * @param  numberOfChannels Number of output channels; this is computed
     * from the output channels of the regions if not provided.
     */
    PolarPointSourcePanner(std::vector<std::unique_ptr<RegionHandler>> regions,
                           boost::optional<int> numberOfChannels = boost::none);

    boost::optional<Eigen::VectorXd> handle(Eigen::Vector3d position) override;

    int numberOfOutputChannels() const override;

   private:
    int _numberOfRequiredChannels();

    const std::vector<std::unique_ptr<RegionHandler>> _regions;
    int _numberOfOutputChannels;
  };

  /** @brief Wrapper around a point source panner with an additional downmix.
   */
  class PointSourcePannerDownmix : public PointSourcePanner {
   public:
    /** @brief Ctor
     *
     * @param  psp Inner point source panner.
     * @param  downmix Downmix matrix (mxn) from m inputs to n outputs.
     */
    PointSourcePannerDownmix(std::shared_ptr<PointSourcePanner> psp,
                             Eigen::MatrixXd downmix);
    ~PointSourcePannerDownmix() = default;

    boost::optional<Eigen::VectorXd> handle(Eigen::Vector3d position) override;

    int numberOfOutputChannels() const override;

   private:
    std::shared_ptr<PointSourcePanner> _psp;
    Eigen::MatrixXd _downmix;
  };

  /** @brief Generate extra loudspeaker positions to fill gaps in layers.
   *
   * @param  layout Original layout without the LFE channels
   *
   * @returns
   *   - list of extra channels (layout.Channel).
   *   - downmix matrix to mix the extra channel outputs to the real channels
   */
  std::pair<std::vector<Channel>, Eigen::MatrixXd> extraPosVerticalNominal(
      Layout layout);

  // given a layout, determine the full set of loudspeaker positions used for
  // panning. returns:
  //
  // - the real position of real and virtual loudspeakers
  // - the nominal position of the real and virtual loudspeakers
  // - the indices of the virtual loudspeakers in the two position lists
  // - a downmix matrix to be applied to the output of the real loudspeakers
  std::tuple<std::vector<Eigen::Vector3d>, std::vector<Eigen::Vector3d>,
             std::set<int>, Eigen::MatrixXd>
  getAugmentedLayout(const Layout& layout);

  std::shared_ptr<PointSourcePanner> configureFullPolarPanner(
      const Layout& layout);

  class StereoPannerDownmix : public RegionHandler {
   public:
    StereoPannerDownmix(Eigen::VectorXi outputChannels,
                        Eigen::MatrixXd positions);

    boost::optional<Eigen::VectorXd> handle(
        Eigen::Vector3d position) const override;

   private:
    std::shared_ptr<PointSourcePanner> _psp;
  };

  class AllocentricPanner : public PointSourcePanner {
   public:
    AllocentricPanner() = default;
    ~AllocentricPanner() = default;

    boost::optional<Eigen::VectorXd> handle(Eigen::Vector3d position) override;
    int numberOfOutputChannels() const override;
  };

  std::shared_ptr<PointSourcePanner> configureStereoPolarPanner(
      const Layout& layout);

  std::shared_ptr<PointSourcePanner> configureFullPolarPanner(
      const Layout& layout);

  std::shared_ptr<PointSourcePanner> configureAllocentricPanner(
      const Layout& layout);

  std::shared_ptr<PointSourcePanner> configurePolarPanner(const Layout& layout);

}  // namespace ear


// Copyright (c) 2015-2021 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "error_rate_stdev.hpp"

#include <numeric>
#include <algorithm>
#include <iterator>
#include <map>
#include <cstddef>

#include <boost/variant.hpp>

#include "basics/cigar_string.hpp"
#include "basics/aligned_read.hpp"
#include "core/types/haplotype.hpp"
#include "io/variant/vcf_record.hpp"
#include "utils/maths.hpp"
#include "../facets/samples.hpp"
#include "../facets/alleles.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string ErrorRateStdev::name_ = "ERS";

std::unique_ptr<Measure> ErrorRateStdev::do_clone() const
{
    return std::make_unique<ErrorRateStdev>(*this);
}

Measure::ValueType ErrorRateStdev::get_value_type() const
{
    return double {};
}

namespace {

auto calculate_error_rate(const AlignedRead& read) noexcept
{
    return static_cast<double>(sum_non_matches(read.cigar())) / sequence_size(read);
}

boost::optional<double>
calculate_error_rate_stdev(const ReadRefSupportSet& reads)
{
    std::vector<double> error_rates(reads.size());
    std::transform(std::cbegin(reads), std::cend(reads), std::begin(error_rates), calculate_error_rate);
    if (!error_rates.empty()) {
        return maths::stdev(error_rates);
    } else {
        return boost::none;
    }
}

} // namespace

Measure::ResultType ErrorRateStdev::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& alleles = get_value<Alleles>(facets.at("Alleles"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments")).alleles;
    const auto num_alleles = 1 + call.num_alt();
    Array<Array<Optional<ValueType>>> result(samples.size(), Array<Optional<ValueType>>(num_alleles));
    for (std::size_t s {0}; s < samples.size(); ++s) {
        const auto& sample_alleles = get(alleles, call, samples[s]);
        assert(sample_alleles.size() == num_alleles);
        const auto& support = assignments.at(samples[s]);
        for (std::size_t a {0}; a < num_alleles; ++a) {
            if (sample_alleles[a]) {
                const auto support_set_itr = support.find(*sample_alleles[a]);
                if (support_set_itr != std::cend(support)) {
                    result[s][a] = calculate_error_rate_stdev(support_set_itr->second);
                }
            }
        }
    }
    return result;
}

Measure::ResultCardinality ErrorRateStdev::do_cardinality() const noexcept
{
    return ResultCardinality::samples_and_alleles;
}

const std::string& ErrorRateStdev::do_name() const
{
    return name_;
}

std::string ErrorRateStdev::do_describe() const
{
    return "Error rate standard deviation in supporting reads";
}

std::vector<std::string> ErrorRateStdev::do_requirements() const
{
    return {"Samples", "Alleles", "ReadAssignments"};
}

boost::optional<Measure::Aggregator> ErrorRateStdev::do_aggregator() const noexcept
{
    return Measure::Aggregator::max;
}

} // namespace csr
} // namespace octopus


/*
 * Copyright 2020, Offchain Labs, Inc.
 *
 * 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.
 */

#ifndef test_ecops_hpp
#define test_ecops_hpp

#include <gmpxx.h>
#include <avm/machinestate/ecops.hpp>
#include <boost/algorithm/hex.hpp>
#include <libff/algebra/curves/alt_bn128/alt_bn128_pp.hpp>

using namespace libff;

inline std::vector<unsigned char> hexToVec(const std::string& hexstr) {
    std::vector<unsigned char> bytes;
    bytes.resize(hexstr.size() / 2);
    boost::algorithm::unhex(hexstr.begin(), hexstr.end(), bytes.begin());
    return bytes;
}

inline uint256_t hexToInt(const std::string& hexstr) {
    auto bytes = hexToVec(hexstr);
    return intx::be::unsafe::load<uint256_t>(bytes.data());
}

struct PairingTestCase {
    std::vector<std::pair<G1Point, G2Point>> points;
    bool valid;

    PairingTestCase(
        std::vector<std::pair<G1<alt_bn128_pp>, G2<alt_bn128_pp>>> pairs_,
        bool valid_)
        : valid(valid_) {
        for (const auto& point : pairs_) {
            points.push_back(
                {toG1ArbPoint(point.first), toG2ArbPoint(point.second)});
        }
    }

    PairingTestCase(const std::string& data, bool valid);
};

std::vector<PairingTestCase> preparePairingCases();

struct ECAddTestCase {
    G1Point a;
    G1Point b;
    G1Point res;
};

std::vector<ECAddTestCase> prepareECAddCases();

struct ECMulTestCase {
    G1Point a;
    uint256_t k;
    G1Point res;
};

inline std::vector<ECMulTestCase> prepareECMulCases() {
    alt_bn128_pp::init_public_params();

    G1<alt_bn128_pp> Pff =
        (Fr<alt_bn128_pp>::random_element()) * G1<alt_bn128_pp>::one();
    uint256_t sui = hexToInt(
        "b7abaaf2f45b6d1c1b23afb835719050a28b98cea191d94bff8feb3025ddbfc8");

    uint8_t sbytes[32];
    intx::be::store(sbytes, sui);
    mpz_t smpz;
    mpz_init(smpz);
    mpz_import(smpz, 32, 1, 1, 1, 0, sbytes);
    bigint<BIG_INT_FOR_UINT256> s(smpz);

    auto P = toG1ArbPoint(Pff);

    G1Point prod = toG1ArbPoint(s * Pff);
    return {{P, sui, prod}};
}

#endif /* test_ecops_hpp */