///
/// if force € [0, FORCE_F_1_2_THRESH] then state 1 : lower cup
/// if force € (FORCE_F_1_2_THRESH, FORCE_F_2_3_THRESH] then state 2 : do nothing
/// if force € (FORCE_F_2_3_THRESH, MAX] then state 3 : raise cup
///
/// Fallback velocity functions
///	Vel.x				                                  Vel.y
/// -180 to -90	  y =  0.004444444*x + 0.8			  -180 to   0	y =  0.004444444*x + 0.4
///  -90 to  90	  y = -0.004444444*x 			           0 to 180	y = -0.004444444*x + 0.4
///   90 to 180	  y =  0.004444444*x - 0.8
///
///
///
/// TODO: IMPLEMENT SAFETY ABOVE 1.5N

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include "mutex"
#include "angles/angles.h"
#include "std_msgs/Int32.h"
#include "audio_emergency/AudioMessage.h"

#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"

#include <hri_package/Sens_Force.h>
#include <kinova_msgs/PoseVelocity.h>

#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>

#define FORCE_F_1_2_THRESH 0.05
#define FORCE_F_2_3_THRESH 0.5
#define NUMBER_OF_ARM_SUB_STATES 10
#define UPPER_FEED_ANGLE_THRESH 180.00 // was 140
#define FORCE_SAFETY 5

#define ARC_DOWN 1
#define ARC_UP 2
#define TRANSLATE_BACK 3
#define RAISE_CUP 4
#define LOWER_CUP 5
#define TRANSLATE_UP 6
#define TRANSLATE_DOWN 7
#define TRANSLATE_FRONT 8
#define END_EFF_FRAME "j2s7s300_end_effector"
#define BASE_FRAME "j2s7s300_link_base"
#define SENSOR_FRAME "forcesensor"

#define MAX_STEPS 200
#define VEL_LIN_MAX 0.04
#define VEL_ANG_MAX 0.4
#define VEL_CMD_DURATION 0.8

#define ACTION_DOWN 0
#define ACTION_STAY 1
#define ACTION_UP 2

double lower_angle_thresh = 85;
float thresh_lin = 0.01;
float thresh_ang = 0.05;
double rotation_step;
bool emergency = false;

int step_count;

std::string result;
std::mutex lock_pose;
std::mutex lock_status;
std::mutex lock_proc;
std::mutex lock_force;
std::mutex lock_emerg;
double force_f, force_b;
bool processing=false;
ros::Publisher cmd_pos;
ros::Publisher cmd_vel;
geometry_msgs::PoseStamped current_pose, initial_pose;

void getRPYFromQuaternionMSG(geometry_msgs::Quaternion orientation, double& roll,double& pitch, double& yaw)
{
  tf::Quaternion quat;
  tf::quaternionMsgToTF(orientation,quat);
  quat.normalize();
  tf::Matrix3x3 mat(quat);
  mat.getRPY(roll, pitch,yaw);
}

void waitForActionCompleted()
{
  std::string temp_res="";
  while(ros::ok())
  {
    ros::spinOnce();
    lock_status.lock();
    temp_res = result;
    lock_status.unlock();

    if (temp_res=="Stopped")
    {
      ROS_INFO_STREAM("Status : " << temp_res);
      //Wait a little extra for arm to settle due to inertia.
      ros::Time start = ros::Time::now();
      while(ros::Time::now()-start < ros::Duration(0.2)) ros::spinOnce();
      break;
    }
  }
}

void setPoseForDirection(int direction, geometry_msgs::PoseStamped& start_pose,double  distance)
{
  geometry_msgs::Quaternion quat;
  switch(direction)
  {
  /// END EFFECTOR FRAME  Z-forward-blue, Y-up-green, X-left-red
  case ARC_DOWN :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(rotation_step),angles::from_degrees(0),angles::from_degrees(0)); //Turn down
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0.01;  // Slightly up
    start_pose.pose.position.z=-0.01; // Slightly back
    start_pose.pose.orientation = quat;
    break;
  case ARC_UP :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(-rotation_step),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=-0.01;
    start_pose.pose.position.z=0.01;
    start_pose.pose.orientation = quat;
    break;
  case TRANSLATE_BACK :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(0),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0;
    start_pose.pose.position.z=-distance;
    start_pose.pose.orientation = quat;
    break;
  case TRANSLATE_FRONT :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(0),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0;
    start_pose.pose.position.z=distance;
    start_pose.pose.orientation = quat;
    break;

  /// SENSORFRAME  X-forward-red, Y-left-green, Z-up-blue
  }
}

geometry_msgs::TwistStamped getTwistForDirection(int direction)
{
  geometry_msgs::TwistStamped twist;
  twist.header.stamp=ros::Time::now();
  twist.header.frame_id="j2s7s300_link_7";
  switch(direction)
  {
    case ARC_DOWN:
      ROS_INFO("Arc down");
      twist.twist.linear.z=VEL_LIN_MAX;
      twist.twist.angular.x=VEL_ANG_MAX;
    break;

    case ARC_UP:
      ROS_INFO("Arc up");
      twist.twist.linear.z=-VEL_LIN_MAX;
      twist.twist.angular.x=-VEL_ANG_MAX;
    break;

    case TRANSLATE_BACK:
      ROS_INFO("Translate back");
      twist.twist.linear.x=-VEL_LIN_MAX;
    break;

    case RAISE_CUP:
      ROS_INFO("Raise Cup");
      twist.twist.angular.x=VEL_ANG_MAX;
    break;

    case LOWER_CUP:
      ROS_INFO("Lower Cup");
      twist.twist.angular.x=-VEL_ANG_MAX;
    break;

    case TRANSLATE_UP:
      ROS_INFO("Translate up");
      twist.twist.linear.z=VEL_LIN_MAX;
    break;

    case TRANSLATE_DOWN:
      ROS_INFO("Translate down");
      twist.twist.linear.z=-VEL_LIN_MAX;
    break;

  }
  return twist;
}

bool checkUpperAngleThreshold();
bool checkLowerAngleThreshold();

void publishTwistForDuration(geometry_msgs::TwistStamped twist_msg, double duration)
{
  ros::Time time_start = ros::Time::now();
  while (ros::Time::now() - time_start < ros::Duration(duration))
  {
    ros::spinOnce();
    cmd_vel.publish(twist_msg);
  }
}

void positionControlDriveForDirection(int direction, double distance)
{
  geometry_msgs::PoseStamped start_pose, pose_in_base;
  tf::TransformListener tf_listener;
  //Wait for TF buffer to fill up
  ros::Duration(0.25).sleep();
  ros::Rate loop_rate(10);

  while(ros::ok())
  {
    try
    {
      lock_pose.lock();
      start_pose = current_pose;
      lock_pose.unlock();

      setPoseForDirection(direction, start_pose, distance);

      tf_listener.transformPose(BASE_FRAME,start_pose,pose_in_base);
      break;

    }
    catch (tf::ExtrapolationException e)
    {
     /* ROS_INFO_STREAM("Waiting for transform "
                      //<< e.what()
                      );*/
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  cmd_pos.publish(pose_in_base);
}

bool isForceSafe()
{
  double local_force_f;
  lock_force.lock();
  local_force_f=force_f;
  lock_force.unlock();

  if (local_force_f >= FORCE_SAFETY)
  {
    ROS_WARN_STREAM("PAIN THRESHOLD BREACHED!!");
    return false;
  }
  else
    return true;
}

bool isAudioSafe()
{
  lock_emerg.lock();
  bool emerg = emergency;
  lock_emerg.unlock();

  if (emerg) ROS_WARN_STREAM("AUDIO EMERGENCY DETECTED!!");
  return !emerg;
}

void fallback(bool emerg=false);

void driveToRollGoalWithVelocity(int direction)
{
  if(direction == RAISE_CUP)
    ROS_INFO_STREAM("Raise cup");
  else if (direction == LOWER_CUP)
    ROS_INFO_STREAM("Lower cup");
  else
  {
    ROS_ERROR_STREAM("Wrong direction given!!");
    return;
  }

  geometry_msgs::PoseStamped start_pose, goal_pose, temp_pose;

  lock_pose.lock();
  start_pose=current_pose;
  lock_pose.unlock();

  double temp_rol, temp_pit, temp_yaw;
  double goal_rol, goal_pit, goal_yaw;
  double start_rol, start_pit, start_yaw;
  double del_rol;

  goal_pose = start_pose;

  tf::Quaternion q_rot = tf::createQuaternionFromRPY(angles::from_degrees( direction==RAISE_CUP?rotation_step:-rotation_step),angles::from_degrees(0),angles::from_degrees(0)); // Rotate about x by 20 degrees
  tf::Quaternion q_start; tf::quaternionMsgToTF(start_pose.pose.orientation, q_start);
  tf::Quaternion q_goal = q_start*q_rot;

  tf::quaternionTFToMsg(q_goal,goal_pose.pose.orientation);

  ros::Rate loop_rate(100);
  //ros::Time start = ros::Time::now();
  while(ros::ok())
  {
    lock_pose.lock();
    temp_pose = current_pose;
    lock_pose.unlock();

    getRPYFromQuaternionMSG(goal_pose.pose.orientation, goal_rol, goal_pit, goal_yaw);
    getRPYFromQuaternionMSG(temp_pose.pose.orientation, temp_rol, temp_pit, temp_yaw);
    getRPYFromQuaternionMSG(start_pose.pose.orientation, start_rol, start_pit, start_yaw);

    if (goal_rol < 0 ) goal_rol = goal_rol + angles::from_degrees(360);
    if (temp_rol < 0 ) temp_rol = temp_rol + angles::from_degrees(360);

    del_rol = goal_rol - temp_rol;

    if (std::fabs(del_rol)>=thresh_ang)
    {
      float linear_vel = 0.02;
      geometry_msgs::TwistStamped twist_msg;
      twist_msg.twist.linear.x = 0;
      twist_msg.twist.linear.y = 0;
      // Comment out next line ONLY if using end-effector mode ie: Mode 1 on jostick is active and 4th blue LED on far right is active.
//      twist_msg.twist.linear.z = direction==RAISE_CUP ? linear_vel : -linear_vel;
      twist_msg.twist.angular.x= del_rol>0?VEL_ANG_MAX:-VEL_ANG_MAX;
      twist_msg.twist.angular.y = 0;
      twist_msg.twist.angular.z= 0;

      if(isForceSafe() && isAudioSafe())
      {
        cmd_vel.publish(twist_msg);
      }
      else
      {
        ROS_WARN("ABORTING SEQUENCE!!!");
        fallback(true);
        ros::shutdown();
      }
    }
    else
    {
      //ROS_INFO_STREAM("Goal reached, breaking loop");
      break;
    }

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

}

void moveCup(int direction, double duration=VEL_CMD_DURATION, double distance=0.1)
{
  if (direction==TRANSLATE_BACK || direction==TRANSLATE_FRONT)
  {
    positionControlDriveForDirection(direction, distance);
    return;
  }
  else
  {
    geometry_msgs::TwistStamped twist_msg = getTwistForDirection(direction);
    publishTwistForDuration(twist_msg, duration);

  }
  return;
}

void poseGrabber(geometry_msgs::PoseStamped pose)
{
  lock_pose.lock();
  current_pose=pose;
  lock_pose.unlock();
}

void forceGrabber(const hri_package::Sens_Force::ConstPtr msg)
{
  lock_force.lock();
  force_f=msg->forceF;
  force_b=msg->forceB;
  lock_force.unlock();
}

void statusGrabber(std_msgs::String::ConstPtr status)
{
  lock_status.lock();
  result=status->data;
  lock_status.unlock();
}

void waitForPoseDataAvailable()
{
  // This loop to wait until subscriber starts returning valid poses.
  ros::Rate loop_rate(9);
  geometry_msgs::PoseStamped temp_pose;
  while(ros::ok())
  {
    lock_pose.lock();
    temp_pose = current_pose;
    lock_pose.unlock();

    ros::spinOnce();
    loop_rate.sleep();

    if (temp_pose.pose.position.x != 0) break;
  }
  ROS_INFO("Pose data available");
}

bool checkUpperAngleThreshold()
{
  geometry_msgs::PoseStamped temp_pose;

  lock_pose.lock();
  temp_pose=current_pose;
  lock_pose.unlock();

  double temp_roll, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation,temp_roll, temp_pitch, temp_yaw);

  if ( angles::to_degrees(temp_roll) > UPPER_FEED_ANGLE_THRESH )
  {
    ROS_WARN_STREAM("MAX UPPER FEED ANGLE REACHED");
    return false;
  }
  else
  {
   return true;
  }
}

bool checkLowerAngleThreshold()
{
  geometry_msgs::PoseStamped temp_pose;

  lock_pose.lock();
  temp_pose=current_pose;
  lock_pose.unlock();

  double temp_roll, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation,temp_roll, temp_pitch, temp_yaw);

  if ((int)angles::to_degrees(temp_roll) - (int)angles::to_degrees(lower_angle_thresh) < rotation_step/2 && temp_roll > 0)
  {
    //ROS_WARN_STREAM("MAX LOWER ANGLE REACHED. LIMIT: " << angles::to_degrees(lower_angle_thresh) << " CURRENT: " << angles::to_degrees(temp_roll));
    return false;
  }
  else
  {
//   ROS_INFO_STREAM("Current roll: " << sangles::to_degrees(temp_roll));
   return true;
  }
}

double getCurrentRoll()
{
  lock_pose.lock();
  geometry_msgs::PoseStamped temp_pose = current_pose;
  lock_pose.unlock();

  double temp_rol, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation, temp_rol, temp_pitch, temp_yaw);

  return temp_rol<0?temp_rol+angles::from_degrees(360):temp_rol;
}

void callFallbackTimer(double duration)
{

  ros::Time start = ros::Time::now();
  ros::Duration run_for(duration);
  ros::Rate loop_rate(10);
  ROS_WARN_STREAM("Starting fallback timeout for: " << duration << "s.");
  while(ros::Time::now() - start <= run_for)
  {
    lock_force.lock();
    double force = force_f;
    lock_force.unlock();

    if (force>= FORCE_F_2_3_THRESH)
    {
      ROS_INFO_STREAM("Force detected, breaking out of timer");
      return;
    }

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

  ROS_WARN_STREAM("Time out! Initialising fallback");
 // moveCup(TRANSLATE_BACK);
  fallback();
  sleep(1);
  ros::shutdown();
}

void fallback(bool emerg)
{
  lock_pose.lock();
  geometry_msgs::PoseStamped  start_pose=current_pose;
  lock_pose.unlock();

  double roll, pitch, yaw;
  getRPYFromQuaternionMSG(start_pose.pose.orientation, roll, pitch, yaw);

  int yaw_degrees = angles::to_degrees(yaw);

  // ROS_INFO_STREAM("Current yaw float: " << angles::to_degrees(yaw) << " int " << yaw_degrees);

  geometry_msgs::TwistStamped twist_cmd;

  // y = mx + c
  double m = 0.004444444;
  double velx_c = 0.8;
  double vely_c = 0.4;

  /// Fallback velocity functions
  ///	Vel.x				                                  Vel.y
  /// -180 to -90	  y =  0.004444444*x + 0.8			  -180 to   0	y =  0.004444444*x + 0.4
  ///  -90 to  90	  y = -0.004444444*x 			           0 to 180	y = -0.004444444*x + 0.4
  ///   90 to 180	  y =  0.004444444*x - 0.8

  if (yaw_degrees >= -180 && yaw_degrees < -90)
  {
    //ROS_INFO_STREAM("Sector 1");
    twist_cmd.twist.linear.x = m * yaw_degrees + velx_c;
    twist_cmd.twist.linear.y = m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= -90 &&  yaw_degrees <0)
  {
    //ROS_INFO_STREAM("Sector 2");
    twist_cmd.twist.linear.x = -m * yaw_degrees;
    twist_cmd.twist.linear.y = m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= 0 &&  yaw_degrees <90)
  {
    //ROS_INFO_STREAM("Sector 3");
    twist_cmd.twist.linear.x = -m * yaw_degrees;
    twist_cmd.twist.linear.y = -m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= 90 &&  yaw_degrees <=180)
  {
    //ROS_INFO_STREAM("Sector 4");
    twist_cmd.twist.linear.x = m * yaw_degrees - velx_c;
    twist_cmd.twist.linear.y = -m * yaw_degrees + vely_c;
  }
  //ROS_INFO_STREAM("Publishing twist : ");
  //std::cout << (twist_cmd) << std::endl;
  if (emerg==true) twist_cmd.twist.angular.x = -1;
  publishTwistForDuration(twist_cmd,1);
}

class CSVReader
{
    std::string fileName;
    std::string delimeter;

public:
    CSVReader(std::string filename, std::string delm = ",") :
        fileName(filename), delimeter(delm)
    { }

    // Function to fetch data from a CSV File
    std::vector<std::vector<std::string> > getData();
};

std::vector<std::vector<std::string> > CSVReader::getData()
{
    std::ifstream file(fileName);

    std::vector<std::vector<std::string> > dataList;

    std::string line = "";
    // Iterate through each line and split the content using delimeter
    while (getline(file, line))
    {
        std::vector<std::string> vec;
        boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
        dataList.push_back(vec);
    }
    // Close the File
    file.close();

    return dataList;
}

std::vector<int> getPolicyFromCsv(std::string file)
{
    std::vector<int> policy;
    /*
  * A class to read data from a csv file.
  */
   CSVReader reader(file);

   std::vector<std::vector<std::string> > dataList = reader.getData();

   // Print the content of row by row on screen
   for(std::vector<std::string> vec : dataList)
   {
     for(std::string data : vec)
     {
      // std::cout<<data << " , ";
       policy.push_back( std::stoi(data));
     }
     std::cout<<std::endl;
   }
    return policy;
}

int calculateCurrentState(double force, int step)
{
  int force_state;

  if (force >= 0 && force < FORCE_F_1_2_THRESH) force_state = 0;
  else if (force >= FORCE_F_1_2_THRESH && force < FORCE_F_2_3_THRESH) force_state = 1;
  else force_state = 2;

  int current_state = (3 * step) + force_state;

  return current_state;
}

std::vector<int> selectActivePolicy(std::string algorithm, char** argv)
{
  if(algorithm == "pi")
  {
    ROS_INFO_STREAM("Using optimal policy for " << algorithm << " from file " << argv[1]);
    return getPolicyFromCsv(argv[1]);
  }
  else if(algorithm == "vi")
  {
    ROS_INFO_STREAM("Using optimal policy for " << algorithm << " from file " << argv[2]);
    return getPolicyFromCsv(argv[2]);
  }
  else if(algorithm == "q")
  {
    ROS_INFO_STREAM("Using optimal policy for " << algorithm << " from file " << argv[3]);
    return getPolicyFromCsv(argv[3]);
  }
  else if(algorithm == "sarsa")
  {
    ROS_INFO_STREAM("Using optimal policy for " << algorithm << " from file " << argv[4]);
    return getPolicyFromCsv(argv[4]);
  }
  else
  {
    ROS_ERROR_STREAM("Wrong algorithm selected"<< algorithm <<". Choices: pi, vi, q sarsa (case sensitive)");
    ros::shutdown();
  }
}

void audioGrabber(audio_emergency::AudioMessage::ConstPtr msg)
{
  lock_emerg.lock();
  if (msg->result == "talk") emergency = true;
  lock_emerg.unlock();
}

void safePauseFor(float duration)
{
  ros::Time start = ros::Time::now();
  ros::Rate loop_rate(10);
  while(ros::Time::now()-start < ros::Duration(duration))
  {
    ros::spinOnce();
    if (!isAudioSafe() || !isForceSafe())
    {
      ROS_WARN_STREAM("ABORTING SEQUENCE!!");
      fallback(true);
      ros::shutdown();
    }
    loop_rate.sleep();
  }
}

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

  tf::TransformListener tf_listener;

  ros::Subscriber sub = nh.subscribe("/force_values", 1000, forceGrabber);
  cmd_vel = nh.advertise<geometry_msgs::TwistStamped>("/RobotControl/VelocityControl", 1000);
  cmd_pos = nh.advertise<geometry_msgs::PoseStamped>("/RobotControl/PoseControl", 1000);
  ros::Subscriber tool_pose = nh.subscribe("/j2s7s300_driver/out/tool_pose",1000, poseGrabber );
  ros::Subscriber control_status = nh.subscribe("/RobotControl/Status",1000, statusGrabber );
  ros::Publisher arm_pose_pub = nh.advertise<std_msgs::Int32>("/arm_state", 1000);
  ros::Subscriber audio_emerg = nh.subscribe("/audio_emergency",1000, audioGrabber );

  waitForPoseDataAvailable();

  double local_force_f;
  bool print_once_only=true;

  ROS_INFO_STREAM("Initial rotation from " << angles::to_degrees(getCurrentRoll()));
  moveCup(RAISE_CUP, VEL_CMD_DURATION);
  ros::Duration(1.0).sleep(); // Allow inertial settlement
  ros::spinOnce();

  lock_pose.lock();
  initial_pose=current_pose;
  lock_pose.unlock();

  double r,p,y;
  getRPYFromQuaternionMSG(initial_pose.pose.orientation,r,p,y);
  lower_angle_thresh = r;

  rotation_step = (UPPER_FEED_ANGLE_THRESH - angles::to_degrees(lower_angle_thresh))/NUMBER_OF_ARM_SUB_STATES;

  std::vector<int> active_policy = selectActivePolicy(argv[5], argv);

  ROS_INFO_STREAM("Lower angle thresh : " << (int)angles::to_degrees(lower_angle_thresh) << ". Upper angle thresh : " << UPPER_FEED_ANGLE_THRESH);
  ROS_INFO_STREAM("No. of arm substates :" <<  NUMBER_OF_ARM_SUB_STATES << ". Rotation per step: " << rotation_step << " Max roll : "
                  << angles::to_degrees(lower_angle_thresh) + (rotation_step * (NUMBER_OF_ARM_SUB_STATES-1))) ;
  ROS_INFO_STREAM("Policy to be used :");
  for (int action:active_policy) std::cout << action << " ";
  std::cout << std::endl;

  step_count = 0;
  int prev_step_count = 0;
  ros::Rate loop_rate(10);

  while(ros::ok())
  {
    ros::spinOnce();

    lock_force.lock();
    local_force_f=force_f;
    lock_force.unlock();

    int state = calculateCurrentState(force_f, step_count);
    int action = active_policy[state];

    switch(action)
    {
      case ACTION_DOWN:
      if(!checkLowerAngleThreshold())
      {
        if(prev_step_count > step_count)
        {
          callFallbackTimer(3);
          break;
        }
        ROS_WARN_STREAM("LOWER THRESHOLD REACHED!!");
        break;
      }
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("Current state : "<< state << " (" <<  force_f   <<"N) " << " Selected action : " << action);
      driveToRollGoalWithVelocity(LOWER_CUP);
//      ros::Duration(1.0).sleep(); // Allow inertial settlement
      safePauseFor(1.0);
      prev_step_count = step_count--;
      ros::spinOnce(); // Update subscribed values ie: current pose
      ROS_WARN_STREAM("Step : " << prev_step_count << " -> " << step_count  << " @ roll : " << angles::to_degrees(getCurrentRoll()));
      print_once_only=true;
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");

      if ((prev_step_count > step_count &&
          std::fabs((int)angles::to_degrees(getCurrentRoll()) - (int)angles::to_degrees(lower_angle_thresh)) <=  0.75*rotation_step) ||
          (prev_step_count == 1 && step_count == 0 ))
      {
        callFallbackTimer(3);
        break;
      }

      case ACTION_STAY:
      if (print_once_only)
          {
            ROS_INFO("---------------------------------------------------------------------");
            ROS_INFO_STREAM("Current state : "<< state << " (" <<  force_f   <<"N) " << " Selected action : " << action);
            ROS_INFO_STREAM("Do nothing.");
            print_once_only=false;
            ros::spinOnce(); // Update subscribed values ie: current pose
            ROS_WARN_STREAM("Step : " << prev_step_count << " -> " << step_count  << " @ roll : " << angles::to_degrees(getCurrentRoll()));
            ROS_INFO("---------------------------------------------------------------------");
            ROS_INFO(" ");
          }
      break;

      case ACTION_UP:
      if(!checkUpperAngleThreshold())
      {
        ROS_INFO_STREAM("UPPER THRESHOLD REACHED!!");
        break;
      }
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("Current state : "<< state << " (" <<  force_f   <<"N) " << " Selected action : " << action);
      driveToRollGoalWithVelocity(RAISE_CUP);
//      ros::Duration(1.0).sleep(); // Allow inertial settlement
      safePauseFor(1.0);
      prev_step_count =  step_count++;
      ros::spinOnce(); // Update subscribed values ie: current pose
      ROS_WARN_STREAM("Step : " << prev_step_count << " -> " << step_count  << " @ roll : " << angles::to_degrees(getCurrentRoll()));
      print_once_only=true;
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");
      break;
    }

    loop_rate.sleep();
    ros::spinOnce();
    std_msgs::Int32 arm_pose_msg;
    arm_pose_msg.data=step_count;
    arm_pose_pub.publish(arm_pose_msg);
    if (!isAudioSafe())
    {
      ROS_WARN_STREAM("ABORTING SEQUENCE!!");
      fallback(true);
      ros::shutdown();
    }

  }
  ros::spinOnce();
  return 0;
}


#pragma once
// TODO: Quaternion class
#define _USE_MATH_DEFINES
#include <cmath>
#include <Eigen/Geometry>

#include <memory>

#include <LeapC.h>

#include "math/ISkeletalAdapter.hpp"

#include "driver/Settings.hpp"
#include "driver/Types.hpp"
#include "math/IVectorFactory.hpp"
#include "math/IMatrixFactory.hpp"
#include "math/Types.hpp"
#include "utility/ILogger.hpp"

namespace northstar {
    namespace math {
        class CSkeletalAdapter : public ISkeletalAdapter {
        public:
            CSkeletalAdapter(
                std::shared_ptr<IVectorFactory> pVectorFactory,
                std::shared_ptr<IMatrixFactory> pMatrixFactory,
                std::shared_ptr<northstar::utility::ILogger> pLogger);

            ~CSkeletalAdapter() = default;
            virtual void FromLeapMotionHandToOVRBonePoseArray(
                const vr::DriverPose_t& sPose,
                const types::AffineMatrix4d& m4dFromLeapSensorToHMDRelativeSpace,
                const types::AffineMatrix4d& m4dFromHMDToWorldSpace,
                const LEAP_HAND& sLeapHand,
                std::array<vr::VRBoneTransform_t, northstar::driver::settings::values::driverConfiguration::k_unBoneCount>& saOpenVRBoneTargets,
                const northstar::driver::types::EHand& eHand) const override final;
        private:
            enum class EBoneType {
                Digit,
                Thumb,
            };

            enum class ESegmentClassification {
                Metacarpal,
                Proximal,
                Intermediate,
                Distal,
                Tip,
                Auxilary,
            };

            void WriteConvertedDigitToTargetOVRBoneIndicies(
                const types::AffineMatrix4d& m4dFromWorldToRootBoneRelativeSpace,
                const types::AffineMatrix4d& m4dFromWorldToWristBoneRelativeSpace,
                const types::AffineMatrix4d& m4dFromLeapSensorToHMDRelativeSpace,
                const types::AffineMatrix4d& m4dFromHMDToWorldSpace,
                const LEAP_DIGIT& sLeapDigit,
                const std::array<northstar::driver::types::EHandeSkeletonBone, 5>& aeDestinationIndicies,
                const northstar::driver::types::EHandeSkeletonBone& eAuxBoneIndex,
                std::array<vr::VRBoneTransform_t, northstar::driver::settings::values::driverConfiguration::k_unBoneCount>& saOpenVRBoneTargets,
                const EBoneType& eBoneType,
                const northstar::driver::types::EHand& eHand) const;

            types::AffineMatrix4d northstar::math::CSkeletalAdapter::ConvertAndWriteBoneToOVRBoneReturningNewWorldToRelativeTransform(
                const types::AffineMatrix4d& m4dFromWorldSpaceToParentRelativeSpace,
                const types::AffineMatrix4d& m4dFromLeapSensorToHMDRelativeSpace,
                const types::AffineMatrix4d& m4dFromHMDToWorldSpace,
                const LEAP_VECTOR& sLeapPosition,
                const LEAP_QUATERNION& sLeapOrientation,
                vr::VRBoneTransform_t& sVRBoneTransform,
                const EBoneType& eBoneType,
                const ESegmentClassification& eSegmentClassification,
                const northstar::driver::types::EHand& eHand) const;

            types::Vector4d GetTranslationSensitiveV4DFromLeapVectorPosition(const LEAP_VECTOR& sPosition) const;
            types::Quaterniond GetQuaterniondFromLeapQuaternionOrientation(const LEAP_QUATERNION& sOrientation) const;
            types::Vector4d ConvertLeapDigitPositionToOpenVRDigitPosition(
                const types::Vector4d& qdLeapDigitPosition,
                const types::Quaterniond& qdLeapDigitOrientation,
                const EBoneType& eBoneType,
                const ESegmentClassification& eSegmentClassification,
                const northstar::driver::types::EHand& eHand) const;

            types::Quaterniond ConvertLeapDigitOrientationToOpenVRDigitOrientation(
                const types::Quaterniond& qdLeapDigitOrientation,
                const EBoneType& eBoneType,
                const ESegmentClassification& eSegmentClassification,
                const northstar::driver::types::EHand& eHand) const;

            void WritePositionAndOrientationIntoVRBonePose(vr::VRBoneTransform_t& sVRBoneTransform, const types::Vector4d& v4dPosition, const types::Quaterniond& qdOrientation) const;

            std::shared_ptr<IVectorFactory> m_pVectorFactory;
            std::shared_ptr<IMatrixFactory> m_pMatrixFactory;
            std::shared_ptr<utility::ILogger> m_pLogger;
        };
    }
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This example shows how to run a CNN based vehicle detector using dlib.  The
    example loads a pretrained model and uses it to find the front and rear ends
    of cars in an image.  The model used by this example was trained by the
    dnn_mmod_train_find_cars_ex.cpp example program on this dataset:
        http://dlib.net/files/data/dlib_front_and_rear_vehicles_v1.tar

    Users who are just learning about dlib's deep learning API should read
    the dnn_introduction_ex.cpp and dnn_introduction2_ex.cpp examples to learn
    how the API works.  For an introduction to the object detection method you
    should read dnn_mmod_ex.cpp.

    You can also see a video of this vehicle detector running on YouTube:
        https://www.youtube.com/watch?v=OHbJ7HhbG74
*/

#include <iostream>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <dlib/gui_widgets.h>
#include <dlib/image_processing.h>

using namespace std;
using namespace dlib;

// The front and rear view vehicle detector network
template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = con<num_filters,5,5,1,1,SUBNET>;
template <typename SUBNET> using downsampler  = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<16,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = relu<affine<con5<55,SUBNET>>>;
using net_type = loss_mmod<con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

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

int main() try
{
    net_type net;
    shape_predictor sp;
    // You can get this file from http://dlib.net/files/mmod_front_and_rear_end_vehicle_detector.dat.bz2
    // This network was produced by the dnn_mmod_train_find_cars_ex.cpp example program.
    // As you can see, the file also includes a separately trained shape_predictor.  To see
    // a generic example of how to train those refer to train_shape_predictor_ex.cpp.
    deserialize("mmod_front_and_rear_end_vehicle_detector.dat") >> net >> sp;

    matrix<rgb_pixel> img;
    load_image(img, "../mmod_cars_test_image2.jpg");

    image_window win;
    win.set_image(img);

    // Run the detector on the image and show us the output.
    for (auto&& d : net(img))
    {
        // We use a shape_predictor to refine the exact shape and location of the detection
        // box.  This shape_predictor is trained to simply output the 4 corner points of
        // the box.  So all we do is make a rectangle that tightly contains those 4 points
        // and that rectangle is our refined detection position.
        auto fd = sp(img,d);
        rectangle rect;
        for (unsigned long j = 0; j < fd.num_parts(); ++j)
            rect += fd.part(j);

        if (d.label == "rear")
            win.add_overlay(rect, rgb_pixel(255,0,0), d.label);
        else
            win.add_overlay(rect, rgb_pixel(255,255,0), d.label);
    }

    cout << "Hit enter to end program" << endl;
    cin.get();
}
catch(image_load_error& e)
{
    cout << e.what() << endl;
    cout << "The test image is located in the examples folder.  So you should run this program from a sub folder so that the relative path is correct." << endl;
}
catch(serialization_error& e)
{
    cout << e.what() << endl;
    cout << "The correct model file can be obtained from: http://dlib.net/files/mmod_front_and_rear_end_vehicle_detector.dat.bz2" << endl;
}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Sebastian Schlenkrich
*/

/*! \file bondoptionengine.cpp
    \brief engine for bond options with Hull White model
*/

#include <time.h>
#include <boost/lexical_cast.hpp>

#include <ql/settings.hpp>
#include <ql/experimental/templatemodels/hullwhite/fixedratebondoption.hpp>
#include <ql/experimental/templatemodels/hullwhite/bondoptionengine.hpp>
#include <ql/cashflows/coupon.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/cashflows/simplecashflow.hpp>

namespace QuantLib {

    // calibrate model based on given swaptions
    void BondOptionEngine::calibrateModel( std::vector< ext::shared_ptr<Swaption> >          swaptions,
                                           const bool                                        contTenorSpread,
                                           const Real                                        tolVola) {
        std::sort(swaptions.begin(), swaptions.end(), lessByExerciseFirstDate);
        // set up inputs for model calibration
        std::vector<Real>                  exercTimes, strikeVals, b76Prices;
        std::vector< std::vector<Real> >   startTimes, payTimes, cashFlows;
        std::vector<Option::Type>          callOrPut;
        // get discount curve and conventions from model
        Handle<YieldTermStructure> discCurve = model_->termStructure();
        DayCounter                 dc        = model_->termStructure()->dayCounter();
        Date                       today     = model_->termStructure()->referenceDate();
        // iterate swaptions...
        referenceSwaptions_.clear();
        for (Size k=0; k<swaptions.size(); ++k) {
            // skip swaptions with equal exercise date since we calibrate by bootstrapping
            if ( (k>0) && (swaptions[k]->exercise()->date(0)==swaptions[k-1]->exercise()->date(0)) ) continue;
            // save reference swaption
            referenceSwaptions_.push_back(swaptions[k]);
            // build an equivalent bond option
            FixedRateBondOption bondOption(swaptions[k], discCurve, contTenorSpread);
            // extract option details
            exercTimes.push_back(dc.yearFraction(today,bondOption.exerciseDates()[0]));
            strikeVals.push_back(bondOption.dirtyStrikeValues()[0]);
            b76Prices.push_back(swaptions[k]->NPV());  // assume we have an engine and market data attached
            callOrPut.push_back(bondOption.callOrPut());
            // extract underlying details
            std::vector<Real> bondStartTimes, bondPayTimes;
            for (Size i=0; i<bondOption.cashflowValues().size(); ++i) {
                bondStartTimes.push_back( dc.yearFraction(today,bondOption.startDates()[i]) );
                bondPayTimes.push_back( dc.yearFraction(today,bondOption.payDates()[i]) );
            }
            startTimes.push_back(bondStartTimes);
            payTimes.push_back(bondPayTimes);
            cashFlows.push_back(bondOption.cashflowValues());
        }
        // calibrate Hull White model
        model_->BermudanCalibration(exercTimes, strikeVals, b76Prices, startTimes, payTimes, cashFlows, callOrPut, tolVola);
    }

    void BondOptionEngine::calculate() const {
        std::vector<Time> startTimes;
        std::vector<Time> payTimes;
        std::vector<Real> cashFlowValues;
        std::vector<Time> exerciseTimes;
        std::vector<Real> strikeValues;
        DayCounter dayCounter = model_->termStructure()->dayCounter();
        Date today = model_->termStructure()->referenceDate();
        Date startDate;
        ext::shared_ptr<Coupon> coupon;
        // set up cash flows
        for (Size i=0; i<arguments_.cashflows.size(); ++i) {
            coupon = ext::dynamic_pointer_cast<Coupon>(arguments_.cashflows[i]);
            if (coupon) { // cast is ok
                startDate = coupon->accrualStartDate();
            }
            else { // cash flow is no coupon, assume redemption payment, startDate = payDate
                startDate = arguments_.cashflows[i]->date();
            }
            if (startDate>today) { // consider only coupons with startDate later than today
                startTimes.push_back(dayCounter.yearFraction(today,startDate));
                payTimes.push_back(dayCounter.yearFraction(today,arguments_.cashflows[i]->date()));
                cashFlowValues.push_back(arguments_.cashflows[i]->amount());
            }
        }
        // set up exercises
        Size Nexc = std::min(arguments_.exerciseDates.size(), arguments_.dirtyStrikeValues.size());
        for (Size i=0; i<Nexc; ++i) {
            if (arguments_.exerciseDates[i]>today) { // consider only exercises later than today
                exerciseTimes.push_back(dayCounter.yearFraction(today,arguments_.exerciseDates[i]));
                strikeValues.push_back(arguments_.dirtyStrikeValues[i]);
            }
        }
        // do not calibrate in pricing engine unless there is a calibrator

        // evaluate Bermudan bond option
        clock_t start_clock = clock();
        results_.value = model_->BermudanBondOption(exerciseTimes,strikeValues,
            startTimes,payTimes,cashFlowValues,arguments_.callOrPut,dimension_,gridRadius_,bermudanTolerance_);
        clock_t end_clock = clock();
        // report additional results here ...
        results_.additionalResults["runtime"] = (Real)(end_clock - start_clock)/CLOCKS_PER_SEC;
        std::vector<Real> europeansAnalytical = model_->europeansAnalytical();
        std::vector<Real> europeansNumerical  = model_->europeansNumerical();
        // (absolute) error is estimated by corresponding European prices
        Real errorEstimate = 0.0;
        for (Size i=0; i<std::min(europeansAnalytical.size(),europeansNumerical.size()); ++i) {
            errorEstimate = std::max(errorEstimate,fabs(europeansNumerical[i]-europeansAnalytical[i]));
        }
        results_.errorEstimate = errorEstimate;
        // report adittional results for
        for (Size i=0; i<referenceSwaptions_.size(); ++i) {
            std::string name = "black76price_";
            name += boost::lexical_cast<std::string>( i+1 );
            results_.additionalResults[name] = referenceSwaptions_[i]->NPV();
        }
        for (Size i=0; i<referenceSwaptions_.size(); ++i) {
            std::string name = "black76vola_";
            name += boost::lexical_cast<std::string>( i+1 );
            std::map<std::string,boost::any>::const_iterator stDev = referenceSwaptions_[i]->additionalResults().find("stdDev");
            if (stDev!=referenceSwaptions_[i]->additionalResults().end()) {
                results_.additionalResults[name] =
                    boost::any_cast<Real>(stDev->second) / std::sqrt(model_->termStructure()->dayCounter().yearFraction(today,referenceSwaptions_[i]->exercise()->dates()[0]));
            }
        }
        for (Size i=0; i<referenceSwaptions_.size(); ++i) {
            std::string name = "black76vega_";
            name += boost::lexical_cast<std::string>( i+1 );
            std::map<std::string,boost::any>::const_iterator vega = referenceSwaptions_[i]->additionalResults().find("vega");
            if (vega!=referenceSwaptions_[i]->additionalResults().end()) {
                results_.additionalResults[name] = 	boost::any_cast<Real>(vega->second);
            }
        }
        // if we have an AD-enabeled model report vega(s) here...
        ext::shared_ptr<MinimADHullWhiteModel> amodel = ext::dynamic_pointer_cast<MinimADHullWhiteModel>(model_);
        if (amodel) {
            // derivative of Bermudan price w.r.t. short rate vola
            std::vector<QuantLib::Real> vegas = amodel->bermudanVega();
            // differentiate calibration; short rate vola w.r.t. B76 prices
            for (Size j=vegas.size(); j>0; --j) {
                for (Size i=j; i<vegas.size(); ++i) vegas[j-1] -= vegas[i]*amodel->calibrationJacobian()[i][j-1];
                vegas[j-1] /= amodel->calibrationJacobian()[j-1][j-1];
            }
            // finally differentiate reference prices w.r.t. Black'76 volas
            for (Size i=0; i<std::min(vegas.size(),referenceSwaptions_.size()); ++i) {
                std::map<std::string,boost::any>::const_iterator vega = referenceSwaptions_[i]->additionalResults().find("vega");
                if (vega!=referenceSwaptions_[i]->additionalResults().end()) {
                    vegas[i] *= boost::any_cast<Real>(vega->second);
                }
            }
            // the sum of vegas represents the sensitivity w.r.t. to a parallel shift of the B76 vola surface
            QuantLib::Real vega=0.0;
            for (Size i=0; i<vegas.size(); ++i) vega += vegas[i];
            // store Bermudan and reference European vega
            results_.additionalResults["vega"] = vega;
            results_.additionalResults["vegas_size"] = (1.0*vegas.size());
            for (Size i=0; i<vegas.size(); ++i) {
                std::string name = "vegas_";
                name += boost::lexical_cast<std::string>( i+1 );
                results_.additionalResults[name] = vegas[i];
            }
        }
    }

}


#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <string>
#include <algorithm>
#include <iterator>
#include <numeric>
#include <ext/numeric>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

#include <stdlib.h>
#include <math.h>
#include <assert.h>

#include "mpi_kmeans.h"

namespace po = boost::program_options;

static void write_assignment(const std::string& output_filename,
								  const std::vector<int> labels) {

	std::cout << "Writing cluster centers to \""
			  << output_filename << "\"" << std::endl;

	std::ofstream wout(output_filename.c_str());
	if (wout.fail()) {
		std::cerr << "Failed to open \"" << output_filename
				  << "\" for writing." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	wout << std::setprecision(1);
	for (unsigned int m=0; m < labels.size(); ++m) {
		wout << labels[m] << std::endl;
	}
	wout.close();

	return;
}

static int read_problem_data(const std::string& filename,
							  std::vector<std::vector<double> >& data) {
	data.clear();

	std::ifstream in(filename.c_str());
	if (in.fail()) {
		std::cerr << "Failed to open file \""
				  << filename << "\" for reading." << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	unsigned int ndims = 0;
	while (in.eof() == false) {
		std::getline(in,line);
		if (line.size() == 0)
			continue; // skip over empty lines

		// remove trailing whitespaces
		line.erase(line.find_last_not_of(" ")+1);

		std::vector<double> current_data;
		std::istringstream is(line);
		while (is.eof() == false) {
			double value;
			is >> value;
			current_data.push_back(value);
		}

		// Ensure the same number of dimensions for each point
		if (ndims == 0)
			ndims = current_data.size();
		assert(ndims == current_data.size());
		data.push_back(current_data);
	}
	in.close();

	if (data.size() == 0) {
		std::cerr << "No points read from file \"" << filename
				  << "\"" << std::endl;
		std::cerr << "Try mpi_assign --help" << std::endl;
		exit(EXIT_FAILURE);
	}

	return(data.size());

}

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

	std::string data_filename;
	std::string cluster_filename;
	std::string assignment_filename;

	// Set Program options
	po::options_description generic("Generic Options");
	generic.add_options()
		("help","Produce help message")
		("verbose","Verbose output")
		;

	po::options_description input_options("Input/Output Options");
	input_options.add_options()
		("data",po::value<std::string>
		 (&data_filename)->default_value("data.txt"),
		 "Data file, one datum per line")
		("cluster",po::value<std::string>
		 (&cluster_filename)->default_value("clustercenter.txt"),
		 "Output file, one cluster center per line")
		("assignment",po::value<std::string>
		 (&assignment_filename)->default_value("assignment.txt"),
		 "Output file, one cluster center per line")
		;

	po::options_description all_options;
	all_options.add(generic).add(input_options);
	po::variables_map vm;
	po::store(po::command_line_parser(argc,argv).options(all_options).run(), vm);
	po::notify(vm);

	bool verbose = vm.count("verbose");

	if (vm.count("help")) {
		std::cerr << "Assigning points to cluster center" << std::endl;
		std::cerr << all_options << std::endl;
		std::cerr << std::endl;
		std::cerr << "Example:" << std::endl;
		std::cerr << "  mpi_assign --data example.txt --cluster clusters.txt --assignment assignment.txt" << std::endl;
		exit(EXIT_SUCCESS);
	}

	// read in the data
	std::cout << "Input file: " << data_filename << std::endl;
	std::vector<std::vector<double> > data_X;
	int nof_points = read_problem_data(data_filename,data_X);
	assert(nof_points>0);

	// read in the clusters
	std::cout << "Clustercenter file: " << cluster_filename << std::endl;
	std::vector<std::vector<double> > data_CX;
	int nof_clusters = read_problem_data(cluster_filename,data_CX);
	assert(nof_clusters>0);

	unsigned int dims = data_X[0].size();
	assert(dims>0);
	if (data_X[0].size() != data_CX[0].size()) {
		std::cerr << "Dimension mismatch between points and clusters" << std::endl;
		exit(EXIT_FAILURE);
	}

	// convert clusters to double*
	double *CX = (double *)malloc(nof_clusters * dims * sizeof(double));
	unsigned int cntr = 0;
	for (unsigned int m=0; m < data_CX.size() ; ++m) {
		for (unsigned int n=0; n < data_CX[m].size() ; ++n) {
			CX[cntr] = data_CX[m][n];
			cntr += 1;
		}
	}

	// start K-Means
	std::cout << "Starting Assignment ..." << std::endl;
	std::cout << " ... with " << nof_points << " training points " <<std::endl;
	std::cout << " ... for " << nof_clusters << " clusters " <<std::endl;
	std::cout << " ... in " << dims << " dimensions " <<std::endl;

	std::vector<int> labels;
	double *x = (double *)malloc(dims * sizeof(double));
	for (unsigned int i=0; i < nof_points ; i++ ) {
		for (unsigned int n=0; n<data_X[i].size() ; ++n)
			x[n] = data_X[i][n];
		labels.push_back(1+assign_point_to_cluster_ordinary(x,CX,dims,nof_clusters));
	}

	std::cout << "Done!" << std::endl;

	// write the clusters
	write_assignment(assignment_filename,labels);

	// done
	exit(EXIT_SUCCESS);
}


#include <boost/test/unit_test.hpp>

#include <scorum/chain/dba/db_accessor.hpp>
#include <scorum/chain/schema/betting_property_object.hpp>
#include <scorum/chain/schema/game_object.hpp>

#include <scorum/protocol/betting/market_kind.hpp>

#include "defines.hpp"
#include "detail.hpp"
#include "database_betting_integration.hpp"
#include "actor.hpp"

namespace bet_operations_tests {

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

struct bet_operations_fixture : public database_fixture::database_betting_integration_fixture
{
    bet_operations_fixture()
    {
        open_database();

        alice.scorum(ASSET_SCR(1e+9));
        actor(initdelegate).create_account(alice);
        actor(initdelegate).give_sp(alice, 1e+9);
        actor(initdelegate).give_scr(alice, alice.scr_amount.amount.value);

        bob.scorum(ASSET_SCR(1e+9));
        actor(initdelegate).create_account(bob);
        actor(initdelegate).give_sp(bob, 1e+9);
        actor(initdelegate).give_scr(bob, bob.scr_amount.amount.value);

        actor(initdelegate).create_account(moderator);
        actor(initdelegate).give_sp(moderator, 1e+9);
        actor(initdelegate).give_scr(moderator, 1e+9);

        empower_moderator(moderator);

        BOOST_REQUIRE_EQUAL(this->betting_moderator(), moderator.name);

        create_game(moderator, { result_home{}, total{ 2000 } }, SCORUM_BLOCK_INTERVAL * 60 * 20);

        generate_block();

        BOOST_REQUIRE_EQUAL(1u, db.get_index<game_index>().indices().size());

        const auto& game = db.get(game_object::id_type(0));

        BOOST_REQUIRE_EQUAL(2u, game.markets.size());
    }

    Actor alice = "alice";
    Actor bob = "bob";
    Actor moderator = "smit";
};

BOOST_FIXTURE_TEST_SUITE(create_and_cancel_bet_tests, bet_operations_fixture)

SCORUM_TEST_CASE(post_bet_operation_check)
{
    generate_block();

    create_bet(gen_uuid("b1"), alice, result_home::yes{}, { 10, 2 }, alice.scr_amount / 2);

    generate_block();

    create_bet(gen_uuid("b2"), bob, result_home::yes{}, { 10, 8 }, bob.scr_amount / 2);

    generate_block();
}

SCORUM_TEST_CASE(cancel_single_pending_bet_by_better_operation_check)
{
    generate_block();

    create_bet(gen_uuid("b1"), alice, result_home::yes{}, { 10, 2 }, alice.scr_amount / 2);

    generate_block();

    cancel_pending_bet(alice, { gen_uuid("b1") });

    generate_block();
}

SCORUM_TEST_CASE(cancel_some_pending_bets_by_better_operation_check)
{
    generate_block();

    create_bet(gen_uuid("b1"), alice, total::under{ 2000 }, { 10, 2 }, alice.scr_amount / 2);

    generate_block();

    create_bet(gen_uuid("b2"), alice, result_home::yes(), { 10, 5 }, alice.scr_amount / 2);

    generate_block();

    create_bet(gen_uuid("b3"), bob, total::over{ 2000 }, { 10, 8 }, bob.scr_amount / 2);

    generate_block();

    cancel_pending_bet(alice, { gen_uuid("b1"), gen_uuid("b2") });

    generate_block();
}

SCORUM_TEST_CASE(throw_on_bet_with_existing_uuid)
{
    generate_block();

    create_bet(gen_uuid("b1"), alice, result_home::yes{}, { 10, 2 }, alice.scr_amount / 2);

    generate_block();

    BOOST_REQUIRE_THROW(create_bet(gen_uuid("b1"), bob, result_home::yes{}, { 10, 8 }, bob.scr_amount / 2),
                        fc::assert_exception);
}

BOOST_AUTO_TEST_SUITE_END()
}


/*************************************************************************
 * SceneML, Copyright (C) 2007, 2008  J.D. Yamokoski
 * All rights reserved.
 * Email: yamokosk at gmail dot com
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation; either version 2.1 of the License,
 * or (at your option) any later version. The text of the GNU Lesser General
 * Public License is included with this library in the file LICENSE.TXT.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the file LICENSE.TXT for
 * more details.
 *
 *************************************************************************/
/*
 * DistanceQuery.cpp
 *
 *  Created on: Dec 5, 2008
 *      Author: yamokosk
 */

#include "DistanceQuery.h"
#include <api/Services.h>
#include <boost/any.hpp>

#include "algorithm/BodyManager.h"

#include <linalg/Vector3.h>

using namespace obrsp::linalg;
using namespace tinysg;

const std::string DistanceQuery::Type("LCDistanceQuery");

void* DistanceQuery::create(plugin::PF_ObjectParams* params)
{
	DistanceQuery* ptr = new DistanceQuery();
	ptr->services = params->platformServices;
	return ptr;
}

int DistanceQuery::destroy(void *p)
{
	if (!p) return -1;

	delete static_cast<DistanceQuery*>(p);

	return 0;
}

DistanceQuery::DistanceQuery()
{

}

DistanceQuery::~DistanceQuery()
{
}

void DistanceQuery::init()
{
	BodyManager::getInstance().init();
}

void DistanceQuery::getInfo(tinysg::QueryInfo* i)
{
}

void DistanceQuery::execute(tinysg::QueryArguments* args)
{
	LOG_MESSAGE(services, "In DistanceQuery::execute()");

	BodyManager& bm = BodyManager::getInstance();

	if ( !bm.computeDistances() )
	{
		LOG_ERROR(services, "Distance query failed for unknown reason.");
		return;
	}

	// Get distance data
	VERTEX3D** critPoints = bm.getCriticalPointArray();
	VERTEX3D** regPoints = bm.getPointArray();
	double** distances = bm.getDistancesArray();
	VERTEX3D pnt;

	std::stringstream ss;
	ss << "Num crit bodies: " << bm.getCriticalBodyCount() << ", Num reg bodies: " << bm.getBodyCount() << std::endl;

	LOG_MESSAGE(services, ss.str());
	for (int ii = 0; ii < bm.getCriticalBodyCount(); ++ii)
	{
		for (int jj = 0; jj < bm.getBodyCount(); ++jj)
		{
			pnt = critPoints[ ii ][ jj ];
			//std::cout << "c: " << critPoints[ ii ][ jj ].x << ", " << critPoints[ ii ][ jj ].y << ", " << critPoints[ ii ][ jj ].z << std::endl;
			//std::cout << "c: " << pnt.x << ", " << pnt.y << ", " << pnt.z << std::endl;
			Vector3 p( (Real)pnt.x, (Real)pnt.y, (Real)pnt.z );

			pnt = regPoints[ ii ][ jj ];
			//std::cout << "r: " << regPoints[ ii ][ jj ].x << ", " << regPoints[ ii ][ jj ].y << ", " << regPoints[ ii ][ jj ].z << std::endl;
			//std::cout << "r: " << pnt.x << ", " << pnt.y << ", " << pnt.z << std::endl;
			Vector3 q( (Real)pnt.x, (Real)pnt.y, (Real)pnt.z );

			float dist = (float)distances[ii][jj];

			args->critpnt.push_back( Point3D(p[0], p[1], p[2]) );
			args->regpnt.push_back( Point3D(q[0], q[1], q[2]) );
			args->distanceMap.push_back(dist);
		}
	}
	LOG_MESSAGE(services, "Leaving DistanceQuery::execute()");
}


#ifndef STOKES_SOLVER_HPP
#define STOKES_SOLVER_HPP

#include "mfem.hpp"
using namespace mfem;

#include <Eigen/Core>

#include "../core/config.hpp"
#include "../mymfem/utilities.hpp"

#include "test_cases.hpp"
#include "coefficients.hpp"
#include "discretisation.hpp"
#include "observer.hpp"

class StokesSolver
{
public:
    StokesSolver (const nlohmann::json& config,
                  std::string mesh_dir,
                  const int lx);

    ~ StokesSolver ();

    std::pair<double, Eigen::VectorXd> operator()(void);

    void init ();

    void solve (BlockVector *U, BlockVector* B) const;

    void set_linear_solver() const;

    void set_preconditioner () const;

private:
    const nlohmann::json& m_config;

    std::string m_mesh_format;
    std::string m_mesh_elem_type;

    std::shared_ptr<Mesh> m_mesh;
    std::unique_ptr<StokesFEM> m_discr;
    std::unique_ptr<StokesObserver> m_observer;

    bool m_bool_mean_free_pressure = true;
    bool m_bool_error;

    Array<int> m_block_offsets;
    mutable Solver *m_solver = nullptr;

    mutable BlockOperator *m_stokesOp = nullptr;
    mutable BlockDiagonalPreconditioner *m_stokesPr = nullptr;
    mutable SparseMatrix *m_stokesMat = nullptr;
};

#endif /// STOKES_SOLVER_HPP


#include <boost/geometry/strategies/cartesian/centroid_average.hpp>


#include <iostream>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>

using namespace std;
using namespace boost::posix_time;
using namespace boost::gregorian;

int main(){
    ptime p(date(2014,1,1),hours(12));
    time_period tp1(p,hours(8));
    time_period tp2(p + hours(8),hours(1));

    if(tp1.end() == tp2.begin() && tp1.is_adjacent(tp2))
        cout << "Okay" << endl;

    tp1.shift(hours(1));
    if(tp1.is_after(p))
        cout << "tp1 is after tp2 " << endl;

    // P 47
    return 0;
}


#include "stdafx.h"
#include "qmath.h"
#include "config.h"

// each test module could contain no more then one 'main' file with init function defined
// alternatively you could define init function yourself
#include <boost/test/unit_test.hpp>

using namespace math;

namespace
{

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB)
{
	//test default constructor
	aabb3f my_box;
	vec3f minus_one(-1.0f, -1.0f, -1.0f);
	vec3f one(1.0f, 1.0f, 1.0f);

	BOOST_CHECK(my_box.minPoint == minus_one && my_box.maxPoint == one);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_MIN_MAX)
{
	//test 2 vec constructor
	vec3f min(1.0f, 2.0f, 3.0f);
	vec3f max(4.0f, 5.0f, 6.0f);

	aabb3f box(min, max);

	BOOST_CHECK(
			box.minPoint == min &&
			box.maxPoint == max
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_MIN)
{
	//test 1 vec constructor
	vec3f min(1.0f, 2.0f, 3.0f);

	aabb3f box(min);

	BOOST_CHECK(
			box.minPoint == min &&
			box.maxPoint == min
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_RESET_MIN)
{
	//test reset
	aabb3f box;

	box.reset(1.0f, 2.0f, 3.0f);

	BOOST_CHECK(
			box.minPoint.x == 1.0f &&
			box.minPoint.y == 2.0f &&
			box.minPoint.z == 3.0f &&
			box.maxPoint.x == 1.0f &&
			box.maxPoint.y == 2.0f &&
			box.maxPoint.z == 3.0f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_RESET_BOX)
{
	//test reset
	aabb3f initial_box(1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
	aabb3f box;

	box.reset(initial_box);

	BOOST_CHECK(
			box.minPoint.x == 1.0f &&
			box.minPoint.y == 2.0f &&
			box.minPoint.z == 3.0f &&
			box.maxPoint.x == 1.0f &&
			box.maxPoint.y == 2.0f &&
			box.maxPoint.z == 3.0f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_RESET_MIN_MAX)
{
	//test reset
	vec3f min(1.0f, 2.0f, 3.0f);
	vec3f max(4.0f, 5.0f, 6.0f);

	aabb3f box;

	box.reset(min, max);

	BOOST_CHECK(
			box.minPoint.x == 1.0f &&
			box.minPoint.y == 2.0f &&
			box.minPoint.z == 3.0f &&
			box.maxPoint.x == 4.0f &&
			box.maxPoint.y == 5.0f &&
			box.maxPoint.z == 6.0f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_RESET_VEC)
{
	//test reset
	vec3f min(1.0, 2.0, 3.0);
	aabb3f box;

	box.reset(min);

	BOOST_CHECK(
			box.minPoint.x == 1.0f &&
			box.minPoint.y == 2.0f &&
			box.minPoint.z == 3.0f &&
			box.maxPoint.x == 1.0f &&
			box.maxPoint.y == 2.0f &&
			box.maxPoint.z == 3.0f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_EXTENT)
{
	//test extent
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	vec3f ext = box.get_extent();
	BOOST_CHECK(
			ext.x == 3.0f &&
			ext.y == 3.0f &&
			ext.z == 3.0f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_RADIUS)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	float rad = box.get_radius();
	BOOST_CHECK(
			math::equals(rad,2.5980761f)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_RADIUS_FAST)
{

	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	float rad = box.get_radius_fast();
	BOOST_CHECK(
			math::equals(rad, 2.5980761f, 0.001f)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_RADIUS_SQ)
{

	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	float rad = box.get_radius_sq();
	BOOST_CHECK(
			rad == 6.75f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_INNER_RADIUS)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	float rad = box.get_inner_radius();
	BOOST_CHECK(
			 rad == 1.5f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_INNER_RADIUS_SQ)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	float rad = box.get_inner_radius_sq();
	BOOST_CHECK(
			rad == 2.25f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_GET_EDGES)
{

	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	vec3f edges[8];
	box.get_edges(edges);

	vec3f e0(1.0, 2.0, 3.0);
	vec3f e1(1.0, 5.0, 3.0);
	vec3f e2(1.0, 2.0, 6.0);
	vec3f e3(1.0, 5.0, 6.0);
	vec3f e4(4.0, 2.0, 3.0);
	vec3f e5(4.0, 5.0, 3.0);
	vec3f e6(4.0, 2.0, 6.0);
	vec3f e7(4.0, 5.0, 6.0);

	BOOST_CHECK(
			edges[0] == e0 &&
			edges[1] == e1 &&
			edges[2] == e2 &&
			edges[3] == e3 &&
			edges[4] == e4 &&
			edges[5] == e5 &&
			edges[6] == e6 &&
			edges[7] == e7
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_IS_EMPTY)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(1.0, 2.0, 3.0));
	bool empty = box.is_empty();
	BOOST_CHECK(
			empty
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_REPAIR)
{
	vec3f min(4.0, 5.0, 6.0);
	vec3f max(1.0, 2.0, 3.0);
	aabb3f box(min, max);
	box.repair();
	BOOST_CHECK(
			box.minPoint == max &&
			box.maxPoint == min
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_GET_INTERPOLATED)
{
	aabb3f box1(vec3f(-4.0, -5.0, -6.0), vec3f(-1.0, -2.0, -3.0));
	aabb3f box2(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	aabb3f box = box1.get_interpolaed(box2, 0.5f);
	vec3f middle(1.5, 1.5, 1.5);

	BOOST_CHECK(
			box.minPoint == -middle &&
			box.maxPoint == middle
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_GET_CENTER)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	vec3f center = box.get_center();
	BOOST_CHECK(
			center.x == 2.5f &&
			center.y == 3.5f &&
			center.z == 4.5f
		);
}
///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_GET_VOLUME)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	float volume = box.get_volume();
	BOOST_CHECK(
			volume == 27.0f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_GET_AREA)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	float area = box.get_area();
	BOOST_CHECK(
			area == 54.0f
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_IS_POINT_INSIDE)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	vec3f center(2.0, 3.0, 4.0);

	BOOST_CHECK(
			box.is_point_inside(center)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_IS_POINT_TOTAL_INSIDE)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	vec3f center(2.0, 3.0, 4.0);
	BOOST_CHECK(
			box.is_point_totally_inside(center)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_IS_FULL_INSIDE)
{
	aabb3f box(vec3f(1.0, 1.0, 1.0), vec3f(4.0, 4.0, 4.0));
	aabb3f box2(vec3f(2.0, 2.0, 2.0), vec3f(3.0, 3.0, 3.0));
	BOOST_CHECK(
			box2.is_fully_inside(box)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_ADD_INTERNAL_POINT_VEC)
{
	vec3f zero(0.0 ,0.0 ,0.0);
	vec3f min(1.0, 2.0, 3.0);
	vec3f max(4.0, 5.0, 6.0);

	aabb3f box(min, max);

	box.add_internal_point(zero);

	BOOST_CHECK(
			box.minPoint == zero &&
			box.maxPoint == max
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_ADD_INTERNAL_POINT)
{
	vec3f zero(0.0 ,0.0 ,0.0);
	vec3f min(1.0, 2.0, 3.0);
	vec3f max(4.0, 5.0, 6.0);

	aabb3f box(min, max);

	box.add_internal_point(0.0 ,0.0 ,0.0);

	BOOST_CHECK(
			box.minPoint == zero &&
			box.maxPoint == max
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_ADD_INTERNAL_BOX)
{
	vec3f min(1.0, 2.0, 3.0);
	vec3f max(4.0, 5.0, 6.0);

	vec3f min2(0.0, 0.0, 0.0);
	vec3f max2(2.0, 3.0, 4.0);

	aabb3f box(min, max);
	aabb3f box2(min2, max2);

	box.add_internal_box(box2);

	BOOST_CHECK(
			box.minPoint == min2 &&
			box.maxPoint == max
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_INTERSECTS_WITH_BOX)
{
	vec3f min(1.0, 2.0, 3.0);
	vec3f max(4.0, 5.0, 6.0);

	vec3f min2(0.0, 0.0, 0.0);
	vec3f max2(2.0, 3.0, 4.0);

	aabb3f box(min, max);
	aabb3f box2(min2, max2);

	BOOST_CHECK(
			box.intersect_with_box(box2)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_INTERSECTS_WITH_LINE_VEC)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));

	vec3f middle(3, 3, 3);
	vec3f vect(1.0, 0.0, 0.0);

	BOOST_CHECK(
			box.intersect_with_line(middle, vect, 2.5)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_INTERSECTS_WITH_LINE)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	line3f line(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	BOOST_CHECK(
			box.intersect_with_line(line)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_INTERSECTS_WITH_RAY)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	line3f line(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	vec3f outNear, outFar;
	BOOST_CHECK(
			box.intersect_with_ray(line, outNear, outFar)
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_OPERATOR_EQUALS)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	aabb3f box2(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	BOOST_CHECK(
			box == box2
		);
}

///////////////////////////////////////////////////////////////////////////////

BOOST_AUTO_TEST_CASE(AABB_OPERATOR_NOT_EQUAL)
{
	aabb3f box(vec3f(1.0, 2.0, 3.0), vec3f(4.0, 5.0, 6.0));
	aabb3f box2(vec3f(1.0, 2.0, 4.0), vec3f(4.0, 5.0, 6.0));
	BOOST_CHECK(
			box != box2
		);
}

///////////////////////////////////////////////////////////////////////////////
}


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE restricted_growth_sequence

#include <iostream>
#include <list>
#include <map>

#include <boost/format.hpp>
#define timer timer_class
#include <boost/test/unit_test.hpp>
#undef timer

#include <core/utils/benchmark_table.hpp>
#include <core/utils/timer.hpp>

#include <reversible/rcbdd.hpp>

void restricted_growth_sequence( unsigned p, double& runtime )
{
  using namespace cirkit;

  //reference_timer t( &runtime );

  unsigned i = 0u;
  std::map<std::pair<unsigned,unsigned>,unsigned> a;

  for ( int k = p; k; --k )
  {
    for ( unsigned j = 0u; j < k; ++j )
    {
      a[{k,j}] = i++;
    }
  }

  unsigned n = i;

  rcbdd cf;
  cf.initialize_manager();
  cf.create_variables( n + 1u, false );
  cf.set_num_inputs( n );
  cf.set_num_outputs( 1u );

  BDD tmp1, tmp2;

  std::vector<BDD> fs( p + 1u, cf.manager().bddOne() );
  for ( int k = p; k; --k )
  {
    for ( unsigned j = 1u; j < k; ++j )
    {
      BDD x = cf.x( a[{k,0u}] );
      fs[0] = x.Ite( fs[j], cf.manager().bddZero() );
      tmp1 = x.Ite( cf.manager().bddZero(), fs[j + 1u] );
      tmp2 = x.Ite( cf.manager().bddZero(), fs[j] );
      for ( i = 1u; i < j; ++i )
      {
        BDD x2 = cf.x( a[{k,i}] );
        fs[0] = x2.Ite( tmp2, fs[0] );
        tmp1 = x2.Ite( cf.manager().bddZero(), tmp1 );
        tmp2 = x2.Ite( cf.manager().bddZero(), tmp2 );
      }
      fs[0] = cf.x( a[{k,j}] ).Ite( tmp1, fs[0] );
      for ( i++; i < k; ++i )
      {
        fs[0] = cf.x( a[{k,i}] ).Ite( cf.manager().bddZero(), fs[0] );
      }
      fs[j] = fs[0];
    }
  }

  BDD f = cf.x( a[{1,0}] ).Ite( fs[1u], cf.manager().bddZero() );

  /* Embed */
  BDD chi = cf.manager().bddOne();

  chi &= !cf.y( 0u ) ^ ( cf.x( 0u ) ^ f );

  for ( unsigned i = 1u; i <= n; ++i )
  {
    chi &= !cf.x( i ) ^ cf.y( i );
  }

  cf.set_chi( chi );
}

BOOST_AUTO_TEST_CASE(simple)
{
  using namespace cirkit;

  benchmark_table<unsigned, unsigned, unsigned, double> table( { "p", "n", "m", "Run-time" }, true );
  double runtime;

  for ( unsigned i = 1; i < 8u; ++i )
  {
    unsigned p = i * 5u;
    restricted_growth_sequence( p, runtime );
    table.add( p, ((p * (p + 1u)) / 2u), 1u, runtime );
  }

  table.print();
}

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


#include <rct_optimizations/dh_chain_kinematic_calibration.h>
#include <rct_optimizations/local_parameterization.h>
#include <rct_optimizations_tests/dh_chain_observation_creator.h>
#include <rct_optimizations_tests/utilities.h>
#include <rct_optimizations_tests/observation_creator.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <gtest/gtest.h>

using namespace rct_optimizations;

/**
 * @brief This test models an environment where a robot (with slightly modified DH parameters)
 * holds a 2D camera which observes a target on a separate DH chain with no active joints
 * (which is located ~1m in front of the robot)
 */
class DHChainCalTest : public ::testing::Test
{
public:
  /**
   * @brief Constructor
   * Note: the calibration problem is initialized with a DH chain with modified values to be
   * more representative of a "real-life" use-case
   */
  DHChainCalTest()
    : camera_chain(test::perturbDHChain(test::createABBIRB2400(), 1.0e-3))
    , target_chain(std::vector<DHTransform>{})
    , problem(camera_chain, target_chain)
  {
  }

  void SetUp() override
  {
    setActualData();
    setInitialGuess();
    setObservations();
    applyMasks();
  }

  virtual void setActualData()
  {
    n_observations = 150;
    residual_threshold_sqr = 1.0e-12;

    camera = test::makeKinectCamera();
    target = test::Target(3, 4, 0.025);
    problem.intr = camera.intr;

    // Create the transform from the camera mount (i.e. robot wrist) to the camera
    camera_mount_to_camera = Eigen::Isometry3d::Identity();
    camera_mount_to_camera.rotate(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()));

    // Create the transform from the target mount (in this case an empty kinematic chain) to the target
    target_mount_to_target = Eigen::Isometry3d::Identity();

    // Create the transform from the base of the camera kinematic chain to the base of the target kinematic chain
    camera_base_to_target_base = Eigen::Isometry3d::Identity();
    camera_base_to_target_base.translate(Eigen::Vector3d(0.940, 0.0, 0.0) + target.center);
  }

  virtual void setInitialGuess()
  {
    problem.camera_mount_to_camera_guess = camera_mount_to_camera;
    problem.target_mount_to_target_guess = target_mount_to_target;
    problem.camera_base_to_target_base_guess = camera_base_to_target_base;
  }

  virtual void setObservations()
  {
    EXPECT_NO_THROW(problem.observations = test::createKinematicObservations(
                        camera_chain, target_chain, camera_mount_to_camera, target_mount_to_target,
                        camera_base_to_target_base, target, camera, n_observations));
  }

  virtual void applyMasks()
  {
  }

  virtual void analyzeResults(const KinematicCalibrationResult& result)
  {
    EXPECT_TRUE(result.converged);
    EXPECT_LT(result.final_cost_per_obs, residual_threshold_sqr);

    // Expect the resulting DH parameter offsets to be the same shape as the input
    // This is an important check of the case where the cost function modifies for 0 DoF chains
    EXPECT_EQ(problem.camera_chain.dof(), result.camera_chain_dh_offsets.rows());
    EXPECT_EQ(problem.target_chain.dof(), result.target_chain_dh_offsets.rows());

    // Test the result by moving the robot around to a lot of positions and seeing of the results match
    DHChain optimized_chain(problem.camera_chain, result.camera_chain_dh_offsets);

    std::cout << "original chain:\n" << problem.camera_chain.getDHTable().matrix() << std::endl << std::endl;
    std::cout << "optimized chain:\n" << optimized_chain.getDHTable().matrix() << std::endl << std::endl;

    namespace ba = boost::accumulators;
    ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> pos_acc;
    ba::accumulator_set<double, ba::stats<ba::tag::mean, ba::tag::variance>> ori_acc;

    const std::size_t n = 1000;
    for (std::size_t i = 0; i < n; ++i)
    {
      Eigen::VectorXd random_pose = camera_chain.createUniformlyRandomPose();

      // Nominal chain
      Eigen::Isometry3d nominal_fk = camera_chain.getFK(random_pose) * camera_mount_to_camera;

      // Optimized chain
      Eigen::Isometry3d optimized_fk = optimized_chain.getFK(random_pose)
                                       * result.camera_mount_to_camera;

      // Compare
      Eigen::Isometry3d diff = nominal_fk.inverse() * optimized_fk;
      pos_acc(diff.translation().norm());
      ori_acc(Eigen::Quaterniond(nominal_fk.linear()).angularDistance(Eigen::Quaterniond(optimized_fk.linear())));
    }

    std::cout << "Position Difference Mean: " << ba::mean(pos_acc) << std::endl;
    std::cout << "Position Difference Std. Dev.: " << std::sqrt(ba::variance(pos_acc)) << std::endl;

    std::cout << "Orientation Difference Mean: " << ba::mean(ori_acc) << std::endl;
    std::cout << "Orientation difference Std. Dev.: " << std::sqrt(ba::variance(ori_acc)) << std::endl;
  }

  double residual_threshold_sqr;
  std::size_t n_observations;

  test::Camera camera;
  test::Target target;

  DHChain camera_chain;
  DHChain target_chain;
  Eigen::Isometry3d camera_mount_to_camera;
  Eigen::Isometry3d target_mount_to_target;
  Eigen::Isometry3d camera_base_to_target_base;

  KinematicCalibrationProblem2D3D problem;
};

/**
 * @brief Tests the Dual DH Chain kinematic calibration algorithm with perfect initial conditions
 */
class DHChainCalTest_PerfectInitial : public DHChainCalTest
{
public:
  using DHChainCalTest::DHChainCalTest;

  virtual void analyzeResults(const KinematicCalibrationResult& result) override
  {
    // Check the nominal expectations
    DHChainCalTest::analyzeResults(result);

    EXPECT_LT(result.initial_cost_per_obs, 1.0e-15);

    EXPECT_TRUE(result.camera_mount_to_camera.isApprox(camera_mount_to_camera));
    EXPECT_TRUE(result.target_mount_to_target.isApprox(target_mount_to_target));
    EXPECT_TRUE(result.camera_base_to_target_base.isApprox(camera_base_to_target_base));

    // Expect the camera DH offsets to remain the same as the input (i.e. all zeros)
    for (Eigen::Index row = 0; row < result.camera_chain_dh_offsets.rows(); ++row)
    {
      for (Eigen::Index col = 0; col < result.camera_chain_dh_offsets.cols(); ++col)
      {
        EXPECT_LT(result.camera_chain_dh_offsets(row, col), 1.0e-15);
      }
    }
  }
};

/**
 * @brief Tests the Dual DH Chain kinematic calibration algorithm with slightly modified DH parameters
 */
class DHChainCalTest_PerturbedDH : public DHChainCalTest
{
public:
  using DHChainCalTest::DHChainCalTest;

  virtual void setActualData() override
  {
    DHChainCalTest::setActualData();

    // Set the residual squared error threshold to 0.5 pixels
    residual_threshold_sqr = 0.5 * 0.5;
  }

  virtual void setInitialGuess() override
  {
    DHChainCalTest::setInitialGuess();

    // Set the initial guess of the camera chain to it's nominal value
    // This is different than the real chain in the test fixture which has "realistically modified" DH parameters
    problem.camera_chain = test::createABBIRB2400();
  }

  virtual void applyMasks() override
  {
    // Mask a few DH parameters
    {
      Eigen::Matrix<bool, Eigen::Dynamic, 4> mask =
          Eigen::Matrix<bool, Eigen::Dynamic, 4>::Constant(problem.camera_chain.dof(), 4, false);

      // Mask the last row because they duplicate the camera mount to camera transform
      mask.bottomRows(1) << true, true, true, true;

      // Mask the joint 2 "r" parameter
      // It seems to have a high correlation with target_mount_to_target_y
      mask(1, 2) = true;

      // Add the mask to the problem
      problem.mask.at(0) = createDHMask(mask);
    }

    /* Mask the camera base to target base transform (duplicated by target mount to target transform when
     * the target chain has no joints */
    problem.mask.at(6) = { 0, 1, 2 };
    problem.mask.at(7) = { 0, 1, 2 };
  }

  virtual void analyzeResults(const KinematicCalibrationResult& result) override
  {
    DHChainCalTest::analyzeResults(result);

    // Expect the masked variables not to have changed
    EXPECT_TRUE(result.camera_chain_dh_offsets.bottomRows(1).isApproxToConstant(0.0));
    EXPECT_DOUBLE_EQ(result.camera_chain_dh_offsets(1, 2), 0.0);
    EXPECT_TRUE(result.camera_base_to_target_base.isApprox(problem.camera_base_to_target_base_guess));
  }
};

/**
 * @brief Tests the Dual DH Chain kinematic calibration algorithm with
 * slightly modified DH parameters and initial transform guesses
 */
class DHChainCalTest_PerturbedDH_PertubedGuess : public DHChainCalTest_PerturbedDH
{
public:
  using DHChainCalTest_PerturbedDH::DHChainCalTest_PerturbedDH;

  virtual void setInitialGuess() override
  {
    // Set the initial guess of the camera chain to it's nominal value
    // This is different than the real chain in the test fixture which has "realistically modified" DH parameters
    problem.camera_chain = test::createABBIRB2400();

    problem.camera_mount_to_camera_guess = test::perturbPose(camera_mount_to_camera, 0.05, 0.05);
    problem.target_mount_to_target_guess = test::perturbPose(target_mount_to_target, 0.05, 0.05);
    problem.camera_base_to_target_base_guess = test::perturbPose(camera_base_to_target_base, 0.05, 0.05);
  }
};

TEST_F(DHChainCalTest, TestCostFunction)
{
  // Initialize the optimization variables
  // Camera mount to camera (cm_to_c) quaternion and translation
  Eigen::Vector3d t_cm_to_c(camera_mount_to_camera.translation());
  Eigen::AngleAxisd rot_cm_to_c(camera_mount_to_camera.rotation());
  Eigen::Vector3d aa_cm_to_c(rot_cm_to_c.angle() * rot_cm_to_c.axis());

  // Target mount to target (cm_to_c) quaternion and translation
  Eigen::Vector3d t_tm_to_t(target_mount_to_target.translation());
  Eigen::AngleAxisd rot_tm_to_t(target_mount_to_target.rotation());
  Eigen::Vector3d aa_tm_to_t(rot_tm_to_t.angle() * rot_tm_to_t.axis());

  // Camera chain base to target_chain_base (ccb_to_tcb) quaternion and translation
  Eigen::Vector3d t_ccb_to_tcb(camera_base_to_target_base.translation());
  Eigen::AngleAxisd rot_ccb_to_tcb(camera_base_to_target_base.rotation());
  Eigen::Vector3d aa_ccb_to_tcb(rot_ccb_to_tcb.angle() * rot_ccb_to_tcb.axis());

  // Create containers for the kinematic chain DH offsets
  // Ceres will not work with parameter blocks of size zero, so create a dummy set of DH offsets for chains with DoF == 0
  Eigen::MatrixX4d camera_chain_dh_offsets = Eigen::MatrixX4d::Zero(camera_chain.dof(), 4);
  Eigen::MatrixX4d target_chain_dh_offsets = Eigen::MatrixX4d::Zero(target_chain.dof(), 4);

  // Create a vector of the pointers to the optimization variables in the order that the cost function expects them
  std::vector<double *> parameters
      = DualDHChainCost2D3D::constructParameters(camera_chain_dh_offsets,
                                                 target_chain_dh_offsets,
                                                 t_cm_to_c,
                                                 aa_cm_to_c,
                                                 t_tm_to_t,
                                                 aa_tm_to_t,
                                                 t_ccb_to_tcb,
                                                 aa_ccb_to_tcb);

  // Create observations
  KinObservation2D3D::Set observations = test::createKinematicObservations(camera_chain,
                                                                           target_chain,
                                                                           camera_mount_to_camera,
                                                                           target_mount_to_target,
                                                                           camera_base_to_target_base,
                                                                           target,
                                                                           camera,
                                                                           n_observations);

  // Test the cost function
  for (const auto &obs : observations)
  {
    for (const auto &corr : obs.correspondence_set)
    {
      DualDHChainCost2D3D cost(corr.in_image,
                               corr.in_target,
                               camera.intr,
                               camera_chain,
                               target_chain,
                               obs.camera_chain_joints,
                               obs.target_chain_joints);

      double residual[2];
      EXPECT_TRUE(cost(parameters.data(), residual));
      EXPECT_LT(residual[0], 1.0e-12);
      EXPECT_LT(residual[1], 1.0e-12);
    }
  }
}

TEST_F(DHChainCalTest_PerfectInitial, PerfectInitialConditions)
{
  KinematicCalibrationResult result = optimize(problem);
  std::cout << result.covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl;
  analyzeResults(result);
}

TEST_F(DHChainCalTest_PerturbedDH, PerfectGuessPerturbedDH)
{
  KinematicCalibrationResult result = optimize(problem);
  std::cout << result.covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl;
  analyzeResults(result);
}

TEST_F(DHChainCalTest_PerturbedDH_PertubedGuess, PerturbedDHPerturbedGuess)
{
  KinematicCalibrationResult result = optimize(problem);
  std::cout << result.covariance.printCorrelationCoeffAboveThreshold(0.5) << std::endl;
  analyzeResults(result);
}

int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#ifndef CFORCEFIELD_HPP
#define CFORCEFIELD_HPP

// std includes
#include <unordered_map>
#include <map>
#include <iostream>

// eigen includes
#include <Eigen/Dense>

#include "EigenMatrix_hash.hpp"

template< typename T >
T findValue( int             index , std::unordered_map<int,T>&             mp );

template< typename T >
T findValue( Eigen::Vector2i index , std::unordered_map<Eigen::Vector2i,T>& mp );

template< typename T >
T findValue( Eigen::Vector3i index , std::unordered_map<Eigen::Vector3i,T>& mp );

template< typename T >
T findValue( Eigen::Vector4i index , std::unordered_map<Eigen::Vector4i,T>& mp );

class CForceField
{

    using TVec3  = Eigen::Vector3d;

public:

    void setAtomicNumber( const std::string& symbol , const std::string& name , int id );
    int symbolToNumber( const std::string& symbol );

    bool loadFromFile( const std::string& filename );

    Eigen::Vector3d force_bond( const Eigen::Vector3d& a1 , const Eigen::Vector3d& a2 , const Eigen::Vector2i& atomicNumbers );
    Eigen::Vector3d force_lenard_jones( const Eigen::Vector3d& a1 , const Eigen::Vector3d& a2 , const Eigen::Vector2i& atomicNumbers );

#define getMacro( Name , MapName , IndexType , ValueType ) \
    ValueType get##Name( const IndexType& index ) \
    {   return findValue( index , MapName );  }

    getMacro( Mass , m_mass , int , double )
    getMacro( Epsilon , m_epsilon , int , double )
    getMacro( RMinHalf , m_r_min_half , int , double )
    getMacro( Kb , m_kb , Eigen::Vector2i , double )
    getMacro( B0 , m_b0 ,  Eigen::Vector2i  , double )
    getMacro( KTheta , m_k_theta , Eigen::Vector3i  , double )
    getMacro( Theta0 , m_theta0 , Eigen::Vector3i , double )
    getMacro( KChi , m_k_chi , Eigen::Vector4i , double )
    getMacro( N , m_n , Eigen::Vector4i , int )
    getMacro( Delta , m_delta , Eigen::Vector4i , double )
    getMacro( KPsi , m_k_psi , Eigen::Vector4i , double )
    getMacro( Psi0 , m_psi0 , Eigen::Vector4i , double )

private:

    inline int atomID( const std::string& ref );

    std::map<std::string,int>   m_atomicNumbers;

    // parameters...

    std::unordered_map<int,double>  m_mass;
    std::map<int,std::string>       m_names;
    std::map<int,std::string>       m_symbols;

    std::unordered_map<int,double> m_epsilon;
    std::unordered_map<int,double> m_r_min_half;

    std::unordered_map<Eigen::Vector2i,double> m_kb;
    std::unordered_map<Eigen::Vector2i,double> m_b0;

    std::unordered_map<Eigen::Vector3i,double> m_k_theta;
    std::unordered_map<Eigen::Vector3i,double> m_theta0;

    std::unordered_map<Eigen::Vector4i,double> m_k_chi;
    std::unordered_map<Eigen::Vector4i,int>    m_n;
    std::unordered_map<Eigen::Vector4i,double> m_delta;

    std::unordered_map<Eigen::Vector4i,double> m_k_psi;
    std::unordered_map<Eigen::Vector4i,double> m_psi0;

};

template< typename T >
T findValue( int index , std::unordered_map<int,T>& mp )
{
    // try to find it directly
    auto it = mp.find( index );
    if( it != mp.end() )
        return it->second;

    std::cout << "Missing parameter for " << index << "\n" << std::flush;

    mp[index] = 0;

    return 0;
}

template< typename T >
T findValue( Eigen::Vector2i index , std::unordered_map<Eigen::Matrix<int,2,1>,T>& mp )
{
    // try to find it directly
    auto it = mp.find( index );
    if( it != mp.end() )
        return it->second;

    // swap a0 and a1
    std::swap( index[0] , index[1] );

    it = mp.find( index );
    if( it != mp.end() )
    {
        T val = it->second;
        // we don't want to do this step every frame, hence insert it into the map
        mp[index] = val;
        return val;
    }

    // report the missing parameter:

    std::cout << "Missing parameter for " << index.transpose() << "\n" << std::flush;

    mp[index] = 0;
    return 0;
}

template< typename T >
T findValue( Eigen::Vector3i index , std::unordered_map<Eigen::Matrix<int,2,1>,T>& mp )
{
    // try to find it directly
    auto it = mp.find( index );
    if( it != mp.end() )
        return it->second;

    // swap a0 and a2
    std::swap( index[0] , index[2] );

    it = mp.find( index );
    if( it != mp.end() )
    {
        T val = it->second;
        // we don't want to do this step every frame, hence insert it into the map
        mp[index] = val;
        return val;
    }
    // report the missing parameter:

    std::cout << "Missing parameter for " << index.transpose() << "\n" << std::flush;

    mp[index] = 0;
    // T is expected to be numeric
    return 0;
}

template< typename T >
T findValue( Eigen::Vector4i index , std::unordered_map<Eigen::Matrix<int,2,1>,T>& mp )
{
    // try to find it directly
    auto it = mp.find( index );
    if( it != mp.end() )
        return *it;

    // swap a0 and a3
    std::swap( index[0] , index[3] );

    it = mp.find( index );
    if( it != mp.end() )
    {
        T val = *it;
        // we don't want to do this step every frame, hence insert it into the map
        mp[index] = val;
        return val;
    }

    // swap a1 and a2
    std::swap( index[1] , index[2] );

    it = mp.find( index );
    if( it != mp.end() )
    {
        T val = *it;
        // we don't want to do this step every frame, hence insert it into the map
        mp[index] = val;
        return val;
    }

    // report the missing parameter:

    std::cout << "Missing parameter for " << index.transpose() << "\n" << std::flush;

    mp[index] = 0;
    return 0;
}

#endif // CFORCEFIELD_HPP


/*
 *  divide_by_inplace.hpp
 *  MTL4
 *
 *  Created by Hui Li (huil@Princeton.EDU)
 *
 */

#ifndef MTL_DIVIDE_BY_INPLACE_INCLUDE
#define MTL_DIVIDE_BY_INPLACE_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/operation/assign_each_nonzero.hpp>
#include <boost/numeric/mtl/operation/divide_by.hpp>

namespace mtl {

	/// Divide collection \p c (from right) by scalar factor \p alpha; \p c is altered
	template <typename Factor, typename Coll>
	void divide_by_inplace(Coll& c, const Factor& alpha, tag::scalar)
	{
	    // assign_each_nonzero(c, boost::lambda::_1 / alpha);
	    assign_each_nonzero(c, tfunctor::divide_by<typename Collection<Coll>::value_type, Factor>(alpha));
	}

	/// Divide collection \p c (from right) by factor \p alpha; \p c is altered
	template <typename Factor, typename Collection>
	void divide_by_inplace(Collection& c, const Factor& alpha)
	{
	    divide_by_inplace(c, alpha, typename traits::category<Factor>::type());
	}

} // namespace mtl

#endif // MTL_DIVIDE_BY_INPLACE_INCLUDE