#include <cmath>

#include <ros/ros.h>
#include <tf2_eigen/tf2_eigen.h>

#include <mav_msgs/conversions.h>
#include <mav_msgs/RateThrust.h>

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

#include "cascaded_pid_control_node.hpp"

namespace cascaded_pid_control {

  inline geometry_msgs::Vector3 MsgVec3(const Eigen::Vector3d& vec) {
    geometry_msgs::Vector3 result;
    result.x = vec[0];
    result.y = vec[1];
    result.z = vec[2];
    return result;
  }

  inline double Constrain(double value, double min, double max) {
    if (value < min) {
      return min;
    }
    if (value > max) {
      return max;
    }
    return value;
  }

  double CascadedPidControl::AltitudeControl(
    const mav_msgs::EigenOdometry& current,
    const mav_msgs::EigenTrajectoryPoint& cmd,
    const Eigen::Vector3d &accel_ff,
    double dt) {

      Eigen::Matrix3d rot_mat = current.orientation_W_B.toRotationMatrix();

      double dz = cmd.position_W[2] - current.position_W[2];
      double dvz = cmd.velocity_W[2] - current.getVelocityWorld()[2];
      double z_dot_dot = kp_z_ * dz + kd_z_ * dvz + accel_ff[2];

      double result;
      result = z_dot_dot * mass_ / rot_mat(2, 2);

      // clamp value
      result = Constrain(result, min_thrust_, max_thrust_);
      return result;
  }

  Eigen::Vector3d CascadedPidControl::LateralPositionControl(
    const mav_msgs::EigenOdometry& current,
    const mav_msgs::EigenTrajectoryPoint& cmd,
    const Eigen::Vector3d& accel_ff,
    double dt) {

    double err_x = cmd.position_W[0] - current.position_W[0];
    double err_y = cmd.position_W[1] - current.position_W[1];

    double err_vx = cmd.velocity_W[0] - current.getVelocityWorld()[0];
    double err_vy = cmd.velocity_W[1] - current.getVelocityWorld()[1];

    Eigen::Vector3d result;
    result << Constrain(kp_x_ * err_x + kd_x_ * err_vx + accel_ff[0], -max_abs_accel_x_, max_abs_accel_x_),
              Constrain(kp_y_ * err_y + kd_y_ * err_vy + accel_ff[1], -max_abs_accel_y_, max_abs_accel_y_),
              0;

    return result;
  }

  Eigen::Vector3d CascadedPidControl::AttitudeControl(
      const mav_msgs::EigenOdometry& pose,
      const Eigen::Vector3d& accel_cmd,
      double thrust_cmd,
      double dt) {
    Eigen::Matrix3d rot_mat = pose.orientation_W_B.toRotationMatrix();

    double bx = rot_mat(0, 2);
    double by = rot_mat(1, 2);

    double c = thrust_cmd / mass_;
    double bx_cmd = accel_cmd[0] / c;
    double by_cmd = accel_cmd[1] / c;

    // clamp bx_cmd and by_cmd be with [-sqrt(2)/2,  sqrt(2)/2]
    // this will result in forcing angle between the thrust vector and x/y axis of world frame
    // be in [45deg, 135deg] (this is not the same with constraining roll and pitch angle)
    bx_cmd = Constrain(bx_cmd, -0.5, 0.5);
    by_cmd = Constrain(by_cmd, -0.5, 0.5);

    double bx_dot = kp_pitch_ * (bx_cmd - bx);
    double by_dot = kp_roll_ * (by_cmd - by);

    double k = 1.0 / rot_mat(2, 2);
    Eigen::Vector3d pq_rate_cmd;
    pq_rate_cmd << k * (rot_mat(1, 0) * bx_dot - rot_mat(0, 0) * by_dot),
                   k * (rot_mat(1, 1) * bx_dot - rot_mat(0, 1) * by_dot),
                   0;

    return pq_rate_cmd;
  }

  double CascadedPidControl::YawControl(
    const mav_msgs::EigenOdometry& current,
    double yaw_cmd,
    double dt) {
    double current_yaw = current.getYaw();
    double err_yaw = yaw_cmd - current_yaw;
    while (err_yaw > PI) {
      err_yaw -= 2 * PI;
    }
    while (err_yaw < -PI) {
      err_yaw += 2 * PI;
    }
    return kp_yaw_ * err_yaw;
  }

  void CascadedPidControl::OdometryCallback(const nav_msgs::OdometryConstPtr &ptr) {
    if (controller_active_) {
      mav_msgs::eigenOdometryFromMsg(*ptr, &last_odometry_);
      double current_time = ptr->header.stamp.toSec();
      double dt = current_time - last_odometry_time_;
      last_odometry_time_ = current_time;

      Eigen::Vector3d accel_ff = set_point_.acceleration_W + default_ff_;

      double thrust = AltitudeControl(last_odometry_, set_point_, accel_ff, dt);
      Eigen::Vector3d accel_cmd = LateralPositionControl(last_odometry_, set_point_, accel_ff, dt);
      Eigen::Vector3d pqr_rate_cmd = AttitudeControl(last_odometry_, accel_cmd, thrust, dt);
      pqr_rate_cmd[2] = YawControl(last_odometry_, set_point_.getYaw(), dt);

      // ROS_INFO_STREAM(thrust<<" "<<accel_cmd<<" "<<pqr_rate_cmd);

      // control command is published as mav_msgs/RateThrust
      mav_msgs::RateThrust rate_thrust;
      rate_thrust.header.stamp = ros::Time::now();
      rate_thrust.header.frame_id = "uav/imu";
      rate_thrust.thrust.z = thrust;
      mav_msgs::vectorEigenToMsg(pqr_rate_cmd, &rate_thrust.angular_rates);

      if (publish_debug_topic_) {
        PublishDebugTopic(last_odometry_);
      }

      rate_thrust_pub_.publish(rate_thrust);
    }
  }

  void CascadedPidControl::Init() {
    dynamic_reconfigure::Server<CascadedPidConfig>::CallbackType cb;
    cb = boost::bind(&CascadedPidControl::DynamicReconfigureCallback, this, _1, _2);
    server_.setCallback(cb);

    controller_active_ = false;
    last_odometry_time_ = 0;
    default_ff_ << 0, 0, GRAVITY_CONST;
    publish_debug_topic_ = private_nh_.param("publish_debug_topic", false);

    rate_thrust_pub_ = private_nh_.advertise<mav_msgs::RateThrust>("rateThrust", 1);
    odometry_sub_ = private_nh_.subscribe<nav_msgs::Odometry>("odometry", 1, &CascadedPidControl::OdometryCallback, this);
    trajectory_sub_ = private_nh_.subscribe<trajectory_msgs::MultiDOFJointTrajectory>("trajectory", 1, &CascadedPidControl::TrajectoryCallbackStartFromNearest2, this);

    if (publish_debug_topic_) {
      position_setpoint_pub_ = private_nh_.advertise<geometry_msgs::Vector3>("positionSetpoint", 8);
      velocity_setpoint_pub_ = private_nh_.advertise<geometry_msgs::Vector3>("velocitySetpoint", 8);
      attitude_setpoint_pub_ = private_nh_.advertise<geometry_msgs::Vector3>("attitudeSetpoint", 8);
      position_error_pub_ = private_nh_.advertise<geometry_msgs::Vector3>("positionError", 8);
      velocity_error_pub_ = private_nh_.advertise<geometry_msgs::Vector3>("velocityError", 8);
      attitude_error_pub_ = private_nh_.advertise<geometry_msgs::Vector3>("attitudeError", 8);
      target_pose_pub_ = private_nh_.advertise<geometry_msgs::PoseStamped>("targetPose", 1);
    }

    timer_ = nh_.createTimer(ros::Duration(0), &CascadedPidControl::TimerCallback, this, true, false);

    ROS_INFO("Cascaded PID control node initialized. Waiting for trajectory.");
  }

  void CascadedPidControl::Destroy() {
    ROS_INFO("Cascaded PID control exited.");
  }

  void CascadedPidControl::DynamicReconfigureCallback(CascadedPidConfig &config, uint32_t level) {
    ROS_INFO("Dynamic reconfigure requested.");
    mass_ = config.mass;
    // motors should generate at least (mass * g) N force, this is the minimum
    // value of max_thrust, and maximu value of min_thrust
    if (config.max_thrust < mass_ * GRAVITY_CONST) {
      config.max_thrust = mass_ * GRAVITY_CONST;
    }
    if (config.min_thrust > mass_ * GRAVITY_CONST) {
      config.min_thrust = mass_ * GRAVITY_CONST;
    }
    min_thrust_ = config.min_thrust;
    max_thrust_ = config.max_thrust;

    kp_x_ = config.kp_x;
    kd_x_ = config.kd_x;
    max_abs_accel_x_ = config.max_abs_accel_x;
    kp_y_ = config.kp_y;
    kd_y_ = config.kd_y;
    max_abs_accel_y_ = config.max_abs_accel_y;
    kp_z_ = config.kp_z;
    kd_z_ = config.kd_z;
    kp_roll_ = config.kp_roll;
    kp_pitch_ = config.kp_pitch;
    kp_yaw_ = config.kp_yaw;

    if (config.xy_same_params) {
      config.kp_y = kp_y_ = kp_x_;
      config.kd_y = kd_y_ = kd_x_;
      config.max_abs_accel_y = max_abs_accel_y_ = max_abs_accel_x_;
    }

    if (config.rp_same_params) {
      config.kp_pitch = kp_pitch_ = kp_roll_;
    }
  }

  void CascadedPidControl::PublishDebugTopic(const mav_msgs::EigenOdometry& odometry) {
    // debug topic includes:
    //  1. set point position
    //  2. set point velocit
    //  3. set point attitude (x component: roll, y component: pitch, z component: yaw)
    geometry_msgs::Vector3 pos_setpoint;
    tf2::toMsg(set_point_.position_W, pos_setpoint);
    geometry_msgs::Vector3 vel_setpoint;
    tf2::toMsg(set_point_.velocity_W, vel_setpoint);
    geometry_msgs::Vector3 att_setpoint;
    Eigen::Vector3d set_att;
    mav_msgs::getEulerAnglesFromQuaternion(set_point_.orientation_W_B, &set_att);
    tf2::toMsg(set_att, att_setpoint);

    position_setpoint_pub_.publish(pos_setpoint);
    velocity_setpoint_pub_.publish(vel_setpoint);
    attitude_setpoint_pub_.publish(att_setpoint);

    //  4. error of position
    //  5. error of velocity
    //  6. error of attitude (x component: roll, y component: pitch, z component: yaw)
    geometry_msgs::Vector3 pos_error;
    tf2::toMsg(set_point_.position_W - odometry.position_W, pos_error);
    geometry_msgs::Vector3 vel_error;
    tf2::toMsg(set_point_.velocity_W - odometry.getVelocityWorld(), vel_error);
    geometry_msgs::Vector3 att_error;
    Eigen::Vector3d ego_att;
    mav_msgs::getEulerAnglesFromQuaternion(odometry.orientation_W_B, &ego_att);
    tf2::toMsg(set_att - ego_att, att_error);

    position_error_pub_.publish(pos_error);
    velocity_error_pub_.publish(vel_error);
    attitude_error_pub_.publish(att_error);
  }

  void CascadedPidControl::TrajectoryCallbackStartFromNearest2(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& ptr) {
    if (ptr->points.size() == 0) {
      ROS_WARN("Empty trajectory.");
      return;
    }

    ROS_INFO("New trajectory arrived.");

    bool restart = traj_points_.size() == 0;

    timer_.stop();
    traj_points_.clear();
    command_wait_time_.clear();

    auto iter = ptr->points.begin();
    double last_time = iter->time_from_start.toSec();
    command_wait_time_.push_back(last_time);
    mav_msgs::EigenTrajectoryPoint eigen_traj_point;
    mav_msgs::eigenTrajectoryPointFromMsg(*iter, &eigen_traj_point);
    traj_points_.push_back(eigen_traj_point);

    double nearest_dist = 1e9;
    bool nearest_found = false;
    double threshold_dist;
    int nearest_idx = 0;

    if (restart) {
      nearest_found = true;
    } else {
      threshold_dist = (set_point_.position_W - eigen_traj_point.position_W).norm();
      nearest_dist = threshold_dist;
    }

    for (++iter; iter != ptr->points.end(); ++iter) {
      double traj_time = iter->time_from_start.toSec();
      command_wait_time_.push_back(traj_time - last_time);
      mav_msgs::eigenTrajectoryPointFromMsg(*iter, &eigen_traj_point);
      traj_points_.push_back(eigen_traj_point);

      if (!nearest_found) {
        double dist = (set_point_.position_W- eigen_traj_point.position_W).norm();
        if (dist > threshold_dist) {
          nearest_found = true;
        } else {
          if (dist < nearest_dist) {
            nearest_dist = dist;
            nearest_idx = iter - ptr->points.begin();
          }
        }
      }

      last_time = traj_time;
    }

    ROS_INFO("The closest trajectory point to the current position is at index %d.", nearest_idx);

    while (traj_points_.size() > 1 && nearest_idx-- > 0) {
      traj_points_.pop_front();
      command_wait_time_.pop_front();
    }

    SetNextPoint(traj_points_.front());
    traj_points_.pop_front();

    if (!traj_points_.empty()) {
      double wait_time = command_wait_time_.front();
      command_wait_time_.pop_front();
      timer_.setPeriod(ros::Duration(wait_time));
      timer_.start();
    }

    controller_active_ = true;
  }

  void CascadedPidControl::TrajectoryCallbackStartFromNearest(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& ptr) {
    // This trajectory handler when receiving a new trajectory, pickup the neartest waypoint to the current position and
    // use it as the next target set point
    if (ptr->points.size() == 0) {
      ROS_WARN("Empty trajectory.");
      return;
    }

    ROS_INFO("New trajectory arrived.");

    timer_.stop();
    traj_points_.clear();
    command_wait_time_.clear();

    double nearest_dist;
    auto iter = ptr->points.begin();
    double last_time = iter->time_from_start.toSec();
    command_wait_time_.push_back(last_time);
    mav_msgs::EigenTrajectoryPoint eigen_traj_point;
    mav_msgs::eigenTrajectoryPointFromMsg(*iter, &eigen_traj_point);
    traj_points_.push_back(eigen_traj_point);

    bool nearest_found = false;
    double threshold_dist;
    int nearest_idx = 0;

    if (last_odometry_time_ <= 0) {
      nearest_found = true;
    } else {
      threshold_dist = (last_odometry_.position_W - eigen_traj_point.position_W).norm();
      nearest_dist = threshold_dist;
    }

    for (++iter; iter != ptr->points.end(); ++iter) {
      double traj_time = iter->time_from_start.toSec();
      command_wait_time_.push_back(traj_time - last_time);
      mav_msgs::eigenTrajectoryPointFromMsg(*iter, &eigen_traj_point);
      traj_points_.push_back(eigen_traj_point);

      if (!nearest_found) {
        double dist = (last_odometry_.position_W - eigen_traj_point.position_W).norm();
        if (dist > threshold_dist) {
          nearest_found = true;
        } else {
          if (dist < nearest_dist) {
            nearest_dist = dist;
            nearest_idx = iter - ptr->points.begin();
          }
        }
      }

      last_time = traj_time;
    }

    ROS_INFO("The closest trajectory point to the current position is at index %d.", nearest_idx);

    while (traj_points_.size() > 1 && nearest_idx-- >= 0) {
      traj_points_.pop_front();
      command_wait_time_.pop_front();
    }

    SetNextPoint(traj_points_.front());
    traj_points_.pop_front();

    if (!traj_points_.empty()) {
      double wait_time = command_wait_time_.front();
      command_wait_time_.pop_front();
      timer_.setPeriod(ros::Duration(wait_time));
      timer_.start();
    }

    controller_active_ = true;
  }

  void CascadedPidControl::TrajectoryCallbackNew(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& ptr) {
    // Merge the new coming trajectory with the current one that being executed.

    // Some preconditions: because the planner with use part of the old trajectory to generate
    // the new trajectory, there should be some waypoints overlapping (otherwise the controller
    // is executing more waypoints than the planner could re-generate. the rate of the planner
    // should be raised)
    //
    // so the idea is, find out the waypoint in the in coming trajectory that is closest to the
    // current set point (in ideal case, the distance should be 0 since they are overlapping).
    // from that point, keep comparing waypoints until they diverge (distance greater than some
    // threshold). from the point they diverge, we have a new trajectory segement. subsitude the new
    // segment with the corresponding part in the trajectory the controller is going to execute.
    //

    if (ptr->points.size() == 0) {
      ROS_WARN("Empty trajectory.");
      return;
    }

    ROS_INFO("New trajectory arrived.");

    bool should_restart = traj_points_.size() == 0;

    mav_msgs::EigenTrajectoryPoint eigen_traj_point;
    std::vector<mav_msgs::EigenTrajectoryPoint> new_trajectory;
    std::vector<double> new_wait_time;
    double closest_dist = 1e9;
    std::size_t best_match = -1;
    auto iter = ptr->points.begin();
    double last_time = -1;
    // find out the best matching waypoint in the new coming trajectory with
    // the current set point.
    for (auto iter = ptr->points.begin(); iter != ptr->points.end(); ++iter) {
      mav_msgs::eigenTrajectoryPointFromMsg(*iter, &eigen_traj_point);
      new_trajectory.push_back(eigen_traj_point);
      double dist = (eigen_traj_point.position_W - set_point_.position_W).norm();
      if (dist < closest_dist) {
        closest_dist = dist;
        best_match = iter - ptr->points.begin();
      }
      if (last_time >= 0) {
        double dt = iter->time_from_start.toSec() - last_time;
        new_wait_time.push_back(dt);
      }
      last_time = iter->time_from_start.toSec();
    }
    ROS_INFO("New trajectory is built. Length: %zu, number of wait commands: %zu", new_trajectory.size(), new_wait_time.size());
    ROS_INFO("Merging the new trajectory with current one.");

    // continue looking forward until the new trajectory diverge with the current trajectory
    std::size_t current_j = 0;
    std::size_t new_j = best_match;
    for (;
         current_j < traj_points_.size() - 1 && new_j < new_trajectory.size() - 1;
         ++current_j, ++new_j) {
      if ((traj_points_[current_j].position_W - new_trajectory[new_j].position_W).norm() > 0.1) {
        break;
      }
    }
    ROS_INFO("New trajectory start overlapping with the current one at waypoint index %zu, diverge from %zu (waypoint index %zu in the current path)", best_match, new_j, current_j);

    // substitude segement in the new trajectory from new_j with the one in the current trajectory
    // from current_j
    std::size_t num_copied = 0;
    std::size_t copy_current_j = current_j;
    std::size_t copy_new_j = new_j;
    while (copy_current_j < traj_points_.size() && copy_new_j < new_trajectory.size()) {
      traj_points_[current_j] = new_trajectory[new_j];
      ++copy_current_j;
      ++copy_new_j;
      ++num_copied;
    }
    ROS_INFO("%zu waypoints copied from new trajectory within from %zu to %zu to the current trajectory from %zu to %zu.",
          num_copied, new_j, copy_new_j, current_j, copy_current_j);
    ROS_INFO("Truncate number of waypoints to %zu.", copy_current_j);
    traj_points_.resize(copy_current_j);

    if (copy_new_j < new_trajectory.size()) {
      ROS_INFO("There're still waypoints in the new trajectory, starting from index %zu", copy_new_j);
      std::copy(new_trajectory.begin() + copy_new_j, new_trajectory.end(), std::back_inserter(traj_points_));
    }

    // command wait time update.
    command_wait_time_.resize(current_j);
    std::copy(new_wait_time.begin() + new_j, new_wait_time.end(), std::back_inserter(command_wait_time_));

    ROS_INFO_STREAM("New trajectory length: " << traj_points_.size() << ", command wait time queue length: " << command_wait_time_.size() << ".");

    if (should_restart) {
      ROS_INFO("Starting/restarting waypoint following routine.");
      if (!traj_points_.empty()) {
        SetNextPoint(traj_points_.front());
        traj_points_.pop_front();
        if (!traj_points_.empty()) {
          double wait_time = command_wait_time_.front();
          command_wait_time_.pop_front();
          timer_.stop();
          timer_.setPeriod(ros::Duration(wait_time));
          timer_.start();
        }
      }
      controller_active_ = true;
    }
    ROS_INFO("New trajectory is successfully merged into the exising trajectory.");
  }

  void CascadedPidControl::TrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& ptr) {
    timer_.stop();
    traj_points_.clear();
    command_wait_time_.clear();

    if (ptr->points.size() == 0) {
      ROS_WARN("Empty trajectory.");
      return;
    }

    ROS_INFO("New trajectory arrived.");

    auto iter = ptr->points.begin();
    double last_time = iter->time_from_start.toSec();
    command_wait_time_.push_back(last_time);
    mav_msgs::EigenTrajectoryPoint eigen_traj_point;
    mav_msgs::eigenTrajectoryPointFromMsg(*iter, &eigen_traj_point);
    traj_points_.push_back(eigen_traj_point);

    for (++iter; iter != ptr->points.end(); ++iter) {
      double traj_time = iter->time_from_start.toSec();
      command_wait_time_.push_back(traj_time - last_time);
      mav_msgs::eigenTrajectoryPointFromMsg(*iter, &eigen_traj_point);
      traj_points_.push_back(eigen_traj_point);

      last_time = traj_time;
    }

    SetNextPoint(traj_points_.front());
    traj_points_.pop_front();

    if (!traj_points_.empty()) {
      double wait_time = command_wait_time_.front();
      command_wait_time_.pop_front();
      timer_.setPeriod(ros::Duration(wait_time));
      timer_.start();
    }

    controller_active_ = true;
  }

  void CascadedPidControl::TimerCallback(const ros::TimerEvent& e) {
    if (traj_points_.empty()) {
      ROS_INFO("All trajectory commands have beed carried out.");
      return;
    }

    SetNextPoint(traj_points_.front());
    traj_points_.pop_front();
    if (!traj_points_.empty()) {
      double wait_time = command_wait_time_.front();
      command_wait_time_.pop_front();
      timer_.stop();
      timer_.setPeriod(ros::Duration(wait_time));
      timer_.start();
    }
  }

  void CascadedPidControl::SetNextPoint(const mav_msgs::EigenTrajectoryPoint& point) {
    set_point_ = point;
    if (publish_debug_topic_) {
      geometry_msgs::PoseStamped msg;
      msg.header.frame_id = "map";
      msg.header.stamp = ros::Time::now();
      mav_msgs::pointEigenToMsg(set_point_.position_W, &msg.pose.position);
      mav_msgs::quaternionEigenToMsg(set_point_.orientation_W_B, &msg.pose.orientation);
      target_pose_pub_.publish(msg);
    }
  }

}

XROS_RUNNABLE_NODE_MAIN(cascaded_pid_control::CascadedPidControl)


/**
 * \file boost/numeric/ublasx/operation/element_pow.hpp
 *
 * \brief Apply the \c std::pow function to each element of a vector or a matrix
 *  expression.
 *
 * Copyright (c) 2015, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_ELEMENT_POW_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_ELEMENT_POW_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_binary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_binary_functor.hpp>
#include <cmath>
#include <complex>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT, typename Arg2T>
struct vector_element_pow_functor1_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument1_type;
	typedef Arg2T signature_argument2_type;
	//typedef signature_argument_type signature_result_type;
	typedef typename promote_traits<
				signature_argument1_type,
				signature_argument2_type
			>::promote_type signature_result_type;
	typedef vector_binary_functor1_traits<
				input_expression_type,
				Arg2T,
				signature_result_type (signature_argument1_type, signature_argument2_type)
			> binary_functor_expression_type;
	typedef typename binary_functor_expression_type::result_type result_type;
	typedef typename binary_functor_expression_type::expression_type expression_type;
};

template <typename Arg1T, typename VectorExprT>
struct vector_element_pow_functor2_traits
{
	typedef VectorExprT input_expression_type;
	typedef Arg1T signature_argument1_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument2_type;
	//typedef signature_argument_type signature_result_type;
	typedef typename promote_traits<
				signature_argument1_type,
				signature_argument2_type
			>::promote_type signature_result_type;
	typedef vector_binary_functor2_traits<
				Arg1T,
				input_expression_type,
				signature_result_type (signature_argument1_type, signature_argument2_type)
			> binary_functor_expression_type;
	typedef typename binary_functor_expression_type::result_type result_type;
	typedef typename binary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT, typename Arg2T>
struct matrix_element_pow_functor1_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument1_type;
	typedef Arg2T signature_argument2_type;
	//typedef signature_argument_type signature_result_type;
	typedef typename promote_traits<
				signature_argument1_type,
				signature_argument2_type
			>::promote_type signature_result_type;
	typedef matrix_binary_functor1_traits<
				input_expression_type,
				Arg2T,
				signature_result_type (signature_argument1_type, signature_argument2_type)
			> binary_functor_expression_type;
	typedef typename binary_functor_expression_type::result_type result_type;
	typedef typename binary_functor_expression_type::expression_type expression_type;
};

template <typename Arg1T, typename MatrixExprT>
struct matrix_element_pow_functor2_traits
{
	typedef MatrixExprT input_expression_type;
	typedef Arg1T signature_argument1_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument2_type;
	//typedef signature_argument_type signature_result_type;
	typedef typename promote_traits<
				signature_argument1_type,
				signature_argument2_type
			>::promote_type signature_result_type;
	typedef matrix_binary_functor2_traits<
				Arg1T,
				input_expression_type,
				signature_result_type (signature_argument1_type, signature_argument2_type)
			> binary_functor_expression_type;
	typedef typename binary_functor_expression_type::result_type result_type;
	typedef typename binary_functor_expression_type::expression_type expression_type;
};

// Wrappers to the std::pow function to avoid compiler errors

template <typename T1, typename T2>
BOOST_UBLAS_INLINE
typename promote_traits<T1,T2>::promote_type element_pow(T1 x, T2 y)
{
    return ::std::pow(x, y);
}

template <typename T1, typename T2>
BOOST_UBLAS_INLINE
std::complex<T1> element_pow(std::complex<T1> const& x, T2 y)
{
    return ::std::pow(x, y);
}

template <typename T1, typename T2>
BOOST_UBLAS_INLINE
std::complex<T1> element_pow(T1 x, std::complex<T2> const& y)
{
	// Remember: if z=(a + ib) is a complex number and c is a scalar => c^z = e^{ln(c)*z}
    return ::std::exp(::std::log(x)*y);
}

} // Namespace detail

/**
 * \brief Applies the \c std::pow function to a given vector expression,
 *  where each element of the vector is treated as the base of the
 *  exponentiation.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \param p The exponent.
 * \return A vector expression representing the application of \c std::pow to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT, typename T>
BOOST_UBLAS_INLINE
typename detail::vector_element_pow_functor1_traits<VectorExprT,T>::result_type element_pow(vector_expression<VectorExprT> const& ve, T p)
{
	typedef typename detail::vector_element_pow_functor1_traits<VectorExprT,T>::expression_type expression_type;
	typedef typename detail::vector_element_pow_functor1_traits<VectorExprT,T>::signature_argument1_type signature_argument1_type;
	typedef typename detail::vector_element_pow_functor1_traits<VectorExprT,T>::signature_argument2_type signature_argument2_type;
	typedef typename detail::vector_element_pow_functor1_traits<VectorExprT,T>::signature_result_type signature_result_type;

//	return expression_type(ve(), detail::element_pow<signature_result_type>);
//	signature_result_type (*)(ptr_element_pow_fun)(signature_argument_type)(BOOST_NUMERIC_UBLASX_OPERATION_POW_NS_::element_pow);
	typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
	fun_ptr_type ptr_element_pow_fun(&detail::element_pow);
	return expression_type(ve(), p, ptr_element_pow_fun);
}

/**
 * \brief Applies the \c std::pow function to a given vector expression,
 *  where each element of the vector is treated as the power of the
 *  exponentiation.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param b The base.
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::pow to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename T, typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_element_pow_functor2_traits<T,VectorExprT>::result_type element_pow(T b, vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_element_pow_functor2_traits<T,VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_element_pow_functor2_traits<T,VectorExprT>::signature_argument1_type signature_argument1_type;
	typedef typename detail::vector_element_pow_functor2_traits<T,VectorExprT>::signature_argument2_type signature_argument2_type;
	typedef typename detail::vector_element_pow_functor2_traits<T,VectorExprT>::signature_result_type signature_result_type;

//	return expression_type(ve(), detail::element_pow<signature_result_type>);
//	signature_result_type (*)(ptr_element_pow_fun)(signature_argument_type)(BOOST_NUMERIC_UBLASX_OPERATION_POW_NS_::element_pow);
	typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
	fun_ptr_type ptr_element_pow_fun(&detail::element_pow);
	return expression_type(b, ve(), ptr_element_pow_fun);
}

/**
 * \brief Applies the \c std::pow function to a given matrix expression,
 *  where each element of the matrix is treated as the base of the
 *  exponentiation.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \param p The exponent.
 * \return A matrix expression representing the application of \c std::pow to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT, typename T>
BOOST_UBLAS_INLINE
typename detail::matrix_element_pow_functor1_traits<MatrixExprT,T>::result_type element_pow(matrix_expression<MatrixExprT> const& me, T p)
{
	typedef typename detail::matrix_element_pow_functor1_traits<MatrixExprT,T>::expression_type expression_type;
	typedef typename detail::matrix_element_pow_functor1_traits<MatrixExprT,T>::signature_argument1_type signature_argument1_type;
	typedef typename detail::matrix_element_pow_functor1_traits<MatrixExprT,T>::signature_argument2_type signature_argument2_type;
	typedef typename detail::matrix_element_pow_functor1_traits<MatrixExprT,T>::signature_result_type signature_result_type;

//	return expression_type(me(), detail::element_pow<signature_result_type>(signature_argument_type));
	typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
	fun_ptr_type ptr_element_pow_fun(&detail::element_pow);
	return expression_type(me(), p, ptr_element_pow_fun);
}

/**
 * \brief Applies the \c std::pow function to a given matrix expression,
 *  where each element of the matrix is treated as the power of the
 *  exponentiation.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param b The base.
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::pow to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename T, typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_element_pow_functor2_traits<T,MatrixExprT>::result_type element_pow(T b, matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_element_pow_functor2_traits<T,MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_element_pow_functor2_traits<T,MatrixExprT>::signature_argument1_type signature_argument1_type;
	typedef typename detail::matrix_element_pow_functor2_traits<T,MatrixExprT>::signature_argument2_type signature_argument2_type;
	typedef typename detail::matrix_element_pow_functor2_traits<T,MatrixExprT>::signature_result_type signature_result_type;

//	return expression_type(me(), detail::element_pow<signature_result_type>(signature_argument_type));
	typedef signature_result_type(*fun_ptr_type)(signature_argument1_type, signature_argument2_type);
	fun_ptr_type ptr_element_pow_fun(&detail::element_pow);
	return expression_type(b, me(), ptr_element_pow_fun);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_ELEMENT_POW_HPP


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward 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 NT2_SDK_MEMORY_META_NEXT_POWER_OF_2_HPP_INCLUDED
#define NT2_SDK_MEMORY_META_NEXT_POWER_OF_2_HPP_INCLUDED

#include <cstddef>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/integral_c.hpp>

namespace nt2 { namespace details
{
  template<std::size_t N> struct next_power_of_2_impl
  {
    BOOST_STATIC_CONSTANT(std::size_t, x0    = N-1            );
    BOOST_STATIC_CONSTANT(std::size_t, x1    = x0 | (x0 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x2    = x1 | (x1 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x3    = x2 | (x2 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x4    = x3 | (x3 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, x5    = x4 | (x4 >> 1) );
    BOOST_STATIC_CONSTANT(std::size_t, value = x5 + 1         );
  };
} }

namespace nt2 { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function computing the power of 2 greater or equal to any
  // integral constant.
  // Documentation: next_power_of_2_c.rst
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t N>
  struct  next_power_of_2_c
        : boost::mpl::size_t<details::next_power_of_2_impl<N>::value> {};

  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function computing the power of 2 greater or equal to any
  // Integral Constant.
  // Documentation: next_power_of_2.rst
  //////////////////////////////////////////////////////////////////////////////
  template<class N>
  struct  next_power_of_2
        : boost::mpl::integral_c< typename N::value_type
                                , next_power_of_2_c<N::value>::value
                                > {};
} }

#endif


#include "tile/math/matrix.h"

#include <algorithm>
#include <set>
#include <string>
#include <utility>

#include <boost/format.hpp>

#include "base/util/compat.h"

namespace vertexai {
namespace tile {
namespace math {

struct DualMatrix {
  size_t size_;
  Matrix lhs_;
  Matrix rhs_;

  explicit DualMatrix(const Matrix& m) : size_(m.size1()), lhs_(IdentityMatrix(size_)), rhs_(m) {}

  void swapRows(size_t r1, size_t r2) {
    lhs_.swapRows(r1, r2);
    rhs_.swapRows(r1, r2);
  }
  void multRow(size_t r, Rational v) {
    lhs_.multRow(r, v);
    rhs_.multRow(r, v);
  }
  void addMultRow(size_t d, size_t s, Rational v) {
    lhs_.addRowMultToRow(d, s, v);
    rhs_.addRowMultToRow(d, s, v);
  }

  std::string toString() const {
    std::string r = "";
    for (size_t i = 0; i < size_; i++) {
      for (size_t j = 0; j < size_; j++) {
        r += str(boost::format("%4s, ") % to_string(lhs_(i, j)));
      }
      r += "      ";
      for (size_t j = 0; j < size_; j++) {
        r += str(boost::format("%4s, ") % to_string(rhs_(i, j)));
      }
      r += "\n";
    }
    return r;
  }

  // Do elimination, returns false if singular
  bool invert() {
    // First zero the lower triangle of the RHS
    for (size_t i = 0; i < size_; i++) {
      // Pivot first to non-zero entry in diagonal
      bool found_nonzero = false;
      for (size_t j = i; j < size_; j++) {
        if (rhs_(j, i) != 0) {
          found_nonzero = true;
          swapRows(i, j);
          break;
        }
      }
      if (!found_nonzero) {
        return false;
      }
      // Divide row to make (i, i) == 1
      multRow(i, 1 / rhs_(i, i));
      // Zero this column all the way down
      for (size_t j = i + 1; j < size_; j++) {
        addMultRow(j, i, -rhs_(j, i));
      }
    }
    // Now, zero the upper triangle of the RHS
    for (ssize_t i = size_ - 1; i >= ssize_t(0); i--) {
      // Zero this column all the way up
      for (ssize_t j = i - 1; j >= ssize_t(0); j--) {
        addMultRow(j, i, -rhs_(j, i));
      }
    }
    return true;
  }
};

void Matrix::swapRows(size_t r, size_t s) {
  for (size_t i = 0; i < size2(); i++) {
    std::swap((*this)(r, i), (*this)(s, i));
  }
}

void Matrix::multRow(size_t r, Rational multiplier) {
  for (size_t i = 0; i < size2(); i++) {
    (*this)(r, i) *= multiplier;
  }
}

void Matrix::addRowMultToRow(size_t dest_row, size_t src_row, const Rational& multiplier) {
  if (multiplier != 0) {
    for (size_t i = 0; i < size2(); i++) {
      (*this)(dest_row, i) += multiplier * (*this)(src_row, i);
    }
  }
}

void Matrix::makePivotAt(size_t row, size_t col) {
  if ((*this)(row, col) == 0) {
    throw std::runtime_error("Cannot pivot matrix at entry containing 0");
  }
  for (size_t r = 0; r < size1(); ++r) {
    if (r == row) {
      continue;
    }
    addRowMultToRow(r, row, -(*this)(r, col) / (*this)(row, col));
  }
  multRow(row, 1 / (*this)(row, col));
}

bool Matrix::invert() {
  if (size1() != size2()) {
    throw std::runtime_error("Trying to invert non-square matrix");
  }
  DualMatrix dm(*this);
  if (!dm.invert()) {
    return false;
  }
  *this = dm.lhs_;
  return true;
}

std::string Matrix::toString() const {
  std::string ret;
  ret += "\n";
  for (size_t i = 0; i < size1(); ++i) {
    ret += "[ ";
    for (size_t j = 0; j < size2(); ++j) {
      ret += ((*this)(i, j)).str() + "\t";
    }
    ret += "]\n";
  }
  return ret;
}

bool Matrix::operator==(const Matrix& m) {
  if (size1() != m.size1()) {
    return false;
  }
  if (size2() != m.size2()) {
    return false;
  }
  for (size_t i = 0; i < size1(); i++) {
    for (size_t j = 0; j < size2(); j++) {
      if ((*this)(i, j) != m(i, j)) {
        return false;
      }
    }
  }
  return true;
}

Vector VectorLit(const std::vector<Rational>& vec) {
  Vector r(vec.size());
  for (size_t i = 0; i < vec.size(); i++) {
    r(i) = vec[i];
  }
  return r;
}

Matrix MatrixLit(const std::vector<std::vector<Rational>>& vecs) {
  size_t rows = vecs.size();
  size_t columns = vecs[0].size();
  Matrix r(rows, columns);
  for (size_t i = 0; i < rows; i++) {
    if (vecs[i].size() != columns) {
      throw std::runtime_error("Non-rectangular matrix literal");
    }
    for (size_t j = 0; j < columns; j++) {
      r(i, j) = vecs[i][j];
    }
  }
  return r;
}

bool operator==(const Vector& a, const Vector& b) {
  if (a.size() != b.size()) {
    return false;
  }
  for (size_t i = 0; i < a.size(); i++) {
    if (a(i) != b(i)) {
      return false;
    }
  }
  return true;
}

std::tuple<Matrix, Vector> FromPolynomials(const std::vector<Polynomial<Rational>>& polys) {
  std::set<std::string> vars;
  for (size_t i = 0; i < polys.size(); i++) {
    for (const auto& kvp : polys[i].getMap()) {
      if (kvp.first != "") {
        vars.insert(kvp.first);
      }
    }
  }
  Matrix mat(polys.size(), vars.size());
  Vector vec(polys.size());
  for (size_t i = 0; i < polys.size(); i++) {
    vec(i) = polys[i].constant();
    size_t j = 0;
    for (const auto& v : vars) {
      mat(i, j) = polys[i][v];
      j++;
    }
  }
  return std::tie(mat, vec);
}

struct HermiteCompute {
  size_t rows_;
  size_t columns_;
  Matrix lhs_;

  void swap(size_t i, size_t j) { lhs_.swapRows(i, j); }

  void mult(size_t i, Integer m) {
    if (m != 1 && m != -1) {
      throw std::runtime_error("Cannot multiply row by nonunit constant in computing HNF.");
    }
    lhs_.multRow(i, m);
  }

  void addMult(size_t d, size_t s, Integer m) {
    IVLOG(6, "  Adding " << m << " * row " << s << " to row " << d);
    lhs_.addRowMultToRow(d, s, m);
  }

  void eliminate(size_t i, size_t j) {
    IVLOG(5, "    Eliminate " << i << ", " << j);
    if (lhs_(j, i) == 0) {
      IVLOG(5, "      Already 0, nothing to do");
      return;
    }
    Integer x, y;
    IVLOG(5, "      Computing XGCD of " << lhs_(i, i) << " and " << lhs_(j, i));
    Rational o = XGCD(lhs_(i, i), lhs_(j, i), x, y);
    IVLOG(5, "o = " << o << ", x = " << x << ", y = " << y);
    if (Abs(o) != lhs_(i, i)) {
      if (Abs(o) == Abs(lhs_(j, i))) {
        IVLOG(5, "      Swapping entry");
        swap(i, j);
        if (lhs_(i, i) < 0) {
          mult(i, -1);
        }
      } else {
        IVLOG(5, "      Updating entry");
        euclidean_reduce(i, j, i);
      }
    }
    Rational m = numerator(-lhs_(j, i) / o);
    IVLOG(5, "  m = " << m);
    addMult(j, i, numerator(-lhs_(j, i) / o));
  }

  void euclidean_reduce(size_t i, size_t j, size_t col) {
    Rational a = lhs_(i, col);
    Rational b = lhs_(j, col);
    if (a < 0) {
      a = -a;
      IVLOG(5, "    Negating row " << i)
      mult(i, -1);
      IVLOG(6, "  state\n" << toString());
    }
    if (b < 0) {
      b = -b;
      IVLOG(5, "    Negating row " << j)
      mult(j, -1);
      IVLOG(6, "  state\n" << toString());
    }
    if (a < b) {
      swap(i, j);
      IVLOG(6, "  state\n" << toString());
      a = lhs_(i, col);
      b = lhs_(j, col);
    }

    // Main Euclidean algorithm
    Rational r;
    Integer q = RatDiv(a, b, r);
    IVLOG(6, "Quotient " << q << ", Remainder " << r);
    while (true) {
      addMult(i, j, -q);
      swap(i, j);
      IVLOG(6, "  a = " << a << ", b = " << b << ", state\n" << toString());
      if (r == 0) {
        IVLOG(6, "Remainder 0, stopping");
        break;
      }
      a = b;
      b = r;
      q = RatDiv(a, b, r);
      IVLOG(6, "Quotient " << q << ", Remainder " << r);
    }
  }

  void normalize(size_t i, size_t j) {
    Integer m = -Floor(lhs_(j, i) / lhs_(i, i));
    addMult(j, i, m);
  }

  std::string toString() {
    std::stringstream ss;
    for (size_t i = 0; i < rows_; i++) {
      for (size_t j = 0; j < columns_; j++) {
        ss << lhs_(i, j).str() << " ";
      }
    }
    return ss.str();
  }

 public:
  explicit HermiteCompute(const Matrix& m) : rows_(m.size1()), columns_(m.size2()), lhs_(m) {}

  bool compute() {
    if (rows_ < columns_) {
      return false;
    }
    IVLOG(4, "Computing HNF, initial state\n" << toString());
    // TODO(T132): Technically, this ordering of the algorithm may be
    // exponential in the bit representation of the rational numbers
    // There is a polynomial time version, but the pivoting is tricker
    // so I'm skipping it for now.
    for (size_t i = 0; i < columns_; i++) {
      IVLOG(5, "Fixing column " << i);
      IVLOG(5, "  state\n" << toString());
      // First, we need to position (i, i) with a non-zero entry if possible
      for (size_t j = i; j < rows_; j++) {
        if (lhs_(j, i) != 0) {
          IVLOG(5, "  Swapping " << i << " and " << j);
          swap(i, j);
          IVLOG(6, "  state\n" << toString());
          break;
        }
      }
      // If they are all zeros, whatevs, on to the next column
      if (lhs_(i, i) == 0) {
        IVLOG(5, "  Skipping due to zeros");
        continue;
      }
      // Otherwise, fix sign and combine with all rows below
      if (lhs_(i, i) < 0) {
        IVLOG(6, " Multiplying " << i << " by -1");
        mult(i, -1);
        IVLOG(6, "  state\n" << toString());
      }
      for (size_t j = i + 1; j < rows_; j++) {
        eliminate(i, j);
        IVLOG(6, "  state\n" << toString());
      }
      // And normalize all rows above
      for (size_t j = 0; j < i; j++) {
        normalize(i, j);
        IVLOG(6, "  state\n" << toString());
      }
    }
    IVLOG(4, "Final state\n" << toString());
    return true;
  }
};

bool HermiteNormalForm(Matrix& m) {  // NOLINT(runtime/references)
  HermiteCompute hc(m);
  bool r = hc.compute();
  m = hc.lhs_;
  return r;
}

}  // namespace math
}  // namespace tile
}  // namespace vertexai


#include <iostream>
#include <Eigen/Dense>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
#include "LR.hpp"
#include "Adadelta.hpp"
#include "Adagrad.hpp"
#include "Adam.hpp"

#define EIGEN_MPL2_ONLY
using namespace Eigen;
using namespace std;

void read(string filepath, MatrixXd& x,VectorXd& l){
  ifstream ifs(filepath);
  if(ifs.fail()){
    cerr << "File do not exist."<<endl;
    exit(0);
  }
  string s;
  int index = 0;
  while(getline(ifs,s)){
    stringstream ss(s);
    int t;
    ss>>t;
    if(t>0){
      l(index) = 1;
    }else{
      l(index) = 0;
    }
    string feature;
    while(ss>>feature){
      string::size_type idx = feature.find(":");
      int n = atoi(feature.substr(0,idx).c_str());
      double value = atof(feature.substr(idx+1).c_str());
      x(index,n-1) = value;
    }
    ++index;
  }
}

int main()
{
  MatrixXd x = MatrixXd::Zero(32561,123);
  VectorXd label(32561);
  MatrixXd x_test = MatrixXd::Zero(16281,123);
  VectorXd label_test(16281);
  int iteration = 250;
  read("a9a",x,label);
  read("a9a.t",x_test,label_test);
  // LR lr = LR(x.rows(),x.cols(),x,label,0.01,0.1,iteration);
  // lr.train();
  // vector<double> scores;
  // lr.predict(x_test,label_test,scores);
  // cout<<"=== LR ==="<<endl;
  // cout <<"Accuracy:"<< lr.Acc(scores,label_test) <<endl;

  // ofstream iofs("iters.txt");
  // for(double d : lr.iterscores){
  //   iofs<<d<<endl;
  // }

  // ofstream wofs("weight.txt");
  // for(int i = 0; i< lr.w.rows();i++){
  //   wofs<<lr.w(i)<<endl;
  // }

  Adam adam = Adam(x.rows(),x.cols(),x,label,0.01,0.002,0.1,0.001,0.000000001,0.00000001,iteration);
  adam.train();
  vector<double> adamscores;
  adam.predict(x_test,label_test,adamscores);
  cout<<"=== Adam ==="<<endl;
  cout <<"Accuracy:"<< adam.Acc(adamscores,label_test) <<endl;

  ofstream adamiofs("adamiters.txt");
  for(double d : adam.iterscores){
    adamiofs<<d<<endl;
  }

  ofstream adamwofs("adamweight.txt");
  for(int i = 0; i< adam.w.rows();i++){
    adamwofs<<adam.w(i)<<endl;
  }

  // Adadelta addlr = Adadelta(x.rows(),x.cols(),x,label,0.01,0.95,0.0000001,iteration);
  // addlr.train();
  // vector<double> addscores;
  // addlr.predict(x_test,label_test,addscores);
  // cout<<"=== Adadelta=="<<endl;
  // cout <<"Accuracy:"<< lr.Acc(addscores,label_test) <<endl;

  // ofstream addiofs("adadelta_iters.txt");
  // for(double d : addlr.iterscores){
  //   addiofs<<d<<endl;
  // }

  // ofstream addwofs("adadelta_weight.txt");
  // for(int i = 0; i< addlr.w.rows();i++){
  //   addwofs<<lr.w(i)<<endl;
  // }

  // Adagrad adglr = Adagrad(x.rows(),x.cols(),x,label,0.01,0.1,iteration);
  // adglr.train();
  // vector<double> adgscores;

  // adglr.predict(x_test,label_test,adgscores);
  // cout<<"=== Adagrad ==="<<endl;
  // cout <<"Accuracy:"<< adglr.Acc(adgscores,label_test) <<endl;

  // ofstream adgiofs("adagrad_iters.txt");
  // for(double d : adglr.iterscores){
  //   adgiofs<<d<<endl;
  // }

  // ofstream adgwofs("adagrad_weight.txt");
  // for(int i = 0; i< adglr.w.rows();i++){
  //   adgwofs<<adglr.w(i)<<endl;
  // }
}


#include <NTL/ZZ.h>
#include <cstdint>
#include <cmath>

#define NUM_BITS_REAL_MANTISSA 54

#include "binomials.hpp"
#include <cmath>

int main(int argc, char* argv[]){
  if(argc < 5){
     std::cout << "Complexity estimation of key enumeration" << std::endl << " Usage "
               << argv[0] << " <p> <d_v> <n_0> <m_0> <m_1> <m_2> <m_3>" << std::endl <<
               "All the non existing m_i should be passed as zeroes " << std::endl;
    return -1;
  }

  InitBinomials();
  NTL::RR::SetPrecision(NUM_BITS_REAL_MANTISSA);
  pi = NTL::ComputePi_RR();
  uint32_t p = atoi(argv[1]);
  uint32_t d_v = atoi(argv[2]);
  uint32_t n_0 = atoi(argv[3]);
  uint32_t m[4];
  m[0] = atoi(argv[4]);
  m[1] = atoi(argv[5]);
  m[2] = atoi(argv[6]);
  m[3] = atoi(argv[7]);

  NTL::RR Henum, Qenum;

  Henum = lnBinom(NTL::RR(p),NTL::RR(d_v));
  Henum = Henum*NTL::RR(n_0) / NTL::log(NTL::RR(2));

  Qenum = NTL::RR(n_0)* ( lnBinom(NTL::RR(p),NTL::RR(m[0])) +
                          lnBinom(NTL::RR(p),NTL::RR(m[1])) +
                          lnBinom(NTL::RR(p),NTL::RR(m[2])) +
                          lnBinom(NTL::RR(p),NTL::RR(m[3])) )/ NTL::log(NTL::RR(2));

  std::cout << "H enum classic/quantum cost :" << Henum << "  " << (Henum/NTL::RR(2)) << std::endl;
  std::cout << "Q enum classic/quantum cost :" << Qenum << "  " << (Qenum/NTL::RR(2)) << std::endl;
  return 0;
}


#ifndef ALGORITHM_DP_PATH_FINDER_HPP_2T9GR5EG
#define ALGORITHM_DP_PATH_FINDER_HPP_2T9GR5EG

#include <stdlib.h>
#include <fstream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <log4cxx/logger.h>
#include <log4cxx/basicconfigurator.h>
#include <log4cxx/helpers/exception.h>
#include "image.hpp"
#include <opencv2/core/core.hpp>

//namespace std {using namespace __gnu_cxx;}
using namespace std;

namespace prhlt {

    class Algorithm_DP_Path_Finder
    {
        public:
            Algorithm_DP_Path_Finder(cv::Mat ex_image_mat, const Eigen::MatrixXd& ex_cost_matrix);
            Algorithm_DP_Path_Finder(cv::Mat ex_image_mat, const Eigen::MatrixXd& ex_cost_matrix, cv::Rect ex_search_area );
            Algorithm_DP_Path_Finder(cv::Mat ex_image_mat, const Eigen::MatrixXd& ex_cost_matrix,int orig_x, int orig_y , int size_x, int size_y);
            ~Algorithm_DP_Path_Finder();
            void run(double ex_alpha, double ex_beta);
            void set_search_limits(vector < vector <cv::Point> > ex_upper_limits, vector < vector <cv::Point> > ex_lower_limits);
            void set_lower_search_limits(vector < vector <cv::Point> > ex_lower_limits);
            void set_upper_search_limits(vector < vector <cv::Point> > ex_lower_limits);
            void set_search_limits(vector <cv::Point> ex_upper_limits, vector <cv::Point> ex_lower_limits);
            void set_lower_search_limits(vector <cv::Point> ex_lower_limits);
            void set_upper_search_limits(vector <cv::Point> ex_lower_limits);
            vector<cv::Point2d> recover_path(int x, int y);
            vector< vector<cv::Point2d> > recover_all_paths();
            vector<cv::Point2d> recover_best_path();
            vector<cv::Point2d> get_best_path_collision_points();
            void save_path_matrix_to_file(string file_name);
        private:
            //GENERAL
            void forward();
            void backward();
            void reset_change_counter();
            bool solution_not_converged();
            void show_search_area();

            //INITIALIZATION
            void initialize_bound_matrix();
            //PATH MANAGEMENT FUNCTIONS
            void update_column(int c);
            void update_cell(int x , int y);
            bool update_cell_from(int from_x, int from_y, int to_x , int to_y);
            void review_column(int c);
            bool review_cell(int x , int y);
            void backtrack_column(int c);
            bool backtrack_cell(int x , int y);
            bool update_bound_matrix(int x, int y, double cost);
            void update_path_matrix(int x, int y, int from_x, int from_y);
            void display_path(int x, int y);
            void display_best_path();
            void save_best_path();
            bool is_collision_point(vector<cv::Point2d> points, int index );
            cv::Point2d localize_point(cv::Point2d point);
            cv::Point2d localize_point(int x , int y);

            //vector<cv::Point2d> recover_all_paths();
            //COST FUNCTIONS
            double base_movement_cost(int from_x, int from_y, int to_x, int to_y);
            double contextual_average(int x, int y, int context_size);
            double future_contextual_average(int x, int y,const int x_context_size, const int y_context_size);
			void calculate_valid_search_area();
            bool precalc_is_valid_point_as_per_search_limits(int x, int y);
            bool is_valid_point_as_per_search_limits(int x, int y);
            int point_position_in_respect_to_line(vector<cv:: Point> ex_line, int x, int y);
            bool restriction_segment_applicable_to_point(vector<cv:: Point> ex_line, int x, int y);
            //DATA
		    int axis_x;
		    int axis_y;
    		int cells_changed;
		    double alpha;
		    double beta;
		    double average_euclidian_distance_cost;
		    double average_grey_distance_cost;
		    Image image;
		    double roof_distance;
        vector < vector <cv::Point> > upper_search_limits;
        vector < vector <cv::Point> > lower_search_limits;
            Eigen::MatrixXd cost_matrix;
            Eigen::MatrixXd bound_matrix;
            Eigen::MatrixXi limits_matrix;
            Eigen::MatrixXi path_matrix_x;
            Eigen::MatrixXi path_matrix_y;
            log4cxx::LoggerPtr logger;
    };

}

#endif /* end of include guard: ALGORITHM_PATH_FINDER_HPP_2T9GR5EG */


#include <opengv/optimization_tools/objective_function_tools/ObjectiveFunctionInfo.hpp>
#include <opengv/optimization_tools/solver_tools/SolverToolsNoncentralRelativePose.hpp>
#include <Eigen/Dense>
#include <opengv/types.hpp>
#include <iostream>

Eigen::Matrix3d SolverToolsNoncentralRelativePose::exp_R( Eigen::Matrix3d & X ){
  double phi = X.norm()/std::sqrt(2);
  Eigen::Matrix3d X1 = X/phi;
  Eigen::Matrix3d I = Eigen::Matrix< double, 3, 3 >::Identity();
  I = I + std::sin(phi)*X1 + ( 1 - std::cos(phi) )*X1*X1;
  return I;
}

opengv::rotation_t SolverToolsNoncentralRelativePose::rotation_solver(opengv::rotation_t & state_rotation, const opengv::translation_t & translation,
								      double &tol, ObjectiveFunctionInfo * info_function, int & k){
  double g = 1.0;
  double erro = 1.0;
  k = 0;
  opengv::rotation_t X = state_rotation;
  opengv::rotation_t previous_X = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Z  = Eigen::Matrix3d::Zero(3,3);
  double zz = 0;
  Eigen::Matrix3d DX = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Pt = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d P  = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Q  = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d Qt = Eigen::Matrix3d::Zero(3,3);
  Eigen::Matrix3d reference = Eigen::Matrix3d::Identity(3,3);
  //std::cout << "Inside rotation solver to check the values: " << std::endl;
  //std::cout << "Before process starts the rotation matrix is: " << X << std::endl;

  while( erro > tol && k < 1e3 )
    {

      //std::cout << "state at the beginning: " << std::endl << X << std::endl << std::endl;
      DX = info_function->rotation_gradient(X, translation);
      //std::cout << "Calculated gradient: " << std::endl << DX << std::endl << std::endl;
      Z   = DX*X.transpose() - X*DX.transpose();
      //std::cout << "Calculated riemaniann gradient: " << std::endl << Z << std::endl << std::endl;
      zz  = 0.5*( Z*Z.transpose() ).trace();
      //std::cout << "Coefficient zz : " << zz << std::endl << std::endl;
      Pt  = -g*Z;
      //std::cout << "Matrix Pt: " << std::endl << Pt << std::endl << std::endl;
      P   = exp_R( Pt );
      //std::cout << "The rotation matrix P: " << std::endl << P << std::endl << std::endl;
      Q   = P*P; // this seems strange
      //std::cout << "Matrix Q: " << std::endl << Q << std::endl << std::endl;
      Qt  = Q*X;
      //std::cout << "Matrix Qt: " << std::endl << Qt << std::endl << std::endl;
      //std::cout << "************************************************************" << std::endl << std::endl;
      //std::cout << "1st CYCLE" << std::endl << std::endl;

      //while( ( objective_function( M, X, translation ) - objective_function(M, Qt, translation ) ) >= g*zz  )
      while( ( info_function->objective_function_value(X, translation ) - info_function->objective_function_value(Qt, translation ) ) >= g*zz  )
        {
          g   = 2*g;
          //In order to prevent NAN's the following restriction is added
          if(g < 64){
            //std::cout << "g: " << g << std::endl;
            P   = Q;
            //std::cout << std::endl << "New P: " << std::endl << P << std::endl;
            Q   = P*P; // this seems strange
            //std::cout << std::endl << "New Q: " << std::endl << Q << std::endl;
            Qt  = Q*X;
            //std::cout << std::endl << "New Qt: " << std::endl << Qt << std::endl;
            //std::cout << "Current value for obj function: " << objective_function(M, X, translation) << std::endl;
            //std::cout << "Current value for new obj function: " << objective_function(M, Qt, translation ) << std::endl;
          }
          else{
            break;
          }
        }
      //std::cout << "End of 1st cycle" << std::endl;
      //std::cout << "******************************************************************" << std::endl;
      //std::cout << "**********************************************************************" << std::endl;
      //std::cout << "Enters 2nd cycle: " << std::endl;
      Qt = P * X;
      //while( ( objective_function( M, X, translation ) - objective_function(M, Qt, translation ) ) < 0.5*g*zz)
      while( ( info_function->objective_function_value( X, translation ) - info_function->objective_function_value( Qt, translation ) ) < 0.5*g*zz)
        {

          //   if ( f_obj( M, N, X, beta) - f_obj( M, N, Qt, beta ) < tol )
          //     break;

          g  = 0.5*g;
          //std::cout << "New g: " << std::endl << g << std::endl << std::endl;
          Pt = -g*Z;
          //std::cout << "New Pt: " << std::endl << Pt << std::endl << std::endl;
          P  = exp_R( Pt );
          //In order to prevent NAN's
          if( (P - reference).norm() < 1e-6){
            break;
          }
          //std::cout << "New P : " << std::endl << P << std::endl << std::endl;
          Qt = P*X;
          //std::cout << "New Qt: " << std::endl << std::endl << Qt << std::endl;
        }
      previous_X = X;
      X    = P*X;
      erro = ( X - previous_X ).norm();
      // std::cout << "Rotation: "    << std::endl << X << std::endl;
      /*std::cout << "inside rotation solver: " << std::endl;
      std::cout << "\nIteration: " << k << std::endl;
      std::cout << "Rotation: "    << std::endl << X << std::endl;
      std::cout << "Euclidean grad: " << std::endl << DX << std::endl;
      std::cout << "Function value: " << info_function->objective_function_value(X, translation) << std::endl;*/
      k++;
    }
  return X;
}

opengv::translation_t SolverToolsNoncentralRelativePose::translation_solver(const opengv::rotation_t & rotation, opengv::translation_t & translation, double &tol, ObjectiveFunctionInfo * info_function, double & step, int & k){

  double error = 1;
  k = 0;
  //std::cout << "Translation gradient: " << std::endl;
  opengv::translation_t state = translation;
  opengv::translation_t grad = info_function->translation_gradient(rotation, state);
  opengv::translation_t new_state = state - step * grad;
  //std::cout << "Beginning of the translation solver: " << std::endl;
  //std::cout << "The rotation used is: " << std::endl << rotation << std::endl;
  while (error > tol  && k < 1000){
    new_state = state - step * grad;
    double f_obj_current = info_function->objective_function_value(rotation, state);
    double f_obj_next = info_function->objective_function_value(rotation, new_state);

    /*std::cout << "current state: " << std::endl     << state      << std::endl;
    std::cout << "new state: "     << std::endl     << new_state  << std::endl;
    std::cout << "f(state): "      << f_obj_current << std::endl;
    std::cout << "f(new_state): "  << f_obj_next    << std::endl;
    std::cout << "gradient:     "  << std::endl     << grad       << std::endl;
    std::cout << std::endl         << std::endl     << std::endl;*/
    if(f_obj_next > f_obj_current){
      break;
    }
    error = std::abs(f_obj_current - f_obj_next);
    grad = info_function->translation_gradient(rotation, state);

    state = new_state;
    k++;
  }
  return state;
}


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

#include <mitrax/sub_matrix.hpp>

using namespace mitrax;
using namespace mitrax::literals;

template < typename T >
auto rt_id(T&& v){
	return boost::typeindex::type_id_runtime(static_cast< T&& >(v));
}

template < typename T >
auto const id = boost::typeindex::type_id< T >();

using point_t = point_t;

constexpr int org[3][3] = {
	{0, 1, 2},
	{3, 4, 5},
	{6, 7, 8}
};

constexpr auto m(){
	return make_matrix(3_CS, 3_RS, org);
}

template < typename M >
constexpr bool check1(M const& m){
	return
		m.cols() == 2_CS &&
		m.rows() == 2_RS &&
		m(0_c, 0_r) == 0 &&
		m(1_c, 0_r) == 1 &&
		m(0_c, 1_r) == 3 &&
		m(1_c, 1_r) == 4;
}

template < typename M >
constexpr bool check2(M const& m){
	return
		m.cols() == 2_CS &&
		m.rows() == 2_RS &&
		m(0_c, 0_r) == 1 &&
		m(1_c, 0_r) == 2 &&
		m(0_c, 1_r) == 4 &&
		m(1_c, 1_r) == 5;
}

template < typename M >
constexpr bool check3(M const& m){
	return
		m.cols() == 2_CS &&
		m.rows() == 2_RS &&
		m(0_c, 0_r) == 3 &&
		m(1_c, 0_r) == 4 &&
		m(0_c, 1_r) == 6 &&
		m(1_c, 1_r) == 7;
}

template < typename M >
constexpr bool check4(M const& m){
	return
		m.cols() == 2_CS &&
		m.rows() == 2_RS &&
		m(0_c, 0_r) == 4 &&
		m(1_c, 0_r) == 5 &&
		m(0_c, 1_r) == 7 &&
		m(1_c, 1_r) == 8;
}

// TODO: add more unit tests
// TODO: check also for result types
// TODO: check move version and const/non-const versions
// TODO: use non square matrices

BOOST_AUTO_TEST_SUITE(suite_sub_matrix)

BOOST_AUTO_TEST_CASE(test_sub_matrix_3x3){
	constexpr auto m = make_matrix(3_CS, 3_RS, org);

	constexpr auto sub1 = sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS);
	auto type1 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub1) == type1);
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RD)));

	constexpr auto sub2 = sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS));
	auto type2 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub2) == type2);
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	constexpr auto sub3 = sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS);
	auto type3 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub3) == type3);
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RD)));

	constexpr auto sub4 = sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS));
	auto type4 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub4) == type4);
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_CASE(test_sub_matrix_3x3_move){
	constexpr auto sub1 = sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RS);
	auto type1 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub1) == type1);
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RD)));

	constexpr auto sub2 = sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RS));
	auto type2 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub2) == type2);
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	constexpr auto sub3 = sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RS);
	auto type3 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub3) == type3);
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RD)));

	constexpr auto sub4 = sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RS));
	auto type4 = id< std_matrix< int, 2_C, 2_R > >;
	BOOST_TEST(rt_id(sub4) == type4);
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_CASE(test_sub_matrix_3rtx3){
	auto m = make_matrix(3_CD, 3_RS, org);

	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_CASE(test_sub_matrix_3rtx3_move){
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_CASE(test_sub_matrix_3x3rt){
	auto m = make_matrix(3_CS, 3_RD, org);

	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_CASE(test_sub_matrix_3x3rt_move){
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_CASE(test_sub_matrix_3rtx3rt){
	auto m = make_matrix(3_CD, 3_RD, org);

	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, 0_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, 1_c, 0_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, 0_c, 1_r, dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RS)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RS)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CS, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CS, 2_RD)));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), 2_CD, 2_RD)));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), 2_CD, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));

	BOOST_TEST(check1(sub_matrix(m, point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check2(sub_matrix(m, point_t(1_c, 0_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check3(sub_matrix(m, point_t(0_c, 1_r), dim_pair(2_CD, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m, point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_CASE(test_sub_matrix_3rtx3rt_move){
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), 0_c, 0_r, dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), 1_c, 1_r, dim_pair(2_CD, 2_RD))));

	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RS)) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RS)) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CS, 2_RD)) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), 2_CD, 2_RD)) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RS)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CS, 2_RD)));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), 2_CD, 2_RD)));

	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RS))) ==
		id< std_matrix< int, 2_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RS))) ==
		id< std_matrix< int, 0_C, 2_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CS, 2_RD))) ==
		id< std_matrix< int, 2_C, 0_R > >));
	BOOST_TEST((rt_id(sub_matrix(m(), point_t(0_c, 0_r), dim_pair(2_CD, 2_RD))) ==
		id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RS))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CS, 2_RD))));
	BOOST_TEST(check4(sub_matrix(m(), point_t(1_c, 1_r), dim_pair(2_CD, 2_RD))));
}

BOOST_AUTO_TEST_SUITE_END()


// json parser
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
namespace pt = boost::property_tree;

#include "Looper.h"
#include <cassert>

Looper::Looper(double start, double end, int number_of_steps, const std::string &scale)
    : start(start), end(end), number_of_steps(number_of_steps), scale(scale) {
  assert(start < end);
  this->calculate_steps();
}

Looper::Looper(const std::string &input_file) {

  // read parameters
  pt::ptree root;
  pt::read_json(input_file, root);
  this->start = root.get<double>("Looper.start");
  this->end = root.get<double>("Looper.end");
  this->number_of_steps = root.get<double>("Looper.steps");
  this->scale = root.get<std::string>("Looper.scale");

  assert(start < end);
  this->calculate_steps();
}

void Looper::calculate_steps() {
  if (scale == "linear") {
    double spacing = (end - start) / ((double)number_of_steps - 1);
    for (int i = 0; i < number_of_steps; i++) {
      this->steps.push_back(start + i * spacing);
    }
  } else if (scale == "log") {
    double spacing = pow(end / start, 1. / ((double)number_of_steps - 1.0));
    for (int i = 0; i < number_of_steps; i++) {
      this->steps.push_back(start * pow(spacing, i));
    }

  } else {
    std::cerr << "Unknown scale: " << scale << std::endl;
    exit(-1);
  }
}