/*
 * Copyright 2017 Yu Kunlin <yukunlin@mail.ustc.edu.cn>
 */
#include <Eigen/Eigen>

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>

#include <pgslam/pgslam.h>

#include <string>
#include <functional>

ros::Publisher node_pub;
ros::Publisher factor_pub;
ros::Publisher map_pub;
nav_msgs::OccupancyGrid global_map;
tf::TransformListener *plistener;

pgslam::Slam slam;

std::string map_frame  = "map";
std::string odom_frame = "odom";
std::string base_frame = "base_link";
double keyscan_threshold = 0.4;
double factor_threshold = 0.9;

void draw_graph() {
  visualization_msgs::Marker points;
  points.header.frame_id = map_frame;
  points.header.stamp = ros::Time::now();
  points.ns = "points_and_lines";
  points.action = visualization_msgs::Marker::ADD;
  points.pose.orientation.w = 1.0;
  points.id = 0;
  points.type = visualization_msgs::Marker::POINTS;
  points.scale.x = 0.05;
  points.scale.y = 0.05;
  points.color.g = 0.5f;
  points.color.a = 1.0;

  auto scans = slam.scans();
  scans[0].set_pose(pgslam::Pose2D());
  for (int i = 0; i < scans.size(); i++) {
    geometry_msgs::Point p;
    p.x = scans[i].pose().pos().x();
    p.y = scans[i].pose().pos().y();
    p.z = 0;
    points.points.push_back(p);
  }
  node_pub.publish(points);

  visualization_msgs::Marker line_list;
  line_list.header.frame_id = map_frame;
  line_list.header.stamp = ros::Time::now();
  line_list.ns = "points_and_lines";
  line_list.action = visualization_msgs::Marker::ADD;
  line_list.pose.orientation.w = 1.0;
  line_list.id = 1;
  line_list.type = visualization_msgs::Marker::LINE_LIST;
  line_list.scale.x = 0.01;
  line_list.color.r = 0.5;
  line_list.color.a = 1.0;

#ifdef USE_ISAM
  auto factors = slam.factors();
  for (size_t i = 0; i < factors.size(); i++) {
    geometry_msgs::Point first;
    first.x = factors[i].first.x();
    first.y = factors[i].first.y();
    line_list.points.push_back(first);
    geometry_msgs::Point second;
    second.x = factors[i].second.x();
    second.y = factors[i].second.y();
    line_list.points.push_back(second);
  }
  factor_pub.publish(line_list);
#endif
}

void draw_map() {
  double resolution = 0.05;
  double draw_range = 6.0;
  ros::param::get("~resolution", resolution);
  ros::param::get("~draw_range", draw_range);

  double max_x = 0.0;
  double min_x = 0.0;
  double max_y = 0.0;
  double min_y = 0.0;
  auto scans = slam.scans();
  for (size_t i = 0; i < scans.size(); i++) {
    if (max_x < scans[i].max_x_in_world())
      max_x = scans[i].max_x_in_world();
    if (min_x > scans[i].min_x_in_world())
      min_x = scans[i].min_x_in_world();
    if (max_y < scans[i].max_y_in_world())
      max_y = scans[i].max_y_in_world();
    if (min_y > scans[i].min_y_in_world())
      min_y = scans[i].min_y_in_world();
  }
  double max_x_i = static_cast<int>((max_x + 1.0) * 10) / 10.0;
  double min_x_i = static_cast<int>((min_x - 1.0) * 10) / 10.0;
  double max_y_i = static_cast<int>((max_y + 1.0) * 10) / 10.0;
  double min_y_i = static_cast<int>((min_y - 1.0) * 10) / 10.0;
  int width  = (max_x_i - min_x_i) / resolution;
  int height = (max_y_i - min_y_i) / resolution;

  // new an eigen map
  Eigen::MatrixXi emap(width, height);
  for (int i = 0; i < width; i++)
    for (int j = 0; j < height; j++)
      emap(i, j) = -1;

  // draw map into eigen map
  Eigen::Vector2d source(min_x_i, min_y_i);
  for (int i = 0; i < scans.size(); i++) {  // for every scan
    Eigen::Vector2d origin = scans[i].pose().pos();
    auto points = scans[i].points();
    for (int j = 1; j < points.cols(); j++) {  // for every point
      Eigen::Vector2d v = points.col(j) - origin;
      if (v.norm() > draw_range)
        v = v.normalized() * (draw_range + resolution);
      int steps = v.norm() / resolution;
      Eigen::Vector2d step = v / steps / resolution;
      Eigen::Vector2d current = (origin - source) / resolution;
      for (int i = 0; i < steps; i++) {  // for every step
        if (emap(current.x(), current.y()) == -1) {
          emap(current.x(), current.y()) = 30;  // white
        } else {
          emap(current.x(), current.y()) =
            static_cast<int>(emap(current.x(), current.y()) * 0.8);
        }
        current += step;
      }
      if (v.norm() < draw_range) {
        if (emap(current.x(), current.y()) == -1) {
          emap(current.x(), current.y()) = 100;  // black
        } else {
          emap(current.x(), current.y()) =
            static_cast<int>(emap(current.x(), current.y()) * 0.2);
          emap(current.x(), current.y()) += 100 * 0.8;
        }
      }
    }
  }

  // copy eigen map to ros map
  global_map.header.stamp = ros::Time::now();
  global_map.header.frame_id = map_frame;
  global_map.info.resolution = resolution;
  global_map.info.map_load_time = ros::Time::now();

  global_map.info.width = width;
  global_map.info.height = height;

  global_map.info.origin.position.x = min_x_i;
  global_map.info.origin.position.y = min_y_i;
  global_map.info.origin.position.z = 0;
  global_map.info.origin.orientation.w = 1.0;

  global_map.data.resize(width * height);

  for (int i = 0; i < width; i++)
    for (int j = 0; j < height; j++)
      global_map.data[j * width + i] = emap(i, j);

  // publish
  map_pub.publish(global_map);
}

pgslam::Pose2D
ListenPose2D(std::string target_frame, std::string source_frame) {
  pgslam::Pose2D pose;
  // listen
  tf::StampedTransform transform;
  std::string s;
  try {
    plistener->waitForTransform(target_frame,
        source_frame, ros::Time(0), ros::Duration(2.0),
        ros::Duration(0.01), &s);
    plistener->lookupTransform(target_frame,
        source_frame, ros::Time(0), transform);
  } catch (tf::TransformException ex) {
    ROS_ERROR("slam lookupTransform error: %s %s", ex.what(), s.c_str());
    return pose;
  }

  // calc pose
  pose.set_x(transform.getOrigin().x());
  pose.set_y(transform.getOrigin().y());

  double roll, pitch, yaw;
  transform.getBasis().getRPY(roll, pitch, yaw);

  pose.set_theta(yaw);

  return pose;
}

void BroadcastMapAndGraph() {
  draw_graph();
  draw_map();
}

void BroadcastPose(pgslam::Pose2D pose) {
  pgslam::Pose2D odom_pose = ListenPose2D(odom_frame, base_frame);
  pgslam::Pose2D delta = odom_pose.inverse() * pose;

  tf::StampedTransform transform;
  transform.setOrigin(tf::Vector3(delta.pos().x(), delta.pos().y(), 0.0));
  tf::Quaternion q;
  q.setRPY(0, 0, delta.theta());
  transform.setRotation(q);

  static tf::TransformBroadcaster br;
  br.sendTransform(tf::StampedTransform(transform,
        ros::Time::now(), map_frame, odom_frame));
}

pgslam::LaserScan
RosLaserScan_T_PGSlamLaserScan(const sensor_msgs::LaserScan& msg) {
  std::vector<pgslam::Echo> echos;
  size_t i = 0;
  for (double angle = msg.angle_min; angle <= msg.angle_max;
      angle += msg.angle_increment, i++) {
    echos.push_back(pgslam::Echo(msg.ranges[i], angle, msg.intensities[i], 0));
  }
  return pgslam::LaserScan(echos);
}

void scanCallback(const sensor_msgs::LaserScan& msg) {
  static pgslam::Pose2D odom_old;
  pgslam::Pose2D odom_new = ListenPose2D(odom_frame, base_frame);
  pgslam::Pose2D odom_delta = odom_new * odom_old.inverse();
  odom_old = odom_new;
  slam.UpdatePoseWithPose(odom_delta);
  slam.UpdatePoseWithLaserScan(RosLaserScan_T_PGSlamLaserScan(msg));
}

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

  ros::Subscriber scan_sub = node.subscribe("/scan", 10, scanCallback);

  node_pub   = node.advertise<visualization_msgs::Marker>("graph_node", 1);
  factor_pub = node.advertise<visualization_msgs::Marker>("graph_factor", 1);
  map_pub    = node.advertise<nav_msgs::OccupancyGrid>("/map", 1);

  std::function<void(void)> MapUpdateCallback = BroadcastMapAndGraph;
  slam.RegisterMapUpdateCallback(MapUpdateCallback);
  std::function<void(pgslam::Pose2D)> PoseUpdateCallback = BroadcastPose;
  slam.RegisterPoseUpdateCallback(PoseUpdateCallback);

  plistener = new tf::TransformListener();

  ros::param::get("~map_frame", map_frame);
  ros::param::get("~odom_frame", odom_frame);
  ros::param::get("~base_frame", base_frame);
  ros::param::get("~keyscan_threshold", keyscan_threshold);
  ros::param::get("~factor_threshold", factor_threshold);

  slam.set_keyscan_threshold(keyscan_threshold);
  slam.set_factor_threshold(factor_threshold);

  ros::spin();

  return 0;
}


// Copyright (C) 2016 Vicente J. Botet Escriba
//
//  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)

// <experimental/ordinal_range.hpp>

#include <experimental/ordinal_range.hpp>
#include "../../Bool.hpp"
#include "../../Bounded.hpp"

#include <boost/detail/lightweight_test.hpp>

int main()
{

  namespace stde = std::experimental;

  using Indx = Bounded<1,4,unsigned char>;

  {
    stde::ordinal_range<Indx> rng;
    auto b = rng.begin();
    BOOST_TEST(b->value==1);
  }
  return ::boost::report_errors();
}


/**
 * @file data_holder.hpp
 * @author Fujii Naomichi
 * @copyright (c) 2021 Fujii Naomichi
 * SPDX-License-Identifier: MIT
 */

#pragma once

#include <stdint.h>
#include <Eigen/Dense>

/// ADC2で測定したデータ
struct Adc2Data_t {
    float dc48v_voltage;
    float dribble_voltage;
    float dribble_current;
};

/// 車体の動きに関するデータ
struct MotionData_t {
    Eigen::Vector3f accelerometer;
    Eigen::Vector3f gyroscope;
    Eigen::Vector3f gravity;
    Eigen::Vector4f wheel_velocity;
    Eigen::Vector4f wheel_current_d;
    Eigen::Vector4f wheel_current_q;
    Eigen::Vector3f body_acceleration;
    Eigen::Vector3f body_velocity;
};

/// 制御に関するデータ
struct ControlData_t {
    Eigen::Vector4f current_ref;
    Eigen::Vector4f body_ref_accel;
};

/**
 * ペリフェラルレジスタ等からのデータの読み出しと変換を一括で行う
 * 読んだ値を保存しアクセスできるようにする
 */
class DataHolder {
public:
    /**
     * 制御ループの前でレジスタを読む
     */
    static void fetchOnPreControlLoop(void);

    /**
     * 制御ループの後でレジスタを読む
     */
    static void fetchOnPostControlLoop(void);

    /**
     * ADC2の変換結果を読む
     */
    static void fetchAdc2Result(void);

    /**
     * Adc2Dataを取得する
     * @return Adc2Dataへの参照
     */
    static const Adc2Data_t& adc2Data(void) {
        return _adc2_data;
    }

    /**
     * MotionDataを取得する
     * @return　MotionDataへの参照
     */
    static const MotionData_t& motionData(void) {
        return _motion_data;
    }

    /**
     * ControlDataを取得する
     * @return　ControlDataへの参照
     */
    static const ControlData_t& controlData(void) {
        return _control_data;
    }

private:
    /// ADC2で測定したデータ
    static Adc2Data_t _adc2_data;

    /// 機体の動きに関するデータ
    static MotionData_t _motion_data;

    /// 制御に関するデータ
    static ControlData_t _control_data;
};


#ifndef ASLAM_BACKEND_SIMPLE_REPROJECTION_ERROR_HPP
#define ASLAM_BACKEND_SIMPLE_REPROJECTION_ERROR_HPP

#include <aslam/backend/ErrorTerm.hpp>
#include <aslam/backend/HomogeneousExpression.hpp>
#include <boost/shared_ptr.hpp>

namespace aslam {
namespace backend {

template<typename FRAME_T>
class SimpleReprojectionError : public ErrorTermFs<FRAME_T::KeypointDimension> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef FRAME_T frame_t;
  typedef typename frame_t::keypoint_t keypoint_t;
  typedef typename frame_t::camera_geometry_t camera_geometry_t;
  enum {
    KeypointDimension = frame_t::KeypointDimension /*!< The dimension of the keypoint associated with this geometry policy */
  };

  typedef Eigen::Matrix<double, KeypointDimension, 1> measurement_t;
  typedef Eigen::Matrix<double, KeypointDimension, KeypointDimension> inverse_covariance_t;
  typedef ErrorTermFs<KeypointDimension> parent_t;

  SimpleReprojectionError();
  SimpleReprojectionError(const frame_t * frame, int keypointIndex,
                          HomogeneousExpression point);
  SimpleReprojectionError(const measurement_t & y,
                          const inverse_covariance_t & invR,
                          HomogeneousExpression point,
                          const camera_geometry_t & geometry);

  virtual ~SimpleReprojectionError();

 protected:
  /// \brief evaluate the error term
  virtual double evaluateErrorImplementation();

  /// \brief evaluate the jacobian
  virtual void evaluateJacobiansImplementation(
      aslam::backend::JacobianContainer & _jacobians);

  /// \brief the frame that this measurement comes from.
  measurement_t _y;

  /// \brief The camera geometry
  const camera_geometry_t * _geometry;

  /// \brief the homogeneous point expressed in the camera frame
  HomogeneousExpression _point;

};
}  // namespace backend
}  // namespace aslam

#include "implementation/SimpleReprojectionError.hpp"

#endif /* ASLAM_BACKEND_SIMPLE_REPROJECTION_ERROR_HPP */


#pragma once
#include "vector.hpp"
#include "quat.hpp"
#include "alignedalloc.hpp"
#include <boost/serialization/access.hpp>

namespace spn {
	enum POSERF {
		PRF_OFFSET = 0x01,
		PRF_ROTATE = 0x02,
		PRF_SCALE = 0x04,
		PRF_ALL = ~0,
		PRF_MAX = PRF_SCALE
	};
	//! 2次元姿勢クラス
	/*! 変換適用順序は[拡縮][回転][平行移動] */
	class Pose2D : public CheckAlign<16,Pose2D> {
		private:
			GAP_MATRIX_DEF(mutable, _finalMat, 3,2,
				(((Vec2, _ofs)))
				(((Vec2, _scale)))
				(((RadF, _angle), (mutable, uint32_t, _accum)))
			)

			friend class boost::serialization::access;
			template <class Archive>
			void serialize(Archive& ar, const unsigned int /*ver*/) {
				ar & BOOST_SERIALIZATION_NVP(_ofs) & BOOST_SERIALIZATION_NVP(_angle) & BOOST_SERIALIZATION_NVP(_scale);
				if(Archive::is_loading::value) {
					_accum = std::rand();
					_rflag = PRF_ALL;
				}
			}
		protected:
			mutable uint32_t	_rflag;

			void _refresh() const;
			const float* _getPtr() const;
			void _setAsChanged();
		public:
			struct Value;
			struct TValue {
				Vec2	ofs, scale;
				RadF	ang;

				TValue() = default;
				TValue(const Value& v);
				TValue(const Pose2D& p);
				TValue& operator = (const Pose2D& p);
				TValue& operator = (const TValue& tv) = default;
				TValue& operator = (const Value& v);
			};
			struct Value {
				Pose2D	&_pose;
				Vec2	&ofs, &scale;
				RadF	∠

				Value(Pose2D& p);
				~Value();
				Value(const Value& v);
				Value& operator = (const TValue& tv);
				void operator = (const Value& v) = delete;
			};

			Pose2D();
			Pose2D(const Pose2D& p);
			Pose2D(const Vec2& pos, RadF ang, const Vec2& sc);
			Pose2D(const TValue& tv);
			void identity();

			// ---- Lua互換用メソッド ----
			AMat33 luaGetToWorld() const;
			AMat33 luaGetToLocal() const;
			bool equal(const Pose2D& ps) const;
			std::string toString() const;
			// ---- getter method ----
			const Vec2& getOffset() const;
			const Vec2& getScale() const;
			const AMat32& getToWorld() const;
			AMat32 getToLocal() const;
			uint32_t getAccum() const;
			RadF getAngle() const;
			// ---- setter method ----
			void setAll(const Vec2& ofs, RadF ang, const Vec2& sc);
			void setScale(const Vec2& ofs);
			void setAngle(RadF ang);
			void setOffset(const Vec2& ofs);
			// ---- reference method ----
			Vec2& refOffset();
			Vec2& refScale();
			RadF& refAngle();
			// ---- helper function ----
			void moveUp(float speed);
			void moveDown(float speed);
			void moveLeft(float speed);
			void moveRight(float speed);
			Vec2 getUp() const;
			Vec2 getRight() const;
			void setUp(const Vec2& up);

			Pose2D lerp(const Pose2D& p1, float t) const;
			Value refValue();
			void apply(const TValue& t);

			Pose2D& operator = (const Pose2D& ps);
			Pose2D& operator = (const TValue& tv);
			bool operator == (const Pose2D& ps) const;
			bool operator != (const Pose2D& ps) const;
			friend std::ostream& operator << (std::ostream&, const Pose2D&);
	};
	std::ostream& operator << (std::ostream& os, const Pose2D& ps);

	//! 3次元姿勢クラス
	class Pose3D : public CheckAlign<16,Pose3D> {
		private:
			AVec3			_ofs;
			AQuat			_rot;
			AVec3			_scale;

			GAP_MATRIX_DEF(mutable, _finalMat, 4,3,
				(((uint32_t, _accum)))
				(((mutable, uint32_t, _rflag)))
			)

			void _refresh() const;

			constexpr static float TURN_THRESHOLD = 1e-5f;
			friend class boost::serialization::access;
			template <class Archive>
			void serialize(Archive& ar, const unsigned int /*ver*/) {
				ar & BOOST_SERIALIZATION_NVP(_ofs) & BOOST_SERIALIZATION_NVP(_rot) & BOOST_SERIALIZATION_NVP(_scale);
				if(Archive::is_loading::value) {
					_accum = std::rand();
					_rflag = PRF_ALL;
				}
			}

		protected:
			const float* _getPtr() const;
			void _setAsChanged();
		public:
			struct Value;
			struct TValue {
				Vec3	ofs, scale;
				Quat	rot;

				TValue() = default;
				TValue(const Value& v);
				TValue(const Pose3D& p);
				TValue& operator = (const TValue& tv) = default;
				TValue& operator = (const Value& v);
				TValue& operator = (const Pose3D& p);
			};
			struct Value {
				Pose3D	&_pose;
				AVec3	&ofs, &scale;
				AQuat	&rot;

				Value(Pose3D& p);
				~Value();
				Value(const Value& v);
				Value& operator = (const TValue& tv);
				void operator = (const Value& v) = delete;
			};

			Pose3D();
			Pose3D(const Pose3D& p);
			Pose3D(const AVec3& pos, const AQuat& rot, const AVec3& sc);
			Pose3D(const TValue& tv);
			Pose3D(const AMat43& m);
			void identity();

			// ---- Lua互換用メソッド ----
			AMat44 luaGetToWorld() const;
			AMat44 luaGetToLocal() const;
			bool equal(const Pose3D& p) const;
			std::string toString() const;
			// ---- helper function ----
			//! 前方への移動(XZ平面限定)
			void moveFwd2D(float speed);
			//! サイド移動(XZ平面限定)
			void moveSide2D(float speed);
			//! 前方への移動(軸フリー)
			void moveFwd3D(float speed);
			//! サイド移動(軸フリー)
			void moveSide3D(float speed);

			//! 方向転換(軸指定)
			void turnAxis(const AVec3& axis, spn::RadF rad);
			//! Yaw Pitch Roll指定の回転
			void turnYPR(spn::RadF yaw, spn::RadF pitch, spn::RadF roll);
			//! 差分入力
			void addRot(const AQuat& q);
			//! 補間付き回転
			/*! 3軸の目標距離を合計した物が閾値以下ならtrueを返す */
			bool lerpTurn(const AQuat& q_tgt, float t, float threshold=TURN_THRESHOLD);
			//! Upベクトルをrollが0になるよう補正
			void adjustNoRoll();

			// ---- getter method ----
			const AVec3& getOffset() const;
			const AQuat& getRot() const;
			const AVec3& getScale() const;
			const AMat43& getToWorld() const;
			AMat43 getToLocal() const;
			uint32_t getAccum() const;

			AVec3 getUp() const;
			AVec3 getRight() const;
			AVec3 getDir() const;

			// ---- setter method ----
			void setAll(const AVec3& ofs, const AQuat& q, const AVec3& sc);
			// Scaleに変更を加える
			void setScale(const AVec3& s);
			// Rotationに変更を加える
			void setRot(const AQuat& q);
			void addAxisRot(const AVec3& axis, RadF radf);
			// Positionに変更を加える
			void setOffset(const AVec3& v);
			void addOffset(const AVec3& ad);

			// ---- reference method ----
			AVec3& refOffset();
			AQuat& refRot();
			AVec3& refScale();

			Pose3D lerp(const Pose3D& p1, float t) const;
			Value refValue();
			void apply(const TValue& t);

			Pose3D& operator = (const Pose3D& ps);
			Pose3D& operator = (const TValue& tv);
			Pose3D& operator = (const AMat43& m);
			// ---- compare method ----
			bool operator == (const Pose3D& ps) const;
			bool operator != (const Pose3D& ps) const;
			friend std::ostream& operator << (std::ostream&, const Pose3D&);
	};
	std::ostream& operator << (std::ostream& os, const Pose3D& ps);
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/LU>

template<typename MatrixType> void determinant(const MatrixType& m)
{
  /* this test covers the following files:
     Determinant.h
  */
  typedef typename MatrixType::Index Index;
  Index size = m.rows();

  MatrixType m1(size, size), m2(size, size);
  m1.setRandom();
  m2.setRandom();
  typedef typename MatrixType::Scalar Scalar;
  Scalar x = ei_random<Scalar>();
  VERIFY_IS_APPROX(MatrixType::Identity(size, size).determinant(), Scalar(1));
  VERIFY_IS_APPROX((m1*m2).eval().determinant(), m1.determinant() * m2.determinant());
  if(size==1) return;
  Index i = ei_random<Index>(0, size-1);
  Index j;
  do {
    j = ei_random<Index>(0, size-1);
  } while(j==i);
  m2 = m1;
  m2.row(i).swap(m2.row(j));
  VERIFY_IS_APPROX(m2.determinant(), -m1.determinant());
  m2 = m1;
  m2.col(i).swap(m2.col(j));
  VERIFY_IS_APPROX(m2.determinant(), -m1.determinant());
  VERIFY_IS_APPROX(m2.determinant(), m2.transpose().determinant());
  VERIFY_IS_APPROX(ei_conj(m2.determinant()), m2.adjoint().determinant());
  m2 = m1;
  m2.row(i) += x*m2.row(j);
  VERIFY_IS_APPROX(m2.determinant(), m1.determinant());
  m2 = m1;
  m2.row(i) *= x;
  VERIFY_IS_APPROX(m2.determinant(), m1.determinant() * x);

  // check empty matrix
  VERIFY_IS_APPROX(m2.block(0,0,0,0).determinant(), Scalar(1));
}

void test_determinant()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( determinant(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_2( determinant(Matrix<double, 2, 2>()) );
    CALL_SUBTEST_3( determinant(Matrix<double, 3, 3>()) );
    CALL_SUBTEST_4( determinant(Matrix<double, 4, 4>()) );
    CALL_SUBTEST_5( determinant(Matrix<std::complex<double>, 10, 10>()) );
    CALL_SUBTEST_6( determinant(MatrixXd(20, 20)) );
  }
  CALL_SUBTEST_6( determinant(MatrixXd(200, 200)) );
}


#include <boost/test/unit_test.hpp>

#include "fixtures.h"
#include <stdexcept>

#if LUAPP_API_VERSION >= 52

struct Udata{int x;};
LUAPP_USERDATA(Udata, "Test.Userdata")

enum Ops {
	Unm = 0,
	Add,
	Sub,
	Mul,
	Div,
	Mod,
	Pow,
#if(LUAPP_API_VERSION >= 53)
	Idiv,
	Band,
	Bor,
	Bxor,
	Bneg,
	Shl,
	Shr,
#endif	// V53+

    Size
};

static int opcount[Ops::Size];
static inline void resetCounts() {for(auto& count: opcount) count = 0;}
static inline bool checkOthers(Ops op)
{
	for(int i = Ops::Unm; i < Ops::Size; ++i)
		if(i != op && opcount[i] != 0)
			return false;
	return true;
}

// We aren't really chasing computational efficiency here
static inline int mypow(int val, int p) {int rv = val; for(int i = 1; i < p; ++i) rv *= val; return rv;}

using lua::Retval;
using lua::Context;
using lua::mkcf;
using lua::Value;

static Retval unm(Context& c) {++ opcount[Ops::Unm]; return c.ret( -c.args[0].cast<Udata>().x);}
static Retval add(Context& c) {++ opcount[Ops::Add]; return c.ret( c.args[0].cast<Udata>().x + c.args[1].cast<int>());}
static Retval sub(Context& c) {++ opcount[Ops::Sub]; return c.ret( c.args[0].cast<Udata>().x - c.args[1].cast<int>());}
static Retval mul(Context& c) {++ opcount[Ops::Mul]; return c.ret( c.args[0].cast<Udata>().x * c.args[1].cast<int>());}
static Retval div(Context& c) {++ opcount[Ops::Div]; return c.ret( c.args[0].cast<Udata>().x / c.args[1].cast<double>());}
static Retval mod(Context& c) {++ opcount[Ops::Mod]; return c.ret( c.args[0].cast<Udata>().x % c.args[1].cast<int>());}
static Retval pow(Context& c) {++ opcount[Ops::Pow]; return c.ret( mypow(c.args[0].cast<Udata>().x, c.args[1].cast<int>()));}
#if(LUAPP_API_VERSION >= 53)
static Retval do_idiv(Context& c) {++ opcount[Ops::Idiv]; return c.ret( c.args[0].cast<Udata>().x / c.args[1].cast<int>());}
static Retval do_band(Context& c) {++ opcount[Ops::Band]; return c.ret( c.args[0].cast<Udata>().x & c.args[1].cast<int>());}
static Retval do_bor (Context& c) {++ opcount[Ops::Bor ]; return c.ret( c.args[0].cast<Udata>().x | c.args[1].cast<int>());}
static Retval do_bxor(Context& c) {++ opcount[Ops::Bxor]; return c.ret( c.args[0].cast<Udata>().x ^ c.args[1].cast<int>());}
static Retval do_bneg(Context& c) {++ opcount[Ops::Bneg]; return c.ret( ~static_cast<unsigned long long>(c.args[0].cast<Udata>().x));}
static Retval do_shl (Context& c) {++ opcount[Ops::Shl ]; return c.ret( c.args[0].cast<Udata>().x << c.args[1].cast<int>());}
static Retval do_shr (Context& c) {++ opcount[Ops::Shr ];	return c.ret( c.args[0].cast<Udata>().x >> c.args[1].cast<int>());}
#endif	// V53+

struct fxArith: public fxContext{

	struct Dummy {
		explicit Dummy(Context& context)
		{
        context.mt<Udata>() = lua::Table::records(context,
			"__unm", mkcf<unm>,
			"__add", mkcf<add>,
			"__sub", mkcf<sub>,
			"__mul", mkcf<mul>,
			"__div", mkcf<div>,
			"__mod", mkcf<mod>,
			"__pow", mkcf<pow>
#if(LUAPP_API_VERSION >= 53)
			,
			"__idiv", mkcf<do_idiv>,
			"__band", mkcf<do_band>,
			"__bor" , mkcf<do_bor >,
			"__bxor", mkcf<do_bxor>,
			"__bnot", mkcf<do_bneg>,
			"__shl" , mkcf<do_shl>,
			"__shr" , mkcf<do_shr>
#endif	// V53+
		);
		}
	} dummy {context};

	Value v2, v3, uv;

	fxArith():
		v2(2, context),
		v3(3, context),
		uv(Udata{2}, context)
	{
		resetCounts();
		context.global["l2"] = 2;
		context.global["l3"] = 3;
		context.global["ul"] = Udata{2};
	}
};

#define l2 context.global["l2"]
#define l3 context.global["l3"]
#define ul context.global["ul"]

BOOST_AUTO_TEST_SUITE(Arithmetics)

BOOST_FIXTURE_TEST_CASE(UnaryMinus, fxArith)
{
	BOOST_CHECK_EQUAL((-v2).cast<int>(), -2);
	BOOST_CHECK_EQUAL((-l2).cast<int>(), -2);
	BOOST_CHECK_EQUAL((-uv).cast<int>(), -2);
	BOOST_CHECK_EQUAL((-ul).cast<int>(), -2);
	BOOST_CHECK_EQUAL(opcount[Ops::Unm], 2);
	BOOST_CHECK(checkOthers(Ops::Unm));
}

BOOST_FIXTURE_TEST_CASE(Plus, fxArith)
{
	BOOST_CHECK_EQUAL((v2 + v3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((l2 + l3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((v2 + l3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((l2 + v3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((v2 + 3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((2 + v3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((l2 + 3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((2 + l3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((uv + 3).cast<int>(), 5);
	BOOST_CHECK_EQUAL((ul + 3).cast<int>(), 5);
	BOOST_CHECK_EQUAL(opcount[Ops::Add], 2);
	BOOST_CHECK(checkOthers(Ops::Add));
}

BOOST_FIXTURE_TEST_CASE(Minus, fxArith)
{
	BOOST_CHECK_EQUAL((v2 - v3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((l2 - l3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((v2 - l3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((l2 - v3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((v2 - 3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((2 - v3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((l2 - 3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((2 - l3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((uv - 3).cast<int>(), -1);
	BOOST_CHECK_EQUAL((ul - 3).cast<int>(), -1);
	BOOST_CHECK_EQUAL(opcount[Ops::Sub], 2);
	BOOST_CHECK(checkOthers(Ops::Sub));
}

BOOST_FIXTURE_TEST_CASE(Multiply, fxArith)
{
	BOOST_CHECK_EQUAL((v2 * v3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((l2 * l3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((v2 * l3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((l2 * v3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((v2 * 3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((2 * v3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((l2 * 3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((2 * l3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((uv * 3).cast<int>(), 6);
	BOOST_CHECK_EQUAL((ul * 3).cast<int>(), 6);
	BOOST_CHECK_EQUAL(opcount[Ops::Mul], 2);
	BOOST_CHECK(checkOthers(Ops::Mul));
}

BOOST_FIXTURE_TEST_CASE(Divide, fxArith)
{
	uv = Udata{10};
	ul = Udata{10};
	BOOST_CHECK_EQUAL((v3 / v2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((l3 / l2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((v3 / l2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((l3 / v2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((v3 / 2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((3 / v2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((l3 / 2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((3 / l2).cast<double>(), 1.5);
	BOOST_CHECK_EQUAL((uv / 3).cast<double>(), 10.0 / 3);
	BOOST_CHECK_EQUAL((ul / 3).cast<double>(), 10.0 / 3);
	BOOST_CHECK_EQUAL(opcount[Ops::Div], 2);
	BOOST_CHECK(checkOthers(Ops::Div));
}

BOOST_FIXTURE_TEST_CASE(Modulus, fxArith)
{
	uv = Udata{10};
	ul = Udata{10};
	BOOST_CHECK_EQUAL((v3 % v2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((l3 % l2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((v3 % l2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((l3 % v2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((v3 % 2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((3 % v2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((l3 % 2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((3 % l2).cast<int>(), 1);
	BOOST_CHECK_EQUAL((uv % 3).cast<int>(), 1);
	BOOST_CHECK_EQUAL((ul % 3).cast<int>(), 1);
	BOOST_CHECK_EQUAL(opcount[Ops::Mod], 2);
	BOOST_CHECK(checkOthers(Ops::Mod));
}

BOOST_FIXTURE_TEST_CASE(Power, fxArith)
{
	BOOST_CHECK_EQUAL((v2 ^ v3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((l2 ^ l3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((v2 ^ l3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((l2 ^ v3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((v2 ^ 3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((2 ^ v3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((l2 ^ 3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((2 ^ l3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((uv ^ 3).cast<int>(), 8);
	BOOST_CHECK_EQUAL((ul ^ 3).cast<int>(), 8);
	BOOST_CHECK_EQUAL(opcount[Ops::Pow], 2);
	BOOST_CHECK(checkOthers(Ops::Pow));
}

BOOST_FIXTURE_TEST_CASE(Chaining, fxArith)
{
	BOOST_CHECK_EQUAL((v2 + l2*v3  + (l3^v2) - l2).cast<int>(), 15);
}

BOOST_FIXTURE_TEST_CASE(Discard, fxArith)
{
	uv + 1;
	ul + 1;
#ifdef LUAPP_NONDISCARDABLE_ARITHMETICS
	BOOST_CHECK_EQUAL(opcount[Ops::Add], 2);
	BOOST_CHECK(checkOthers(Ops::Add));
#else
	BOOST_CHECK_EQUAL(opcount[Ops::Add], 0);
	BOOST_CHECK(checkOthers(Ops::Add));
#endif // LUAPP_NODISCARD_ARITHMETICS
}

#if(LUAPP_API_VERSION >= 53)
BOOST_FIXTURE_TEST_CASE( IntegerDivision, fxArith)
{
	uv = Udata{10};
	ul = Udata{10};
	BOOST_CHECK_EQUAL(idiv(v3, v2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(l3, l2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(v3, l2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(l3, v2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(v3, 2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(3, v2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(l3, 2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(3, l2).cast<double>(), 1);
	BOOST_CHECK_EQUAL(idiv(uv, 3).cast<double>(), 3);
	BOOST_CHECK_EQUAL(idiv(ul, 3).cast<double>(), 3);
	BOOST_CHECK_EQUAL(opcount[Ops::Idiv], 2);
	BOOST_CHECK(checkOthers(Ops::Idiv));
}

BOOST_FIXTURE_TEST_CASE(BinaryAnd, fxArith)
{
    BOOST_CHECK_EQUAL(band(v3, v2).to<int>(), 2);
    BOOST_CHECK_EQUAL(band(l3, l2).to<int>(), 2);
    BOOST_CHECK_EQUAL(band(v3, l2).to<int>(), 2);
    BOOST_CHECK_EQUAL(band(l3, v2).to<int>(), 2);
	BOOST_CHECK_EQUAL(band(v3, 2).to<int>(), 2);
	BOOST_CHECK_EQUAL(band(3, v2).to<int>(), 2);
	BOOST_CHECK_EQUAL(band(l3, 2).to<int>(), 2);
	BOOST_CHECK_EQUAL(band(3, l2).to<int>(), 2);
	BOOST_CHECK_EQUAL(band(uv, 3).to<int>(), 2);
	BOOST_CHECK_EQUAL(band(ul, 3).to<int>(), 2);
	BOOST_CHECK_EQUAL(opcount[Ops::Band], 2);
	BOOST_CHECK(checkOthers(Ops::Band));
}

BOOST_FIXTURE_TEST_CASE(BinaryOr, fxArith)
{
    BOOST_CHECK_EQUAL(bor(v3, v2).to<int>(), 3);
    BOOST_CHECK_EQUAL(bor(l3, l2).to<int>(), 3);
    BOOST_CHECK_EQUAL(bor(v3, l2).to<int>(), 3);
    BOOST_CHECK_EQUAL(bor(l3, v2).to<int>(), 3);
	BOOST_CHECK_EQUAL(bor(v3, 2).to<int>(), 3);
	BOOST_CHECK_EQUAL(bor(3, v2).to<int>(), 3);
	BOOST_CHECK_EQUAL(bor(l3, 2).to<int>(), 3);
	BOOST_CHECK_EQUAL(bor(3, l2).to<int>(), 3);
	BOOST_CHECK_EQUAL(bor(uv, 3).to<int>(), 3);
	BOOST_CHECK_EQUAL(bor(ul, 3).to<int>(), 3);
	BOOST_CHECK_EQUAL(opcount[Ops::Bor], 2);
	BOOST_CHECK(checkOthers(Ops::Bor));
}

BOOST_FIXTURE_TEST_CASE(BinaryXor, fxArith)
{
    BOOST_CHECK_EQUAL(bxor(v3, v2).to<int>(), 1);
    BOOST_CHECK_EQUAL(bxor(l3, l2).to<int>(), 1);
    BOOST_CHECK_EQUAL(bxor(v3, l2).to<int>(), 1);
    BOOST_CHECK_EQUAL(bxor(l3, v2).to<int>(), 1);
	BOOST_CHECK_EQUAL(bxor(v3,  2).to<int>(), 1);
	BOOST_CHECK_EQUAL(bxor( 3, v2).to<int>(), 1);
	BOOST_CHECK_EQUAL(bxor(l3,  2).to<int>(), 1);
	BOOST_CHECK_EQUAL(bxor( 3, l2).to<int>(), 1);
	BOOST_CHECK_EQUAL(bxor(uv,  3).to<int>(), 1);
	BOOST_CHECK_EQUAL(bxor(ul,  3).to<int>(), 1);
	BOOST_CHECK_EQUAL(opcount[Ops::Bxor], 2);
	BOOST_CHECK(checkOthers(Ops::Bxor));
}

BOOST_FIXTURE_TEST_CASE(BinaryNeg, fxArith)
{
    BOOST_CHECK_EQUAL(bneg(v2).to<unsigned long long>(), 0xFFFFFFFFFFFFFFFDull);
    BOOST_CHECK_EQUAL(bneg(l2).to<unsigned long long>(), 0xFFFFFFFFFFFFFFFDull);
	BOOST_CHECK_EQUAL(bneg(uv).to<unsigned long long>(), 0xFFFFFFFFFFFFFFFDull);
	BOOST_CHECK_EQUAL(bneg(ul).to<unsigned long long>(), 0xFFFFFFFFFFFFFFFDull);
	BOOST_CHECK_EQUAL(opcount[Ops::Bneg], 2);
	BOOST_CHECK(checkOthers(Ops::Bneg));
}

BOOST_FIXTURE_TEST_CASE(ShiftLeft, fxArith)
{
    BOOST_CHECK_EQUAL(shl(v3, v2).to<int>(), 12);
    BOOST_CHECK_EQUAL(shl(l3, l2).to<int>(), 12);
    BOOST_CHECK_EQUAL(shl(v3, l2).to<int>(), 12);
    BOOST_CHECK_EQUAL(shl(l3, v2).to<int>(), 12);
	BOOST_CHECK_EQUAL(shl(v3,  2).to<int>(), 12);
	BOOST_CHECK_EQUAL(shl( 3, v2).to<int>(), 12);
	BOOST_CHECK_EQUAL(shl(l3,  2).to<int>(), 12);
	BOOST_CHECK_EQUAL(shl( 3, l2).to<int>(), 12);
	BOOST_CHECK_EQUAL(shl(uv,  3).to<int>(), 16);
	BOOST_CHECK_EQUAL(shl(ul,  3).to<int>(), 16);
	BOOST_CHECK_EQUAL(opcount[Ops::Shl], 2);
	BOOST_CHECK(checkOthers(Ops::Shl));
}

BOOST_FIXTURE_TEST_CASE(ShiftRight, fxArith)
{
	v3 = 13;
	l3 = 13;
	uv = Udata{17};
	ul = Udata{17};
    BOOST_CHECK_EQUAL(shr(v3, v2).to<int>(), 3);
    BOOST_CHECK_EQUAL(shr(l3, l2).to<int>(), 3);
    BOOST_CHECK_EQUAL(shr(v3, l2).to<int>(), 3);
    BOOST_CHECK_EQUAL(shr(l3, v2).to<int>(), 3);
	BOOST_CHECK_EQUAL(shr(v3,  2).to<int>(), 3);
	BOOST_CHECK_EQUAL(shr(13, v2).to<int>(), 3);
	BOOST_CHECK_EQUAL(shr(l3,  2).to<int>(), 3);
	BOOST_CHECK_EQUAL(shr(13, l2).to<int>(), 3);
	BOOST_CHECK_EQUAL(shr(uv,  3).to<int>(), 2);
	BOOST_CHECK_EQUAL(opcount[Ops::Shr], 1);
	BOOST_CHECK_EQUAL(shr(ul,  3).to<int>(), 2);
	BOOST_CHECK_EQUAL(opcount[Ops::Shr], 2);
	BOOST_CHECK(checkOthers(Ops::Shr));
}
#endif	// V53+

BOOST_AUTO_TEST_SUITE_END()

#endif	// V52+


//
// Copyright (c) 2015, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH.
// 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 <fstream>
#include <boost/archive/binary_iarchive.hpp>
#include <maps/grid/MLSMap.hpp>

#include <boost/test/unit_test.hpp>
#include <envire_collider_mls/MLSCollision.hpp>

#define	DEGTORAD			0.01745329251994329577f				//!< PI / 180.0, convert degrees to radians
//#include <boost/shared_ptr.hpp>

using namespace envire::collision;

BOOST_AUTO_TEST_CASE(test_box_collision)
{

    dInitODE();
    {
		MLSCollision* c = MLSCollision::getInstance();
		BOOST_CHECK(c != NULL);
		BOOST_CHECK(c->getGeomID() == -1);

		std::string env_path("./mlsdata/MLSMapKalman_waves.bin");
		std::ifstream input(env_path,  std::ios::binary);
		//std::ifstream input("MLSMapKalman_waves.bin",  std::ios::binary);
		boost::archive::binary_iarchive  ia(input);

		maps::grid::MLSMapKalman mls_kalman;
		ia >> mls_kalman;

	    boost::shared_ptr<maps::grid::MLSMapKalman> mls(&mls_kalman);

 	    BOOST_TEST_MESSAGE("mls resolution: " << mls->getResolution().x() <<", "<< mls->getResolution().y());
        BOOST_TEST_MESSAGE("mls cell number: "<< mls->getNumCells().x() <<", "<< mls->getNumCells().y());
 	    BOOST_TEST_MESSAGE("mls size: "<< mls->getSize().x()<<", "<<mls->getSize().y());
        BOOST_TEST_MESSAGE("mls mean value on (x,y): "<< mls->at(0,0).begin()->getMean());

		// create first geom
		dGeomID geom_mls = c->createNewCollisionObject(mls);
		BOOST_CHECK(geom_mls->type == 14);

        c->widthX  = mls->getSize().x();
        c->widthY  = mls->getSize().y();

    dVector3 pos;
	pos[ 0 ] = 0;
	pos[ 1 ] = 0;
	pos[ 2 ] = 0;

	// Rotate so Z is up, not Y (which is the default orientation)
	dMatrix3 R;
	dRSetIdentity( R );
	//dRFromAxisAndAngle( R, 1, 0, 0, DEGTORAD * 90 );

	// Place it.
	dGeomSetRotation( geom_mls, R );
	dGeomSetPosition( geom_mls, pos[0], pos[1], pos[2] );

        // check interface
        BOOST_CHECK(c->getGeomID() >= dFirstUserClass);
        BOOST_CHECK(dGeomGetClass(geom_mls) == c->getGeomID());

        boost::shared_ptr<maps::grid::MLSMapKalman> user_data_a2 = c->getUserData(geom_mls);

        //BOOST_CHECK(user_data_a.use_count() == 3);
        BOOST_CHECK(mls == user_data_a2);

        // create second geom
        dxSphere* geom_sphere = new dxSphere(0, 0.05);
		float ini_z = 1.0f;
        int maxNumContacts = 5;
		dContact contact[maxNumContacts];   // the number of maximum contacts per a collision

        for(int k=0;k<100;k++)
        {
			dGeomSetPosition (geom_sphere,0.0,0.0,ini_z-0.01*(float)k);
			const dReal *pos2 = dGeomGetPosition(geom_sphere);
			BOOST_TEST_MESSAGE("pos of geom_sphere: "<< pos2[2]);

			if(int numc = dCollide(geom_mls, geom_sphere, maxNumContacts,  &contact[0].geom, sizeof(dContact)))
			{
              for (int i=0; i<numc; i++) {
			   BOOST_TEST_MESSAGE("collision x: "<< contact[i].geom.pos[0]<<", y: "<<contact[i].geom.pos[1]<<", z: "<<contact[i].geom.pos[2]);
			  }
			}

		}
        dGeomDestroy(geom_mls);
        dGeomDestroy(geom_sphere);
    }
    dCloseODE();

}


#include <boost/program_options.hpp>
#include <cstdint>
#include <iostream>
#include <queue>
#include <random>
#include <string>
#include <vector>

#include "config.h"
#include "midgard/logging.h"
#include "midgard/util.h"
#include "sif/edgelabel.h"

#include "baldr/double_bucket_queue.h"

using namespace valhalla::midgard;
using namespace valhalla::baldr;
using namespace valhalla::sif;

namespace bpo = boost::program_options;

/**
 * Benchmark of adjacency list. Constructs a large number of random numbers,
 * adds EdgeLabels to the AdjacencyList with those as the sortcost. Then
 * removes them from the list. This compares performance of an STL
 * priority_queue with the custom approximate double bucket sorting used
 * in adjacencylist.cc.
 */
int Benchmark(const uint32_t n, const float maxcost, const float bucketsize) {
  // Create a set of random costs
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<> dis(0, 1);
  std::vector<uint32_t> costs(n);
  for (uint32_t i = 0; i < n; i++) {
    costs[i] = static_cast<uint32_t>(dis(gen) * maxcost);
  }

  // Test performance of STL priority queue. Construct labels
  // and add to the priority queue
  std::clock_t start = std::clock();
  std::priority_queue<EdgeLabel> pqueue;
  for (uint32_t i = 0; i < n; i++) {
    EdgeLabel el;
    el.SetSortCost(costs[i]);
    pqueue.push(std::move(el));
  }

  uint32_t count = 0;
  std::vector<uint32_t> ordered_cost1;
  while (!pqueue.empty()) {
    EdgeLabel el = pqueue.top();
    ordered_cost1.push_back(el.sortcost());
    pqueue.pop();
    count++;
  }
  uint32_t ms = (std::clock() - start) / static_cast<double>(CLOCKS_PER_SEC / 1000);
  LOG_INFO("Priority Queue: Added and removed " + std::to_string(count) + " edgelabels in " +
           std::to_string(ms) + " ms");

  // Didn't alter the sort order of priority queue, so reverse the vector
  std::reverse(ordered_cost1.begin(), ordered_cost1.end());

  // Test performance of double bucket adjacency list. Set the bucket maxcost
  // such that EmptyOverflow is called once
  std::vector<EdgeLabel> edgelabels;
  // Set up lambda to get sort costs
  const auto edgecost = [&edgelabels](const uint32_t label) { return edgelabels[label].sortcost(); };
  start = std::clock();
  DoubleBucketQueue adjlist(0, maxcost / 2, bucketsize, edgecost);

  // Construct EdgeLabels and add to adjacency list

  for (uint32_t i = 0; i < n; i++) {
    EdgeLabel el;
    el.SetSortCost(costs[i]);
    edgelabels.push_back(std::move(el));
    adjlist.add(i);
  }

  // Get edge label indexes from the adj list. Accumulate total cost to make
  // sure compiler doesn't optimize too much.
  count = 0;
  std::vector<uint32_t> ordered_cost2;
  while (true) {
    uint32_t idx = adjlist.pop();
    if (idx == kInvalidLabel) {
      break;
    }

    // Copy the edge label - simulates what is done in PathAlgorithm
    EdgeLabel el = edgelabels[idx];
    ordered_cost2.push_back(el.sortcost());
    count++;
  }
  ms = (std::clock() - start) / static_cast<double>(CLOCKS_PER_SEC / 1000);
  LOG_INFO("Bucketed Adj. List: Added and removed " + std::to_string(count) + " edgelabels in " +
           std::to_string(ms) + " ms");

  // Verify order
  for (uint32_t i = 0; i < count; i++) {
    if (ordered_cost1[i] != ordered_cost2[i]) {
      LOG_INFO("Costs: " + std::to_string(ordered_cost1[i]) + "," + std::to_string(ordered_cost2[i]));
    }
  }
  return 0;
}

int main(int argc, char* argv[]) {

  bpo::options_description options(
      "valhalla " VALHALLA_VERSION "\n"
      "\n"
      " Usage: adjlistbenchmark [options]\n"
      "\n"
      "adjlistbenchmark is benchmark comparing performance of an STL priority_queue"
      "to the approximate double bucket adjacency list class supplied with Valhalla."
      "\n"
      "\n");

  options.add_options()("help,h", "Print this help message.")("version,v",
                                                              "Print the version of this software.");

  bpo::variables_map vm;
  try {
    bpo::store(bpo::command_line_parser(argc, argv).options(options).run(), vm);
    bpo::notify(vm);

  } catch (std::exception& e) {
    std::cerr << "Unable to parse command line options because: " << e.what() << "\n"
              << "This is a bug, please report it at " PACKAGE_BUGREPORT << "\n";
    return EXIT_FAILURE;
  }

  if (vm.count("help")) {
    std::cout << options << "\n";
    return EXIT_SUCCESS;
  }

  if (vm.count("version")) {
    std::cout << "AdjacencyListBenchmark " << VALHALLA_VERSION << "\n";
    return EXIT_SUCCESS;
  }

  // Benchmark with count, maxcost, and bucketsize
  Benchmark(1000000, 50000, 1);
  LOG_INFO("Done Benchmark!");

  return EXIT_SUCCESS;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <iostream>
#include <memory>
#include <random>

#include <BayesFilters/BootstrapCorrection.h>
#include <BayesFilters/DrawParticles.h>
#include <BayesFilters/GaussianLikelihood.h>
#include <BayesFilters/InitSurveillanceAreaGrid.h>
#include <BayesFilters/SimulatedLinearSensor.h>
#include <BayesFilters/LikelihoodModel.h>
#include <BayesFilters/MeasurementModelDecorator.h>
#include <BayesFilters/ParticleSet.h>
#include <BayesFilters/ParticleSetInitialization.h>
#include <BayesFilters/PFCorrectionDecorator.h>
#include <BayesFilters/PFPredictionDecorator.h>
#include <BayesFilters/SimulatedStateModel.h>
#include <BayesFilters/Resampling.h>
#include <BayesFilters/StateModelDecorator.h>
#include <BayesFilters/SIS.h>
#include <BayesFilters/WhiteNoiseAcceleration.h>
#include <BayesFilters/utils.h>
#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

class DecoratedWNA : public StateModelDecorator
{
public:
    DecoratedWNA(std::unique_ptr<StateModel> state_model) noexcept :
        StateModelDecorator(std::move(state_model))
    { };

    void motion(const Eigen::Ref<const Eigen::MatrixXd>& cur_states, Eigen::Ref<Eigen::MatrixXd> mot_states) override
    {
        std::cout << "Decorator: DecoratedWNA::motion()." << std::endl;

        StateModelDecorator::motion(cur_states, mot_states);
    }
};

class DecoratedLinearSensor : public MeasurementModelDecorator
{
public:
    DecoratedLinearSensor(std::unique_ptr<MeasurementModel> observation_model) noexcept :
        MeasurementModelDecorator(std::move(observation_model))
    { }

    std::pair<bool, bfl::Data> measure() const override
    {
        std::cout << "Decorator: DecoratedLinearSensor::measure()." << std::endl;

        return MeasurementModelDecorator::measure();
    }
};

class DecoratedDrawParticles : public PFPredictionDecorator
{
public:
    DecoratedDrawParticles(std::unique_ptr<PFPrediction> prediction) noexcept :
        PFPredictionDecorator(std::move(prediction))
    { }

protected:
    void predictStep(const ParticleSet& prev_particles, ParticleSet& pred_particles) override
    {
        std::cout << "Decorator: DecoratedDrawParticles::predictStep()." << std::endl;

        PFPredictionDecorator::predictStep(prev_particles, pred_particles);
    }
};

class DecoratedBootstrapCorrection : public PFCorrectionDecorator
{
public:
    DecoratedBootstrapCorrection(std::unique_ptr<PFCorrection> correction) noexcept :
        PFCorrectionDecorator(std::move(correction))
    { }

protected:
    void correctStep(const ParticleSet& pred_particles, ParticleSet& cor_particles) override
    {
        std::cout << "Decorator: DecoratedBootstrapCorrection::correctStep()." << std::endl;

        PFCorrectionDecorator::correctStep(pred_particles, cor_particles);
    }
};

class SISSimulation : public SIS
{
public:
    SISSimulation
    (
        unsigned int num_particle,
        std::size_t state_size,
        unsigned int simulation_steps,
        std::unique_ptr<ParticleSetInitialization> initialization,
        std::unique_ptr<PFPrediction> prediction,
        std::unique_ptr<PFCorrection> correction,
        std::unique_ptr<Resampling> resampling
    ) noexcept :
        SIS(num_particle, state_size, std::move(initialization), std::move(prediction), std::move(correction), std::move(resampling)),
        simulation_steps_(simulation_steps)
    { }

protected:
    bool runCondition()
    {
        if (getFilteringStep() < simulation_steps_)
            return true;
        else
            return false;
    }

private:
    unsigned int simulation_steps_;
};

int main()
{
    /* A set of parameters needed to run a SIS particle filter in a simulated environment. */
    double surv_x = 1000.0;
    double surv_y = 1000.0;
    unsigned int num_particle_x = 100;
    unsigned int num_particle_y = 100;
    unsigned int num_particle = num_particle_x * num_particle_y;
    Vector4d initial_state(10.0f, 0.0f, 10.0f, 0.0f);
    unsigned int simulation_time = 10;
    std::size_t state_size = 4;

    /* Step 1 - Initialization */
    /* Initialize initialization class. */
    std::unique_ptr<ParticleSetInitialization> grid_initialization = utils::make_unique<InitSurveillanceAreaGrid>(surv_x, surv_y, num_particle_x, num_particle_y);

    /* Step 2 - Prediction */
    /* Step 2.1 - Define the state model */
    /* Initialize a white noise acceleration state model. */
    std::unique_ptr<StateModel> wna = utils::make_unique<WhiteNoiseAcceleration>();

    /* Step 2.1.1 - Define a decoration for the state model */
    /* Initialize a white noise acceleration decorator. */
    std::unique_ptr<StateModel> decorated_wna = utils::make_unique<DecoratedWNA>(std::move(wna));

    /* Step 2.2 - Define the prediction step */
    /* Initialize the particle filter prediction step and pass the ownership of the state model. */
    std::unique_ptr<PFPrediction> pf_prediction = utils::make_unique<DrawParticles>();
    pf_prediction->setStateModel(std::move(decorated_wna));

    /* Step 2.2.1 - Define a decoration for the prediction step */
    /* Initialize a particle filter prediction decorator. */
    std::unique_ptr<PFPrediction> decorated_prediction = utils::make_unique<DecoratedDrawParticles>(std::move(pf_prediction));

    /* Step 3 - Correction */
    /* Step 3.1 - Define where the measurement are originated from (either simulated or from a real process) */
    /* Initialize simulaterd target model, a white noise acceleration, and measurements, a MeasurementModel decoration for the linear sensor. */
    std::unique_ptr<StateModel> target_model = utils::make_unique<WhiteNoiseAcceleration>();
    std::unique_ptr<SimulatedStateModel> simulated_state_model = utils::make_unique<SimulatedStateModel>(std::move(target_model), initial_state, simulation_time);

    /* Initialize a measurement model (a linear sensor reading x and y coordinates). */
    std::unique_ptr<MeasurementModel> simulated_linear_sensor = utils::make_unique<SimulatedLinearSensor>(std::move(simulated_state_model));

    /* Step 3.1.1 - Define a decoration for the measurement model */
    /* Initialize a white noise acceleration decorator */
    std::unique_ptr<MeasurementModel> decorated_linearsensor = utils::make_unique<DecoratedLinearSensor>(std::move(simulated_linear_sensor));

    /* Step 3.2 - Define the likelihood model */
    /* Initialize the the exponential likelihood, a PFCorrection decoration of the particle filter correction step. */
    std::unique_ptr<LikelihoodModel> exp_likelihood = utils::make_unique<GaussianLikelihood>();

    /* Step 3.3 - Define the correction step */
    /* Initialize the particle filter correction step and pass the ownership of the measurement model. */
    std::unique_ptr<PFCorrection> pf_correction = utils::make_unique<BoostrapCorrection>();
    pf_correction->setLikelihoodModel(std::move(exp_likelihood));
    pf_correction->setMeasurementModel(std::move(decorated_linearsensor));

    /* Initialize a update particle decorator */
    std::unique_ptr<PFCorrection> decorated_correction = utils::make_unique<DecoratedBootstrapCorrection>(std::move(pf_correction));

    /* Step 4 - Resampling */
    /* Initialize a resampling algorithm */
    std::unique_ptr<Resampling> resampling = utils::make_unique<Resampling>();

    /* Step 5 - Assemble the particle filter */
    std::cout << "Constructing SIS particle filter..." << std::flush;
    SISSimulation sis_pf(num_particle, state_size, simulation_time, std::move(grid_initialization), std::move(decorated_prediction), std::move(decorated_correction), std::move(resampling));
    std::cout << "done!" << std::endl;

    /* Step 6 - Prepare the filter to be run */
    std::cout << "Booting SIS particle filter..." << std::flush;
    sis_pf.boot();
    std::cout << "completed!" << std::endl;

    /* Step 7 - Run the filter and wait until it is closed */
    /* Note that since this is a simulation, the filter will end upon simulation termination */
    std::cout << "Running SIS particle filter..." << std::flush;
    sis_pf.run();
    std::cout << "waiting..." << std::endl;
    if (!sis_pf.wait())
        return EXIT_FAILURE;
    std::cout << "completed!" << std::endl;

    return EXIT_SUCCESS;
}


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

#include <boost/simd/swar/functions/interleave_second.hpp>
#include <boost/simd/include/functions/bitwise_cast.hpp>
#include <boost/simd/swar/functions/details/shuffle.hpp>
#include <boost/simd/swar/functions/details/perm.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_second_
                                   , boost::simd::tag::avx_
                                   , (A0)(A1)
                                   , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<single_<A1>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m256 const a0, __m256 const a1) const
    {
      result_type that0 = details::perm2<1, 1>(a0, a0);
      that0 = details::shuffle_pairs<0, 1, 1, 3>(that0, that0);
      result_type that1 = details::perm2<1, 1>(a1, a1);
      that1 = details::shuffle_pairs<0, 1, 1, 3>(that1, that1);
      return _mm256_unpacklo_ps(that0,that1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_second_
                                   , boost::simd::tag::avx_
                                   , (A0)(A1)
                                   , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<double_<A1>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m256d const a0, __m256d const a1) const
    {
      result_type that0 = details::perm2<1, 1>(a0, a0);
      that0 = details::shuffle<0, 1, 1, 3>(that0, that0);
      result_type that1 = details::perm2<1, 1>(a1, a1);
      that1 = details::shuffle<0, 1, 1, 3>(that1, that1);
      return _mm256_unpacklo_pd(that0,that1);

    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_second_
                                   , boost::simd::tag::avx_
                                   , (A0)(A1)
                                   , ((simd_<type32_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<type32_<A1>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(__m256i const a0, __m256i const a1) const
    {
      typedef typename boost::dispatch::meta::as_floating<A0>::type  ftype;
      return  bitwise_cast<result_type>(interleave_second(bitwise_cast<ftype>(a0), bitwise_cast<ftype>(a1)));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::interleave_second_
                                   , boost::simd::tag::avx_
                                   , (A0)(A1)
                                   , ((simd_<type64_<A0>,boost::simd::tag::avx_>))
                                     ((simd_<type64_<A1>,boost::simd::tag::avx_>))
                                   )
  {
    typedef A0 result_type;

    result_type operator()(A0 const& a0, A1 const& a1) const
    {
      typedef typename boost::dispatch::meta::as_floating<A0>::type  ftype;
      return bitwise_cast<result_type>(interleave_second(bitwise_cast<ftype>(a0), bitwise_cast<ftype>(a1)));
    }
  };

} } }

#endif
#endif


/*
    BSD 3-Clause License
    Copyright (c) 2018, Roboy
            All rights reserved.
    Redistribution and use in source and binary forms, with or without
            modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.
    * Neither the name of the copyright holder nor the names of its
    contributors may be used to endorse or promote products derived from
    this software without specific prior written permission.
    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
            IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
    author: Simon Trendel ( st@gi.ai ), 2019
    description: The Robot class implementing the Cable model of vrpuppets
*/

#pragma once

#include <ros/ros.h>
#include <Eigen/Core>
#include <Eigen/Dense>

#include <iDynTree/Model/FreeFloatingState.h>
#include <iDynTree/KinDynComputations.h>
#include <iDynTree/ModelIO/ModelLoader.h>
#include <iDynTree/Core/EigenHelpers.h>
#include <iDynTree/InverseKinematics.h>

#include "tinyxml.h"

#include "kindyn/cable.hpp"
#include "kindyn/EigenExtension.hpp"
#include "kindyn/controller/cardsflow_state_interface.hpp"
#include "kindyn/controller/cardsflow_command_interface.hpp"

#include <actionlib/server/simple_action_server.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/JointState.h>
#include <roboy_simulation_msgs/Tendon.h>
#include <roboy_simulation_msgs/ControllerType.h>
#include <roboy_simulation_msgs/JointState.h>
#include <roboy_middleware_msgs/ForwardKinematics.h>
#include <roboy_middleware_msgs/InverseKinematics.h>
#include <roboy_middleware_msgs/InverseKinematicsMultipleFrames.h>
#include <roboy_middleware_msgs/MotorCommand.h>
#include <roboy_middleware_msgs/MotorStatus.h>
#include <roboy_control_msgs/MoveEndEffectorAction.h>
#include <roboy_control_msgs/GetLinkPose.h>

#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_msg.h>

#include <qpOASES.hpp>

#include <controller_manager/controller_manager.h>
#include <controller_manager_msgs/LoadController.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>

#include <boost/numeric/odeint.hpp>

#include <common_utilities/rviz_visualization.hpp>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <thread>

using namespace qpOASES;
using namespace std;
using namespace Eigen;
using iDynTree::toEigen;
using iDynTree::fromEigen;

using namespace cardsflow::kindyn;

namespace cardsflow {
    namespace vrpuppet {
        class Robot:public rviz_visualization{
        public:
            /**
             * Constructor
             */
            Robot();
            /**
             * Destructor
             */
            ~Robot();

            /**
             * initializes everything, call before use!
             * @param urdf_file_path path to robot urdf
             * @param viapoints_file_path path to viapoints xml
             * @param joint_names a vector of joint_names to be considered from the model
             */
            void init(string urdf_file_path, string viapoints_file_path, vector<string> joint_names);
            /**
             * Updates the model
             */
            void update();

            /**
             * This is the read function and should implement reading the state of your robot
             */
            virtual void read(){
                ROS_WARN_STREAM_THROTTLE(1, "reading virtual, "
                                            "you probably forgot to implement your own read function?!");
            };
            /**
             * This is the write function and should implement writing commands to your robot
             */
            virtual void write(){
                ROS_WARN_STREAM_THROTTLE(1, "writing virtual, "
                                            "you probably forgot to implement your own write function?!");
            };
        private:

            /**
             * Move endeffector action server
             * @param goal
             */
            void MoveEndEffector(const roboy_control_msgs::MoveEndEffectorGoalConstPtr &goal);
            /**
             * parses the cardsflow xml file for viapoint definitions
             * @param viapoints_file_path path to the cardsflow.xml file
             * @param cables will be filled with the parsed viapoints of the defined cables
             * @return success
             */
            bool parseViapoints(const string &viapoints_file_path, vector<Cable> &cables);
            /**
             * Forward kinematic service for endeffectors
             * @param req endeffector, requested joint angles
             * @param res 3d resulting position of endeffector
             * @return success
             */
            bool ForwardKinematicsService(roboy_middleware_msgs::ForwardKinematics::Request &req,
                                          roboy_middleware_msgs::ForwardKinematics::Response &res);
            /**
             * Inverse kinematic service for endeffectors
             * @param req endeffector and ik type
             * @param res joint configuration solution
             * @return success
             */
            bool InverseKinematicsService(roboy_middleware_msgs::InverseKinematics::Request &req,
                                          roboy_middleware_msgs::InverseKinematics::Response &res);

            bool ExecuteIK(roboy_middleware_msgs::InverseKinematics::Request &req,
                                          roboy_middleware_msgs::InverseKinematics::Response &res);

            bool InverseKinematicsMultipleFramesService(roboy_middleware_msgs::InverseKinematicsMultipleFrames::Request &req,
                                                        roboy_middleware_msgs::InverseKinematicsMultipleFrames::Response &res);

            bool GetLinkPoseService(roboy_control_msgs::GetLinkPose::Request &req,
                                    roboy_control_msgs::GetLinkPose::Response &res);

            /**
             * Callback for Interactive Marker Feedback of endeffectors. When the Interactive Marker is released in rviz,
             * the IK routine is called and the solution directly applied to the robot q_target angles
             * @param msg Interactive Marker Feedback message
             */
            void InteractiveMarkerFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &msg);

            /**
             * Callback for the joint state of the robot. This can come from gazebo, the real robot or else where.
             * @param msg message containing joint_name/angle information
             */
            void JointState(const sensor_msgs::JointStateConstPtr &msg);

            /**
             * Callback for the joint target of the robot.
             * @param msg message containing joint_name/angle information
             */
            void JointTarget(const sensor_msgs::JointStateConstPtr &msg);

            /**
             * Callback for the floating base world pose. This can come from gazebo, the real robot or else where.
             * @param msg message containing the 6 DoF pose of the floating base
             */
            void FloatingBase(const geometry_msgs::PoseConstPtr &msg);

            Matrix3d *link_to_link_transform;

            VectorXd resolve_function(MatrixXd &A_eq, VectorXd &b_eq, VectorXd &f_min, VectorXd &f_max);

            ros::NodeHandlePtr nh; /// ROS node handle
            boost::shared_ptr <ros::AsyncSpinner> spinner; /// async ROS spinner
            ros::Publisher robot_state_pub, tendon_state_pub, joint_state_pub, cardsflow_joint_states_pub; /// ROS robot pose and tendon publisher
            ros::Publisher robot_state_target_pub, tendon_state_target_pub, joint_state_target_pub; /// target publisher
            ros::Subscriber controller_type_sub, joint_state_sub, joint_target_sub, floating_base_sub, interactive_marker_sub; /// ROS subscribers
            ros::ServiceServer ik_srv, ik_two_frames_srv, fk_srv, execute_ik_srv, link_pose_srv;
            map<string,boost::shared_ptr<actionlib::SimpleActionServer<roboy_control_msgs::MoveEndEffectorAction>>> moveEndEffector_as;

            map<string,iDynTree::KinDynComputations> ik_models; /// the robot models for each endeffector
            map<string,iDynTree::InverseKinematics> ik; /// the ik for each endeffector
            map<string,string> ik_base_link; /// the base link of each endeffector
            struct iDynTreeRobotState
            {
                void resize(int nrOfInternalDOFs)
                {
                    jointPos.resize(nrOfInternalDOFs);
                    jointVel.resize(nrOfInternalDOFs);
                }

                iDynTree::Transform world_H_base;
                iDynTree::VectorDynSize jointPos;
                iDynTree::Twist         baseVel;
                iDynTree::VectorDynSize jointVel;
                iDynTree::Vector3       gravity;
            }robotstate;
        public:
            iDynTree::KinDynComputations kinDynComp, kinDynCompTarget; /// the full robot model
            size_t number_of_dofs = 0; /// number of degrees of freedom of the whole robot
            vector<string> endeffectors; /// names of the endeffectors
            map<string,size_t> endeffector_index;
            vector<size_t> endeffector_number_of_dofs, endeffector_dof_offset; /// number of degrees of freedom of each endeffector
            size_t number_of_joints = 0; /// number of joints of the whole robot
            size_t number_of_cables = 0; /// number of cables, ie muscles of the whole robot
            size_t number_of_links = 0; /// number of links of the whole robot
            Matrix4d world_H_base; /// floating base 6-DoF pose
            vector<Matrix4d> world_to_link_transform, link_to_world_transform, frame_transform;
            Eigen::Matrix<double,6,1> baseVel; /// the velocity of the floating base
            Vector3d gravity; /// gravity vector (default: (0,0,-9.81)
            MatrixXd M; /// The Mass matrix of the robot
            VectorXd CG; /// The Coriolis+Gravity term of the robot
            VectorXd q, qd, qdd; /// joint positon, velocity, acceleration
            VectorXd q_min, q_max; /// joint limits
            VectorXd q_target, qd_target, qdd_target; /// joint positon, velocity, acceleration targets
            VectorXd q_target_prev, qd_target_prev, qdd_target_prev; /// joint positon, velocity, acceleration targets
            VectorXd l_int, l, l_target; /// tendon length and length change
            vector<VectorXd> Ld; // tendon velocity per endeffector
            VectorXd torques; /// joint torques
            VectorXd cable_forces; /// the cable forces in Newton
            vector<VectorXd> ld; /// tendon length changes for each controller
            MatrixXd L, L_t; /// L and -L^T

            MatrixXd S, P, V, W; /// matrices of cable model
            vector <vector<pair < ViaPointPtr, ViaPointPtr>>> segments; /// cable segments
            bool external_robot_state; /// indicates if we get the robot state externally
            bool simulated = false; /// indicates if the robots is simulated or hardware is used
            bool initialized = false;
        protected:
            iDynTree::FreeFloatingGeneralizedTorques bias; /// Coriolis+Gravity term
            iDynTree::MatrixDynSize Mass; /// Mass matrix

            bool torque_position_controller_active = false, force_position_controller_active = false, cable_length_controller_active = false;
            VectorXd qdd_torque_control, qdd_force_control;

            vector<Cable> cables; /// all cables of the robot
            vector <VectorXd> joint_axis; /// joint axis of each joint
            vector <string> link_names, joint_names; /// link and joint names of the robot
            map<string, int> link_index, joint_index; /// link and joint indices of the robot
            vector<ros::Publisher> joint_command_pub;
            vector<int> controller_type; /// currently active controller type
            double integration_time =0; /// odeint integration time
            typedef boost::array< double , 2 > state_type; /// second order dynamics integration
            vector<state_type> joint_state, motor_state; /// joint and cable states
            bool first_time_solving = true;
            int nWSR = 1000; /// qp working sets
            VectorXd f_min, f_max;
            int qp_print_level = PL_NONE; /// qpoases print level
            SQProblem qp_solver; /// qpoases quadratic problem solver
            real_t *H, *g, *A, *lb, *ub, *b, *FOpt; /// quadratic problem variables
            ros::Time last_visualization; /// timestamp for visualization at reasonable intervals
            Eigen::IOFormat fmt; /// formator for terminal printouts
            hardware_interface::JointStateInterface joint_state_interface; /// ros control joint state interface
            hardware_interface::EffortJointInterface joint_command_interface; /// ros control joint command interface
            hardware_interface::CardsflowStateInterface cardsflow_state_interface; /// cardsflow state interface
            hardware_interface::CardsflowCommandInterface cardsflow_command_interface; /// cardsflow command interface
            bool first_update = true;
        };
    }
}

typedef boost::shared_ptr<cardsflow::vrpuppet::Robot> RobotPtr; /// typedef for convenience


#include "alchemist.h"
#include "data_stream.h"
#include <iostream>
#include <fstream>
#include <cstdlib>
#include <sys/stat.h>
#include <map>
#include <random>
#include <sstream>
#include <boost/asio.hpp>
#include <boost/chrono.hpp>
#include <boost/thread/thread.hpp>
#include <boost/tokenizer.hpp>
#include "arpackpp/arrssym.h"
#include "spdlog/spdlog.h"
#include <time.h>

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace alchemist {

using namespace El;

struct Driver {
  mpi::communicator world;
  DataInputStream input;
  DataOutputStream output;
  std::vector<WorkerInfo> workers;
  std::map<MatrixHandle, MatrixDescriptor> matrices;
  uint32_t nextMatrixId;
  std::shared_ptr<spdlog::logger> log;

  Driver(const mpi::communicator &world, std::istream &is, std::ostream &os, std::shared_ptr<spdlog::logger> log);
  void issue(const Command &cmd);
  MatrixHandle registerMatrix(size_t numRows, size_t numCols);
  void reshapeMatrix(MatrixHandle handle, size_t numRows, size_t numCols);
  int main();

  void handle_newMatrix();
  void handle_matrixMul();
  void handle_matrixDims();
  void handle_computeThinSVD();
  void handle_getMatrixRows();
  void handle_getTranspose();
  void handle_kmeansClustering();
  void handle_truncatedSVD();
  void handle_SkylarkKRR();
  void handle_SkylarkLSQR();
  void handle_FactorizedCGSolver();
  void handle_RandomFourierFeatures();
  void handle_ReadHDF5();
  void handle_normalizeMatInPlace();
};

Driver::Driver(const mpi::communicator &world, std::istream &is, std::ostream &os, std::shared_ptr<spdlog::logger> log) :
    world(world), input(is), output(os), log(log), nextMatrixId(42) {
}

void Driver::issue(const Command &cmd) {
  const Command *cmdptr = &cmd;
  mpi::broadcast(world, cmdptr, 0);
}

MatrixHandle Driver::registerMatrix(size_t numRows, size_t numCols) {
  MatrixHandle handle{nextMatrixId++};
  MatrixDescriptor info(handle, numRows, numCols);
  matrices.insert(std::make_pair(handle, info));
  return handle;
}

void Driver::reshapeMatrix(MatrixHandle handle, size_t numRows, size_t numCols) {
    matrices.erase(handle);
    MatrixDescriptor info(handle, numRows, numCols);
    matrices.insert(std::make_pair(handle, info));
}

int Driver::main() {

  // get WorkerInfo
  auto numWorkers = world.size() - 1;
  workers.resize(numWorkers);
  for(auto id = 0; id < numWorkers; ++id) {
    world.recv(id + 1, 0, workers[id]);
  }
  log->info("{} workers ready, sending hostnames and ports to Spark", numWorkers);

  // handshake
  ENSURE(input.readInt() == 0xABCD);
  ENSURE(input.readInt() == 0x1);
  output.writeInt(0xDCBA);
  output.writeInt(0x1);
  output.writeInt(numWorkers);
  for(auto id = 0; id < numWorkers; ++id) {
    output.writeString(workers[id].hostname);
    output.writeInt(workers[id].port);
  }
  output.flush();

  bool shouldExit = false;
  while(!shouldExit) {
    uint32_t typeCode = input.readInt();
    log->info("Received code {:#x}", typeCode);

    switch(typeCode) {
      // shutdown
      case 0xFFFFFFFF:
        shouldExit = true;
        issue(HaltCommand());
        output.writeInt(0x1);
        output.flush();
        break;

      // new matrix
      case 0x1:
        handle_newMatrix();
        break;

      // matrix multiplication
      case 0x2:
        handle_matrixMul();
        break;

      // get matrix dimensions
      case 0x3:
        handle_matrixDims();
        break;

      // return matrix to Spark
      case 0x4:
        handle_getMatrixRows();
        break;

      case 0x5:
        handle_computeThinSVD();
        break;

      case 0x6:
        handle_getTranspose();
        break;

      case 0x7:
        handle_kmeansClustering();
        break;

      case 0x8:
        handle_truncatedSVD();
        break;

      case 0x9:
        handle_SkylarkKRR();
        break;

      case 0x10:
        handle_SkylarkLSQR();
        break;

      case 0x11:
        handle_FactorizedCGSolver();
        break;

      case 0x12:
        handle_RandomFourierFeatures();
        break;

      case 0x13:
        handle_ReadHDF5();
        break;

      case 0x14:
        handle_normalizeMatInPlace();
        break;

      default:
        log->error("Unknown typeCode {#x}", typeCode);
        abort();
    }
    log->info("Waiting on next command");
  }

  // wait for workers to reach exit
  world.barrier();
  return EXIT_SUCCESS;
}

// A = USV
/*
void Driver::handle_computeLowrankSVD() {
  uint32_t inputMat = input.readInt();
  uint32_t kRank = input.readInt();
  uint32_t whichFactors = input.readInt(); // 0 = U, 1 = U and S, 2 = V and S, 3/default = U, S, and V

  MatrixHandle U;
  MatrixHandle S;
  MatrixHandle V;

  // TODO: register the U, S, V factors with false newmatrixcommands to track them
  switch(whichFactors) {
    case 0: U = MatrixHandle{nextMatrixId++};
            break;
    case 1: U = MatrixHandle{nextMatrixId++};
            S = MatrixHandle{nextMatrixId++};
            break;
    case 2: V = MatrixHandle{nextMatrixId++};
            S = MatrixHandle{nextMatrixId++};
            break;
    default: U = MatrixHandle{nextMatrixId++};
             S = MatrixHandle{nextMatrixId++};
             V = MatrixHandle{nextMatrixId++};
             break;
  }

  LowrankSVDCommand cmd(MatrixHandle{inputMat}, whichFactors, krank, U, S, V);
  issue(cmd);

  output.writeInt(0x1); // statusCode
  switch(whichFactors) {
    case 0: output.writeInt(U.id);
            break;
    case 1: output.writeInt(U.id);
            output.writeInt(S.id);
            break;
    case 2: output.writeInt(V.id);
            output.writeInt(S.id);
            break;
    default: output.writeInt(U.id);
             output.writeInt(S.id);
             output.writeInt(V.id);
             break;
  }
  output.flush();

  // wait for command to finish
  world.barrier();
  output.writeInt(0x1);
  output.flush();
}
*/

void Driver::handle_RandomFourierFeatures() {
    MatrixHandle A{input.readInt()};
    uint32_t numRandFeatures = input.readInt();
    double sigma = input.readDouble();
    uint32_t seed = input.readInt();

    auto numRows = matrices[A].numRows;
    uint32_t d = matrices[A].numCols;
    MatrixHandle X = registerMatrix(numRows, numRandFeatures);

    log->info("Computing Fourier Random Features on feature matrix {}");
    log->info("Input dim: {}, Output dim: {}, number of points: {}", d, numRandFeatures, numRows);

    RandomFourierFeaturesCommand cmd(A, X, numRandFeatures, sigma, seed);
    issue(cmd);

    world.barrier(); // wait for it to finish
    output.writeInt(0x1);
    output.writeInt(X.id);
    output.flush();
    log->info("Finished computing Fourier Random Features, stored matrix as {}", X);
}

void Driver::handle_normalizeMatInPlace() {
    MatrixHandle A{input.readInt()};
    log->info("Normalizing matrix {} in place by zero meaning the rows and setting the columns to have stdev 1", A);

    NormalizeMatInPlaceCommand cmd(A);
    issue(cmd);

    output.writeInt(0x1);
    log->info("Finished normalizing matrix {} in place", A);
}

void Driver::handle_ReadHDF5() {
    std::string fname = input.readString();
    std::string varname = input.readString();
    int colreplicas = input.readInt();
    MatrixHandle A = registerMatrix(0, 0);

    log->info("Reading variable {} from file {} and replicating columnwise {} times", varname, fname, colreplicas);
    ReadHDF5Command cmd(A, fname, varname, colreplicas);
    issue(cmd);

    El::Int numRows, numCols;
    world.recv(1, mpi::any_tag, numRows);
    world.recv(1, mpi::any_tag, numCols);
    world.barrier();

    reshapeMatrix(A, numRows, numCols);
    log->info("Loaded matrix is {}-by-{}", matrices[A].numRows, matrices[A].numCols);
    output.writeInt(0x1);
    output.writeInt(A.id);
    output.flush();
    log->info("Finished loading variable from HDF5 file");
}

void Driver::handle_FactorizedCGSolver() {
  MatrixHandle featureMat{input.readInt()};
  MatrixHandle targetMat{input.readInt()};
  double lambda = input.readDouble();
  uint32_t maxIters = input.readInt();

  auto numfeatures = matrices[featureMat].numCols;
  auto numtargets = matrices[targetMat].numCols;
  MatrixHandle coefs = registerMatrix(numfeatures, numtargets);

  log->info("Starting factorized CG solver on feature matrix {} and target matrix {}", featureMat, targetMat);
  log->info("lambda={}, maxIters={}", lambda, maxIters);

  FactorizedCGSolverCommand cmd(featureMat, targetMat, coefs, lambda, maxIters);
  issue(cmd);

  world.barrier(); // wait for it to finish
  output.writeInt(0x1);
  output.writeInt(coefs.id);
  output.flush();
  log->info("Finished CG solve, stored coefficient matrix as {}", coefs);
}

void Driver::handle_SkylarkKRR() {
    MatrixHandle featureMat{input.readInt()};
    MatrixHandle targetMat{input.readInt()};
    bool regression = input.readInt() > 0? true : false;
    uint32_t lossfunction = input.readInt();
    uint32_t regularizer = input.readInt();
    uint32_t kernel = input.readInt();
    double kernelparam = input.readDouble();
    double kernelparam2 = input.readDouble();
    double kernelparam3 = input.readDouble();
    double lambda = input.readDouble();
    uint32_t maxiter = input.readInt();
    double tolerance = input.readDouble();
    double rho = input.readDouble();
    uint32_t seed = input.readInt();
    uint32_t randomfeatures = input.readInt();
    uint32_t numfeaturepartitions = input.readInt();

    log->info("Starting Skylark's ADMM KRR solver on feature matrix {} and target matrix {}", featureMat, targetMat);
    log->info("<should list arguments here>");

    auto numfeatures = matrices[featureMat].numCols;
    auto numtargets = matrices[targetMat].numCols;
    MatrixHandle coefs = registerMatrix(numfeatures, numtargets);

    SkylarkKernelSolverCommand cmd(featureMat, targetMat, coefs, regression,
        lossfunction, regularizer, kernel, kernelparam, kernelparam2,
        kernelparam3, lambda, maxiter, tolerance, rho, seed, randomfeatures,
        numfeaturepartitions);
    issue(cmd);

    world.barrier(); // wait for it to finish
    log->info("Finished calling Skylark's ADMM Kernel solver for this KRR problem");
    output.writeInt(0x1);
    output.writeInt(coefs.id);
    output.flush();
    log->info("Finished KRR solve, stored coeffcient matrix as {}", coefs);
}

void Driver::handle_SkylarkLSQR() {
  MatrixHandle A{input.readInt()};
  MatrixHandle B{input.readInt()};
  double tolerance = input.readDouble();
  uint32_t iter_lim = input.readInt();

  auto p = matrices[A].numCols;
  auto m = matrices[B].numCols;
  MatrixHandle X = registerMatrix(p, m);

  log->info("Starting Skylark's LSQR solver on feature matrix {} and target matrix {}", A, B);
  log->info("Tolerance: {}", tolerance);
  log->info("Iteration Limit: {}", iter_lim);
  log->info("Result will be a {}-by-{} matrix", p, m);

  SkylarkLSQRSolverCommand cmd(A, B, X, tolerance, iter_lim);
  issue(cmd);

  world.barrier(); // wait for it to finish
  log->info("Finished call to Skylark's LSQR solver");
  output.writeInt(0x1);
  output.writeInt(X.id);
  output.flush();
  log->info("Finished LSQR computation");
}

// TODO: the cluster centers should be stored locally on driver and reduced/broadcasted. the current
// way of updating kmeans centers is ridiculous
// TODO: currently only implements kmeans||
void Driver::handle_kmeansClustering() {
  MatrixHandle inputMat{input.readInt()};
  uint32_t numCenters = input.readInt();
  uint32_t maxnumIters = input.readInt(); // how many iteration of Lloyd's algorithm to use
  uint32_t initSteps = input.readInt(); // number of initialization steps to use in kmeans||
  double changeThreshold = input.readDouble(); // if all the centers change by Euclidean distance less than changeThreshold, then we stop the iterations
  uint32_t method = input.readInt(); // which initialization method to use to choose initial cluster center guesses
  uint64_t seed = input.readLong(); // randomness seed used in driver and workers

  log->info("Starting K-means on matrix {}", inputMat);
  log->info("numCenters = {}, maxnumIters = {}, initSteps = {}, changeThreshold = {}, method = {}, seed = {}",
      numCenters, maxnumIters, initSteps, changeThreshold, method, seed);

  if (changeThreshold < 0.0 || changeThreshold > 1.0) {
    log->error("Unreasonable change threshold in k-means: {}", changeThreshold);
    abort();
  }
  if (method != 1) {
    log->warn("Sorry, only k-means|| initialization has been implemented, so ignoring your choice of method {}", method);
  }

  auto n = matrices[inputMat].numRows;
  auto d = matrices[inputMat].numCols;
  MatrixHandle centersHandle = this->registerMatrix(numCenters, d);
  MatrixHandle assignmentsHandle = this->registerMatrix(n, 1);
  KMeansCommand cmd(inputMat, numCenters, method, initSteps, changeThreshold, seed, centersHandle, assignmentsHandle);
  issue(cmd); // initial call initializes stuff and waits for next command

  /******** START of kmeans|| initialization ********/
  std::mt19937 gen(seed);
  std::uniform_int_distribution<unsigned long> dis(0, n-1);
  uint32_t rowidx = dis(gen);
  std::vector<double> initialCenter(d);

  mpi::broadcast(world, rowidx, 0); // tell the workers which row to use as initialization in kmeans||
  world.barrier(); // wait for workers to return oversampled cluster centers and sizes

  std::vector<uint32_t> clusterSizes;
  std::vector<MatrixXd> initClusterCenters;
  world.recv(1, mpi::any_tag, clusterSizes);
  world.recv(1, mpi::any_tag, initClusterCenters);
  world.barrier();

  log->info("Retrieved the k-means|| oversized set of potential cluster centers");
  log->debug("{}", initClusterCenters);

  // use kmeans++ locally to find the initial cluster centers
  std::vector<double> weights;
  weights.reserve(clusterSizes.size());
  std::for_each(clusterSizes.begin(), clusterSizes.end(), [&weights](const uint32_t & cnt){ weights.push_back((double) cnt); });
  MatrixXd clusterCenters(numCenters, d);

  kmeansPP(gen(), initClusterCenters, weights, clusterCenters, 30); // same number of maxIters as spark kmeans

  log->info("Ran local k-means on the driver to determine starting cluster centers");
  log->debug("{}", clusterCenters);

  mpi::broadcast(world, clusterCenters.data(), numCenters*d, 0);
  /******** END of kMeans|| initialization ********/

  /******** START of Lloyd's algorithm iterations ********/
  double percentAssignmentsChanged = 1.0;
  bool centersMovedQ = true;
  uint32_t numChanged = 0;
  uint32_t numIters = 0;
  std::vector<uint32_t> parClusterSizes(numCenters);
  std::vector<uint32_t> zerosVector(numCenters);

  for(uint32_t clusterIdx = 0; clusterIdx < numCenters; clusterIdx++)
    zerosVector[clusterIdx] = 0;

  uint32_t command = 1; // do another iteration
  while (centersMovedQ && numIters++ < maxnumIters)  {
    log->info("Starting iteration {} of Lloyd's algorithm, {} percentage changed in last iter",
        numIters, percentAssignmentsChanged*100);
    numChanged = 0;
    for(uint32_t clusterIdx = 0; clusterIdx < numCenters; clusterIdx++)
      parClusterSizes[clusterIdx] = 0;
    command = 1; // do a basic iteration
    mpi::broadcast(world, command, 0);
    mpi::reduce(world, (uint32_t) 0, numChanged, std::plus<int>(), 0);
    mpi::reduce(world, zerosVector.data(), numCenters, parClusterSizes.data(), std::plus<uint32_t>(), 0);
    world.recv(1, mpi::any_tag, centersMovedQ);
    percentAssignmentsChanged = ((double) numChanged)/n;

    for(uint32_t clusterIdx = 0; clusterIdx < numCenters; clusterIdx++) {
      if (parClusterSizes[clusterIdx] == 0) {
        // this is an empty cluster, so randomly pick a point in the dataset
        // as that cluster's centroid
        centersMovedQ = true;
        command = 2; // reinitialize this cluster center
        uint32_t rowIdx = dis(gen);
        mpi::broadcast(world, command, 0);
        mpi::broadcast(world, clusterIdx, 0);
        mpi::broadcast(world, rowIdx, 0);
        world.barrier();
      }
    }

  }
  command = 0xf; // terminate and finalize the k-means centers and assignments as distributed matrices
  mpi::broadcast(world, command, 0);
  double objVal = 0.0;
  mpi::reduce(world, 0.0, objVal, std::plus<double>(), 0);
  world.barrier();

  /******** END of Lloyd's iterations ********/

  log->info("Finished Lloyd's algorithm: took {} iterations, final objective value {}", numIters, objVal);
  output.writeInt(0x1);
  output.writeInt(assignmentsHandle.id);
  output.writeInt(centersHandle.id);
  output.writeInt(numIters);
  output.flush();
}

void Driver::handle_getTranspose() {
  MatrixHandle inputMat{input.readInt()};
  log->info("Constructing the transpose of matrix {}", inputMat);

  auto numRows = matrices[inputMat].numCols;
  auto numCols = matrices[inputMat].numRows;
  MatrixHandle transposeHandle = registerMatrix(numRows, numCols);
  TransposeCommand cmd(inputMat, transposeHandle);
  issue(cmd);

  world.barrier(); // wait for command to finish
  output.writeInt(0x1);
  output.writeInt(transposeHandle.id);
  log->info("Wrote handle for transpose");
  output.writeInt(0x1);
  output.flush();
}

// CAVEAT: Assumes tall-and-skinny for now, doesn't allow many options for controlling
// LIMITATIONS: assumes V small enough to fit on one machine (so can use ARPACK instead of PARPACK), but still distributes U,S,V and does distributed computations not needed
void Driver::handle_truncatedSVD() {
  log->info("Starting truncated SVD computation");
  MatrixHandle inputMat{input.readInt()};
  uint32_t k = input.readInt();
  uint32_t method = input.readInt();

  int LOCALEIGS = 0; // TODO: make these an enumeration, and global to Alchemist
  int LOCALEIGSPRECOMPUTE = 1;
  int DISTEIGS = 2;

  MatrixHandle UHandle{nextMatrixId++};
  MatrixHandle SHandle{nextMatrixId++};
  MatrixHandle VHandle{nextMatrixId++};

  auto m = matrices[inputMat].numRows;
  auto n = matrices[inputMat].numCols;
  TruncatedSVDCommand cmd(inputMat, UHandle, SHandle, VHandle, k, method);
  issue(cmd);

  if (method == DISTEIGS) {
      log->info("using distributed mat-vec prods against A, then A tranpose");
  }
  if (method == LOCALEIGS) {
      log->info("using local mat-vec prods computed on the fly against the local Gramians");
  }
  if (method == LOCALEIGSPRECOMPUTE) {
      log->info("using local mat-vec prods against the precomputed local Gramians");
  }

  ARrcSymStdEig<double> prob(n, k, "LM");
  uint32_t command;
  std::vector<double> zerosVector(n);
  for(uint32_t idx = 0; idx < n; idx++)
    zerosVector[idx] = 0;

  int iterNum = 0;

  while (!prob.ArnoldiBasisFound()) {
    prob.TakeStep();
    ++iterNum;
    if(iterNum % 20 == 0) {
        log->info("Computed {} mv products", iterNum);
    }
    if (prob.GetIdo() == 1 || prob.GetIdo() == -1) {
      command = 1;
      mpi::broadcast(world, command, 0);
      if (method == LOCALEIGS || method == LOCALEIGSPRECOMPUTE) {
          mpi::broadcast(world, prob.GetVector(), n, 0);
          mpi::reduce(world, zerosVector.data(), n, prob.PutVector(), std::plus<double>(), 0);
      }
      if (method == DISTEIGS) {
          world.send(1, 0, prob.GetVector(), n);
          world.recv(1, 0, prob.PutVector(), n);
      }
    }
  }

  prob.FindEigenvectors();
  uint32_t nconv = prob.ConvergedEigenvalues();
  uint32_t niters = prob.GetIter();
  log->info("Done after {} Arnoldi iterations, converged to {} eigenvectors of size {}", niters, nconv, n);

  //NB: it may be the case that n*nconv > 4 GB, then have to be careful!
  // assuming tall and skinny A for now
  MatrixXd rightVecs(n, nconv);
  log->info("Allocated matrix for right eigenvectors of A'*A");
  // Eigen uses column-major layout by default!
  for(uint32_t idx = 0; idx < nconv; idx++)
    std::memcpy(rightVecs.col(idx).data(), prob.RawEigenvector(idx), n*sizeof(double));
  log->info("Copied right eigenvectors into allocated storage");

  // Populate U, V, S
  command = 2;
  mpi::broadcast(world, command, 0);
  mpi::broadcast(world, nconv, 0);
  log->info("Broadcasted command and number of converged eigenvectors");
  mpi::broadcast(world, rightVecs.data(), n*nconv, 0);
  log->info("Broadcasted right eigenvectors");
  mpi::broadcast(world, prob.RawEigenvalues(), nconv, 0);
  log->info("Broadcasted eigenvalues");

  MatrixDescriptor Uinfo(UHandle, m, nconv);
  MatrixDescriptor Sinfo(SHandle, nconv, 1);
  MatrixDescriptor Vinfo(VHandle, n, nconv);
  ENSURE(matrices.insert(std::make_pair(UHandle, Uinfo)).second);
  ENSURE(matrices.insert(std::make_pair(SHandle, Sinfo)).second);
  ENSURE(matrices.insert(std::make_pair(VHandle, Vinfo)).second);

  log->info("Waiting on workers to store U,S,V");

  world.barrier();
  log->info("Writing ok status followed by U,S,V handles");
  output.writeInt(0x1);
  output.writeInt(UHandle.id);
  output.writeInt(SHandle.id);
  output.writeInt(VHandle.id);
  output.flush();
}

void Driver::handle_computeThinSVD() {
  MatrixHandle inputMat{input.readInt()};

  // this needs to be done automatically rather than hand-coded. e.g. what if
  // we switch to determining rank by sing-val thresholding instead of doing thin SVD?
  auto m = matrices[inputMat].numRows;
  auto n = matrices[inputMat].numCols;
  auto k = std::min(m,n);
  MatrixHandle Uhandle = registerMatrix(m, k);
  MatrixHandle Shandle = registerMatrix(k, 1);
  MatrixHandle Vhandle = registerMatrix(n, k);
  ThinSVDCommand cmd(inputMat, Uhandle, Shandle, Vhandle);
  issue(cmd);

  output.writeInt(0x1); // statusCode
  output.writeInt(Uhandle.id);
  output.writeInt(Shandle.id);
  output.writeInt(Vhandle.id);
  output.flush();

  // wait for command to finish
  world.barrier();
  log->info("Done with SVD computation");
  output.writeInt(0x1);
  output.flush();
}

void Driver::handle_matrixMul() {
  MatrixHandle matA{input.readInt()};
  MatrixHandle matB{input.readInt()};
  log->info("Multiplying matrices {} and {}", matA, matB);

  auto numRows = matrices[matA].numRows;
  auto numCols = matrices[matB].numCols;
  MatrixHandle destHandle = registerMatrix(numRows, numCols);
  MatrixMulCommand cmd(destHandle, matA, matB);
  issue(cmd);

  // tell spark id of resulting matrix
  output.writeInt(0x1); // statusCode
  output.writeInt(destHandle.id);
  output.flush();

  // wait for it to finish
  world.barrier();
  output.writeInt(0x1);
  output.flush();
}

void Driver::handle_matrixDims() {
  MatrixHandle matrixHandle{input.readInt()};
  log->info("Looking up dimensions for matrix {}", matrixHandle.id);
  auto info = matrices[matrixHandle];
  log->info("Returning dimensions for matrix {}: {}-by-{}", matrixHandle.id, info.numRows, info.numCols);
  output.writeInt(0x1);
  output.writeLong(info.numRows);
  output.writeLong(info.numCols);
  output.flush();

}

void Driver::handle_getMatrixRows() {
  MatrixHandle handle{input.readInt()};
  uint64_t layoutLen = input.readLong();
  std::vector<uint32_t> layout;
  layout.reserve(layoutLen);
  for(uint64_t part = 0; part < layoutLen; ++part) {
    layout.push_back(input.readInt());
  }
  log->info("Returning matrix {} to Spark", handle);

  MatrixGetRowsCommand cmd(handle, layout);
  issue(cmd);

  // tell Spark to start asking for rows
  output.writeInt(0x1);
  output.flush();

  // wait for it to finish
  world.barrier();
  output.writeInt(0x1);
  output.flush();
}

void Driver::handle_newMatrix() {
  // read args
  uint64_t numRows = input.readLong();
  uint64_t numCols = input.readLong();

  // assign id and notify workers
  MatrixHandle handle = registerMatrix(numRows, numCols);
  NewMatrixCommand cmd(matrices[handle]);
  log->info("Recieving new matrix {}, with dimensions {}x{}", handle, numRows, numCols);
  issue(cmd);

  output.writeInt(0x1);
  output.writeInt(handle.id);
  output.flush();

  // tell spark which worker expects each row
  std::vector<int> rowWorkerAssignments(numRows, 0);
  std::vector<uint64_t> rowsOnWorker;
  std::vector<uint32_t> numRowsOnEachWorker(world.size() -1, 0);
  for(int workerIdx = 1; workerIdx < world.size(); workerIdx++) {
    world.recv(workerIdx, 0, rowsOnWorker);
    numRowsOnEachWorker[workerIdx - 1] = rowsOnWorker.size();
    world.barrier();
    for(auto rowIdx: rowsOnWorker) {
      rowWorkerAssignments[rowIdx] = workerIdx;
    }
  }

  log->info("Sending list of which worker each row should go to");
  output.writeInt(0x1); // statusCode
  std::stringstream ss;
  for(auto workerIdx: rowWorkerAssignments) {
    output.writeInt(workerIdx);
    ALCHEMIST_TRACE(ss << workerIdx << ' ');
  }
  ALCHEMIST_TRACE(log->info(ss.str().c_str()));

  for(uint32_t workerIdx = 1; workerIdx < world.size(); workerIdx++)
    log->info("Worker with rank {} will receive {} rows", workerIdx, numRowsOnEachWorker[workerIdx-1]);

  output.flush();

  log->info("Waiting for spark to finish sending data to the workers");
  world.barrier();
  output.writeInt(0x1);  // statusCode
  output.flush();
  log->info("Entire matrix has been received");
}

inline bool exist_test (const std::string& name) {
    struct stat buffer;
    return (stat(name.c_str(), &buffer) == 0);
}

int driverMain(const mpi::communicator &world, int argc, char *argv[]) {
  //log to console as well as file (single-threaded logging)
  //TODO: allow to specify log directory, log level, etc.
  time_t rawtime = time(0);
  struct tm * timeinfo = localtime(&rawtime);
  char buf[80];
  strftime(buf, 80, "%F-%T", timeinfo);
  std::shared_ptr<spdlog::logger> log;
  std::vector<spdlog::sink_ptr> sinks;
  sinks.push_back(std::make_shared<spdlog::sinks::ansicolor_stderr_sink_st>());
  sinks.push_back(std::make_shared<spdlog::sinks::simple_file_sink_st>(str(format("driver-%s.log") % buf)));
  log = std::make_shared<spdlog::logger>("driver", std::begin(sinks), std::end(sinks));
  //log->flush_on(spdlog::level::warn); // flush whenever warning or more critical message is logged
  //log->set_level(spdlog::level::info); // only log stuff at or above info level, for production
  log->flush_on(spdlog::level::info); // flush always, for debugging
  log->info("Started Driver");
  log->info("Max number of OpenMP threads: {}", omp_get_max_threads());

  char machine[255];
  char port[255];

  if (argc == 3) { // we are on a non-NERSC system, so passed in Spark driver machine name and port
      log->info("Non-NERSC system assumed");
      log->info("Connecting to Spark executor at {}:{}", argv[1], argv[2]);
      std::strcpy(machine, argv[1]);
      std::strcpy(port, argv[2]);
  } else { // assume we are on NERSC, so look in a specific location for a file containing the machine name and port
      char const* tmp = std::getenv("SPARK_WORKER_DIR");
      std::string sockPath;
      if (tmp == NULL) {
          log->info("Couldn't find the SPARK_WORKER_DIR variable");
          world.abort(1);
      } else {
        sockPath = std::string(tmp) + "/connection.info";
      }
      log->info("NERSC system assumed");
      log->info("Searching for connection information in file {}", sockPath);

      while(!exist_test(sockPath)) {
          boost::this_thread::sleep_for(boost::chrono::milliseconds(50));
      }
      // now wait for a while for the connection file to be completely written, hopefully is enough time
      // TODO: need a more robust way of ensuring this is the case
      boost::this_thread::sleep_for(boost::chrono::milliseconds(500));

      std::string sockSpec;
      std::ifstream infile(sockPath);
      std::getline(infile, sockSpec);
      infile.close();
      boost::tokenizer<> tok(sockSpec);
      boost::tokenizer<>::iterator iter=tok.begin();
      std::string machineName = *iter;
      std::string portName = *(++iter);

      log->info("Connecting to Spark executor at {}:{}", machineName, portName);
      strcpy(machine, machineName.c_str());
      strcpy(port, portName.c_str());
  }

  using boost::asio::ip::tcp;
  boost::asio::ip::tcp::iostream stream(machine, port);
  ENSURE(stream);
  stream.rdbuf()->non_blocking(false);
  auto result = Driver(world, stream, stream, log).main();
  return result;
}

} // namespace alchemist