#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE MatrixIterator

#include <iostream>
#include <boost/test/unit_test.hpp>
#include <geomc/linalg/mtxdetail/MatrixLayout.h>

using namespace geom;
using namespace std;

namespace std {

    template <typename T, bool Const>
    std::ostream& operator<<(std::ostream& o, const geom::TransposeIterator<T,Const>& i) {
        o << "<" << i.base << "[" << (i.p - i.base) << "] = " << (*i) << ">";
        return o;
    }
}

#define TEST_ROWS 13
#define TEST_COLS  7

// xxx todo: test negative offsets

typedef FlatMatrixLayout<index_t, ROW_MAJOR> rowmaj;
typedef FlatMatrixLayout<index_t, COL_MAJOR> colmaj;

struct MatrixFixture {
    const index_t rows;
    const index_t cols;
    index_t rowmtx[TEST_ROWS * TEST_COLS];
    index_t colmtx[TEST_ROWS * TEST_COLS];

    MatrixFixture():
            rows(TEST_ROWS),
            cols(TEST_COLS) {
        index_t i = 0;
        for (index_t r = 0; r < rows; ++r) {
            for (index_t c = 0; c < cols; ++c, ++i) {
                rowmtx[r * cols + c] = i;
                colmtx[c * rows + r] = i;
            }
        }
    }

    virtual ~MatrixFixture() {}
};

BOOST_FIXTURE_TEST_SUITE(matrix_iterator, MatrixFixture)

BOOST_AUTO_TEST_CASE(verify_mtx_index) {
    // verify that row and column layouts are mutual transposes
    for (index_t r = 0; r < rows; ++r) {
        for (index_t c = 0; c < cols; ++c) {
            BOOST_CHECK_EQUAL(
                rowmaj::index(r,c, rows, cols),
                colmaj::index(c,r, cols, rows));
        }
    }
}

BOOST_AUTO_TEST_CASE(verify_row_order) {
    // verify that row and column layouts agree about the ordering of/within individual rows
    index_t i = 0;
    for (index_t r = 0; r < rows; ++r) {
        auto r_i = rowmaj::row<false>(rowmtx, r, rows, cols);
        auto c_i = colmaj::row<false>(colmtx, r, rows, cols);
        for (index_t c = 0; c < cols; ++c, ++r_i, ++c_i, ++i) {
            BOOST_CHECK_EQUAL(*r_i, *c_i); // agreement?
            BOOST_CHECK_EQUAL(*r_i, i);    // not just wrong in the same way?
        }
    }
}

BOOST_AUTO_TEST_CASE(verify_col_order) {
    // verify that row and column layouts agree about the ordering of/within individual columns
    for (index_t c = 0; c < cols; ++c) {
        auto r_i = rowmaj::col<false>(rowmtx, c, rows, cols);
        auto c_i = colmaj::col<false>(colmtx, c, rows, cols);
        for (index_t r = 0; r < rows; ++r, ++r_i, ++c_i) {
            BOOST_CHECK_EQUAL(*r_i, *c_i);
        }
    }
}

BOOST_AUTO_TEST_CASE(verify_row_iteration) {
    // verify that row and column layouts row-traverse the entire array in the same order
    // (may fail, e.g., at row or column boundaries)
    auto r_ir = rowmaj::row<false>(rowmtx, 0, rows, cols);
    auto c_ir = colmaj::row<false>(colmtx, 0, rows, cols);
    for (index_t i = 0; i < rows * cols; ++i, ++r_ir, ++c_ir) {
        BOOST_CHECK_EQUAL(*r_ir, *c_ir);
    }
}

BOOST_AUTO_TEST_CASE(verify_col_iteration) {
    // verify that row and column layouts column-traverse the entire array in the same order
    // (may fail, e.g., at row or column boundaries)
    auto r_ic = rowmaj::col<false>(rowmtx, 0, rows, cols);
    auto c_ic = colmaj::col<false>(colmtx, 0, rows, cols);
    for (index_t i = 0; i < rows * cols; ++i, ++r_ic, ++c_ic) {
        BOOST_CHECK_EQUAL(*r_ic, *c_ic);
    }
}

BOOST_AUTO_TEST_CASE(verify_offset_increment_row) {
    // verify that "offsetting by `n`" and "incrementing `n` times" are the same for row iterators.
    for (index_t r = 0; r < rows; ++r) {
        auto r_i = rowmaj::row<false>(rowmtx, r, rows, cols);
        auto c_i = colmaj::row<false>(colmtx, r, rows, cols);
        for (index_t c = 0; c < cols; ++c, ++r_i, ++c_i) {
            auto r_j = rowmaj::row<false>(rowmtx, r, rows, cols) + c;
            auto c_j = colmaj::row<false>(colmtx, r, rows, cols) + c;
            // iters consider themselves equal?
            BOOST_CHECK_EQUAL(r_i, r_j);
            BOOST_CHECK_EQUAL(c_i, c_j);
            // iters point to same location?
            BOOST_CHECK_EQUAL(*r_i, *r_j);
            BOOST_CHECK_EQUAL(*c_i, *c_j);
        }
    }
}

BOOST_AUTO_TEST_CASE(verify_offset_increment_col) {
    // verify that "offsetting by `n`" and "incrementing `n` times" are the same for column iterators.
    for (index_t c = 0; c < cols; ++c) {
        auto r_i = rowmaj::col<false>(rowmtx, c, rows, cols);
        auto c_i = colmaj::col<false>(colmtx, c, rows, cols);
        for (index_t r = 0; r < rows; ++r, ++r_i, ++c_i) {
            auto r_j = rowmaj::col<false>(rowmtx, c, rows, cols) + r;
            auto c_j = colmaj::col<false>(colmtx, c, rows, cols) + r;
            // iters consider themselves equal?
            BOOST_CHECK_EQUAL(r_i, r_j);
            BOOST_CHECK_EQUAL(c_i, c_j);
            // iters point to same location?
            BOOST_CHECK_EQUAL(*r_i, *r_j);
            BOOST_CHECK_EQUAL(*c_i, *c_j);
        }
    }
}

BOOST_AUTO_TEST_CASE(verify_distance) {
    // verify that distance() and advance() agree
    for (index_t r_src = 0; r_src < rows; ++r_src) {
        for (index_t c_src = 0; c_src < cols; ++c_src) {
            auto src_i_r = rowmaj::row<false>(rowmtx, r_src, rows, cols) + c_src;
            auto src_i_c = colmaj::row<false>(colmtx, r_src, rows, cols) + c_src;
            for (index_t r_dst = 0; r_dst < rows; ++r_dst) {
                for (index_t c_dst = 0; c_dst < cols; ++c_dst) {
                    auto dst_i_r = rowmaj::row<false>(rowmtx, r_dst, rows, cols) + c_dst;
                    auto dst_i_c = colmaj::row<false>(colmtx, r_dst, rows, cols) + c_dst;
                    auto r_dx = dst_i_r - src_i_r;
                    auto c_dx = dst_i_c - src_i_c;
                    // iterators consider themselves the same?
                    BOOST_CHECK_EQUAL(src_i_r + r_dx, dst_i_r);
                    BOOST_CHECK_EQUAL(src_i_c + c_dx, dst_i_c);
                    // iters point to same location?
                    BOOST_CHECK_EQUAL(*(src_i_r + r_dx), *(dst_i_r));
                    BOOST_CHECK_EQUAL(*(src_i_c + c_dx), *(dst_i_c));
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


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

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

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing cosd capabilities

    cosine of the input in degree.

    @par Semantic:

    For every parameter of floating type T

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

    is similar to:

    @code
    T r = cos(inrad(x));
    @endcode

    As other cosine functions cosd can be used with two parameters as
    @code
    T r = cos(x,  range_);
    @endcode

    see @ref cos for further details

    @see sincosd, cos, cospi

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

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

#endif


#ifndef __TWO_LIDARS_ODOM_HH__
#define __TWO_LIDARS_ODOM_HH__

#include <cmath>
#include <vector>
#include <limits>
#include <numeric>
#include <iostream>

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

#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

#include <laser_proc.hh>
#include <utils.hh>

/*
	This class gets inputs from the two lidars scanners with a configuration
	as in the Inspection KHex platform and IMU data to estimate the incremental
	pose of the robot.
*/

class TwoLidarsOdom{
private:
	Eigen::Matrix4d _pose;
	int _max_iter;
	double _xyz_tol;
	double _rot_tol;
public:
	TwoLidarsOdom(int max_iter = 23, double xyz_tol = 0.001, double yaw_tol = 0.001);
	// This function provides an interface to provide laser scan
	// data to the odometry estimator. The 0th indices are assumed
	// to be older set of lidar data. The output is the two-frames
	// odometry. The global pose of the robot can be obtained by
	// concatenation of two-frames odometry estimates over time
	// by the wrapper. This class holds its own internal pose estimate
	// which can be retrieved through 'get_pose(...)' function.
	// 'init_pose' is the initial pose estimate. This function also
	// assumes that 3D points are generated but not transformed.
	// If 'switch_order == true' then the second element in the
	// input arrays are taken as the first (older) lidar data.
	Eigen::Matrix4d estimate_odometry(const LaserProc bottom_laser_proc[2],
									  const LaserProc    top_laser_proc[2],
									  const Eigen::Matrix4d &init_pose,
									  bool reset_pose = false,
									  bool switch_order = false);
	// This function returns the interval accumulated two-frames
	// odometry estimates, in other words the global pose of the
	// robot.
	bool get_pose(Eigen::Matrix4d &pose){
		pose = _pose;
		return true;
	}
};

#endif


// Copyright 2004, 2005 The Trustees of Indiana University.

// 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)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP
#define BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/topology.hpp> // For topology concepts
#include <vector>
#include <list>
#include <algorithm> // for std::min and std::max
#include <numeric> // for std::accumulate
#include <cmath> // for std::sqrt and std::fabs
#include <functional>

namespace boost {

struct square_distance_attractive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::edge_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return d * d / k;
  }
};

struct square_distance_repulsive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::vertex_descriptor,
             typename graph_traits<Graph>::vertex_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return k * k / d;
  }
};

template<typename T>
struct linear_cooling {
  typedef T result_type;

  linear_cooling(std::size_t iterations)
    : temp(T(iterations) / T(10)), step(0.1) { }

  linear_cooling(std::size_t iterations, T temp)
    : temp(temp), step(temp / T(iterations)) { }

  T operator()()
  {
    T old_temp = temp;
    temp -= step;
    if (temp < T(0)) temp = T(0);
    return old_temp;
  }

 private:
  T temp;
  T step;
};

struct all_force_pairs
{
  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    vertex_iterator v, end;
    for (boost::tie(v, end) = vertices(g); v != end; ++v) {
      vertex_iterator u = v;
      for (++u; u != end; ++u) {
        apply_force(*u, *v);
        apply_force(*v, *u);
      }
    }
  }
};

template<typename Topology, typename PositionMap>
struct grid_force_pairs
{
  typedef typename property_traits<PositionMap>::value_type Point;
  BOOST_STATIC_ASSERT (Point::dimensions == 2);
  typedef typename Topology::point_difference_type point_difference_type;

  template<typename Graph>
  explicit
  grid_force_pairs(const Topology& topology,
                   PositionMap position, const Graph& g)
    : topology(topology), position(position)
  {
    two_k = 2. * this->topology.volume(this->topology.extent()) / std::sqrt((double)num_vertices(g));
  }

  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef std::list<vertex_descriptor> bucket_t;
    typedef std::vector<bucket_t> buckets_t;

    std::size_t columns = std::size_t(topology.extent()[0] / two_k + 1.);
    std::size_t rows = std::size_t(topology.extent()[1] / two_k + 1.);
    buckets_t buckets(rows * columns);
    vertex_iterator v, v_end;
    for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v) {
      std::size_t column =
        std::size_t((get(position, *v)[0] + topology.extent()[0] / 2) / two_k);
      std::size_t row    =
        std::size_t((get(position, *v)[1] + topology.extent()[1] / 2) / two_k);

      if (column >= columns) column = columns - 1;
      if (row >= rows) row = rows - 1;
      buckets[row * columns + column].push_back(*v);
    }

    for (std::size_t row = 0; row < rows; ++row)
      for (std::size_t column = 0; column < columns; ++column) {
        bucket_t& bucket = buckets[row * columns + column];
        typedef typename bucket_t::iterator bucket_iterator;
        for (bucket_iterator u = bucket.begin(); u != bucket.end(); ++u) {
          // Repulse vertices in this bucket
          bucket_iterator v = u;
          for (++v; v != bucket.end(); ++v) {
            apply_force(*u, *v);
            apply_force(*v, *u);
          }

          std::size_t adj_start_row = row == 0? 0 : row - 1;
          std::size_t adj_end_row = row == rows - 1? row : row + 1;
          std::size_t adj_start_column = column == 0? 0 : column - 1;
          std::size_t adj_end_column = column == columns - 1? column : column + 1;
          for (std::size_t other_row = adj_start_row; other_row <= adj_end_row;
               ++other_row)
            for (std::size_t other_column = adj_start_column;
                 other_column <= adj_end_column; ++other_column)
              if (other_row != row || other_column != column) {
                // Repulse vertices in this bucket
                bucket_t& other_bucket
                  = buckets[other_row * columns + other_column];
                for (v = other_bucket.begin(); v != other_bucket.end(); ++v) {
                  double dist =
                    topology.distance(get(position, *u), get(position, *v));
                  if (dist < two_k) apply_force(*u, *v);
                }
              }
        }
      }
  }

 private:
  const Topology& topology;
  PositionMap position;
  double two_k;
};

template<typename PositionMap, typename Topology, typename Graph>
inline grid_force_pairs<Topology, PositionMap>
make_grid_force_pairs
  (const Topology& topology,
   const PositionMap& position, const Graph& g)
{ return grid_force_pairs<Topology, PositionMap>(topology, position, g); }

template<typename Graph, typename PositionMap, typename Topology>
void
scale_graph(const Graph& g, PositionMap position, const Topology& topology,
            typename Topology::point_type upper_left, typename Topology::point_type lower_right)
{
  if (num_vertices(g) == 0) return;

  typedef typename Topology::point_type Point;
  typedef typename Topology::point_difference_type point_difference_type;

  // Find min/max ranges
  Point min_point = get(position, *vertices(g).first), max_point = min_point;
  BGL_FORALL_VERTICES_T(v, g, Graph) {
    min_point = topology.pointwise_min(min_point, get(position, v));
    max_point = topology.pointwise_max(max_point, get(position, v));
  }

  Point old_origin = topology.move_position_toward(min_point, 0.5, max_point);
  Point new_origin = topology.move_position_toward(upper_left, 0.5, lower_right);
  point_difference_type old_size = topology.difference(max_point, min_point);
  point_difference_type new_size = topology.difference(lower_right, upper_left);

  // Scale to bounding box provided
  BGL_FORALL_VERTICES_T(v, g, Graph) {
    point_difference_type relative_loc = topology.difference(get(position, v), old_origin);
    relative_loc = (relative_loc / old_size) * new_size;
    put(position, v, topology.adjust(new_origin, relative_loc));
  }
}

namespace detail {
  template<typename Topology, typename PropMap, typename Vertex>
  void
  maybe_jitter_point(const Topology& topology,
                     const PropMap& pm, Vertex v,
                     const typename Topology::point_type& p2)
  {
    double too_close = topology.norm(topology.extent()) / 10000.;
    if (topology.distance(get(pm, v), p2) < too_close) {
      put(pm, v,
          topology.move_position_toward(get(pm, v), 1./200,
                                        topology.random_point()));
    }
  }

  template<typename Topology, typename PositionMap, typename DisplacementMap,
           typename RepulsiveForce, typename Graph>
  struct fr_apply_force
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef typename Topology::point_type Point;
    typedef typename Topology::point_difference_type PointDiff;

    fr_apply_force(const Topology& topology,
                   const PositionMap& position,
                   const DisplacementMap& displacement,
                   RepulsiveForce repulsive_force, double k, const Graph& g)
      : topology(topology), position(position), displacement(displacement),
        repulsive_force(repulsive_force), k(k), g(g)
    { }

    void operator()(vertex_descriptor u, vertex_descriptor v)
    {
      if (u != v) {
        // When the vertices land on top of each other, move the
        // first vertex away from the boundaries.
        maybe_jitter_point(topology, position, u, get(position, v));

        double dist = topology.distance(get(position, u), get(position, v));
        typename Topology::point_difference_type dispv = get(displacement, v);
        if (dist == 0.) {
          for (std::size_t i = 0; i < Point::dimensions; ++i) {
            dispv[i] += 0.01;
          }
        } else {
          double fr = repulsive_force(u, v, k, dist, g);
          dispv += (fr / dist) *
                   topology.difference(get(position, v), get(position, u));
        }
        put(displacement, v, dispv);
      }
    }

  private:
    const Topology& topology;
    PositionMap position;
    DisplacementMap displacement;
    RepulsiveForce repulsive_force;
    double k;
    const Graph& g;
  };

} // end namespace detail

template<typename Topology, typename Graph, typename PositionMap,
         typename AttractiveForce, typename RepulsiveForce,
         typename ForcePairs, typename Cooling, typename DisplacementMap>
void
fruchterman_reingold_force_directed_layout
 (const Graph&    g,
  PositionMap     position,
  const Topology& topology,
  AttractiveForce attractive_force,
  RepulsiveForce  repulsive_force,
  ForcePairs      force_pairs,
  Cooling         cool,
  DisplacementMap displacement)
{
  typedef typename Topology::point_type Point;
  typedef typename graph_traits<Graph>::vertex_iterator   vertex_iterator;
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::edge_iterator     edge_iterator;

  double volume = topology.volume(topology.extent());

  // assume positions are initialized randomly
  double k = pow(volume / num_vertices(g), 1. / (double)(Topology::point_difference_type::dimensions));

  detail::fr_apply_force<Topology, PositionMap, DisplacementMap,
                         RepulsiveForce, Graph>
    apply_force(topology, position, displacement, repulsive_force, k, g);

  do {
    // Calculate repulsive forces
    vertex_iterator v, v_end;
    for (boost::tie(v, v_end) = vertices(g); v != v_end; ++v)
      put(displacement, *v, typename Topology::point_difference_type());
    force_pairs(g, apply_force);

    // Calculate attractive forces
    edge_iterator e, e_end;
    for (boost::tie(e, e_end) = edges(g); e != e_end; ++e) {
      vertex_descriptor v = source(*e, g);
      vertex_descriptor u = target(*e, g);

      // When the vertices land on top of each other, move the
      // first vertex away from the boundaries.
      ::boost::detail::maybe_jitter_point(topology, position, u, get(position, v));

      typename Topology::point_difference_type delta =
        topology.difference(get(position, v), get(position, u));
      double dist = topology.distance(get(position, u), get(position, v));
      double fa = attractive_force(*e, k, dist, g);

      put(displacement, v, get(displacement, v) - (fa / dist) * delta);
      put(displacement, u, get(displacement, u) + (fa / dist) * delta);
    }

    if (double temp = cool()) {
      // Update positions
      BGL_FORALL_VERTICES_T (v, g, Graph) {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        double disp_size = topology.norm(get(displacement, v));
        put(position, v, topology.adjust(get(position, v), get(displacement, v) * (min BOOST_PREVENT_MACRO_SUBSTITUTION (disp_size, temp) / disp_size)));
        put(position, v, topology.bound(get(position, v)));
      }
    } else {
      break;
    }
  } while (true);
}

namespace detail {
  template<typename DisplacementMap>
  struct fr_force_directed_layout
  {
    template<typename Topology, typename Graph, typename PositionMap,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        const Topology& topology,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        DisplacementMap displacement,
        const bgl_named_params<Param, Tag, Rest>&)
    {
      fruchterman_reingold_force_directed_layout
        (g, position, topology, attractive_force, repulsive_force,
         force_pairs, cool, displacement);
    }
  };

  template<>
  struct fr_force_directed_layout<param_not_found>
  {
    template<typename Topology, typename Graph, typename PositionMap,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        const Topology& topology,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        param_not_found,
        const bgl_named_params<Param, Tag, Rest>& params)
    {
      typedef typename Topology::point_difference_type PointDiff;
      std::vector<PointDiff> displacements(num_vertices(g));
      fruchterman_reingold_force_directed_layout
        (g, position, topology, attractive_force, repulsive_force,
         force_pairs, cool,
         make_iterator_property_map
         (displacements.begin(),
          choose_const_pmap(get_param(params, vertex_index), g,
                            vertex_index),
          PointDiff()));
    }
  };

} // end namespace detail

template<typename Topology, typename Graph, typename PositionMap, typename Param,
         typename Tag, typename Rest>
void
fruchterman_reingold_force_directed_layout
  (const Graph&    g,
   PositionMap     position,
   const Topology& topology,
   const bgl_named_params<Param, Tag, Rest>& params)
{
  typedef typename get_param_type<vertex_displacement_t, bgl_named_params<Param,Tag,Rest> >::type D;

  detail::fr_force_directed_layout<D>::run
    (g, position, topology,
     choose_param(get_param(params, attractive_force_t()),
                  square_distance_attractive_force()),
     choose_param(get_param(params, repulsive_force_t()),
                  square_distance_repulsive_force()),
     choose_param(get_param(params, force_pairs_t()),
                  make_grid_force_pairs(topology, position, g)),
     choose_param(get_param(params, cooling_t()),
                  linear_cooling<double>(100)),
     get_param(params, vertex_displacement_t()),
     params);
}

template<typename Topology, typename Graph, typename PositionMap>
void
fruchterman_reingold_force_directed_layout
  (const Graph&    g,
   PositionMap     position,
   const Topology& topology)
{
  fruchterman_reingold_force_directed_layout
    (g, position, topology,
     attractive_force(square_distance_attractive_force()));
}

} // end namespace boost

#ifdef BOOST_GRAPH_USE_MPI
#  include <boost/graph/distributed/fruchterman_reingold.hpp>
#endif

#endif // BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP


// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_DYN_LINK
#define SPATIAL_ENABLE_ASSERT // detect interal issues that should not occur

#include <boost/test/unit_test.hpp>

#define  SPATIAL_SAFER_ARITHMETICS
#include "../../src/neighbor_iterator.hpp"
#include "spatial_test_fixtures.hpp"

BOOST_AUTO_TEST_CASE_TEMPLATE
( test_neighbor_decrement, Tp, int2_maps )
{
  // Prove that you can iterate N nodes, down to 1 node
  {
    Tp fix(100, randomize(-20, 20));
    int2 target;
    typedef typename neighbor_iterator<typename Tp::container_type>
      ::distance_type distance_type;
    while (!fix.container.empty())
      {
        randomize(-22, 22)(target, 0, 0);
        size_t countdown = fix.container.size();
        std::reverse_iterator<neighbor_iterator<typename Tp::container_type> >
          iter(--neighbor_end(fix.container, target)),
          end(neighbor_begin(fix.container, target));
        distance_type max_dist = distance(iter.base());
        for(; --countdown != 0 && iter != end; ++iter)
          {
            distance_type tmp = distance(iter.base());
            BOOST_CHECK_LE(tmp, max_dist);
            max_dist = tmp;
          }
        BOOST_CHECK_EQUAL(countdown, 0u);
        BOOST_CHECK(iter == end);
        fix.container.erase(fix.container.begin());
      }
  }
  // Prove that you can iterate a very unbalanced tree
  {
    Tp fix(40, increase());
    int2 target;
    typedef typename neighbor_iterator<typename Tp::container_type>
      ::distance_type distance_type;
    while (!fix.container.empty())
      {
        randomize(0, 40)(target, 0, 0);
        size_t countdown = fix.container.size();
        std::reverse_iterator<neighbor_iterator<typename Tp::container_type> >
          iter(--neighbor_end(fix.container, target)),
          end(neighbor_begin(fix.container, target));
        distance_type max_dist = distance(iter.base());
        for(; --countdown != 0 && iter != end; ++iter)
          {
            distance_type tmp = distance(iter.base());
            BOOST_CHECK_LE(tmp, max_dist);
            max_dist = tmp;
          }
        BOOST_CHECK_EQUAL(countdown, 0u);
        BOOST_CHECK(iter == end);
        fix.container.erase(fix.container.begin());
      }
  }
  // Prove that you can iterate an opposite unbalanced tree
  {
    Tp fix(40, decrease());
    int2 target;
    typedef typename neighbor_iterator<typename Tp::container_type>
      ::distance_type distance_type;
    while (!fix.container.empty())
      {
        randomize(0, 40)(target, 0, 0);
        size_t countdown = fix.container.size();
        std::reverse_iterator<neighbor_iterator<typename Tp::container_type> >
          iter(--neighbor_end(fix.container, target)),
          end(neighbor_begin(fix.container, target));
        distance_type max_dist = distance(iter.base());
        for(; --countdown != 0 && iter != end; ++iter)
          {
            distance_type tmp = distance(iter.base());
            BOOST_CHECK_LE(tmp, max_dist);
            max_dist = tmp;
          }
        BOOST_CHECK_EQUAL(countdown, 0u);
        BOOST_CHECK(iter == end);
        fix.container.erase(fix.container.begin());
      }
  }
  // Prove that you can iterate equivalent nodes
  {
    int2 target;
    same()(target, 0, 100);
    Tp fix(100, same());
    std::reverse_iterator<neighbor_iterator<typename Tp::container_type> >
      iter(neighbor_end(fix.container, target)),
      end(neighbor_begin(fix.container, target));
    size_t count = 1;
    ++iter;
    for(; iter != end && count < fix.container.size(); ++iter, ++count)
      {
        BOOST_CHECK_CLOSE(distance(iter.base()), 0.0, 0.000000001);
      }
    BOOST_CHECK(iter == end);
    BOOST_CHECK_EQUAL(count, fix.container.size());
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE
( test_neighbor_upper_bound, Tp, quad_sets )
{
  typedef quadrance<typename Tp::container_type, int, quad_diff> metric_type;
  typedef typename metric_type::distance_type distance_type;
  typedef neighbor_iterator<typename Tp::container_type, metric_type>
    neighbor_iterator_type;
  // Prove that you can find lower bound with N nodes, down to 1 node
  {
    Tp fix(100, randomize(-20, 20));
    metric_type metric;
    quad target;
    while (!fix.container.empty())
      {
        randomize(-22, 22)(target, 0, 0);
        // Find min and max dist first
        distance_type min_dist;
        distance_type max_dist;
        typename Tp::container_type::iterator it = fix.container.begin();
        min_dist = max_dist
          = metric.distance_to_key(fix.container.rank()(), *it, target);
        ++it;
        for (; it != fix.container.end(); ++it)
          {
            distance_type tmp
              = metric.distance_to_key(fix.container.rank()(), *it, target);
            if (tmp < min_dist) min_dist = tmp;
            if (tmp > max_dist) max_dist = tmp;
          }
        distance_type avg_dist = (min_dist + max_dist) / 2;
        // use this knowledge to test the upper bound
        neighbor_iterator_type i
          = neighbor_upper_bound(fix.container, metric, target, min_dist);
        BOOST_CHECK(i != neighbor_begin(fix.container, metric, target));
        if (i != neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_LT(min_dist, distance(i)); }
        BOOST_CHECK_EQUAL(min_dist, distance(--i));
        i = neighbor_upper_bound(fix.container, metric, target, max_dist);
        BOOST_CHECK(i == neighbor_end(fix.container, metric, target));
        i = neighbor_upper_bound(fix.container, metric, target, avg_dist);
        if (i != neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_GT(distance(i), avg_dist); }
        if (i == neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_LE(distance(--i), avg_dist); }
        fix.container.erase(i);
      }
  }
  // Prove that you can find the upper bound when node and target are same
  {
    Tp fix(100, same());
    metric_type metric;
    quad target;
    same()(target, 0, 100);
    // All points and targets are the same.
    while (!fix.container.empty())
      {
        neighbor_iterator_type i
          = neighbor_upper_bound(fix.container, metric, target, 1);
        BOOST_CHECK(i == neighbor_end(fix.container, metric, target));
        i = neighbor_upper_bound(fix.container, metric, target, 0);
        BOOST_CHECK(i == neighbor_end(fix.container, metric, target));
        fix.container.erase(--i);
      }
  }
  // Prove that you can find the upper bound in an unbalanced tree
  {
    Tp fix(100, increase());
    metric_type metric;
    quad target;
    while (!fix.container.empty())
      {
        randomize(0, 100)(target, 0, 0);
        // Find min and max dist first
        distance_type min_dist;
        distance_type max_dist;
        typename Tp::container_type::iterator it = fix.container.begin();
        min_dist = max_dist
          = metric.distance_to_key(fix.container.rank()(), *it, target);
        ++it;
        for (; it != fix.container.end(); ++it)
          {
            distance_type tmp
              = metric.distance_to_key(fix.container.rank()(), *it, target);
            if (tmp < min_dist) min_dist = tmp;
            if (tmp > max_dist) max_dist = tmp;
          }
        distance_type avg_dist = (min_dist + max_dist) / 2;
        // Use this knowledge to test the upper bound
        neighbor_iterator_type i
          = neighbor_upper_bound(fix.container, metric, target, min_dist);
        BOOST_CHECK(i != neighbor_begin(fix.container, metric, target));
        if (i != neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_LT(min_dist, distance(i)); }
        BOOST_CHECK_EQUAL(min_dist, distance(--i));
        i = neighbor_upper_bound(fix.container, metric, target, max_dist);
        BOOST_CHECK(i == neighbor_end(fix.container, metric, target));
        i = neighbor_upper_bound(fix.container, metric, target, avg_dist);
        if (i != neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_GT(distance(i), avg_dist); }
        if (i == neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_LE(distance(--i), avg_dist); }
        fix.container.erase(i);
      }
  }
  // Prove that you can find the upper bound in an unbalanced tree
  {
    Tp fix(100, decrease());
    metric_type metric;
    quad target;
    while (!fix.container.empty())
      {
        randomize(0, 100)(target, 0, 0);
        // Find min and max dist first
        distance_type min_dist;
        distance_type max_dist;
        typename Tp::container_type::iterator it = fix.container.begin();
        min_dist = max_dist
          = metric.distance_to_key(fix.container.rank()(), *it, target);
        ++it;
        for (; it != fix.container.end(); ++it)
          {
            distance_type tmp
              = metric.distance_to_key(fix.container.rank()(), *it, target);
            if (tmp < min_dist) min_dist = tmp;
            if (tmp > max_dist) max_dist = tmp;
          }
        distance_type avg_dist = (min_dist + max_dist) / 2;
        // Use this knowledge to test the upper bound
        neighbor_iterator_type i
          = neighbor_upper_bound(fix.container, metric, target, min_dist);
        BOOST_CHECK(i != neighbor_begin(fix.container, metric, target));
        if (i != neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_LT(min_dist, distance(i)); }
        BOOST_CHECK_EQUAL(min_dist, distance(--i));
        i = neighbor_upper_bound(fix.container, metric, target, max_dist);
        BOOST_CHECK(i == neighbor_end(fix.container, metric, target));
        i = neighbor_upper_bound(fix.container, metric, target, avg_dist);
        if (i != neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_GT(distance(i), avg_dist); }
        if (i == neighbor_end(fix.container, metric, target))
          { BOOST_CHECK_LE(distance(--i), avg_dist); }
        fix.container.erase(i);
      }
  }
}

BOOST_AUTO_TEST_CASE_TEMPLATE
(test_manhattan_neighbor, Tp, double6_maps)
{
  Tp fix(3, randomize(-2, 2));
  double6 target; same()(target, 0, 2);
  {
    // With default diff with float
    manhattan_neighbor_iterator<typename Tp::container_type, double>
      i,
      j = manhattan_neighbor_begin(fix.container, target),
      k = manhattan_neighbor_end(fix.container, target),
      l = manhattan_neighbor_lower_bound(fix.container, target, 0.0),
      m = manhattan_neighbor_upper_bound(fix.container, target, 0.0);
    i = j;
    BOOST_CHECK(i == j);
    BOOST_CHECK_EQUAL(distance(i), distance(j));
    BOOST_CHECK(j != k);
    BOOST_CHECK(i == l);
    BOOST_CHECK(m == l);
  }
  {
    // With custom diff and const
    manhattan_neighbor_iterator<const typename Tp::container_type,
                                double, double6_diff>
      i,
      j = manhattan_neighbor_cbegin(fix.container, double6_diff(), target),
      k = manhattan_neighbor_cend(fix.container, double6_diff(), target),
      l = manhattan_neighbor_clower_bound(fix.container, double6_diff(),
                                          target, 0.0),
      m = manhattan_neighbor_cupper_bound(fix.container, double6_diff(),
                                          target, 0.0);
    i = j;
    BOOST_CHECK(i == j);
    BOOST_CHECK_EQUAL(distance(i), distance(j));
    BOOST_CHECK(j != k);
    BOOST_CHECK(i == l);
    BOOST_CHECK(m == l);
  }
  {
    // With default diff with float and a pair
    manhattan_neighbor_iterator_pair<typename Tp::container_type, double>
      i,
      j = manhattan_neighbor_range(fix.container, target);
    i = j;
    BOOST_CHECK(i == j);
    BOOST_CHECK_EQUAL(distance(i.first), distance(j.first));
  }
  {
    // With custom diff and const
    manhattan_neighbor_iterator_pair<const typename Tp::container_type,
                                     double, double6_diff>
      i,
      j = manhattan_neighbor_crange(fix.container, double6_diff(), target);
    i = j;
    BOOST_CHECK(i == j);
    BOOST_CHECK_EQUAL(distance(i.first), distance(j.first));
  }
  // Need to test the pair
}


#pragma once

#include <boost/assert.hpp>
#include <boost/lexical_cast.hpp>

#include "base/buffered_output_array.hpp"
#include "base/coeff_array.hpp"
#include "export/hdf_writer.hpp"
#include "spectral/macroscopic_quantities.hpp"
#include "spectral/polar_to_nodal.hpp"

class OutputHandler
{
 private:
  template <int dim>
  using buffer_t = BufferType<dim>;

 public:
  template <typename POLAR_BASIS>
  OutputHandler(const POLAR_BASIS &basis,
                const Map &xmap,
                const boltzmann::Polar2Nodal<POLAR_BASIS> &p2n,
                int freq_mom = -1,
                int dump = -1,
                int bufsize = 1);

  void compute(const CoeffArray<double> &full_vector, unsigned int timestep, double time);

 private:
  /// DoF distribution
  Map xmap_;
  /// Polar2Nodal
  const boltzmann::Polar2Nodal<> p2n_;
  /// moments output frequency
  int freq_mom_;
  /// solution output frequency (full solution vector)
  int dump_;
  /// buffer size
  int bufsize_;
  /// buffer counter
  int ctr_ = 0;
  /// density (scalar)
  buffer_t<1> m_;
  /// energy (scalar)
  buffer_t<1> e_;
  /// velocity (vector)
  buffer_t<3> v_;
  /// heat flow (vector)
  buffer_t<3> q_;
  /// energy flow (vector)
  buffer_t<3> r_;
  /// momentum flow  (tensor)
  buffer_t<9> M_;
  /// pressure (tensor)
  buffer_t<9> P_;
  /// object to compute macroscopic quantities
  boltzmann::MQEval mq_coeffs_;

  std::vector<double> timesteps_;
};

template <typename POLAR_BASIS>
OutputHandler::OutputHandler(const POLAR_BASIS &basis,
                             const Map &xmap,
                             const boltzmann::Polar2Nodal<POLAR_BASIS> &p2n,
                             int freq_mom,
                             int dump,
                             int bufsize)
    : xmap_(xmap)
    , p2n_(p2n)
    , freq_mom_(freq_mom)
    , dump_(dump)
    , bufsize_(bufsize)
    , m_(xmap, bufsize)
    , e_(xmap, bufsize)
    , v_(xmap, bufsize)
    , q_(xmap, bufsize)
    , r_(xmap, bufsize)
    , M_(xmap, bufsize)
    , P_(xmap, bufsize)
{
  mq_coeffs_.init(basis);
}

void
OutputHandler::compute(const CoeffArray<double> &full_vector, unsigned int timestep, double time)
{
  typedef typename CoeffArray<double>::map_t map_t;
  typedef map_t::index_t index_t;

  BOOST_ASSERT(full_vector.get_map() == xmap_);

  if (timestep % freq_mom_ == 0) {
    timesteps_.push_back(time);
// check if buffer is full? => write
#pragma omp parallel
    {
      // boltzmann::Polar2Nodal_Evaluator<> p2n_adapter(p2n_);
      // obtain thread-safe evaluator
      auto evaluator = mq_coeffs_.evaluator();
      Eigen::ArrayXd cp(p2n_.N());
      int K = p2n_.K();

#pragma omp for schedule(static)
      for (index_t lid = 0; lid < full_vector.get_map().lsize(); ++lid) {
        // TODO: call p2n, reshape full_vector.get(lid) first
        typedef Eigen::MatrixXd matrix_t;
        Eigen::Map<const matrix_t> cn(full_vector.get(lid).data(), K, K);
        p2n_.to_polar(cp, cn);
        double norm = cp.abs2().sum();
        evaluator(cp);
        // access results and copy to buffer
        m_.fill(evaluator.m, ctr_, lid);
        e_.fill(evaluator.e, ctr_, lid);
        v_.fill(evaluator.v, ctr_, lid);
        q_.fill(evaluator.q, ctr_, lid);
        r_.fill(evaluator.r, ctr_, lid);
        M_.fill(evaluator.M, ctr_, lid);
        P_.fill(evaluator.P, ctr_, lid);
      }
    }
    ctr_++;  // increment buffer counter
    if (ctr_ == bufsize_) {
      // write HDF
      char fname[256];
      std::sprintf(fname, "solution_vector%06d.h5", timestep);
      PHDFWriter exporter(fname, full_vector.get_map().comm());
      exporter.write(m_, "m");
      exporter.write(e_, "e");
      exporter.write(v_, "v");
      exporter.write(q_, "q");
      exporter.write(r_, "r");
      exporter.write(M_, "M");
      exporter.write(P_, "P");
      // todo write meta data ...
      // reset counter
      ctr_ = 0;
    }
  }

  if (timestep % dump_ == 0) {
    char fname[256];
    std::sprintf(fname, "fsolution_vector%06d.h5", timestep);
    PHDFWriter exporter(fname, full_vector.get_map().comm());
    exporter.write(full_vector, "C");
  }
}


#include "dynamic_grid.hpp"
#include "grid_storage.hpp"
#include "multiindex.hpp"

#include <boost/test/unit_test.hpp>

using std::size_t;

// ---------------------------------------------------------------------------------------------
//          GridStorage test cases
// ---------------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(grid_storage)

BOOST_AUTO_TEST_CASE( grid_storage_ctor )
{
    GridStorage storage = GridStorage::create<double>(10);

    BOOST_CHECK_EQUAL( storage.getStride(), sizeof(double) );
    BOOST_CHECK_EQUAL( storage.getType().name(), typeid(double).name() );
    BOOST_CHECK_EQUAL( storage.size(), 10 );
    BOOST_CHECK_NE( storage.getStartingAddress(), (void*)nullptr);      // cast to void* to disambiguate operator<<
}

BOOST_AUTO_TEST_CASE( grid_storage_access )
{
    GridStorage storage = GridStorage::create<double>(10);

    BOOST_CHECK_EQUAL( std::distance(&storage.at<double>(2), &storage.at<double>(8)), 6 );
    BOOST_CHECK_EQUAL( storage.getStartingAddress(), &storage.at<double>(0));
}

BOOST_AUTO_TEST_SUITE_END()

// --------------------------------------------------------------------------------------------------
//          DynamicGrid test cases
// --------------------------------------------------------------------------------------------------

BOOST_AUTO_TEST_SUITE(dynamic_grid_test)

BOOST_AUTO_TEST_CASE(dynamic_grid_ctors)
{
    size_t dimension = 3;
    size_t size = 32;
    default_grid grid(dimension, size, TransformationType::IDENTITY);

    // check data
    BOOST_CHECK_EQUAL( grid.getDimension(), dimension );
    BOOST_CHECK_EQUAL( grid.getExtents().size(), dimension );
    BOOST_CHECK_EQUAL( grid.getExtents()[0], size );
    BOOST_CHECK_EQUAL( grid.getExtents()[1], size );
    BOOST_CHECK_EQUAL( grid.getExtents()[2], size );

    BOOST_CHECK( grid.getAccessMode() == TransformationType::IDENTITY);

    // check container: size
    BOOST_CHECK_EQUAL( grid.getContainer().size(), pow_int(size, dimension) );
    BOOST_CHECK_EQUAL( grid.getContainer().size(), grid.getElementCount() );

    // non square constructor
    std::vector<std::size_t> extent({32, 64});
    default_grid grid2( extent );

    // check data
    BOOST_CHECK_EQUAL( grid2.getDimension(), 2 );
    BOOST_CHECK_EQUAL( grid2.getExtents().size(), 2);
    BOOST_CHECK_EQUAL( grid2.getExtents()[0], 32);
    BOOST_CHECK_EQUAL( grid2.getExtents()[1], 64);
    BOOST_CHECK( grid2.getAccessMode() == TransformationType::IDENTITY);

    // check container: size
    BOOST_CHECK_EQUAL( grid2.getContainer().size(), 32*64 );
    BOOST_CHECK_EQUAL( grid2.getContainer().size(), grid2.getElementCount() );
}

/// \todo the following two tests are almost identical
BOOST_AUTO_TEST_CASE( edit_roundtrip )
{
    size_t dimension = 3;
    size_t size = 32;
    default_grid grid(dimension, size, TransformationType::IDENTITY);

    std::vector<int> index{0,0,0};
    grid(index) = 5;
    BOOST_CHECK_EQUAL( grid(index), 5);

    index = {3,6,2};
    grid(index) = 3;
    BOOST_CHECK_EQUAL( grid(index), 3);
}

BOOST_AUTO_TEST_CASE( edit_roundtrip_nonsquare )
{
    std::vector<std::size_t> sizes{16, 32, 48};
    default_grid grid(sizes, TransformationType::IDENTITY);

    std::vector<int> index{0,0,0};
    grid(index) = 5;
    BOOST_CHECK_EQUAL( grid(index), 5);

    index = {3,6,2};
    grid(index) = 3;
    BOOST_CHECK_EQUAL( grid(index), 3);
}

BOOST_AUTO_TEST_CASE( offset_of )
{
    size_t dimension = 3;
    size_t size = 32;
    default_grid grid(dimension, size, TransformationType::IDENTITY);

    std::vector<int> index{0,1,0};
    grid(index) = 5;
    std::size_t offset = grid.getOffsetOf( index );
    BOOST_CHECK_EQUAL( grid.getContainer().at<double>(offset), grid(index));
}

BOOST_AUTO_TEST_CASE( check_adress_order )
{
    size_t dimension = 3;
    size_t size = 32;
    default_grid grid(dimension, size, TransformationType::IDENTITY);

    MultiIndex index(3);
    index.setLowerBound(0);
    index.setUpperBound(32);
    index.init();
    std::size_t o1 = grid.getOffsetOf( index );
    double* p1 = &grid(index);
    ++index;
    std::size_t o2 = grid.getOffsetOf( index );
    double* p2 = &grid(index);

    BOOST_CHECK_EQUAL( o1 + 1, o2);
    BOOST_CHECK_EQUAL( p1 + 1, p2);
}

BOOST_AUTO_TEST_CASE( check_nonsquare_grid )
{
    default_grid grid(std::vector<std::size_t>({16, 8}), TransformationType::IDENTITY);

    for(MultiIndex idx = grid.getIndex(); idx.valid(); ++idx)
    {
        grid(idx) = 42;
    }

    for(int i = 0; i < 16*8; ++i)
    {
        BOOST_CHECK_EQUAL( grid[i], 42 );
    }

    std::vector<int> idx(2);
    int i = 0;
    for(int x = 0; x < 16; ++x)
    for(int y = 0; y < 8; ++y)
    {
        idx[0] = x;
        idx[1] = y;
        BOOST_CHECK_EQUAL( grid.getOffsetOf(idx), i++);
        BOOST_CHECK_EQUAL( grid(idx), 42 );
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef PYTHONIC_INCLUDE_NUMPY_ISNEGINF_HPP
#define PYTHONIC_INCLUDE_NUMPY_ISNEGINF_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/is_inf.hpp>
#include <boost/simd/function/is_negative.hpp>

namespace pythonic
{

  namespace numpy
  {
    namespace wrapper
    {
      template <class T>
      auto isneginf(T const &t)
          -> decltype(boost::simd::is_inf(t) and boost::simd::is_negative(t));
    }

#define NUMPY_NARY_FUNC_NAME isneginf
#define NUMPY_NARY_FUNC_SYM wrapper::isneginf
#include "pythonic/include/types/numpy_nary_expr.hpp"
  }
}

#endif


/*
 * @Description: g2o edge for LIO lidar frontend relative pose measurement
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_RELATIVE_POSE_HPP_
#define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_RELATIVE_POSE_HPP_

#include <iostream>

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

#include <sophus/so3.hpp>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/slam3d_addons/types_slam3d_addons.h>

#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp"

#include <g2o/core/base_binary_edge.h>

typedef Eigen::Matrix<double, 6, 1> Vector6d;

namespace g2o {

class EdgePRVAGRelativePose : public g2o::BaseBinaryEdge<6, Vector6d, g2o::VertexPRVAG, g2o::VertexPRVAG> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const int INDEX_P = 0;
    static const int INDEX_R = 3;

	EdgePRVAGRelativePose()
	: g2o::BaseBinaryEdge<6, Vector6d, g2o::VertexPRVAG, g2o::VertexPRVAG>() {
	}

	virtual void computeError() override {
        const g2o::VertexPRVAG* v0 = static_cast<const g2o::VertexPRVAG*>(_vertices[0]);
        const g2o::VertexPRVAG* v1 = static_cast<const g2o::VertexPRVAG*>(_vertices[1]);

		const Eigen::Vector3d &pos_i = v0->estimate().pos;
		const Sophus::SO3d    &ori_i = v0->estimate().ori;

		const Eigen::Vector3d &pos_j = v1->estimate().pos;
		const Sophus::SO3d    &ori_j = v1->estimate().ori;

		const Eigen::Vector3d &pos_ij = _measurement.block<3, 1>(INDEX_P, 0);
		const Eigen::Vector3d &ori_ij = _measurement.block<3, 1>(INDEX_R, 0);

		_error.block(INDEX_P, 0, 3, 1) = ori_i.inverse() * (pos_j - pos_i) - pos_ij;
		_error.block(INDEX_R, 0, 3, 1) = (Sophus::SO3d::exp(ori_ij).inverse()*ori_i.inverse()*ori_j).log();
    }

    virtual void setMeasurement(const Vector6d& m) override {
		_measurement = m;
	}

	virtual bool read(std::istream& is) override {
		Vector6d v;
		is >> v(INDEX_P + 0) >> v(INDEX_P + 1) >> v(INDEX_P + 2)
		   >> v(INDEX_R + 0) >> v(INDEX_R + 1) >> v(INDEX_R + 2);

    	setMeasurement(v);

		for (int i = 0; i < information().rows(); ++i) {
			for (int j = i; j < information().cols(); ++j) {
				is >> information()(i, j);
				// update cross-diagonal element:
				if (i != j) {
					information()(j, i) = information()(i, j);
				}
			}
		}

		return true;
	}

	virtual bool write(std::ostream& os) const override {
    	Vector6d v = _measurement;

		os << v(INDEX_P + 0) << " " <<v(INDEX_P + 1) << " " << v(INDEX_P + 2) << " "
		   << v(INDEX_R + 0) << " " <<v(INDEX_R + 1) << " " << v(INDEX_R + 2) << " ";

		for (int i = 0; i < information().rows(); ++i)
			for (int j = i; j < information().cols(); ++j)
				os << " " << information()(i, j);

		return os.good();
	}
};

} // namespace g2o

#endif // LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_RELATIVE_POSE_HPP_


/*
 Copyright (c) 2013, The Cinder Project (http://libcinder.org)
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and
	the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
	the following disclaimer in the documentation and/or other materials provided with the distribution.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 POSSIBILITY OF SUCH DAMAGE.
*/

#include "cinder/ConvexHull.h"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace cinder {

typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<double> > polygon;

namespace {

template<typename T>
PolyLine<Vec2<T> > toPolyLine( const polygon &p )
{
	PolyLine<Vec2<T> > result;

	for( auto pt = p.outer().begin(); pt != p.outer().end(); ++pt )
		result.push_back( Vec2<T>( (T)boost::geometry::get<0>(*pt), (T)boost::geometry::get<1>(*pt) ) );

	return result;
}

boost::geometry::model::d2::point_xy<double> makePoint( const Vec2f &p )
{
	return boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( p.x, p.y );
}

void includePathExtremeties( const Path2d &p, polygon *output )
{
	size_t firstPoint = 0;
	for( size_t s = 0; s < p.getSegments().size(); ++s ) {
		switch( p.getSegments()[s] ) {
			case Path2d::CUBICTO: {
				float monotoneT[4];
				int monotoneCnt = Path2d::calcCubicBezierMonotoneRegions( &(p.getPoints()[firstPoint]), monotoneT );
				for( int monotoneIdx = 0; monotoneIdx < monotoneCnt; ++monotoneIdx )
					output->outer().push_back( makePoint( Path2d::calcCubicBezierPos( &(p.getPoints()[firstPoint]), monotoneT[monotoneIdx] ) ) );

				output->outer().push_back( makePoint( p.getPoints()[firstPoint+0] ) );
				output->outer().push_back( makePoint( p.getPoints()[firstPoint+1] ) );
				output->outer().push_back( makePoint( p.getPoints()[firstPoint+2] ) );
			}
			break;
			case Path2d::QUADTO: {
				float monotoneT[2];
				int monotoneCnt = Path2d::calcCubicBezierMonotoneRegions( &(p.getPoints()[firstPoint]), monotoneT );
				for( int monotoneIdx = 0; monotoneIdx < monotoneCnt; ++monotoneIdx )
					output->outer().push_back( makePoint( Path2d::calcQuadraticBezierPos( &(p.getPoints()[firstPoint]), monotoneT[monotoneIdx] ) ) );
				output->outer().push_back( makePoint( p.getPoints()[firstPoint+0] ) );
				output->outer().push_back( makePoint( p.getPoints()[firstPoint+1] ) );
			}
			break;
			case Path2d::LINETO:
				output->outer().push_back( makePoint( p.getPoints()[firstPoint+0] ) );
			break;
			case Path2d::CLOSE:
				output->outer().push_back( makePoint( p.getPoints()[firstPoint+0] ) );
			break;
			default:
				throw Path2dExc();
		}

		firstPoint += Path2d::sSegmentTypePointCounts[p.getSegments()[s]];
	}
}

} // anonymous namespace

PolyLine2f calcConvexHull( const std::vector<Vec2f> &points )
{
	return calcConvexHull( &points[0], points.size() );
}

PolyLine2f calcConvexHull( const Vec2f *points, size_t numPoints )
{
	polygon poly;
	for( size_t p = 0; p < numPoints; ++p )
		poly.outer().push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( points[p].x, points[p].y ) );

	polygon result;
	boost::geometry::convex_hull( poly, result );

	return toPolyLine<float>( result );
}

PolyLine2f calcConvexHull( const Shape2d &shape )
{
	polygon poly;
	for( auto contourIt = shape.getContours().begin(); contourIt != shape.getContours().end(); ++contourIt )
		includePathExtremeties( *contourIt, &poly );

	polygon result;
	boost::geometry::convex_hull( poly, result );

	return toPolyLine<float>( result );
}

PolyLine2f calcConvexHull( const Path2d &path )
{
	polygon poly;
	includePathExtremeties( path, &poly );

	polygon result;
	boost::geometry::convex_hull( poly, result );

	return toPolyLine<float>( result );
}

PolyLine2f calcConvexHull( const PolyLine2f &polyLine )
{
	polygon poly;
	for( auto ptIt = polyLine.begin(); ptIt != polyLine.end(); ++ptIt )
		poly.outer().push_back( boost::geometry::make<boost::geometry::model::d2::point_xy<double> >( ptIt->x, ptIt->y ) );

	polygon result;
	boost::geometry::convex_hull( poly, result );

	return toPolyLine<float>( result );
}

} // namespace cinder


// Copyright 2016 by Glukhov V. O. All Rights Reserved.
//
// 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.
#pragma once

#include "util.hpp"
#include "image_loader.hpp"
#include "input_layer.hpp"
#include "activation_function.hpp"
#include "cost_function.hpp"
#include "softmax_layer.hpp"
#include "fully_connected_layer.hpp"
#include "pooling_layer.hpp"
#include "convolutional_layer.hpp"
#include <armadillo>

#include <memory>
#include <vector>
#include <fstream>
#include <cstddef>

namespace cnn
{
	namespace nn
	{
		class NeuralNetwork
		{
		public:
			NeuralNetwork(std::unique_ptr<BaseImageLoader> loader,
						  std::unique_ptr<BaseCostFunction> costFunction);

			void AppendLayer(std::unique_ptr<BaseLayer> layer);
			std::size_t Size() const noexcept;
			const std::wstring& LabelName(std::size_t id) const;

			void InitWeights() noexcept;
			bool is_initialized() const noexcept;
			bool LoadWeights(std::ifstream& in);
			bool SaveWeights(std::ofstream& out) const;

			bool LoadTestImage();
			bool LoadTrainImage();
			void SetInputImage(std::shared_ptr<arma::Cube<double>> image);

			std::shared_ptr<arma::Cube<double>> Hypothesis() const noexcept;;
			std::shared_ptr<arma::Cube<double>> Output(std::size_t layerIdx) const noexcept;
			std::shared_ptr<arma::Cube<double>> ReceptiveField(std::size_t layerIdx) const noexcept;
			double Error();

			tensor4d& Weights(std::size_t layerIdx) noexcept
			{
				return layers_[layerIdx]->Weights();
			}
			tensor4d& BiasWeights(std::size_t layerIdx) noexcept
			{
				return layers_[layerIdx]->BiasWeights();
			}

			//propagate signals from bottom
			void Forward();
			// compute Gradient
			std::vector<std::pair<tensor4d, tensor4d>> Backpropagation();
			// compute Hessian
			std::vector<std::pair<tensor4d, tensor4d>> Backpropagation_2nd();
		private:
			std::vector<std::unique_ptr<BaseLayer>> layers_;
			std::unique_ptr<BaseCostFunction> costFunc_;
			std::unique_ptr<InputLayer> in_;

			bool initialized_;
		};

		inline
		NeuralNetwork::NeuralNetwork(std::unique_ptr<BaseImageLoader> loader,
									 std::unique_ptr<BaseCostFunction> costFunction)
			: in_(std::make_unique<InputLayer>(std::move(loader))),
			costFunc_(std::move(costFunction)),
			initialized_(false)
		{}

		inline void NeuralNetwork::AppendLayer(std::unique_ptr<BaseLayer> layer)
		{
			layers_.emplace_back(std::move(layer));
		}

		inline std::size_t NeuralNetwork::Size() const noexcept
		{
			return layers_.size();
		}

		inline const std::wstring& NeuralNetwork::LabelName(std::size_t id) const
		{
			return in_->LabelName(id);
		}

		inline void NeuralNetwork::InitWeights() noexcept
		{
			for(std::unique_ptr<BaseLayer> & item : layers_)
				item->InitWeights();
			initialized_ = true;
		}

		inline bool NeuralNetwork::is_initialized() const noexcept
		{
			return initialized_;
		}

		inline bool NeuralNetwork::LoadWeights(std::ifstream& in)
		{
			bool flag = true;
			for (std::unique_ptr<BaseLayer> & item : layers_) {
				flag = item->LoadWeights(in);
				if (!flag)
					return flag;
			}
			initialized_ = true;
			return flag;
		}

		inline bool NeuralNetwork::SaveWeights(std::ofstream& out) const
		{
			if (!initialized_ || layers_.empty())
				return false;
			bool flag = true;
			for (const std::unique_ptr<BaseLayer> & item : layers_) {
				flag = item->SaveWeights(out);
				if (!flag)
					return flag;
			}
			return flag;
		}

		inline bool NeuralNetwork::LoadTestImage()
		{
			return in_->LoadTestImage();
		}

		inline bool NeuralNetwork::LoadTrainImage()
		{
			return in_->LoadTrainImage();
		}

		inline void NeuralNetwork::SetInputImage(std::shared_ptr<arma::Cube<double>> image)
		{
			in_->SetCustomImage(std::move(image));
		}

		inline std::shared_ptr<arma::Cube<double>> NeuralNetwork::Hypothesis() const noexcept
		{
			return layers_.back()->Output();
		}

		inline
		std::shared_ptr<arma::Cube<double>> NeuralNetwork::ReceptiveField(
			std::size_t layerIdx) const noexcept
		{
#ifndef NDEBUG
			assert(layerIdx < layers_.size());
			assert(initialized_);
#endif
			return layers_[layerIdx]->ReceptiveField();
		}

		inline
		std::shared_ptr<arma::Cube<double>> NeuralNetwork::Output(arma::uword layerIdx) const noexcept
		{
#ifndef NDEBUG
			assert(layerIdx < layers_.size());
			assert(initialized_);
#endif
			return layers_[layerIdx]->Output();
		}

		inline double NeuralNetwork::Error()
		{
#ifndef NDEBUG
			assert(!in_->Labels().empty());
#endif

			if (SoftMaxLayer *output = dynamic_cast<SoftMaxLayer*>(layers_.back().get())) {
				return costFunc_->Compute(in_->Labels(),
										  layers_.back()->ReceptiveField()->slice(0).col(0));
			} else {
				return costFunc_->Compute(in_->Labels(), layers_.back()->Output()->slice(0).col(0));
			}
		}

	}
}


#ifndef __GRAPH_HPP__
#define __GRAPH_HPP__

#include "types.hpp"
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/labeled_graph.hpp>
#include <boost/graph/connected_components.hpp>
using namespace boost;

// Vertex name - possible negative index from eternal legs
typedef boost::adjacency_list<vecS, vecS, undirectedS, property<vertex_name_t, int>,property<edge_index_t, std::size_t> > IndexedGraph;

// typedef labeled_graph<AdjGraph, int> IndexedGraph;

typedef boost::graph_traits<IndexedGraph>::vertex_descriptor VD;

// Subgraph containing only fermion lines
// type = F and type = M.
// External legs are connected to vertex E

class FermionGraph
{
  IndexedGraph g;

  std::vector<int> vnames;
  std::map<int,VD> vidx;

  size_t fermion_lines;

  size_t open_lines;
  size_t closed_lines;

public:
  FermionGraph(const DiagramRecord& dr)
  {

    // size_t v_idx_shift = dr.legs.size(); // Negative vertex index shift by number of eternal legs
    for(std::vector<Prop>::const_iterator pi = dr.props.begin(); pi != dr.props.end(); ++pi)
      {
        // Add only Dirac and Majorana fermion edges
        if((pi->type == "F") || (pi->type == "M"))
          {
            VD u,v;
            std::vector<int>::iterator vit = std::find(vnames.begin(), vnames.end(), pi->u);
            if(vit  != vnames.end())
              {
                u = (vit - vnames.begin());

                std::cout << "u From vec " << u <<std::endl;
              }
            else
              {
                u = add_vertex(IndexedGraph::vertex_property_type(pi->u), g);

                std::cout << "u To   vec " << u <<std::endl;
                vnames.push_back(pi->u);
                assert(u == vnames.size() - 1);
              }

            vit = std::find(vnames.begin(), vnames.end(), pi->v);
            if( vit != vnames.end())
              {
                v = (vit - vnames.begin());
                std::cout << "v From vec " << v <<std::endl;
              }
            else
              {
                v = add_vertex(IndexedGraph::vertex_property_type(pi->v), g);
                std::cout << "v To   vec " << v <<std::endl;
                vnames.push_back(pi->v);
                assert(v == vnames.size() - 1);
              }

            add_edge (u, v, pi->id, g);

            std::cout << "Edge " << pi->type << " added" << std::endl;
          }
      }

    std::cout << "Fermion flow graph has " << num_edges(g) << " edges from props and " << num_vertices(g) << " verts " << std::endl;

    for(std::vector<Leg>::const_iterator li = dr.legs.begin(); li != dr.legs.end(); ++li)
      {
        // Add only Dirac and Majorana fermion legs
        if((li->type == "F") || (li->type == "M"))
          {
            VD u,v;
            std::vector<int>::iterator vit = std::find(vnames.begin(), vnames.end(), li->u);
            if(vit  != vnames.end())
              {
                u = (vit - vnames.begin());

                std::cout << "u From vec " << u <<std::endl;
              }
            else
              {
                u = add_vertex(IndexedGraph::vertex_property_type(li->u), g);

                std::cout << "u To   vec " << u <<std::endl;
                vnames.push_back(li->u);
                assert(u == vnames.size() - 1);
              }

            vit = std::find(vnames.begin(), vnames.end(), li->v);
            if( vit != vnames.end())
              {
                v = (vit - vnames.begin());
                std::cout << "v From vec " << v <<std::endl;
              }
            else
              {
                v = add_vertex(IndexedGraph::vertex_property_type(li->v), g);
                std::cout << "v To   vec " << v <<std::endl;
                vnames.push_back(li->v);
                assert(v == vnames.size() - 1);
              }

            add_edge (u, v, li->id, g);

            std::cout << "Leg " << li->type << " added" << std::endl;

            //     std::cout << "Leg " << li->type << " added" << std::endl;
          }

      }
    std::cout << "Fermion flow graph has " << num_edges(g) << " edges from props and legs" << std::endl;

    std::vector<int> component(num_vertices(g));
    fermion_lines = connected_components(g, &component[0]);

    // By default all lines are closed
    std::vector<bool> line_is_closed(fermion_lines, true);
    for (int i = 0; i != component.size(); ++i)
      if(vnames[i] < 0) line_is_closed[component[i]] = false;

    std::cout << "Total number of components: " << fermion_lines << std::endl;
    for (int i = 0; i != component.size(); ++i)
      std::cout << "Vertex " << i <<" is in component " << component[i] << std::endl;
    std::cout << std::endl;

    open_lines = std::count(line_is_closed.begin(), line_is_closed.end(), false);
    closed_lines = std::count(line_is_closed.begin(), line_is_closed.end(), true);

    std::cout << "We have " << std::count(line_is_closed.begin(), line_is_closed.end(), true) << " closed loops" << std::endl;

  }

  size_t open() const
  {
    return open_lines;
  }

  size_t closed() const
  {
    return closed_lines;
  }

};

#endif  // __GRAPH_HPP__


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/dispatch/function/overload.hpp>

#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/is_eqz.hpp>
#endif

#include <boost/simd/function/raw.hpp>
#include <boost/simd/function/pedantic.hpp>
#include <boost/simd/function/std.hpp>

#include <boost/simd/detail/constant/limitexponent.hpp>
#include <boost/simd/detail/constant/mask1frexp.hpp>
#include <boost/simd/detail/constant/mask2frexp.hpp>
#include <boost/simd/detail/constant/maxexponentm1.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_andnot.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/bitwise_or.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/config.hpp>
#include <utility>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD ( ifrexp_
                          , (typename A0)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using i_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE std::pair<A0,i_t> operator()(const pedantic_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      if (a0 == 0 || is_invalid(a0))
      {
        return {a0, i_t(0)};
      }
      else
      {
        auto m1f  = Mask1frexp<A0>();
        auto nmb  = Nbmantissabits<A0>();

        i_t r1    = simd::bitwise_cast<i_t>(bitwise_and(m1f, a0));  // extract exp.

#ifndef BOOST_SIMD_NO_DENORMALS
        i_t t = i_t(0);
        if(is_eqz(r1)) // denormal
        {
          a0 *= Twotonmb<A0>();
          r1  = simd::bitwise_cast<i_t>(bitwise_and(m1f, a0));  // extract exp. again
          t   = nmb;
        }
#endif

        A0 x  = bitwise_andnot(a0, m1f);               // clear exp. in a0
        r1 = shr(r1,nmb)- Maxexponentm1<A0>();         // compute exp.
        if (r1 > Limitexponent<A0>()) return {a0, i_t(0)};

#ifndef BOOST_SIMD_NO_DENORMALS
        r1 -= t;
#endif

        return {bitwise_or(x,Mask2frexp<A0>()), r1};
      }
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ifrexp_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using i_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE std::pair<A0,i_t> operator()(A0 a0) const BOOST_NOEXCEPT
    {
      if(!a0) return {A0(0),i_t(0)};
      else    return raw_(simd::ifrexp)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ifrexp_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::raw_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using i_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE std::pair<A0,i_t> operator()(const raw_tag &,  A0 a0) const BOOST_NOEXCEPT
    {
      auto m1f  = Mask1frexp<A0>();
      auto r1   = bitwise_cast<i_t>(bitwise_and(m1f, a0));
      A0    x   = bitwise_andnot(a0, m1f);

      return  { bitwise_or(x,Mask2frexp<A0>())
              , shr(r1,Nbmantissabits<A0>()) - Maxexponentm1<A0>()
              };
    }
  };

  BOOST_DISPATCH_OVERLOAD ( ifrexp_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    using i_t = bd::as_integer_t<A0, signed>;
    BOOST_FORCEINLINE std::pair<A0,i_t> operator()(const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      int e(0);
      A0 r = std::frexp(a0, &e);
      return {r, i_t(e)};
    }
  };
} } }

#endif


// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions 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.

#include <Eigen/Core>

#include "SurgSim/Collision/BoxCapsuleContact.h"

#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Collision/Representation.h"
#include "SurgSim/Math/Geometry.h"
#include "SurgSim/Math/Vector.h"

using SurgSim::DataStructures::Location;
using SurgSim::Math::BoxShape;
using SurgSim::Math::CapsuleShape;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Vector3d;
using SurgSim::Math::doesIntersectBoxCapsule;
using SurgSim::Math::distancePointSegment;
using SurgSim::Math::intersectionsSegmentBox;
using SurgSim::Math::Geometry::DistanceEpsilon;

namespace
{

typedef Eigen::AlignedBox<double, 3>::CornerType CornerType;

const std::array<std::pair<CornerType, CornerType>, 12> edges =
{
	std::make_pair(CornerType::BottomLeftFloor, CornerType::TopLeftFloor),
	std::make_pair(CornerType::BottomRightFloor, CornerType::TopRightFloor),
	std::make_pair(CornerType::BottomLeftCeil, CornerType::TopLeftCeil),
	std::make_pair(CornerType::BottomRightCeil, CornerType::TopRightCeil),

	std::make_pair(CornerType::BottomLeftFloor, CornerType::BottomRightFloor),
	std::make_pair(CornerType::BottomLeftCeil, CornerType::BottomRightCeil),
	std::make_pair(CornerType::TopLeftFloor, CornerType::TopRightFloor),
	std::make_pair(CornerType::TopLeftCeil, CornerType::TopRightCeil),

	std::make_pair(CornerType::BottomLeftFloor, CornerType::BottomLeftCeil),
	std::make_pair(CornerType::BottomRightFloor, CornerType::BottomRightCeil),
	std::make_pair(CornerType::TopLeftFloor, CornerType::TopLeftCeil),
	std::make_pair(CornerType::TopRightFloor, CornerType::TopRightCeil)
};
};

namespace SurgSim
{
namespace Collision
{

std::pair<int, int> BoxCapsuleContact::getShapeTypes()
{
	return std::pair<int, int>(SurgSim::Math::SHAPE_TYPE_BOX, SurgSim::Math::SHAPE_TYPE_CAPSULE);
}

std::list<std::shared_ptr<Contact>> BoxCapsuleContact::calculateDcdContact(
									 const SurgSim::Math::BoxShape& boxShape,
									 const SurgSim::Math::RigidTransform3d& boxPose,
									 const SurgSim::Math::CapsuleShape& capsuleShape,
									 const SurgSim::Math::RigidTransform3d& capsulePose) const
{
	std::list<std::shared_ptr<Contact>> contacts;

	RigidTransform3d capsuleToBoxTransform = boxPose.inverse() * capsulePose;
	Vector3d capsuleBottom = capsuleToBoxTransform * capsuleShape.bottomCenter();
	Vector3d capsuleTop = capsuleToBoxTransform * capsuleShape.topCenter();
	double capsuleRadius = capsuleShape.getRadius();

	Vector3d boxRadii = boxShape.getSize() / 2.0;
	Eigen::AlignedBox<double, 3> box(-boxRadii, boxRadii);

	if (doesIntersectBoxCapsule(capsuleBottom, capsuleTop, capsuleRadius, box))
	{
		Vector3d normal, segmentPoint, deepestBoxPoint, deepestCapsulePoint;
		distancePointSegment(Vector3d::Zero().eval(), capsuleBottom, capsuleTop, &segmentPoint);
		if (!segmentPoint.isZero(DistanceEpsilon))
		{
			// The capsule's segment does not pass through the box center.
			if (box.contains(segmentPoint))
			{
				// The capsule's segment passes through the box.
				Eigen::Index closestFace;
				(boxRadii - segmentPoint.cwiseAbs()).minCoeff(&closestFace);
				normal.setZero();
				normal[closestFace] = -segmentPoint[closestFace];
				normal.normalize();
				deepestBoxPoint = boxRadii.array() * (1 - 2 * (segmentPoint.array() < 0).cast<double>());
				deepestCapsulePoint = segmentPoint - capsuleRadius * segmentPoint.normalized();
			}
			else
			{
				// The closest point on the capsule's segment to the center of the box is outside the box.
				deepestBoxPoint = segmentPoint.array().min(box.max().array()).max(box.min().array());
				normal = deepestBoxPoint - segmentPoint;
				if (normal.norm() > capsuleRadius)
				{
					// The closest point to the box center is too far away.
					// Find the closest point to all 12 box edges.
					double minDistance = 2.0 * capsuleRadius;
					for (auto edge : edges)
					{
						Vector3d tempSegmentPoint;
						Vector3d tempBoxPoint;
						double tempDistance = SurgSim::Math::distanceSegmentSegment(capsuleBottom, capsuleTop,
											  box.corner(edge.first), box.corner(edge.second),
											  &tempSegmentPoint, &tempBoxPoint);
						if (tempDistance < minDistance)
						{
							minDistance = tempDistance;
							segmentPoint = tempSegmentPoint;
							deepestBoxPoint = tempBoxPoint;
						}
					}
					normal = deepestBoxPoint - segmentPoint;
					if (normal.norm() > capsuleRadius)
					{
						// The closest point to any edge is too far away.
						// Check the endpoints.
						segmentPoint = capsuleTop;
						deepestBoxPoint = segmentPoint.array().min(box.max().array()).max(box.min().array());
						normal = deepestBoxPoint - segmentPoint;
						if (normal.norm() > capsuleRadius)
						{
							segmentPoint = capsuleBottom;
							deepestBoxPoint = segmentPoint.array().min(box.max().array()).max(box.min().array());
							normal = deepestBoxPoint - segmentPoint;
						}
					}
				}
				normal.normalize();
				deepestCapsulePoint = segmentPoint + capsuleRadius * normal;
			}
		}
		else
		{
			// The capsule's segment passes through the box center.
			if (capsuleTop.isZero(DistanceEpsilon) && capsuleBottom.isZero(DistanceEpsilon))
			{
				// The capsule's segment has no length and is located at the box center.
				Eigen::Index closestFace;
				boxRadii.minCoeff(&closestFace);
				normal.setZero();
				normal[closestFace] = -boxRadii[closestFace];
				normal.normalize();
			}
			else
			{
				// The capsule's segment has a length, pick the closest endpoint to the box center.
				if (capsuleTop.squaredNorm() < capsuleBottom.squaredNorm())
				{
					segmentPoint = capsuleTop;
					normal = -capsuleBottom.normalized();
				}
				else
				{
					segmentPoint = capsuleBottom;
					normal = -capsuleTop.normalized();
				}
			}
			deepestBoxPoint = boxRadii.array() * (1 - 2 * (normal.array() > 0).cast<double>());
			deepestCapsulePoint = segmentPoint + capsuleRadius * normal;
		}

		double distance = (deepestCapsulePoint - deepestBoxPoint).dot(normal);
		normal = boxPose.linear() * normal;
		std::pair<Location, Location> penetrationPoints = std::make_pair(Location(deepestBoxPoint),
				Location(capsulePose.inverse() * boxPose * deepestCapsulePoint));
		contacts.emplace_back(std::make_shared<Contact>(COLLISION_DETECTION_TYPE_DISCRETE, distance, 1.0,
							  Vector3d::Zero(), normal, penetrationPoints));
	}
	return contacts;
}

}; // namespace Collision
}; // namespace SurgSim


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SUCCESSOR_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SUCCESSOR_HPP_INCLUDED

#include <boost/simd/toolbox/ieee/functions/successor.hpp>
#include <boost/simd/include/functions/simd/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/next.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::successor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return  seladd(neq(a0, Valmax<A0>()), a0, One<A0>());
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::successor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::next(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::successor_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<integer_<A0>,X>))((simd_<integer_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return seladd( boost::simd::gt(Valmax<A0>()-boost::simd::abs(a1), a0), a0, boost::simd::abs(a1));
    }
  };
} } }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is floating_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::successor_, tag::cpu_
                            , (A0)(A1)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<integer_<A1>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return select(eq(a0, Inf<A0>()), a0,  bitfloating(bitinteger(a0)+boost::simd::abs(a1)));
    }
  };
} } }

#endif


#include "bigssMath.h"

#include <iostream>
#include <algorithm>

#include <cisstVector/vctFixedSizeMatrixTypes.h>

#if USE_CISST
#include <cisstNumerical/nmrSVD.h>
#include <cisstNumerical/nmrPInverse.h>
#endif

#include <Eigen/Dense>

/// Convert from a cisst dynamic matrix to an eigen representation
/// \param X The cisst representation
/// \return The eigen representation
Eigen::MatrixXd BIGSS::convertCisstToEigen(const vctDynamicMatrix<double> &X)
{
  Eigen::MatrixXd xeig = Eigen::MatrixXd::Zero(X.rows(), X.cols());

  for (unsigned int i = 0; i < X.rows(); i++) {
    for (unsigned int j = 0; j < X.cols(); j++) {
      xeig(i, j) = X(i, j);
    }
  }
  return xeig;
}

/// Convert from an eigen matrix to cisst representation
/// \param X the eigen representation
/// \return The cisst representation
vctDynamicMatrix<double> BIGSS::convertEigenToCisst(const Eigen::MatrixXd &X)
{
  vctDynamicMatrix<double> xcisst(X.rows(), X.cols());
  for (auto i = 0; i < X.rows(); i++) {
    for (auto j = 0; j < X.cols(); j++) {
      xcisst(i, j) = X(i, j);
    }
  }
  return xcisst;
}

void BIGSS::ax_xb(vctDoubleMat &A, vctDoubleMat &B, vctFrm4x4 &X)
{

#if USE_CISST
  vctDouble3x3 mList, xtmp, mat;
  vctRot3 rotMat;
  vctDynamicMatrixRef<double> Atmp, Btmp;
  vctAxAnRot3 Arot, Brot;
  size_t nX = A.rows() / 4;

  mat.SetAll(0.0);
  mList.SetAll(0.0);
  xtmp.SetAll(0.0);

  vctDynamicMatrix<double> C(3 * nX, 3);
  vctDynamicVector<double> d(3 * nX);
  vctDynamicMatrixRef<double> Cref;
  vctDynamicVectorRef<double> dref;

  const unsigned int size = 3;
  vctDynamicMatrix<double> U(size, size);
  vctDynamicMatrix<double> Vt(size, size);
  vctDynamicVector<double> S(size);
  vctDynamicMatrix<double> svdCopy;

  for (size_t i = 0; i < nX; i++) {
    Atmp.SetRef(A, 4 * i, 0, 3, 3);
    Btmp.SetRef(B, 4 * i, 0, 3, 3);

    rotMat.Assign(Atmp);
    Arot.From(rotMat);
    rotMat.Assign(Btmp);
    Brot.From(rotMat);

    xtmp.OuterProductOf(Brot.Axis(), Arot.Axis());

    mList += Brot.Angle() * Arot.Angle() * xtmp;
  }

  svdCopy = mList;
  try {
    nmrSVD(svdCopy, U, S, Vt);
  }
  catch (...) {
    std::cout << "An exception occured, check cisstLog.txt." << std::endl;
  }

  mat.Diagonal().Assign(1 / S.X(), 1 / S.Y(), 1 / S.Z());
  vctFixedSizeMatrix<double, 3, 3> vv = Vt;
  xtmp = vv.Transpose() * mat * vv * mList.Transpose();
  X.Rotation().FromNormalized(vctRot3(xtmp));

  for (size_t i = 0; i < nX; i++) {
    Cref.SetRef(C, 3 * i, 0, 3, 3);
    dref.SetRef(d, 3 * i, 3);

    Atmp.SetRef(A, 4 * i, 0, 3, 3);
    Cref = vctMat::Eye(3) - Atmp;
    Atmp.SetRef(A, 4 * i, 3, 3, 1);
    Btmp.SetRef(B, 4 * i, 3, 3, 1);
    vctDynamicMatrix<double> cc = Atmp - vctDynamicMatrix<double>(xtmp) * Btmp;
    dref = cc.Column(0);
  }
  vctDynamicMatrix<double> inv(3, 3);
  vctDynamicMatrix<double> P = C.Transpose() * C;
  nmrPInverse(P, inv);
  X.Translation().Assign(inv * C.Transpose() * d);
#else
  size_t k = A.rows();
  Eigen::MatrixXd A_eig = Eigen::MatrixXd::Zero(k, 4);
  Eigen::MatrixXd B_eig = Eigen::MatrixXd::Zero(k, 4);
  Eigen::Matrix4d X_eig;

  for (size_t i = 0; i < k; i++)
  {
    A_eig.row(i) << A.Row(i).X(), A.Row(i).Y(), A.Row(i).Z(), A.Row(i).W();
    B_eig.row(i) << B.Row(i).X(), B.Row(i).Y(), B.Row(i).Z(), B.Row(i).W();
  }
  BIGSS::ax_xb(A_eig, B_eig, X_eig);

  X.Rotation().FromNormalized(vctRot3(vct3x3(
    X_eig(0, 0), X_eig(0, 1), X_eig(0, 2),
    X_eig(1, 0), X_eig(1, 1), X_eig(1, 2),
    X_eig(2, 0), X_eig(2, 1), X_eig(2, 2)
  )));
  X.Translation().Assign(X_eig(0, 3), X_eig(1, 3), X_eig(2, 3));
#endif // USE_CISST
}

void BIGSS::circleFitToOrigin(std::vector< vctFrm4x4 > data, vctFrm4x4 &centerPose, double &radius)
{
  vctDynamicMatrix<double> p(data.size(), 3);
  vctDynamicMatrix<double> pCentered = p;
  vct3 pMean(0.0, 0.0, 0.0);

  for(unsigned int i=0; i<data.size(); i++) {
    p.Row(i).Assign( data[i].Translation() );
    pMean += vct3(p.Row(i));
  }

  pMean /= static_cast<double>(data.size());

  for(unsigned int i=0; i<data.size(); i++) {
    pCentered.Row(i).Assign( vct3(p.Row(i)) - pMean );
  }

  // fit a plane
  // AB = 0, B = [b1 b2 b3 b4]
  // b1*X + b2*Y + b3*Z = B4 = 0
  vctDynamicMatrix<double> A(data.size(), 4, 1.0, VCT_COL_MAJOR);
  vctDynamicMatrixRef<double> ARef;
  ARef.SetRef(A, 0, 0, data.size(), 3);
  ARef.Assign(pCentered);

#if CISST_HAS_NETLIB
  vctDynamicMatrix<double> Vt(4, 4, VCT_COL_MAJOR);
  vctDynamicMatrix<double> input = A;
  vctDynamicMatrix<double> U(data.size(), 4, VCT_COL_MAJOR);
  vctDynamicVector<double> S(4);
  try {
    nmrSVDEconomy(input, U, S, Vt);
  } catch( ... ) {
    vtkErrorMacro(<< "Unable to compute SVD, check cisstLog.txt");
    centerPose = vctFrm4x4();
    radius = 0;
    return;
  }

  // Grab row since this is the transpose.
  vct3 Z(Vt.Row(3).X(), Vt.Row(3).Y(), Vt.Row(3).Z());
#else

  Eigen::MatrixXd Ad = Eigen::MatrixXd::Zero(data.size(), 4);
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Ad, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::MatrixXd v = svd.matrixV();

  vct3 Z(v.row(3).x(), v.row(3).y(), v.row(3).z());
#endif

  vct3 X(0.0, 0.0, -1.0);
  vct3 Y = vctCrossProduct(Z, X);
  X = vctCrossProduct(Y, Z);

  vctDynamicMatrix<double> coeff(3, 3);
  coeff.Column(0).Assign(X);
  coeff.Column(1).Assign(Y);
  coeff.Column(2).Assign(Z);

  vctDynamicMatrix<double> fit = pCentered * coeff;

  // fit a circle to XY plane
  vctDynamicVector<double> XPts = fit.Column(0);
  vctDynamicVector<double> YPts = fit.Column(1);
  vctDynamicVector<double> X2 = XPts;
  vctDynamicVector<double> Y2 = YPts;
  vctDynamicVector<double> XY;
  vctDynamicMatrix<double> CircleMat(data.size(), 3, 1.0);
  CircleMat.Column(0) = XPts;
  CircleMat.Column(1) = YPts;

  vctDynamicMatrix<double> CircleMatInv(3, data.size());

  X2.ElementwiseProductOf(XPts, XPts);
  Y2.ElementwiseProductOf(YPts, YPts);
  XY = X2 + Y2;
  XY *= -1;

#if CISST_HAS_NETLIB
  nmrPInverse(CircleMat, CircleMatInv);
#else
  Eigen::MatrixXd ceig = Eigen::MatrixXd::Zero(data.size(), 3);

  for (unsigned int i = 0; i < data.size(); i++)
  {
    ceig(i, 0) = CircleMat(i, 0);
    ceig(i, 1) = CircleMat(i, 1);
    ceig(i, 2) = CircleMat(i, 2);
  }

  // todo
  // perform pseudoinverse
  Eigen::MatrixXd cinv;

  for (unsigned int i = 0; i < data.size(); i++)
  {
    CircleMatInv(i, 0) = cinv(i, 0);
    CircleMatInv(i, 1) = cinv(i, 1);
    CircleMatInv(i, 2) = cinv(i, 2);
  }
#endif

  vct3 a;
  a = CircleMatInv * XY;

  double xc = -0.5 * a[0];
  double yc = -0.5 * a[1];
  radius = sqrt( (a[0]*a[0] + a[1]*a[1])/4 - a[2] );
  centerPose.Translation().Assign( coeff * vctDynamicVector<double>(3, xc, yc, 0.0) + vctDynamicVector<double>(pMean) );
}

bool BIGSS::rotationCalibration(std::vector< vctFrm4x4 > data, vctFrm4x4 &mat)
{
  vctFrm4x4 centerPose;

  auto mRows = data.size()-1;
  int mCols = 3;
  unsigned int i = 0;

  if( mRows < 4) {
    // want at least 4 frames
    return false;
  }

  // compute the center axis
  vctFrm4x4 base = data[0];
  vctDynamicMatrix<double> relRodAng(mRows, mCols, VCT_COL_MAJOR);
  vct3 centroid(0.0, 0.0, 0.0);

  for(i=1; i < data.size(); i++) {
    vctRodriguezRotation3<double> rodAng( vctRot3( base.Rotation().Transpose() * data[i].Rotation() ).Normalized() );
    relRodAng.Row(i-1).Assign(rodAng);
    centroid += rodAng;
  }

  centroid /= (static_cast<double>(data.size())-1.0);

  // Line Fitting in 3D
#ifdef CISST_HAS_NETLIB

  // translate the points to the centroid.
  vctDynamicMatrix<double> linePts = relRodAng;
  for(i=0; i < mRows; i++) {
    linePts.Row(i).X() -= centroid.X();
    linePts.Row(i).Y() -= centroid.Y();
    linePts.Row(i).Z() -= centroid.Z();
  }

  vctDynamicMatrix<double> Vt(mCols, mCols, VCT_COL_MAJOR);
  vctDynamicVector<double> S(mCols);

  vctDynamicMatrix<double> U(mRows, mCols, VCT_COL_MAJOR);

  try {
    nmrSVDEconomy(linePts, U, S, Vt);
  } catch( ... ) {
    vtkErrorMacro( << "Unable to compute SVD for rotation calibration");
    return false;
  }

  // find the index of the largest singular value and the corresponding
  // right singular vector
  double max = -1 * std::numeric_limits<double>::max();
  int maxidx = 0;
  for (i = 0; i<S.size(); i++) {
    if (max < S[i]) {
      max = S[i];
      maxidx = i;
    }
  }

  // Grab row since this is the transpose.
  vct3 Z = Vt.Row(maxidx);

#else

  // translate the points to the centroid.
  Eigen::MatrixXd linePts = Eigen::MatrixXd::Zero(relRodAng.rows(), relRodAng.cols());
  for (i = 0; i < mRows; i++) {
    linePts(i, 0) = relRodAng.Row(i).X() - centroid.X();
    linePts(i, 1) = relRodAng.Row(i).Y() - centroid.Y();
    linePts(i, 2) = relRodAng.Row(i).Z() - centroid.Z();
  }

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(linePts, Eigen::ComputeThinU | Eigen::ComputeThinV);

  Eigen::VectorXd sv = svd.singularValues();
  Eigen::VectorXd::Index max;
  sv.maxCoeff(&max);

  Eigen::MatrixXd v = svd.matrixV();

  vct3 Z(v.row(max).x(), v.row(max).y(), v.row(max).z());
#endif

  // End line fitting

  vctDynamicVector<double> angles(data.size());
  vctDynamicVectorRef<double> anglesSubRef(angles, 1, data.size()-1);
  anglesSubRef.Assign( relRodAng * vctDynamicVector<double>(Z) );
  angles[0] = 0;

  // compute center of frames by fitting a circle
  double radius;
  BIGSS::circleFitToOrigin(data, centerPose, radius);

  vct3 X = base.Rotation().Transpose() * (data[0].Translation()-centerPose.Translation()).Normalized();
  vct3 Y = vctCrossProduct(Z, X);
  Y.NormalizedSelf();
  X = vctCrossProduct(Y, Z);
  X.NormalizedSelf();
  vct3x3 coeff;
  coeff.Column(0).Assign(X);
  coeff.Column(1).Assign(Y);
  coeff.Column(2).Assign(Z);
  centerPose.Rotation().Assign( base.Rotation() * coeff );

  // compute translation and rotation part of offset
  vctDynamicMatrix<double> C(data.size()*3, 3, 0.0, VCT_COL_MAJOR);
  vctDynamicMatrix<double> CInv(3, data.size() * 3, VCT_COL_MAJOR);
  vctDynamicVector<double> d(data.size()*3, 0.0);
  vct3 OffsetRod(0.0, 0.0, 0.0);
  vctDynamicMatrixRef<double> CSubRef;
  vctDynamicVectorRef<double> dSubRef;
  vct3 ZAxis(0.0, 0.0, 1.0);

  for(i=0; i < data.size(); i++) {
    CSubRef.SetRef(C, i*3, 0, 3, 3);
    dSubRef.SetRef(d, i*3, 3);

    vctMatrixRotation3<double> zRot(vctAxisAngleRotation3<double> (ZAxis, angles[i]));
    CSubRef.Assign( centerPose.Rotation() * zRot );
    dSubRef.Assign( data[i].Translation() - centerPose.Translation() );

    vctRodriguezRotation3<double> rodAng( vctRot3( (centerPose.Rotation() * zRot).Transpose() * data[i].Rotation() ).Normalized() );
    OffsetRod += rodAng;
  }

#if CISST_HAS_NETLIB
  nmrPInverse(C, CInv);
#else
  Eigen::MatrixXd ceig = Eigen::MatrixXd::Zero(data.size() * 3, 3);

  for (unsigned int i = 0; i < data.size() * 3; i++)
  {
    ceig(i, 0) = C(i, 0);
    ceig(i, 1) = C(i, 1);
    ceig(i, 2) = C(i, 2);
  }

  // todo
  // perform pseudoinverse
  Eigen::MatrixXd cinv;

  for (unsigned int i = 0; i < data.size() * 3; i++)
  {
    CInv(i, 0) = cinv(i, 0);
    CInv(i, 1) = cinv(i, 1);
    CInv(i, 2) = cinv(i, 2);
  }

#endif

  mat.Translation().Assign( CInv*d );

  OffsetRod /= static_cast<double>(data.size());
  mat.Rotation().Assign( vctMatrixRotation3<double>( vctRodriguezRotation3<double>(OffsetRod) ) );

  return true;
}

bool BIGSS::pivotCalibration( std::vector< vctFrm4x4 > frames, vct3 &tooltip, vct3 &pivot, double &errorRMS )
{

  // Create equations
  auto numPoints = frames.size();

  if( numPoints < 4 ) {
    // want at least 4 frames
    std::cerr << "PivotCalibration: too few frames" << std::endl;
    return false;
  }

#if CISST_HAS_NETLIB
  // compute pivot calibration from a series of measured frames (vctFrm4x4)
  // ( method is based on Prof. Taylor's CIS class )
  // this is pulled from mtsNDISerial.cpp

  vctMat A(3 * numPoints, 6, VCT_COL_MAJOR);
  vctMat b(3 * numPoints, 1, VCT_COL_MAJOR);

  for (unsigned int i = 0; i < numPoints; i++) {

    vctDynamicMatrixRef<double> rotation(3, 3, 1, numPoints*3, A.Pointer(i*3, 0));
    rotation.Assign(frames[i].Rotation());

    vctDynamicMatrixRef<double> identity(3, 3, 1, numPoints*3, A.Pointer(i*3, 3));
    identity.Assign(-vctRot3::Identity());

    vctDynamicVectorRef<double> translation(3, b.Pointer(i*3, 0));
    translation.Assign(frames[i].Translation());
  }

  nmrLSSolver calibration(A, b);
  calibration.Solve(A, b);

  for (unsigned int i = 0; i < 3; i++) {
    tooltip.Element(i) = -b.at(i, 0);
    pivot.Element(i) = -b.at(i+3, 0);
  }
#else
  Eigen::MatrixXd A = Eigen::MatrixXd(3 * numPoints, 6);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(3 * numPoints);

  for (unsigned int i = 0; i < numPoints; i++) {

    A.block<1, 3>(3 * i, 0) << frames[i].Rotation().Row(0).X(), frames[i].Rotation().Row(0).Y(), frames[i].Rotation().Row(0).Z();
    A.block<1, 3>(3 * i + 1, 0) << frames[i].Rotation().Row(1).X(), frames[i].Rotation().Row(1).Y(), frames[i].Rotation().Row(1).Z();
    A.block<1, 3>(3 * i + 2, 0) << frames[i].Rotation().Row(2).X(), frames[i].Rotation().Row(2).Y(), frames[i].Rotation().Row(2).Z();
    A.block<3, 3>(3 * i, 3) = -Eigen::Matrix3d::Identity();
    b.segment<3>(3 * i) << frames[i].Translation().X(), frames[i].Translation().Y(), frames[i].Translation().Z();
  }

  Eigen::VectorXd soln = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

  for (unsigned int i = 0; i < 3; i++) {
    tooltip.Element(i) = -soln(i);
    pivot.Element(i) = -soln(i + 3);
  }
#endif

  vct3 error;
  double errorSquareSum = 0.0;
  for (unsigned int i = 0; i < numPoints; i++) {
    error = (frames[i] * tooltip) - pivot;
//    CMN_LOG_CLASS_RUN_DEBUG << error << std::endl;
    errorSquareSum += error.NormSquare();
  }
  errorRMS = sqrt(errorSquareSum / numPoints);

  return true;
}

vctDynamicMatrix<double> BIGSS::princomp(const vctDynamicMatrix<double> &X)
{
  Eigen::MatrixXd xeig = convertCisstToEigen(X);
  Eigen::MatrixXd Vt = princomp(xeig);
  return convertEigenToCisst(Vt);
}

void BIGSS::fit3DLine(const vctDynamicMatrix<double> &points, vct3 &point, vct3 &vec)
{
  Eigen::MatrixXd pts = convertCisstToEigen(points);
  Eigen::Vector3d p, v;
  fit3DLine(pts, p, v);
  point.Assign(p[0], p[1], p[2]);
  vec.Assign(v[0], v[1], v[2]);
}