#include <disposer/core/generate_module.hpp>
#include <disposer/core/directory.hpp>

#define BOOST_TEST_MODULE disposer module_register
#include <boost/test/included/unit_test.hpp>

using namespace disposer;

using namespace disposer::literals;
using namespace std::literals::string_view_literals;

using type_index = boost::typeindex::type_index;

template < std::size_t D >
using ic = index_component< D >;

template < typename ... T > struct morph{};

BOOST_AUTO_TEST_CASE(register_0){
	directory dir;

	struct exec{
		constexpr void operator()()const{}
	};

	constexpr module_init_fn state_dummy{};
	constexpr exec_fn exec_dummy{exec{}};

	{
		auto fn = generate_module("description",
			module_configure{}, state_dummy, exec_dummy);
		fn("register_0", dir.declarant());
	}
}

BOOST_AUTO_TEST_CASE(register_1){
	directory dir;

	constexpr auto list = dimension_list{
			dimension_c< double, char, float >
		};

	struct exec{
		constexpr void operator()()const{}
	};

	constexpr module_init_fn state_dummy{};
	constexpr exec_fn exec_dummy{exec{}};

	auto const generate_fn = [=](std::size_t i){
			return generate_module("description", list,
				module_configure{
					set_dimension_fn([i](auto const&){
						return solved_dimensions{index_component< 0 >{i}};
					})
				}, state_dummy, exec_dummy);
		};

	{
		auto fn = generate_fn(0);
		fn("register_1_1", dir.declarant());
	}
	{
		auto fn = generate_fn(1);
		fn("register_1_2", dir.declarant());
	}
	{
		auto fn = generate_fn(2);
		fn("register_1_3", dir.declarant());
	}
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef BOOST_GRAPH_DIJKSTRA_HPP
#define BOOST_GRAPH_DIJKSTRA_HPP

#include <functional>
#include <boost/limits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/relax.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/exception.hpp>
#include <boost/pending/relaxed_heap.hpp>

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING
#  include <boost/pending/mutable_queue.hpp>
#endif // BOOST_GRAPH_DIJKSTRA_TESTING

namespace boost {

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING
  static bool dijkstra_relaxed_heap = true;
#endif

  template <class Visitor, class Graph>
  struct DijkstraVisitorConcept {
    void constraints() {
      function_requires< CopyConstructibleConcept<Visitor> >();
      vis.initialize_vertex(u, g);
      vis.discover_vertex(u, g);
      vis.examine_vertex(u, g);
      vis.examine_edge(e, g);
      vis.edge_relaxed(e, g);
      vis.edge_not_relaxed(e, g);
      vis.finish_vertex(u, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits<Graph>::vertex_descriptor u;
    typename graph_traits<Graph>::edge_descriptor e;
  };

  template <class Visitors = null_visitor>
  class dijkstra_visitor : public bfs_visitor<Visitors> {
  public:
    dijkstra_visitor() { }
    dijkstra_visitor(Visitors vis)
      : bfs_visitor<Visitors>(vis) { }

    template <class Edge, class Graph>
    void edge_relaxed(Edge e, Graph& g) {
      invoke_visitors(this->m_vis, e, g, on_edge_relaxed());
    }
    template <class Edge, class Graph>
    void edge_not_relaxed(Edge e, Graph& g) {
      invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed());
    }
  private:
    template <class Edge, class Graph>
    void tree_edge(Edge u, Graph& g) { }
  };
  template <class Visitors>
  dijkstra_visitor<Visitors>
  make_dijkstra_visitor(Visitors vis) {
    return dijkstra_visitor<Visitors>(vis);
  }
  typedef dijkstra_visitor<> default_dijkstra_visitor;

  namespace detail {

    template <class UniformCostVisitor, class UpdatableQueue,
      class WeightMap, class PredecessorMap, class DistanceMap,
      class BinaryFunction, class BinaryPredicate>
    struct dijkstra_bfs_visitor
    {
      typedef typename property_traits<DistanceMap>::value_type D;

      dijkstra_bfs_visitor(UniformCostVisitor vis, UpdatableQueue& Q,
                           WeightMap w, PredecessorMap p, DistanceMap d,
                           BinaryFunction combine, BinaryPredicate compare,
                           D zero)
        : m_vis(vis), m_Q(Q), m_weight(w), m_predecessor(p), m_distance(d),
          m_combine(combine), m_compare(compare), m_zero(zero)  { }

      template <class Edge, class Graph>
      void tree_edge(Edge e, Graph& g) {
        m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                            m_combine, m_compare);
        if (m_decreased)
          m_vis.edge_relaxed(e, g);
        else
          m_vis.edge_not_relaxed(e, g);
      }
      template <class Edge, class Graph>
      void gray_target(Edge e, Graph& g) {
        m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                            m_combine, m_compare);
        if (m_decreased) {
          m_Q.update(target(e, g));
          m_vis.edge_relaxed(e, g);
        } else
          m_vis.edge_not_relaxed(e, g);
      }

      template <class Vertex, class Graph>
      void initialize_vertex(Vertex /*u*/, Graph& /*g*/) { }
      template <class Edge, class Graph>
      void non_tree_edge(Edge, Graph&) { }
      template <class Vertex, class Graph>
      void discover_vertex(Vertex u, Graph& g) { m_vis.discover_vertex(u, g); }
      template <class Vertex, class Graph>
      void examine_vertex(Vertex u, Graph& g) { m_vis.examine_vertex(u, g); }
      template <class Edge, class Graph>
      void examine_edge(Edge e, Graph& g) {
        if (m_compare(get(m_weight, e), m_zero))
          throw negative_edge();
        m_vis.examine_edge(e, g);
      }
      template <class Edge, class Graph>
      void black_target(Edge, Graph&) { }
      template <class Vertex, class Graph>
      void finish_vertex(Vertex u, Graph& g) { m_vis.finish_vertex(u, g); }

      UniformCostVisitor m_vis;
      UpdatableQueue& m_Q;
      WeightMap m_weight;
      PredecessorMap m_predecessor;
      DistanceMap m_distance;
      BinaryFunction m_combine;
      BinaryPredicate m_compare;
      bool m_decreased;
      D m_zero;
    };

  } // namespace detail

  // Call breadth first search with default color map.
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistZero>
  inline void
  dijkstra_shortest_paths_no_init
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistZero zero,
     DijkstraVisitor vis)
  {
    std::vector<default_color_type> color(num_vertices(g));
    default_color_type c = white_color;
    dijkstra_shortest_paths_no_init( g, s, predecessor, distance, weight,
      index_map, compare, combine, zero, vis,
        make_iterator_property_map(&color[0], index_map, c));
  }

  // Call breadth first search
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistZero, class ColorMap>
  inline void
  dijkstra_shortest_paths_no_init
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistZero zero,
     DijkstraVisitor vis, ColorMap color)
  {
    typedef indirect_cmp<DistanceMap, Compare> IndirectCmp;
    IndirectCmp icmp(distance, compare);

    typedef typename graph_traits<VertexListGraph>::vertex_descriptor Vertex;

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING
    if (!dijkstra_relaxed_heap) {
      typedef mutable_queue<Vertex, std::vector<Vertex>, IndirectCmp, IndexMap>
        MutableQueue;

      MutableQueue Q(num_vertices(g), icmp, index_map);

      detail::dijkstra_bfs_visitor<DijkstraVisitor, MutableQueue, WeightMap,
        PredecessorMap, DistanceMap, Combine, Compare>
      bfs_vis(vis, Q, weight, predecessor, distance, combine, compare, zero);

      breadth_first_visit(g, s, Q, bfs_vis, color);
      return;
    }
#endif // BOOST_GRAPH_DIJKSTRA_TESTING

    typedef relaxed_heap<Vertex, IndirectCmp, IndexMap> MutableQueue;

    MutableQueue Q(num_vertices(g), icmp, index_map);

    detail::dijkstra_bfs_visitor<DijkstraVisitor, MutableQueue, WeightMap,
      PredecessorMap, DistanceMap, Combine, Compare>
        bfs_vis(vis, Q, weight, predecessor, distance, combine, compare, zero);

    breadth_first_visit(g, s, Q, bfs_vis, color);
  }

  // Initialize distances and call breadth first search with default color map
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistInf, class DistZero>
  inline void
  dijkstra_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistInf inf, DistZero zero,
     DijkstraVisitor vis)
  {
    std::vector<default_color_type> color(num_vertices(g));
    default_color_type c = white_color;
    dijkstra_shortest_paths(g, s, predecessor, distance, weight, index_map,
                            compare, combine, inf, zero, vis,
                            make_iterator_property_map(&color[0], index_map,
                                                       c));
  }

  // Initialize distances and call breadth first search
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistInf, class DistZero, class ColorMap>
  inline void
  dijkstra_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistInf inf, DistZero zero,
     DijkstraVisitor vis, ColorMap color)
  {
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;
    typename graph_traits<VertexListGraph>::vertex_iterator ui, ui_end;
    for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
      vis.initialize_vertex(*ui, g);
      put(distance, *ui, inf);
      put(predecessor, *ui, *ui);
      put(color, *ui, Color::white());
    }
    put(distance, s, zero);

    dijkstra_shortest_paths_no_init(g, s, predecessor, distance, weight,
                            index_map, compare, combine, zero, vis, color);
  }

  namespace detail {

    // Handle defaults for PredecessorMap and
    // Distance Compare, Combine, Inf and Zero
    template <class VertexListGraph, class DistanceMap, class WeightMap,
              class IndexMap, class Params, class ColorMap>
    inline void
    dijkstra_dispatch2
      (const VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       DistanceMap distance, WeightMap weight, IndexMap index_map,
       const Params& params, ColorMap color)
    {
      // Default for predecessor map
      dummy_property_map p_map;

      typedef typename property_traits<DistanceMap>::value_type D;
      dijkstra_shortest_paths
        (g, s,
         choose_param(get_param(params, vertex_predecessor), p_map),
         distance, weight, index_map,
         choose_param(get_param(params, distance_compare_t()),
                      std::less<D>()),
         choose_param(get_param(params, distance_combine_t()),
                      closed_plus<D>()),
         choose_param(get_param(params, distance_inf_t()),
                      (std::numeric_limits<D>::max)()),
         choose_param(get_param(params, distance_zero_t()),
                      D()),
         choose_param(get_param(params, graph_visitor),
                      make_dijkstra_visitor(null_visitor())),
         color);
    }

    template <class VertexListGraph, class DistanceMap, class WeightMap,
              class IndexMap, class Params, class ColorMap>
    inline void
    dijkstra_dispatch1
      (const VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       DistanceMap distance, WeightMap weight, IndexMap index_map,
       const Params& params, ColorMap color)
    {
      // Default for distance map
      typedef typename property_traits<WeightMap>::value_type D;
      typename std::vector<D>::size_type
        n = is_default_param(distance) ? num_vertices(g) : 1;
      std::vector<D> distance_map(n);

      // Default for color map
      typename std::vector<default_color_type>::size_type
        m = is_default_param(color) ? num_vertices(g) : 1;
      std::vector<default_color_type> color_map(m);

      detail::dijkstra_dispatch2
        (g, s, choose_param(distance, make_iterator_property_map
                            (distance_map.begin(), index_map,
                             distance_map[0])),
         weight, index_map, params,
         choose_param(color, make_iterator_property_map
                      (color_map.begin(), index_map,
                       color_map[0])));
    }
  } // namespace detail

  // Named Parameter Variant
  template <class VertexListGraph, class Param, class Tag, class Rest>
  inline void
  dijkstra_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     const bgl_named_params<Param,Tag,Rest>& params)
  {
    // Default for edge weight and vertex index map is to ask for them
    // from the graph.  Default for the visitor is null_visitor.
    detail::dijkstra_dispatch1
      (g, s,
       get_param(params, vertex_distance),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
       params,
       get_param(params, vertex_color));
  }

} // namespace boost

#endif // BOOST_GRAPH_DIJKSTRA_HPP


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

#ifndef openOR_core_Math_matrixconcept_hpp
#define openOR_core_Math_matrixconcept_hpp

#include <boost/type_traits/add_const.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>
#include <boost/concept/usage.hpp>
#include <boost/concept_check.hpp>
#include <boost/math/quaternion.hpp>

#include <openOR/Math/utilities.hpp>
#include <openOR/Math/traits.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Math/ublasmatrix.hpp>
#include <openOR/Math/ublasvector.hpp>

namespace openOR {
   namespace Math {

      template <int I, int J, typename P>
         typename if_const < P,
                     typename MatrixTraits<typename boost::remove_const<P>::type>::Access::ConstAccessType,
                     typename MatrixTraits<typename boost::remove_const<P>::type>::Access::AccessType >
      ::type get(P& p) {
         return MatrixTraits<typename boost::remove_const<P>::type>::Access::template get<I, J>(p);
      }

      namespace Impl {

         template < class Mat,
         int BeginRow = 0,
         int EndRow = MatrixTraits<Mat>::RowDimension::value,
         int BeginCol = 0,
         int EndCol = MatrixTraits<Mat>::ColDimension::value >
         class MatrixCompileTimeIterator {
               static const int SizeRow = EndRow - BeginRow;
               static const int SizeCol = EndCol - BeginCol;

            public:

               template<template<class, int, int> class Op>
               inline void apply() const {
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  Apply0<0, SizeRow * SizeCol, Op>()();
               }

               template<template<class, class, int, int> class Op, class Mat2>
               inline void apply(Mat& mat, const Mat2& mat2) {
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  Apply2<0, SizeRow * SizeCol, Op, Mat2>()(mat, mat2);
               }

               template<template <class, int, int> class Op, typename Result, template<class> class Reducer>
               inline Result reduce(const Mat& mat) const {
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  return Reduce1 < 0, SizeRow * SizeCol - 1, Op, Result, Reducer > ()(mat);
               }

               template<template <class, class, int, int> class Op, class Mat2, typename Result, template<class> class Reducer>
               inline Result reduce(const Mat& mat, const Mat2& mat2) const {
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  return Reduce2 < 0, SizeRow * SizeCol - 1, Op, Mat2, Result, Reducer > ()(mat, mat2);
               }

            private:

               template<int I, int Count, template<class, int, int> class Op>
               struct Apply0 {
                  inline void operator()() const {
                     Op < Mat, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()();
                     Apply0 < I + 1, Count, Op > ()();
                  }
               };

               template<int Count, template<class, int, int> class Op>
               struct Apply0<Count, Count, Op> {
                  inline void operator()() const {}
               };

               template<int I, int Count, template<class, class, int, int> class Op, class Mat2>
               struct Apply2 {
                  inline void operator()(Mat& mat, const Mat2& mat2) const {
                     Op < Mat, Mat2, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()(mat, mat2);
                     Apply2 < I + 1, Count, Op, Mat2 > ()(mat, mat2);
                  }
               };

               template<int Count, template<class, class, int, int> class Op, class Mat2>
               struct Apply2<Count, Count, Op, Mat2> {
                  inline void operator()(Mat& mat, const Mat2& mat2) const {}
               };

               template<int I, int Count, template <class, int, int> class Op, typename Result, template<class> class Reducer>
               struct Reduce1 {
                  inline Result operator()(const Mat& mat) const {
                     return Reducer<Result>()(Op < Mat, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()(mat),
                                              Reduce1 < I + 1, Count, Op, Result, Reducer > ()(mat));
                  }
               };

               template<int Count, template <class, int, int> class Op, typename Result, template<class> class Reducer>
               struct Reduce1<Count, Count, Op, Result, Reducer> {
                  inline Result operator()(const Mat& mat) {
                     return Op < Mat, BeginRow + (Count % SizeRow), BeginCol + (Count / SizeRow) > ()(mat);
                  }
               };

               template<int I, int Count, template <class, class, int, int> class Op, class Mat2, typename Result, template<class> class Reducer>
               struct Reduce2 {
                  inline Result operator()(const Mat& mat, const Mat2& mat2) {
                     return Reducer<Result>()(Op < Mat, Mat2, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()(mat, mat2),
                                              Reduce2 < I + 1, Count, Op, Mat2, Result, Reducer > ()(mat, mat2));
                  }
               };

               template<int Count, template <class, class, int, int> class Op, class Mat2, typename Result, template<class> class Reducer>
               struct Reduce2<Count, Count, Op, Mat2, Result, Reducer> {
                  inline Result operator()(const Mat& mat, const Mat2& mat2) {
                     return Op < Mat, Mat2, BeginRow + (Count % SizeRow), BeginCol + (Count / SizeRow) > ()(mat, mat2);
                  }
               };

         };

      }

      namespace Concept {

         template <typename Type>
         class ConstMatrix {
            private:

               BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));

               typedef typename MatrixTraits<Type>::ValueType ValueType;
               typedef typename MatrixTraits<Type>::RowVectorType RowVectorType;
               typedef typename MatrixTraits<Type>::ColVectorType ColVectorType;
               enum { ROW_DIMENSION = MatrixTraits<Type>::RowDimension::value,
                      COL_DIMENSION = MatrixTraits<Type>::ColDimension::value
                 };

               template <class M, int I, int J>
               struct AccessCheck {
                  inline void operator()() const {
                     const M* p = 0;
                     typename MatrixTraits<M>::ValueType val(Math::get<I, J>(*p));
                     (void)sizeof(val); // To avoid "unused variable" warnings
                  }
               };

            public :
               /// BCCL macro to check the ConstPoint concept
               BOOST_CONCEPT_USAGE(ConstMatrix) {
                  Impl::MatrixCompileTimeIterator<Type, 0, ROW_DIMENSION, 0, COL_DIMENSION>().template apply<AccessCheck>();
                  Type* pMat = NULL;
                  const RowVectorType* pRowVector = NULL;
                  ColVectorType colvec(Math::prod(*pMat, *pRowVector));
               }
         };

         template<class Type>
         class Matrix : public boost::DefaultConstructible<Type>, public boost::CopyConstructible<Type>, public boost::Assignable<Type> {
            private:

               BOOST_CONCEPT_ASSERT((Concept::ConstMatrix<Type>));
               BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));

               typedef typename MatrixTraits<Type>::ValueType ValueType;
               enum { ROW_DIMENSION = MatrixTraits<Type>::RowDimension::value,
                      COL_DIMENSION = MatrixTraits<Type>::ColDimension::value
                 };

               template <class M, int I, int J>
               struct AccessCheck {
                  void operator()() const {
                     M* p = NULL;
                     get<I, J>(*p) = typename MatrixTraits<M>::ValueType();
                  }
               };

               template<typename P, int RowCount, int ColCount>
               struct MatrixProductChecker {
                  static void check() {}
               };

               template<typename P, int Count>
               struct MatrixProductChecker<P, Count, Count> {
                  static void check() {
                     P* p = 0;
                     *p = Math::prod(*p, *p);
                  }
               };

            public:
               /// BCCL macro to check the Point concept
               BOOST_CONCEPT_USAGE(Matrix) {
                  Impl::MatrixCompileTimeIterator<Type, 0, ROW_DIMENSION, 0, COL_DIMENSION>().template apply<AccessCheck>();
                  MatrixProductChecker<Type, ROW_DIMENSION, COL_DIMENSION>::check();
                  ValueType v;
                  Type t;
                  t *= v;
                  t = v * t;
                  t = t * v;
                  t = t + t;
                  t = t - t;
                  t += t;
                  t -= t;
               }
         };

      }

   }
}

#endif


// Copyright 2022 TIER IV, 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 IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_
#define IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_

#define EIGEN_MPL2_ONLY

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <autoware_auto_perception_msgs/msg/shape.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <sensor_msgs/msg/region_of_interest.hpp>

#include <vector>

namespace image_projection_based_fusion
{

using autoware_auto_perception_msgs::msg::Shape;
using geometry_msgs::msg::Pose;

double calcIoU(
  const sensor_msgs::msg::RegionOfInterest & roi_1,
  const sensor_msgs::msg::RegionOfInterest & roi_2);

double calcIoUX(
  const sensor_msgs::msg::RegionOfInterest & roi_1,
  const sensor_msgs::msg::RegionOfInterest & roi_2);

double calcIoUY(
  const sensor_msgs::msg::RegionOfInterest & roi_1,
  const sensor_msgs::msg::RegionOfInterest & roi_2);

void objectToVertices(
  const Pose & pose, const Shape & shape, std::vector<Eigen::Vector3d> & vertices);

void boundingBoxToVertices(
  const Pose & pose, const Shape & shape, std::vector<Eigen::Vector3d> & vertices);

void cylinderToVertices(
  const Pose & pose, const Shape & shape, std::vector<Eigen::Vector3d> & vertices);

void polygonToVertices(
  const Pose & pose, const Shape & shape, std::vector<Eigen::Vector3d> & vertices);

void transformPoints(
  const std::vector<Eigen::Vector3d> & input_points, const Eigen::Affine3d & affine_transform,
  std::vector<Eigen::Vector3d> & output_points);

}  // namespace image_projection_based_fusion

#endif  // IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_


#include <boost/test/unit_test.hpp>

#include <scorum/chain/dba/db_accessor.hpp>
#include <scorum/chain/evaluators/post_bet_evalulator.hpp>
#include <scorum/chain/evaluators/cancel_pending_bets_evaluator.hpp>

#include "betting_common.hpp"
#include "object_wrapper.hpp"

#include "detail.hpp"

namespace {

using namespace scorum;
using namespace scorum::chain;
using namespace scorum::protocol;

SCORUM_TEST_CASE(post_bet_operation_validate_odds)
{
    post_bet_operation op;
    op.better = "aclice";
    op.uuid = gen_uuid("game");
    op.wincase = correct_score_home::yes();
    op.stake = ASSET_SCR(100'000'000);

    BOOST_CHECK_EQUAL(1001u, SCORUM_MIN_ODDS.inverted().numerator);
    BOOST_CHECK_EQUAL(1u, SCORUM_MIN_ODDS.inverted().denominator);

    BOOST_CHECK_EQUAL(1001u, SCORUM_MIN_ODDS.base().numerator);
    BOOST_CHECK_EQUAL(1000u, SCORUM_MIN_ODDS.base().denominator);

    op.odds = { SCORUM_MIN_ODDS.inverted().numerator, SCORUM_MIN_ODDS.inverted().denominator }; // max odds
    BOOST_CHECK_NO_THROW(op.validate());

    op.odds = { SCORUM_MIN_ODDS.base().numerator, SCORUM_MIN_ODDS.base().denominator };
    BOOST_CHECK_NO_THROW(op.validate());

    op.odds = { SCORUM_MIN_ODDS.inverted().numerator + 1, SCORUM_MIN_ODDS.inverted().denominator };
    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);

    op.odds = { SCORUM_MIN_ODDS.base().numerator - 1, SCORUM_MIN_ODDS.base().denominator };
    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);

    op.odds = { 1, 1 };
    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
}

SCORUM_TEST_CASE(post_bet_evaluator_operation_validate_check)
{
    post_bet_operation test_op;
    test_op.better = "aclice";
    test_op.uuid = gen_uuid("game");
    test_op.wincase = correct_score_home::yes();
    test_op.odds = { 3, 1 };
    test_op.stake = ASSET_SCR(100'000'000);

    post_bet_operation op = test_op;

    BOOST_CHECK_NO_THROW(op.validate());

    op.better = "";
    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
    op = test_op;

    op.odds = { 1, 10 };
    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
    op = test_op;

    op.stake.amount = 0;
    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
    op = test_op;

    op.stake = ASSET_SP(1e+9);
    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
    op = test_op;
}

struct cancel_pending_bets_evaluator_fixture : public shared_memory_fixture
{
    using check_account_existence_ptr
        = void (account_service_i::*)(const account_name_type&, const fc::optional<const char*>&) const;
    using is_exists_ptr = bool (pending_bet_service_i::*)(const uuid_type&) const;
    using get_ptr = const pending_bet_object& (pending_bet_service_i::*)(const uuid_type&)const;

    MockRepository mocks;

    data_service_factory_i* dbs_factory = mocks.Mock<data_service_factory_i>();
    betting_service_i* betting_svc = mocks.Mock<betting_service_i>();
    account_service_i* acc_svc = mocks.Mock<account_service_i>();
    pending_bet_service_i* pending_bet_svc = mocks.Mock<pending_bet_service_i>();

    cancel_pending_bets_evaluator_fixture()
    {
        mocks.OnCall(dbs_factory, data_service_factory_i::account_service).ReturnByRef(*acc_svc);
        mocks.OnCall(dbs_factory, data_service_factory_i::pending_bet_service).ReturnByRef(*pending_bet_svc);
    }
};

BOOST_FIXTURE_TEST_SUITE(cancel_pending_bets_evaluator_tests, cancel_pending_bets_evaluator_fixture)

SCORUM_TEST_CASE(bet_id_existance_check_should_throw)
{
    cancel_pending_bets_operation op;
    op.better = "better";
    op.bet_uuids = { gen_uuid("0") };

    cancel_pending_bets_evaluator ev(*dbs_factory, *betting_svc);

    mocks.ExpectCallOverload(acc_svc, (check_account_existence_ptr)&account_service_i::check_account_existence);
    mocks.ExpectCallOverload(pending_bet_svc, (is_exists_ptr)&pending_bet_service_i::is_exists)
        .With(gen_uuid("0"))
        .Return(false);

    BOOST_CHECK_THROW(ev.do_apply(op), fc::assert_exception);
}

SCORUM_TEST_CASE(better_mismatch_should_throw)
{
    cancel_pending_bets_operation op;
    op.better = "better";
    op.bet_uuids = { gen_uuid("0") };

    auto obj = create_object<pending_bet_object>(shm, [](pending_bet_object& o) { o.data.better = "cartman"; });

    mocks.OnCallOverload(acc_svc, (check_account_existence_ptr)&account_service_i::check_account_existence);
    mocks.OnCallOverload(pending_bet_svc, (is_exists_ptr)&pending_bet_service_i::is_exists)
        .With(gen_uuid("0"))
        .Return(true);
    mocks.ExpectCallOverload(pending_bet_svc, (get_ptr)&pending_bet_service_i::get_pending_bet)
        .With(gen_uuid("0"))
        .ReturnByRef(obj);

    cancel_pending_bets_evaluator ev(*dbs_factory, *betting_svc);
    BOOST_CHECK_THROW(ev.do_apply(op), fc::assert_exception);
}

SCORUM_TEST_CASE(should_cancel_bets)
{
    cancel_pending_bets_operation op;
    op.better = "better";
    op.bet_uuids = { gen_uuid("0"), gen_uuid("1") };

    // clang-format off
    auto obj1 = create_object<pending_bet_object>(shm, [&](pending_bet_object& o){ o.data.better = "better"; o.id = 0; o.data.uuid = gen_uuid("0"); });
    auto obj2 = create_object<pending_bet_object>(shm, [&](pending_bet_object& o){ o.data.better = "better"; o.id = 1; o.data.uuid = gen_uuid("1"); });

    mocks.OnCallOverload(acc_svc, (check_account_existence_ptr)&account_service_i::check_account_existence);
    mocks.OnCallOverload(pending_bet_svc, (is_exists_ptr)&pending_bet_service_i::is_exists).With(gen_uuid("0")).Return(true);
    mocks.OnCallOverload(pending_bet_svc, (is_exists_ptr)&pending_bet_service_i::is_exists).With(gen_uuid("1")).Return(true);
    mocks.OnCallOverload(pending_bet_svc, (get_ptr)&pending_bet_service_i::get_pending_bet).With(gen_uuid("0")).ReturnByRef(obj1);
    mocks.OnCallOverload(pending_bet_svc, (get_ptr)&pending_bet_service_i::get_pending_bet).With(gen_uuid("1")).ReturnByRef(obj2);
    mocks.ExpectCall(betting_svc, betting_service_i::cancel_pending_bet).With(0);
    mocks.ExpectCall(betting_svc, betting_service_i::cancel_pending_bet).With(1);
    // clang-format on

    cancel_pending_bets_evaluator ev(*dbs_factory, *betting_svc);
    BOOST_CHECK_NO_THROW(ev.do_apply(op));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(cancel_pending_bets_operation_validate_tests)

SCORUM_TEST_CASE(throw_exception_when_bet_uuids_is_empty)
{
    cancel_pending_bets_operation op;
    op.better = "better";
    op.bet_uuids = {};

    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
}

SCORUM_TEST_CASE(dont_throw_exception_when_bet_uuids_set)
{
    cancel_pending_bets_operation op;
    op.better = "better";
    op.bet_uuids = { gen_uuid("1") };

    BOOST_CHECK_NO_THROW(op.validate());
}

SCORUM_TEST_CASE(throw_exception_when_better_is_empty)
{
    cancel_pending_bets_operation op;
    op.better = "";
    op.bet_uuids = { gen_uuid("1") };

    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
}

SCORUM_TEST_CASE(throw_exception_when_bets_not_unique)
{
    cancel_pending_bets_operation op;
    op.better = "better";
    op.bet_uuids = { gen_uuid("1"), gen_uuid("1") };

    BOOST_CHECK_THROW(op.validate(), fc::assert_exception);
}

BOOST_AUTO_TEST_SUITE_END()
}


#include "atomistic.hpp"
#include "formats.hpp"
#include "types.hpp"
#include "io.hpp"

#include <iostream>
#include <fstream>
#include <iterator>
#include <vector>
#include <string>

#include <boost/program_options.hpp>

#include <ctime>
time_t t = clock();
//#include <gsl/gsl_const_mksa.h>

namespace po = boost::program_options;
namespace at = atomistic;
namespace cp2k = formats::cp2k;
namespace stm = formats::stm;

// Returns true, if parsing went ok
bool parse(int ac, char* av[], po::variables_map& vm);

// Prepares extrapolation
bool prepare(types::String levelFileName,
             std::vector<types::String> wfnCubes,
             types::String hartreeFileName,
             types::String mode,
             double start,
             double width,
             double isoValue,
             double approachFrom,
             double kPlaneMax,
             types::Uint nLayers,
             double ballRadius
             );

int main(int ac, char* av[]) {

    po::variables_map args;
    if (parse(ac, av, args)) {
        prepare(args["levels"].as< types::String >(),
                args["wfncubes"].as< std::vector<types::String> >(),
                args["hartree"].as< types::String >(),
                args["mode"].as< types::String >(),
                args["start"].as< double >(),
                args["width"].as< double >(),
                args["isovalue"].as< double >(),
                args["approach-from"].as< double >(),
                args["k-cutoff"].as< double >(),
                args["nlayers"].as< types::Uint >(),
                args["ball-radius"].as< double >()
               );

    }

    return 0;
}

bool prepare(types::String levelFileName,
             std::vector<types::String> wfnCubes,
             types::String hartreeFileName,
             types::String mode,
             double start,
             double width,
             double isoValue,
             double approachFrom,
             double kPlaneMax,
             types::Uint nLayers,
             double ballRadius
            ){

    // Read energy levels
    cp2k::Spectrum spectrum = cp2k::Spectrum();
    spectrum.readFromCp2k(levelFileName.c_str());

    // Read hartree cube file
    formats::Cube hartree = formats::Cube();
    hartree.readCubeFile(hartreeFileName.c_str());

    // Write Zprofile of hartree potential
    std::string hartreeZProfile = io::getFileName(hartreeFileName);
    hartreeZProfile += ".zprofile";
    std::cout << "Writing Z profile of Hartree potential to "
        << hartreeZProfile << std::endl;
    hartree.writeZProfile(hartreeZProfile);

    std::cout << "Time to prepare : " << (clock() -t)/1000.0 << " ms\n";
    t = clock();

    // Iterate over cubes
    stm::WfnExtrapolation::Mode m ;
    types::Real var1;
    if( mode == "plane" ){
        m = stm::WfnExtrapolation::plane;
        var1 = start;
    }
    else if ( mode == "isosurface" ){
        m = stm::WfnExtrapolation::isoSurface;
        var1 = isoValue;
    }
    else if ( mode == "rolling-ball" ){
        m = stm::WfnExtrapolation::rollingBall;
        var1 = ballRadius;
    }
    stm::WfnExtrapolation extrapolation = stm::WfnExtrapolation(
            wfnCubes,
            spectrum,
            hartree,
            m,
            var1,
            width,
            approachFrom,
            kPlaneMax,
            nLayers
            );
    extrapolation.execute();

    return true;
}

bool parse(int ac, char* av[], po::variables_map& vm) {
    types::String input_file;
    // Declare regular options
    po::options_description desc("Allowed options");
    desc.add_options()
    ("help,h", "produce help message")
    ("version,v", "print version information")
    ("input-file,i", po::value<types::String>(&input_file), "Input file specifying all or several of the following options")
    ("levels", po::value<types::String>(), "CP2K output file containing energy levels")
    ("wfncubes", po::value< std::vector<types::String> >(), "List of wavefunction cubes you wish to extrapolate")
    ("hartree", po::value<types::String>(), "cube file of hartree potential from CP2K")
    ("mode", po::value<types::String>()->default_value("plane"), "may be 'plane', 'isosurface', 'rolling-ball'")
    ("start", po::value<double>()->default_value(5), "mode 'plane': distance between extrapolation plane and outermost atom in a.u.")
    ("width", po::value<double>()->default_value(15), "length of extrapolation in a.u.")
    ("isovalue", po::value<double>()->default_value(0.0), "mode 'isosurface': isovalue of the Hartree potential [a.u.] ")
    ("approach-from", po::value<double>()->default_value(-1.0), "mode 'isosurface': z [a.u.] from where you want to go down to find the isosurface (default: top z of cube file).")
//  ("decay-cutoff", po::value<double>()->default_value(2.0), "Maximum decay constant k [1/a.u.] to be retained for z decay  10^(-k*z).")
    ("k-cutoff", po::value<double>()->default_value(3.0), "Restrict basis functions to maximum wave number k = 1/lambda [1/a.u.] in xy-plane.")
    ("nlayers", po::value<types::Uint>()->default_value(1), "Number of layers to fit the wave function values.")
    ("ball-radius", po::value<double>()->default_value(5.0), "mode 'rolling-ball': Radius of ball rolling on top of the atoms [a.u.] ")
    ;

    // Register positional options
    po::positional_options_description p;
    p	.add("input-file", 1) .add("wfncubes", -1);

    // Parse
    po::store(po::command_line_parser(ac,av).
              options(desc).positional(p).run(), vm);
    po::notify(vm);

    // If specified, try to parse conig file
    if (vm.count("input-file")){
        std::ifstream ifs(input_file.c_str());
        if(!ifs) throw types::fileAccessError() << boost::errinfo_file_name(input_file);
        store(parse_config_file(ifs, desc), vm);
        notify(vm);
    }

    // Display help message
    if (	vm.count("help") ||
            !vm.count("levels") ||
            !vm.count("hartree") ||
            !vm.count("wfncubes") ||
            !vm.count("width")	) {
        std::cout << "Usage: extrapolate [options]\n";
        std::cout << desc << "\n";
    } else if (vm.count("version")) {
        std::cout << "October 9th 2012\n";
    } else if ( vm.count("mode")  &&
                vm["mode"].as< types::String >() != "plane" &&
                vm["mode"].as< types::String >() != "rolling-ball" &&
                vm["mode"].as< types::String >() != "isosurface") {
                std::cout << "Error: invalid mode specified.\n";
    } else if ( (!vm.count("mode")  || vm["mode"].as< types::String >() == "plane") &&
               !(vm.count("start") && vm.count("width"))) {
                std::cout << "Error: Need to specify 'start' and 'width' for mode='plane'.\n";
    } else if ( vm["mode"].as< types::String >() == "isosurface" &&
               !(vm.count("isovalue") && vm.count("width"))) {
                std::cout << "Error: Need to specify 'isovalue' and 'width' for mode='isosurface'.\n";
    } else if ( vm["mode"].as< types::String >() == "rolling-ball" &&
               !(vm.count("ball-radius") && vm.count("width"))) {
                std::cout << "Error: Need to specify 'ball-radius' and 'width' for mode='rolling-ball'.\n";
    } else if ( vm["k-cutoff"].as< types::Real >() < 0 ){
                std::cout << "Error: K-cutoff must be non-negative.\n";
    } else {
        return true;
    }

    return false;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>

#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tensor/EagerMath/DotProduct.hpp"
#include "DataStructures/Tensor/EagerMath/Magnitude.hpp"  // For Tags::Normalized
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/InterfaceHelpers.hpp"
#include "Domain/Tags.hpp"
#include "Domain/TagsTimeDependent.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

namespace domain {
namespace Tags {
/// Compute the characteristic speeds on the moving mesh given the
/// characteristic speeds if the mesh were stationary.
///
/// \note Assumes that `typename CharSpeedsComputeTag::return_type` is a
/// `std::array<DataVector, NumberOfCharSpeeds>`
template <typename CharSpeedsComputeTag, size_t Dim>
struct CharSpeedCompute : CharSpeedsComputeTag::base, db::ComputeTag {
  using base = typename CharSpeedsComputeTag::base;
  using return_type = typename CharSpeedsComputeTag::return_type;

  template <typename... Ts, typename T, size_t NumberOfCharSpeeds>
  static void function(
      const gsl::not_null<std::array<T, NumberOfCharSpeeds>*> result,
      const boost::optional<tnsr::I<DataVector, Dim, Frame::Inertial>>&
          grid_velocity,
      const tnsr::i<DataVector, Dim, Frame::Inertial>& unit_normal_covector,
      const Ts&... ts) noexcept {
    // Note that while the CharSpeedsComputeTag almost certainly also needs the
    // unit normal covector for computing the original characteristic speeds, we
    // don't know which of the `ts` it is, and thus we need the unit normal
    // covector to be passed explicitly.
    CharSpeedsComputeTag::function(result, ts...);
    if (static_cast<bool>(grid_velocity)) {
      const Scalar<DataVector> normal_dot_velocity =
          dot_product(*grid_velocity, unit_normal_covector);
      for (size_t i = 0; i < result->size(); ++i) {
        gsl::at(*result, i) -= get(normal_dot_velocity);
      }
    }
  }

  using argument_tags =
      tmpl::push_front<typename CharSpeedsComputeTag::argument_tags,
                       MeshVelocity<Dim, Frame::Inertial>,
                       ::Tags::Normalized<UnnormalizedFaceNormal<Dim>>>;
  using volume_tags = get_volume_tags<CharSpeedsComputeTag>;
};
}  // namespace Tags
}  // namespace domain


#include "generate_data/shadowed.h"

#include "generate_data/clip_by_plane.h"
#include "generate_data/get_points_from_subset.h"
#include "generate_data/triangle.h"
#include "generate_data/triangle_subset.h"
#include "generate_data/triangle_subset_intersection.h"
#include "intersect/triangle.h"
#include "intersect/triangle_impl.h"
#include "lib/array_vec.h"
#include "lib/span.h"

#include <boost/geometry.hpp>
#include <unordered_set>

#include "dbg.h"
#include "generate_data/print_region.h"
#include "lib/info/print_triangle.h"

namespace std {
// from
// https://codereview.stackexchange.com/questions/171999/specializing-stdhash-for-stdarray
template <class T, size_t N> struct hash<array<T, N>> {
  auto operator()(const array<T, N> &key) const {
    size_t result = 0;
    for (size_t i = 0; i < N; ++i) {
      result = result * 31 + hasher(key[i]);
    }
    return result;
  }

  std::hash<T> hasher;
};
} // namespace std

namespace generate_data {

// TODO: fix epsilons
// TODO: could be sooooo much faster probably
ATTR_PURE_NDEBUG PartiallyShadowedInfo partially_shadowed(
    const Triangle &from, const TriangleSubset &from_clipped_region,
    const Triangle &blocker, const TriangleSubset &blocker_clipped_region,
    const Triangle &onto, const TriangleSubset &onto_clipped_region,
    bool flip_onto_normal) {
  auto from_pb = get_points_from_subset_with_baryo(from, from_clipped_region);
  const auto blocker_pb =
      get_points_from_subset_with_baryo(blocker, blocker_clipped_region);
  const auto &from_vertices = from_pb.points;
  const auto &from_baryo = from_pb.baryo;
  const auto &blocker_vertices = blocker_pb.points;
  const auto &blocker_baryo = blocker_pb.baryo;
  debug_assert(from_vertices.size() == from_baryo.size());
  debug_assert(blocker_vertices.size() == blocker_baryo.size());
  debug_assert(!from_vertices.empty());
  debug_assert(!blocker_vertices.empty());

  auto normal = *onto.normal();
  if (flip_onto_normal) {
    normal = -normal;
  }
  auto plane_vertex = onto.vertices[0];

  double plane_offset = normal.dot(plane_vertex);
  auto plane_pos = [&](const Eigen::Vector3d &point) {
    return point.dot(normal) - plane_offset;
  };

#ifndef NDEBUG
  // double from_max_plane_pos = std::numeric_limits<double>::lowest();
  for (const auto &p : from_vertices) {
    double pos = plane_pos(p);
    // should already be clipped by plane!
    debug_assert(pos > -1e-6);
    // from_max_plane_pos = std::max(pos, from_max_plane_pos);
  }

  // shouldn't be coplaner
  // debug_assert(from_max_plane_pos > 1e-13);

  for (const auto &p : blocker_vertices) {
    // should already be clipped by plane!
    [[maybe_unused]] double pos = plane_pos(p);
    debug_assert(pos > -1e-6);
  }
#endif

  VectorT<Eigen::Vector2d> points_for_hull;
  // TODO: could be handled just using an angle range
  VectorT<Eigen::Vector2d> directions;
  points_for_hull.reserve(from_vertices.size() * blocker_vertices.size() * 6);
  directions.reserve(from_vertices.size() * blocker_vertices.size());

  VectorT<PartiallyShadowedInfo::RayItem> ray_items;
  ray_items.reserve(from_vertices.size() * blocker_vertices.size() * 6);

  std::unordered_set<std::array<double, 4>> added_items;

  // NOTE: this has_all approach only really makes sense with clipping!
  bool has_all = false;
  auto run_for_points =
      [&](const BaryoPoint &baryo_origin, const BaryoPoint &baryo_endpoint,
          const Eigen::Vector3d &origin, const Eigen::Vector3d &endpoint) {
        std::array key{baryo_origin.x(), baryo_origin.y(), baryo_endpoint.x(),
                       baryo_endpoint.y()};
        if (!added_items.insert(key).second) {
          // already added
          return;
        }

        auto add_point = [&](const Eigen::Vector3d &point) -> Eigen::Vector2d {
          auto baryo = onto.baryo_values(point);
          Eigen::Vector2d baryo_eigen{baryo[0], baryo[1]};
          points_for_hull.push_back(baryo_eigen);

          return baryo_eigen;
        };

        double origin_plane_position = plane_pos(origin);
        double endpoint_plane_position = plane_pos(endpoint);

        // TODO: https://github.com/boostorg/geometry/issues/861
        if (origin_plane_position - endpoint_plane_position <= -1e-5) {
          return;
        }
        // debug_assert(origin_plane_position - endpoint_plane_position >
        // -1e-5);

        Eigen::Vector3d direction = endpoint - origin;

        auto result = [&]() -> PartiallyShadowedInfo::RayItem::Result {
          if (direction.squaredNorm() < 1e-16) {
            has_all = true;
            // intersecting/overlapping point case
            return {tag_v<RayItemResultType::ClosePoint>, {}};
          }

          if (origin_plane_position - endpoint_plane_position < 1e-5) {
            // coplanar case
            if (std::abs(endpoint_plane_position) < 1e-6) {
              // TODO: is this case important?
              // endpoint is also on onto
              add_point(endpoint);
            }

            auto origin_on_plane_baryo =
                onto.baryo_values(origin - normal * origin_plane_position);
            auto endpoint_on_plane =
                onto.baryo_values(endpoint - normal * endpoint_plane_position);
            Eigen::Vector2d direction{
                endpoint_on_plane[0] - origin_on_plane_baryo[0],
                endpoint_on_plane[1] - origin_on_plane_baryo[1]};
            direction.normalize();
            directions.push_back(direction);
            return {tag_v<RayItemResultType::Ray>, direction};
          } else {
            double denom = normal.dot(direction);
            debug_assert(std::abs(denom) > 1e-15);
            double t = -origin_plane_position / denom;
            debug_assert(t > 1. - 1e-3); // should hit plane AFTER endpoint
            return {tag_v<RayItemResultType::Intersection>,
                    add_point(t * direction + origin)};
          }
        }();

        ray_items.push_back({
            .baryo_origin = baryo_origin,
            .baryo_endpoint = baryo_endpoint,
            .origin = origin,
            .endpoint = endpoint,
            .result = result,
        });
      };

  // TODO: could make this much more efficient in many different ways...
  // Also, this has pretty terrible numerical properties...
  for (unsigned i = 0; i < from_vertices.size(); ++i) {
    auto clipped =
        clip_by_plane(-normal, from_vertices[i].dot(-normal), blocker);
    auto new_blocker_region =
        triangle_subset_intersection(blocker_clipped_region, clipped);

    const auto new_blocker_bp =
        get_points_from_subset_with_baryo(blocker, new_blocker_region);
    const auto &new_blocker_vertices = new_blocker_bp.points;
    const auto &new_blocker_baryo = new_blocker_bp.baryo;
    debug_assert(new_blocker_vertices.size() == new_blocker_baryo.size());

    for (unsigned j = 0; j < new_blocker_vertices.size(); ++j) {
      run_for_points(from_baryo[i], new_blocker_baryo[j], from_vertices[i],
                     new_blocker_vertices[j]);
    }
  }
  for (unsigned j = 0; j < blocker_vertices.size(); ++j) {
    auto new_from_region = triangle_subset_intersection(
        from_clipped_region,
        clip_by_plane(normal, blocker_vertices[j].dot(normal), from));

    const auto new_from_bp =
        get_points_from_subset_with_baryo(from, new_from_region);
    const auto &new_from_vertices = new_from_bp.points;
    const auto &new_from_baryo = new_from_bp.baryo;
    debug_assert(new_from_vertices.size() == new_from_baryo.size());

    for (unsigned i = 0; i < new_from_vertices.size(); ++i) {
      run_for_points(new_from_baryo[i], blocker_baryo[j], new_from_vertices[i],
                     blocker_vertices[j]);
    }
  }

  if (has_all) {
    return {.partially_shadowed = {tag_v<TriangleSubsetType::All>, {}},
            .ray_items = ray_items};
  }
  if (points_for_hull.empty()) {
    // hmm, does this mishandle only direction case? (should be very very edge
    // case...)
    return {.partially_shadowed = {tag_v<TriangleSubsetType::None>, {}},
            .ray_items = ray_items};
  }

  boost::geometry::model::multi_point<BaryoPoint> multi_point_for_hull;
  auto add_final_point = [&](const Eigen::Vector2d &p) {
    boost::geometry::append(multi_point_for_hull, BaryoPoint{p.x(), p.y()});
  };
  for (const auto &point : points_for_hull) {
    add_final_point(point);
  }

  // this is jank and slow - probably could do this better...
  // Also has numerical issues...
  double edge_total = 0.;
  for (unsigned i = 0; i < onto.vertices.size(); ++i) {
    unsigned next_i = (i + 1) % onto.vertices.size();
    edge_total += (onto.vertices[next_i] - onto.vertices[i]).norm();
  }
  double base_multiplier = edge_total + 2.;
  for (const auto &dir : directions) {
    for (const auto &point : points_for_hull) {
      auto dist_from_0 = point.norm();
      // multiplier just needs to be sufficiently large
      double multiplier = 2. * (dist_from_0 + base_multiplier);
      add_final_point(point + dir * multiplier);
    }
  }

  TriPolygon poly;
  boost::geometry::convex_hull(multi_point_for_hull, poly);

  TriPolygon triangle{{{0.0, 0.0}, {0.0, 1.0}, {1.0, 0.0}, {0.0, 0.0}}};
  debug_assert(boost::geometry::is_valid(triangle));

  auto partially_shadowed = triangle_subset_intersection(
      triangle_subset_intersection({tag_v<TriangleSubsetType::Some>, poly},
                                   {tag_v<TriangleSubsetType::Some>, triangle}),
      onto_clipped_region);

  if (partially_shadowed.type() == TriangleSubsetType::Some) {
    auto poly = partially_shadowed.get(tag_v<TriangleSubsetType::Some>);
    if (boost::geometry::area(poly) > 0.5 - 1e-12) {
      partially_shadowed = {tag_v<TriangleSubsetType::All>, {}};
    }
  }

  return {.partially_shadowed = partially_shadowed, .ray_items = ray_items};
}

ATTR_PURE_NDEBUG TriangleSubset shadowed_from_point(
    const Eigen::Vector3d &point,
    SpanSized<const Eigen::Vector3d> blocker_points, const Triangle &onto,
    const TriangleSubset &onto_clipped_region) {
  TriangleSubset full_intersection = onto_clipped_region;
  debug_assert(blocker_points.size() >= 3);

  for (unsigned j = 0; j < blocker_points.size(); ++j) {
    unsigned next_j = (j + 1) % blocker_points.size();
    unsigned next_next_j = (j + 2) % blocker_points.size();

    auto vec_0 = blocker_points[j] - point;
    auto vec_1 = blocker_points[next_j] - point;
    Eigen::Vector3d normal = vec_0.cross(vec_1).normalized();
    auto point_on_plane = blocker_points[next_j];
    // other vertex should be on positive side of plane (blocker_points is
    // assumed to be convex, planar polygon)
    if (normal.dot(blocker_points[next_next_j] - point_on_plane) < 0.) {
      normal *= -1.;
    }

    auto clipped = clip_by_plane_point(normal, point_on_plane, onto);

    full_intersection =
        triangle_subset_intersection(full_intersection, clipped);
  }

  return full_intersection;
}

ATTR_PURE_NDEBUG TotallyShadowedInfo totally_shadowed(
    const Triangle &from, const TriangleSubset &from_clipped_region,
    const Triangle &blocker, const TriangleSubset &blocker_clipped_region,
    const Triangle &onto, const TriangleSubset &onto_clipped_region) {
  // no need for the func to be called in these cases
  always_assert(from_clipped_region.type() != TriangleSubsetType::None);
  always_assert(blocker_clipped_region.type() != TriangleSubsetType::None);

  auto from_points = get_points_from_subset(from, from_clipped_region);
  auto blocker_points = get_points_from_subset(blocker, blocker_clipped_region);
  VectorT<TriangleSubset> from_each_point(from_points.size());
  TriangleSubset totally_shadowed = {tag_v<TriangleSubsetType::All>, {}};
  for (unsigned i = 0; i < from_points.size(); ++i) {
    const auto &origin = from_points[i];
    from_each_point[i] =
        shadowed_from_point(origin, blocker_points, onto, onto_clipped_region);
    totally_shadowed =
        triangle_subset_intersection(totally_shadowed, from_each_point[i]);
  }

  return {
      .totally_shadowed = totally_shadowed,
      .from_each_point = from_each_point,
  };
}
} // namespace generate_data


/*
 * Copyright 2020 Robert Bosch GmbH
 *
 * 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.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
/**
 * \file osi_omni_sensor.cpp
 * \see  osi_omni_sensor.hpp
 */

#include "osi_omni_sensor.hpp"

#include <math.h>     // for atan
#include <algorithm>  // for max
#include <cassert>    // for assert
#include <map>        // for map<>

#include <Eigen/Geometry>  // for Isometry3d, Vector3d

#include <cloe/component/lane_boundary.hpp>  // for LaneBoundary
#include <cloe/component/object.hpp>         // for Object
#include <cloe/core.hpp>                     // for Duration
#include <cloe/simulator.hpp>                // for ModelError
#include <cloe/utility/geometry.hpp>         // for quaternion_from_rpy

#include "osi_common.pb.h"           // for Timestamp, Identifier, BaseMoving, ..
#include "osi_detectedobject.pb.h"   // for DetectedMovingObject
#include "osi_hostvehicledata.pb.h"  // for HostVehicleData
#include "osi_object.pb.h"           // for MovingObject
#include "osi_sensordata.pb.h"       // for SensorData, DetectedEntityHeader
#include "osi_sensorview.pb.h"       // for SensorView

#include "osi_ground_truth.hpp"  // for OsiGroundTruth
#include "osi_utils.hpp"         // for osi_require, ..

namespace osii {

Eigen::Isometry3d osi_position_orientation_to_pose_alt(const osi3::BaseMoving& base,
                                                       const osi3::BaseMoving& base_gt) {
  const osi3::BaseMoving* base_p;
  if (base.has_orientation()) {
    base_p = &base;
  } else {
    osi_require("GroundTruth-BaseMoving::orientation", base_gt.has_orientation());
    base_p = &base_gt;
  }
  Eigen::Quaterniond quaternion = cloe::utility::quaternion_from_rpy(
      base_p->orientation().roll(), base_p->orientation().pitch(), base_p->orientation().yaw());

  osi_require("base::position", base.has_position());
  Eigen::Vector3d translation = osi_vector3d_xyz_to_vector3d(base.position());
  return cloe::utility::pose_from_rotation_translation(quaternion, translation);
}

/**
 * Convert from OSI moving object type to Cloe object classification. Note that
 * vehicles are treated explicitly in osi_mov_veh_class_map.
 */
const std::map<osi3::MovingObject::Type, cloe::Object::Class> osi_mov_obj_type_map = {
    {osi3::MovingObject_Type_TYPE_UNKNOWN, cloe::Object::Class::Unknown},
    {osi3::MovingObject_Type_TYPE_OTHER, cloe::Object::Class::Unknown},
    {osi3::MovingObject_Type_TYPE_ANIMAL, cloe::Object::Class::Unknown},
    {osi3::MovingObject_Type_TYPE_PEDESTRIAN, cloe::Object::Class::Pedestrian},
};

/**
 * Convert from OSI moving vehicle type to Cloe object classification. Note that
 * objects other than vehicles are treated explicitly in osi_mov_obj_type_map.
 */
const std::map<osi3::MovingObject::VehicleClassification::Type, cloe::Object::Class>
    osi_mov_veh_class_map = {
        {osi3::MovingObject_VehicleClassification_Type_TYPE_UNKNOWN, cloe::Object::Class::Unknown},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_OTHER, cloe::Object::Class::Unknown},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, cloe::Object::Class::Car},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, cloe::Object::Class::Car},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, cloe::Object::Class::Car},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_LUXURY_CAR, cloe::Object::Class::Car},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_DELIVERY_VAN,
         cloe::Object::Class::Truck},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_HEAVY_TRUCK,
         cloe::Object::Class::Truck},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_SEMITRAILER,
         cloe::Object::Class::Truck},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_TRAILER, cloe::Object::Class::Unknown},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE,
         cloe::Object::Class::Motorbike},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_BICYCLE, cloe::Object::Class::Bike},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_BUS, cloe::Object::Class::Truck},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_TRAM, cloe::Object::Class::Unknown},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_TRAIN, cloe::Object::Class::Unknown},
        {osi3::MovingObject_VehicleClassification_Type_TYPE_WHEELCHAIR,
         cloe::Object::Class::Unknown},
};

/**
 * Convert from OSI lane boundary types to Cloe types.
 */
const std::map<osi3::LaneBoundary_Classification_Type, cloe::LaneBoundary::Type>
    osi_lane_bdry_type_map = {
        // clang-format off
        {osi3::LaneBoundary_Classification_Type_TYPE_UNKNOWN, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_OTHER, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_NO_LINE, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_SOLID_LINE, cloe::LaneBoundary::Type::Solid},
        {osi3::LaneBoundary_Classification_Type_TYPE_DASHED_LINE, cloe::LaneBoundary::Type::Dashed},
        {osi3::LaneBoundary_Classification_Type_TYPE_BOTTS_DOTS, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_ROAD_EDGE, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_SNOW_EDGE, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_GRASS_EDGE, cloe::LaneBoundary::Type::Grass},
        {osi3::LaneBoundary_Classification_Type_TYPE_GRAVEL_EDGE, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_SOIL_EDGE, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_GUARD_RAIL, cloe::LaneBoundary::Type::Unknown},
        {osi3::LaneBoundary_Classification_Type_TYPE_CURB, cloe::LaneBoundary::Type::Curb},
        {osi3::LaneBoundary_Classification_Type_TYPE_STRUCTURE, cloe::LaneBoundary::Type::Unknown},
        // clang-format on
};

/**
 * Convert from OSI lane boundary colors to Cloe colors.
 */
const std::map<int, cloe::LaneBoundary::Color> osi_lane_bdry_color_map = {
    {osi3::LaneBoundary_Classification_Color_COLOR_UNKNOWN, cloe::LaneBoundary::Color::Unknown},
    {osi3::LaneBoundary_Classification_Color_COLOR_OTHER, cloe::LaneBoundary::Color::Unknown},
    {osi3::LaneBoundary_Classification_Color_COLOR_NONE, cloe::LaneBoundary::Color::Unknown},
    {osi3::LaneBoundary_Classification_Color_COLOR_WHITE, cloe::LaneBoundary::Color::White},
    {osi3::LaneBoundary_Classification_Color_COLOR_YELLOW, cloe::LaneBoundary::Color::Yellow},
    {osi3::LaneBoundary_Classification_Color_COLOR_RED, cloe::LaneBoundary::Color::Red},
    {osi3::LaneBoundary_Classification_Color_COLOR_BLUE, cloe::LaneBoundary::Color::Blue},
    {osi3::LaneBoundary_Classification_Color_COLOR_GREEN, cloe::LaneBoundary::Color::Green},
    {osi3::LaneBoundary_Classification_Color_COLOR_VIOLET, cloe::LaneBoundary::Color::Unknown},
};

cloe::Duration osi_timestamp_to_time(const osi3::Timestamp& timestamp) {
  return std::chrono::duration_cast<cloe::Duration>(std::chrono::seconds(timestamp.seconds()) +
                                                    std::chrono::nanoseconds(timestamp.nanos()));
}

cloe::Duration OsiOmniSensor::osi_timestamp_to_simtime(const osi3::Timestamp& timestamp) const {
  return osi_timestamp_to_time(timestamp) - this->init_time_;
}

void from_osi_identifier(const osi3::Identifier& osi_id, int& id) {
  id = static_cast<int>(osi_id.value());
}

void from_osi_host_vehicle_data(const osi3::HostVehicleData& osi_hv, cloe::Object& obj) {
  from_osi_base_moving(osi_hv.location(), obj);
}

void from_osi_detected_item_header(const osi3::DetectedItemHeader& osi_hdr, cloe::Object& obj) {
  osi_require("ground_truth_id_size == 1", osi_hdr.ground_truth_id_size() == 1);
  // Multiple ground truth objects melt into one detected item are currently not
  // supported.
  auto osi_obj_gt_id = osi_hdr.ground_truth_id(0);
  from_osi_identifier(osi_obj_gt_id, obj.id);
  // Existence probability
  if (osi_hdr.has_existence_probability()) {
    obj.exist_prob = osi_hdr.existence_probability();
  } else {
    obj.exist_prob = 1.0;
  }
}

void from_osi_detected_moving_object(const osi3::DetectedMovingObject& osi_mo, cloe::Object& obj) {
  // object id = ground truth id
  osi_require("DetectedMovingObject::header", osi_mo.has_header());
  from_osi_detected_item_header(osi_mo.header(), obj);

  // Object classification
  if (osi_mo.candidate_size() > 0) {
    osi_require("candidate_size == 1", osi_mo.candidate_size() == 1);
    from_osi_mov_obj_type_classification(osi_mo.candidate(0), obj.classification);
    // TODO(tobias): Need to additionally handle classification probability.
  } else {
    obj.classification = cloe::Object::Class::Unknown;
  }

  // DetectedMovingObject::base: "The bounding box does NOT include mirrors for
  // vehicles. The parent frame of base is the sensor's [vehicle frame]."
  from_osi_base_moving(osi_mo.base(), obj);
  // TODO(tobias): handle sensor-specific data: if (osi_mo.has_radar_specifics())
}

void from_osi_detected_moving_object_alt(const osi3::DetectedMovingObject& osi_mo,
                                         const OsiGroundTruth& ground_truth,
                                         cloe::Object& obj) {
  // Object id = ground truth id
  osi_require("DetectedMovingObject::header", osi_mo.has_header());
  from_osi_detected_item_header(osi_mo.header(), obj);

  // Get ground truth info for this object as fallback for missing data.
  osi3::MovingObject osi_mo_gt(*(ground_truth.get_moving_object(obj.id)));
  osi3::MovingObject osi_ego_gt(*(ground_truth.get_moving_object(ground_truth.get_ego_id())));
  // Transform coordinates to osi detected object convention, i.e. into ego
  // vehicle frame.
  osi_transform_base_moving(osi_ego_gt.base(), *(osi_mo_gt.mutable_base()));

  // Object classification
  if (osi_mo.candidate_size() > 0) {
    osi_require("candidate_size == 1", osi_mo.candidate_size() == 1);
    from_osi_mov_obj_type_classification(osi_mo.candidate(0), obj.classification);
    // TODO(tobias): Need to additionally handle classification probability.
  } else {
    from_osi_mov_obj_type_classification(osi_mo_gt, obj.classification);
  }

  assert(obj.id != static_cast<int>(ground_truth.get_ego_id()));
  // DetectedMovingObject::base: "The bounding box does NOT include mirrors for
  // vehicles. The parent frame of base is the sensor's [vehicle frame]."
  from_osi_base_moving_alt(osi_mo.base(), osi_mo_gt.base(), obj);
  // TODO(tobias): handle sensor-specific data: if (osi_mo.has_radar_specifics())
}

void from_osi_base_moving(const osi3::BaseMoving& osi_bm, cloe::Object& obj) {
  obj.type = cloe::Object::Type::Dynamic;

  obj.pose = osi_position_orientation_to_pose(osi_bm);

  osi_require("BaseMoving::dimension", osi_bm.has_dimension());
  obj.dimensions = osi_dimension3d_lwh_to_vector3d(osi_bm.dimension());

  osi_require("BaseMoving::acceleration", osi_bm.has_acceleration());
  obj.acceleration = osi_vector3d_xyz_to_vector3d(osi_bm.acceleration());

  osi_require("BaseMoving::velocity", osi_bm.has_velocity());
  obj.velocity = osi_vector3d_xyz_to_vector3d(osi_bm.velocity());

  osi_require("BaseMoving::orientation_rate", osi_bm.has_orientation_rate());
  obj.angular_velocity = osi_orientation3d_rpy_to_vector3d(osi_bm.orientation_rate());
}

void from_osi_base_moving_alt(const osi3::BaseMoving& osi_bm, const osi3::BaseMoving& osi_bm_gt,
                              cloe::Object& obj) {
  obj.type = cloe::Object::Type::Dynamic;

  obj.pose = osi_position_orientation_to_pose_alt(osi_bm, osi_bm_gt);

  assert(osi_bm.has_dimension());
  obj.dimensions = osi_dimension3d_lwh_to_vector3d(osi_bm.dimension());

  assert(osi_bm.has_acceleration());
  obj.acceleration = osi_vector3d_xyz_to_vector3d(osi_bm.acceleration());

  assert(osi_bm.has_velocity());
  obj.velocity = osi_vector3d_xyz_to_vector3d(osi_bm.velocity());

  if (osi_bm.has_orientation_rate()) {
    obj.angular_velocity = osi_orientation3d_rpy_to_vector3d(osi_bm.orientation_rate());
  } else {
    assert(osi_bm_gt.has_orientation_rate());
    obj.angular_velocity = osi_orientation3d_rpy_to_vector3d(osi_bm_gt.orientation_rate());
  }
}

template <typename T>
void from_osi_mov_obj_type_classification(const T& osi_mo, cloe::Object::Class& oc) {
  if (!osi_mo.has_type()) {
    throw cloe::ModelError("OSI missing moving object type");
  }

  if (osi_mo.type() == osi3::MovingObject_Type_TYPE_VEHICLE) {
    if (!osi_mo.has_vehicle_classification()) {
      throw cloe::ModelError("OSI missing moving vehicle classification");
    }
    if (!osi_mo.vehicle_classification().has_type()) {
      throw cloe::ModelError("OSI missing moving vehicle classification type");
    }
  }

  from_osi_mov_obj_type_classification(osi_mo.type(), osi_mo.vehicle_classification().type(), oc);
}

template void from_osi_mov_obj_type_classification<osi3::MovingObject>(
    const osi3::MovingObject& osi_mo, cloe::Object::Class& oc);
template void
from_osi_mov_obj_type_classification<osi3::DetectedMovingObject::CandidateMovingObject>(
    const osi3::DetectedMovingObject::CandidateMovingObject& osi_mo, cloe::Object::Class& oc);

void from_osi_mov_obj_type_classification(
    const osi3::MovingObject::Type& osi_ot,
    const osi3::MovingObject::VehicleClassification::Type& osi_vt,
    cloe::Object::Class& oc) {
  if (osi_ot == osi3::MovingObject_Type_TYPE_VEHICLE) {
    oc = osi_mov_veh_class_map.at(osi_vt);
  } else {
    oc = osi_mov_obj_type_map.at(osi_ot);
  }
}

void transform_ego_coord_from_osi_data(const Eigen::Vector3d& dimensions_gt, cloe::Object& obj) {
  // obj->pose: Change object position from bbox-center to vehicle reference
  // point (rear axle/street level):
  //  - Shift (x,y) to rear axis center using given osi bbcenter_to_rear vector.
  //  - Shift (z) to street level using bbox half-height.
  Eigen::Vector3d bbcenter_to_rear_street{obj.cog_offset(0), obj.cog_offset(1),
                                          -0.5 * dimensions_gt(2)};

  // Transform translation vector from vehicle frame into world frame.
  Eigen::Vector3d pos_veh_origin =
      obj.pose.translation() + obj.pose.rotation() * bbcenter_to_rear_street;

  obj.pose.translation() = pos_veh_origin;

  // cog is on street level, i.e. only x-offset is non-zero. Here, the direction
  // is opposite as defined in the OSI standard.
  obj.cog_offset = Eigen::Vector3d(-1.0 * obj.cog_offset(0), 0.0, 0.0);

  // Convert ego velocity and acceleration into ego vehicle frame coordinates.
  obj.velocity = obj.pose.rotation().inverse() * obj.velocity;
  obj.acceleration = obj.pose.rotation().inverse() * obj.acceleration;
}

void transform_obj_coord_from_osi_data(const Eigen::Isometry3d& sensor_pose,
                                       const Eigen::Vector3d& dimensions_gt, cloe::Object& obj) {
  // obj->pose/velocity/acceleration/angular_velocity:
  // Transform the location and orientation of the detected object from the ego
  // vehicle frame into the sensor reference frame.
  Eigen::Vector3d obj_pos_sensor_frame =
      sensor_pose.rotation().inverse() * (obj.pose.translation() - sensor_pose.translation());
  obj.pose.translation() = obj_pos_sensor_frame;
  obj.pose.rotate(sensor_pose.rotation().inverse());

  obj.velocity = sensor_pose.rotation().inverse() * obj.velocity;
  obj.acceleration = sensor_pose.rotation().inverse() * obj.acceleration;
  obj.angular_velocity = sensor_pose.rotation().inverse() * obj.angular_velocity;

  // obj->pose: Change the object position reference point from the bounding box
  // center to the vehicle reference point (rear axle/street level).
  Eigen::Vector3d bbcenter_to_rear_street{obj.cog_offset(0), obj.cog_offset(1),
                                          -0.5 * dimensions_gt(2)};

  // Transform translation vector from the object reference frame into the
  // sensor frame.
  obj_pos_sensor_frame = obj.pose.translation() + obj.pose.rotation() * bbcenter_to_rear_street;

  obj.pose.translation() = obj_pos_sensor_frame;

  // cog is on street level, i.e. only x-offset is non-zero. Here, the direction
  // is opposite as defined in the OSI standard.
  obj.cog_offset = Eigen::Vector3d(-1.0 * obj.cog_offset(0), 0.0, 0.0);
}

void OsiOmniSensor::step(const cloe::Sync& s, const bool& restart, cloe::Duration& sim_time) {
  // Cycle until sensor data has been received.
  int n_msg{0};
  while (n_msg == 0 || restart) {
    auto osi_msg = osi_comm_->receive_sensor_data();
    if (osi_msg.size() > 0) {
      osi_logger()->trace("OsiOmniSensor: processing {} messages at Cloe frame no {}",
                          osi_msg.size(), s.step());
      // 1st. timestep: Store the simulation reference (e.g. start) time.
      this->process(osi_msg[0]->timestamp());
    }
    for (auto m : osi_msg) {
      this->process(m.get(), sim_time);
      ++n_msg;
    }
  }

  if (abs(sim_time.count() - s.time().count()) >= s.step_width().count() / 100) {
    // Sensor data time deviates from cloe time by more than 1% of the time step.
    osi_logger()->warn("OsiOmniSensor: inconsistent timestamps [t_sensor={}ns, t_cloe={}ns]",
                       sim_time.count(), s.time().count());
  }

  osi_logger()->trace("OsiOmniSensor: completed processing messages [frame={}, time={}ns]",
                      s.step(), s.time().count());
}

void OsiOmniSensor::process(const osi3::Timestamp& timestamp) {
  // TODO(tobias): probably needs to be changed for restarts
  if (init_time_.count() >= 0.0) {
    return;
  }
  init_time_ = osi_timestamp_to_time(timestamp);
}

void OsiOmniSensor::process(osi3::SensorData* osi_sd, cloe::Duration& sim_time) {
  if (osi_sd == nullptr) {
    return;
  }

  if (osi_sd->ByteSize() == 0) {
    return;
  }

  osi_require("v3.x.x", !osi_sd->has_version() || osi_sd->version().version_major() > 2);

  // TODO(tobias): handle restart

  // Read the time when the message was sent, which is after capturing and
  // processing the sensor raw signal.
  if (osi_sd->has_timestamp()) {
    sim_time = osi_timestamp_to_simtime(osi_sd->timestamp());
    osi_logger()->trace("OsiOmniSensor: message @ {} ns", sim_time.count());
  } else {
    throw cloe::ModelError("OsiOmniSensor: No timestamp in SensorData. FMU properly loaded?");
  }

  // Read the time of the ground truth scene that was processed.
  if (osi_sd->has_last_measurement_time()) {
    cloe::Duration meas_time = osi_timestamp_to_simtime(osi_sd->last_measurement_time());
    osi_logger()->trace("OsiOmniSensor: measurement @ {} ns", meas_time.count());
  } else {
    osi_logger()->info("OsiOmniSensor: last_measurement_time not available in SensorData.");
  }

  // Obtain ego data from sensor views (sensor model input), i.e. ground truth.
  osi_require("SensorData::SensorView", osi_sd->sensor_view_size() > 0);
  const osi3::MountingPosition* mnt_pos{nullptr};
  for (int i_sv = 0; i_sv < osi_sd->sensor_view_size(); ++i_sv) {
    this->process(osi_sd->sensor_view(i_sv));
    if (osi_sd->sensor_view(i_sv).has_mounting_position()) {
      mnt_pos = &(osi_sd->sensor_view(i_sv).mounting_position());
    }
  }

  if (osi_sd->has_mounting_position()) {
    // Give higher priority to the sensor model output (SensorData) than to SensorView.
    mnt_pos = &(osi_sd->mounting_position());
  }

  // Store sensor mounting position and orientation for reference frame transformations.
  if (mnt_pos) {
    osi_sensor_pose_ = osi_position_orientation_to_pose(*mnt_pos);
  } else {
    if (this->get_mock_level(SensorMockTarget::MountingPosition) !=
        SensorMockLevel::OverwriteNone) {
      osi_sensor_pose_ =
          get_static_mounting_position(ground_truth_->get_veh_coord_sys_info(owner_id_),
                                       ground_truth_->get_mov_obj_dimensions(owner_id_));
    } else {
      throw cloe::ModelError("OSI sensor mounting position is not available");
    }
  }

  if (osi_sd->has_host_vehicle_location()) {
    // Sensor has its own estimate of the vehicle location, which we could use
    // to overwrite the ego pose that was taken from ground truth.
    throw cloe::ModelError("OSI host_vehicle_location handling is not yet available");
  }

  // Process detected moving objects.
  for (int i_mo = 0; i_mo < osi_sd->moving_object_size(); ++i_mo) {
    this->process(osi_sd->has_moving_object_header(), osi_sd->moving_object_header(),
                  osi_sd->moving_object(i_mo));
  }

  // TODO(tobias): Process detected stationary objects.

  // Process lane boundaries.
  switch (this->get_mock_level(SensorMockTarget::DetectedLaneBoundary)) {
    case SensorMockLevel::OverwriteAll: {
      mock_detected_lane_boundaries();
      break;
    }
    default: {
      //  TODO(tobias): Detected road marking handling is not yet available.
      break;
    }
  }

  // TODO(tobias): Process detected lanes once supported by Cloe data model.

  // TODO(tobias): Process detected traffic signs.

  // TODO(tobias): Process detected traffic lights once supported by Cloe data model.

  store_sensor_meta_data(ground_truth_->get_veh_coord_sys_info(owner_id_),
                         ground_truth_->get_mov_obj_dimensions(owner_id_));

  // Cleanup
  ground_truth_->reset();
}

void OsiOmniSensor::process(const osi3::SensorView& osi_sv) {
  if (osi_sv.ByteSize() == 0) {
    return;
  }

  // Fill the coordinate system info from ground truth.
  osi_require("SensorView::GroundTruth", osi_sv.has_global_ground_truth());
  const osi3::GroundTruth* osi_gt = &(osi_sv.global_ground_truth());
  ground_truth_->set(*osi_gt);

  for (int i_mo = 0; i_mo < osi_gt->moving_object_size(); ++i_mo) {
    osi3::MovingObject osi_mo = osi_gt->moving_object(i_mo);
    int obj_id;
    from_osi_identifier(osi_mo.id(), obj_id);

    // Store geometric information of different object reference frames.
    if (osi_mo.has_vehicle_attributes()) {
      ground_truth_->store_veh_coord_sys_info(obj_id, osi_mo.vehicle_attributes());
    }

    // Store object bounding box dimensions for cooordinate transformations.
    osi_require("GroundTruth::MovingObject::base", osi_mo.has_base());
    if (osi_mo.has_base()) {
      osi_require("GroundTruth-BaseMoving::dimension", osi_mo.base().has_dimension());
      ground_truth_->store_mov_obj_dimensions(obj_id, osi_mo.base().dimension());
    }
  }

  // Process ego vehicle info. For the ego, we may use ground truth information.
  // Note: osi.sv.host_vehicle_id() may not be populated.
  auto osi_ego = ground_truth_->get_moving_object(ground_truth_->get_ego_id());
  process(osi_sv.has_host_vehicle_data(), osi_sv.host_vehicle_data(), *osi_ego);
}

void OsiOmniSensor::process(const bool has_veh_data, const osi3::HostVehicleData& osi_hv,
                            const osi3::MovingObject& osi_ego) {
  auto obj = std::make_shared<cloe::Object>();
  obj->exist_prob = 1.0;
  // Object id
  from_osi_identifier(osi_ego.id(), obj->id);
  assert(obj->id == static_cast<int>(owner_id_));

  // Ego pose
  if (has_veh_data) {
    // Ego data that was explicitly made available to the sensor (e.g. gps
    // location & rmse).
    from_osi_host_vehicle_data(osi_hv, *obj);
  } else {
    // Use ground truth object information
    from_osi_base_moving(osi_ego.base(), *obj);
  }

  // Data extracted from ground truth:
  //  - Vehicle type
  from_osi_mov_obj_type_classification(osi_ego, obj->classification);
  //  - Offset to vehicle frame origin
  obj->cog_offset = ground_truth_->get_veh_coord_sys_info(obj->id);

  // Store ego pose.
  osi_ego_pose_ = obj->pose;
  osi_ego_pose_.translation() = obj->pose.translation() + obj->pose.rotation() * obj->cog_offset;

  // Object attributes are all set:
  //  - 1a) osi3::HostVehicleData: "All coordinates and orientations are relative
  //        to the global ground truth coordinate system."
  //  - 1b) "All position coordinates refer to the center of the bounding box of
  //         the object (vehicle or otherwise)."
  //  - 2 ) osi3::MovingObject::VehicleAttributes::bbcenter_to_rear: "The vector
  //        pointing from the bounding box center point to the middle of the rear
  //        axle under neutral load conditions. In object coordinates."
  // Now transform the data into the Cloe reference frame convention:
  //  - 1a) obj->velocity/acceleration: Convert from world frame into vehicle
  //        frame coordinates.
  //  - 1b) obj->pose: Change object position from bbox-center to vehicle
  //        reference point (rear axle/street level).
  //  - 2 ) obj->cog_offset: cog should be on street level, i.e. only x-offset is
  //        non-zero. Here, the direction is opposite as defined by OSI.
  transform_ego_coord_from_osi_data(ground_truth_->get_mov_obj_dimensions(obj->id), *obj);
  store_ego_object(obj);  // XXX is this fine for multiple sensor views?
}

void OsiOmniSensor::process(const bool has_eh, const osi3::DetectedEntityHeader& osi_eh,
                            const osi3::DetectedMovingObject& osi_mo) {
  auto obj = std::make_shared<cloe::Object>();

  // Get object information. The sensor (model) may not provide all required data.
  if (has_eh) {
    // TODO(tobias): handle entity header, if needed
    osi_logger()->warn(
        "VtdOsiSensor: DetectedEntityHeader not yet handled. measurement_time = {}ns",
        osi_timestamp_to_simtime(osi_eh.measurement_time()).count());
  }
  switch (this->get_mock_level(SensorMockTarget::DetectedMovingObject)) {
    case SensorMockLevel::OverwriteNone: {
      from_osi_detected_moving_object(osi_mo, *obj);
      break;
    }
    case SensorMockLevel::InterpolateMissing: {
      from_osi_detected_moving_object_alt(osi_mo, *ground_truth_, *obj);
      break;
    }
    case SensorMockLevel::OverwriteAll: {
      throw cloe::ModelError(
          "OSI SensorMockLevel::OverwriteAll not available for DetectedMovingObject");
      break;
    }
  }

  assert(obj->id != static_cast<int>(owner_id_));

  // Offset to the vehicle frame origin
  obj->cog_offset = ground_truth_->get_veh_coord_sys_info(obj->id);

  // Object attributes are all set:
  //  - 1a) DetectedMovingObject::base: "The parent frame of base is the sensor's
  //        [vehicle frame]."
  //  - 1b) "All position coordinates refer to the center of the bounding box of
  //         the object (vehicle or otherwise)."
  //  - 2 ) osi3::MovingObject::VehicleAttributes::bbcenter_to_rear: "The vector
  //        pointing from the bounding box center point to the middle of the rear
  //        axle under neutral load conditions. In object coordinates."
  // Now transform the data to the Cloe reference frame:
  //  - 1a) obj->pose/velocity/acceleration/angular_velocity: Transform detected
  //        object location from the ego vehicle frame into the sensor frame.
  //  - 1b) obj->pose: Change object position from bbox-center to vehicle
  //        reference point (rear axle/street level).
  //  - 2 ) obj->cog_offset: cog should be on street level, i.e. only x-offset is
  //        non-zero. Here, the direction is opposite as defined by OSI.
  transform_obj_coord_from_osi_data(osi_sensor_pose_,
                                    ground_truth_->get_mov_obj_dimensions(obj->id), *obj);

  // Fill the object list
  store_object(obj);
}

void OsiOmniSensor::from_osi_boundary_points(const osi3::LaneBoundary& osi_lb,
                                             cloe::LaneBoundary& lb) {
  assert(osi_lb.boundary_line_size() > 0);
  for (int i = 0; i < osi_lb.boundary_line_size(); ++i) {
    const auto& osi_pt = osi_lb.boundary_line(i);
    Eigen::Vector3d position = osi_vector3d_xyz_to_vector3d(osi_pt.position());
    // Transform points from the inertial into the sensor reference frame.
    cloe::utility::transform_to_child_frame(osi_ego_pose_, &position);
    cloe::utility::transform_to_child_frame(osi_sensor_pose_, &position);
    lb.points.push_back(position);
  }
  // Compute clothoid segment. TODO(tobias): implement curved segments.
  lb.dx_start = lb.points.front()(0);
  lb.dy_start = lb.points.front()(1);
  lb.heading_start = std::atan((lb.points.back()(1) - lb.points.front()(1)) /
                               (lb.points.back()(0) - lb.points.front()(0)));
  lb.curv_hor_start = 0.0;
  lb.curv_hor_change = 0.0;
  lb.dx_end = lb.points.back()(0);
}

void OsiOmniSensor::mock_detected_lane_boundaries() {
  const auto& osi_gt = ground_truth_->get_gt();
  int lb_id = 0;
  // If some of the OSI data does not have an id, avoid id clashes.
  for (const auto& osi_lb : osi_gt.lane_boundary()) {
    if (osi_lb.has_classification() && osi_lb.has_id()) {
      int id;
      from_osi_identifier(osi_lb.id(), id);
      lb_id = std::max(lb_id, id + 1);
    }
  }
  // Set lane boundary data.
  for (const auto& osi_lb : osi_gt.lane_boundary()) {
    if (osi_lb.has_classification()) {
      cloe::LaneBoundary lb;
      if (osi_lb.has_id()) {
        from_osi_identifier(osi_lb.id(), lb.id);
      } else {
        lb.id = lb_id;
      }
      lb.exist_prob = 1.0;
      lb.prev_id = -1;  // no concatenated line segments for now
      lb.next_id = -1;
      ++lb_id;
      from_osi_boundary_points(osi_lb, lb);
      lb.type = osi_lane_bdry_type_map.at(osi_lb.classification().type());
      lb.color = osi_lane_bdry_color_map.at(osi_lb.classification().color());
      store_lane_boundary(lb);
    }
  }
}

}  // namespace osii


#ifndef __WALD_SEPARATE_METHOD_H__
#define __WALD_SEPARATE_METHOD_H__

#include <string>
#include <vector>

#include <armadillo>

#include <besiq/method/method.hpp>
#include <besiq/stats/log_scale.hpp>

/**
 * This class is responsible for executing the closed form
 * wald test for a logistic regression model.
 */
class wald_separate_method
: public method_type
{
public:
    /**
     * Constructor.
     *
     * @param data Additional data required by all methods, such as
     *             covariates.
     */
    wald_separate_method(method_data_ptr data, bool is_lm);

    /**
     * @see method_type::init.
     */
    virtual std::vector<std::string> init();

    /**
     * @see method_type::run.
     */
    virtual double run(const snp_row &row1, const snp_row &row2, float *output);
private:
    void compute_lm(const snp_row &row1, const snp_row &row2, float *output);
    void compute_binomial(const snp_row &row1, const snp_row &row2, float *output);
    /**
     * Weight for each sample.
     */
    arma::vec m_weight;

    /**
     * Indicates whether this is a linear model or not.
     */
    bool m_is_lm;
};

#endif /* End of __WALD_SEPARATE_METHOD_H__ */


#include <algorithm>
#include <string>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>

#include "main.hpp"
#include "imagereconstruct.hpp"
#include "image.hpp"
#include "segments.hpp"
#include "descriptors.hpp"
#include "classify.hpp"
#include "misc.hpp"

using namespace std;
using namespace cv;
namespace fs = boost::filesystem;

int main(int argc, char ** argv) {
    const int RESIZE_FACTOR = 2;
    const string DATA_PATH = "/workspace/steam/opencv-steam-captcha/data";
    const string OUTPUT_PATH = "/workspace/steam/opencv-steam-captcha/output/";
int notX=1;
    // 0, 1, 5, 6, I, O and S are never used
    const string ALLOWED_CHARS = "234789ABCDEFGHJKLMNPQRTUVWXYZ@&%";

    // Images
    Mat sourceImage, finalImage, histogramImage;
    Mat histogram;

    // Initialize character counters
    map < string, int > counter;
    for (int i = 0; i < ALLOWED_CHARS.length(); i++) {
      string letter(1, ALLOWED_CHARS[i]);
      counter[letter] = 0;
    }

    // Check if data folder exists
    fs::path folder(DATA_PATH);
    if (!exists(folder))
      return -1;

    // Create output folder structure
    if (!createFolderStructure(OUTPUT_PATH, ALLOWED_CHARS))
      return -1;

    fs::directory_iterator endItr;
    for (fs::directory_iterator itr(folder); itr != endItr; itr++) {
      string fullPath = itr -> path().string();
      string fileName = itr -> path().filename().string();

      // Skip all dot files
      if (fileName[0] == '.')
        continue;

      // Retrieve captcha string
      string captchaCode = boost::replace_all_copy(fileName, ".png", "");
      captchaCode = aliasToSpecialChar(captchaCode);

      // Load our base image
      sourceImage = imread(fullPath, CV_LOAD_IMAGE_GRAYSCALE);

      // Is it loaded?
      if (!sourceImage.data)
        return -1;

      // Resize the image by resize factor
      resize(sourceImage, sourceImage, Size(sourceImage.cols * RESIZE_FACTOR, sourceImage.rows * RESIZE_FACTOR));

      // Define our final image
      finalImage = sourceImage.clone();

      // Apply adaptive threshold
      //adaptiveThreshold(finalImage, finalImage, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, 3, 1);

      // Use the thresholded image as a mask
      Mat tmp;
      sourceImage.copyTo(tmp, finalImage);
      tmp.copyTo(finalImage);
      tmp.release();

      // Let's calculate histogram for our image
      histogram = createHistogram(finalImage);

      // Calculate final threshold value
      int thresholdValue = getIdealThreshold(histogram);

      // Draw histogram image
      histogramImage = drawHistogram(histogram, thresholdValue);

      // Apply binary threshold
      //threshold(finalImage, finalImage, thresholdValue, 255, THRESH_BINARY);

      // Morphological closing
      Mat element = getStructuringElement(MORPH_ELLIPSE, Size(3, 3));
      dilate(finalImage, finalImage, element);
      erode(finalImage, finalImage, element);

      // Segments
      int * segH = horizontalSegments(finalImage);
      int * segV = verticalSegments(finalImage);

      Mat segHImage = drawHorizontalSegments(segH, finalImage.rows, finalImage.cols);
      Mat segVImage = drawVerticalSegments(segV, finalImage.rows, finalImage.cols);

      // Create pairs
      vector < pair < int, int > > verticalPairs = filterVerticalPairs(createSegmentPairs(segV, finalImage.rows));
      vector < pair < int, int > > horizontalPairs = splitLarge(filterHorizontalPairs(createSegmentPairs(segH, finalImage.cols), finalImage.cols));

      // Get segment squares
      vector < Rectangle > squares = takeRectangles(shrinkRectangles(finalImage, getRectangles(verticalPairs, horizontalPairs)), 6);

      // Save the squares
      saveRectangles(sourceImage, squares, OUTPUT_PATH, captchaCode, counter);
      // Let's draw the rectangles
      drawRectangles(finalImage, squares);
      drawRectangles(sourceImage, squares);

      // Display the images if necessary

	  if(notX ==12){
       imshow("Final image", finalImage);
       imshow("Source image", sourceImage);
       imshow("HSeg", segHImage);
       imshow("VSeg", segVImage);
       imshow("Histogram", histogramImage);
       waitKey();
	   notX=2;
	}
      sourceImage.release();
      finalImage.release();
    }

    Mat trainingData, classLabels;

    // Create training data
    getSimpleTrainingData(trainingData, classLabels, OUTPUT_PATH, "G", "Y", 10);

    // This part is highly simplified and was mostly done just to test
    // classification based on two characters with the highest frequency
    CvSVMParams params;
    params.svm_type = CvSVM::C_SVC;
    params.kernel_type = CvSVM::LINEAR;
    params.term_crit = cvTermCriteria(CV_TERMCRIT_ITER, 100, 1e-6);

    CvSVM SVM;
    SVM.train(trainingData, classLabels, Mat(), Mat(), params);

    int success = 0;
	int fail=0;
    for (int i = 0; i < 10; i++) {

      Mat letterImage = imread(OUTPUT_PATH + "2/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      float result = classify(SVM, letterImage);
      if (result == 1) {
        cout << "2 classified as: 2" << endl;
        success++;
      } else {
		cout << "!!! 2 misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
	  letterImage.release();

	  letterImage = imread(OUTPUT_PATH + "3/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 2) {
        cout << "3 classified as: 3" << endl;
        success++;
      } else {
		  		cout << "!!! 3 misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
      letterImage.release();
	letterImage = imread(OUTPUT_PATH + "4/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 3) {
        cout << "4 classified as: 4" << endl;
        success++;
      } else {
		  		cout << "!!! 4 misrecognised" << endl;

        // cout << "FAIL G" << endl;

 fail++;
      }
      letterImage.release();

	  letterImage = imread(OUTPUT_PATH + "7/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 4) {
        cout << "7 classified as: 7" << endl;
        success++;
      } else {
		  		cout << "!!! 7 misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
      letterImage.release();

	  letterImage = imread(OUTPUT_PATH + "8/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 5) {
        cout << "8 classified as: 8" << endl;
        success++;
      } else {
		  		cout << "!!! 8 misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
      letterImage.release();

	  letterImage = imread(OUTPUT_PATH + "9/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 6) {
        cout << "9 classified as: 9" << endl;
        success++;
      } else {
		  		cout << "!!! 9 misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
      letterImage.release();

	  letterImage = imread(OUTPUT_PATH + "and/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 33) {
        cout << "& classified as: &" << endl;
        success++;
      } else {
		  		cout << "!!! & misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
      letterImage.release();

	  letterImage = imread(OUTPUT_PATH + "at/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 34) {
        cout << "@ classified as: @" << endl;
        success++;
      } else {
		  		cout << "!!! @ misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
      letterImage.release();

  	  letterImage = imread(OUTPUT_PATH + "pct/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
      result = classify(SVM, letterImage);
      if (result == 35) {
        cout << "% classified as: %" << endl;
        success++;
      } else {
		  		cout << "!!! % misrecognised" << endl;

        // cout << "FAIL G" << endl;
 fail++;
      }
      letterImage.release();

	  for (int tmp = 66; tmp <= 90; tmp++) {
        if (tmp == 73 || tmp == 79 || tmp == 83)
          continue;
        else {

          letterImage = imread(OUTPUT_PATH + char(tmp)+"/" + to_string(i) + ".png", CV_LOAD_IMAGE_GRAYSCALE);
          result = classify(SVM, letterImage);
          //cout<<"R: "<<result<<endl;

          if (result == tmp-66+8) {
            cout << "Letter "<<char(tmp)<<" classified as: "<<char(result-7+65) << endl;
            success++;
          } else {
            // cout << "FAIL Y" << endl;
			            cout << "!!!Letter "<<char(tmp)<<" classified as: "<<char(result-7+65) <<"[R:"<<result<<"]"<<endl;
						fail++;
          }
          letterImage.release();

        }

      }

      trainingData.release();
      classLabels.release();
}
float rate = ((success*100/(success+fail)));
      cout << "Success rate: " << success << "/" << (success+fail) <<" = "<< rate<<"%"<< endl;

      return 0;
    }