#include <bluetoe/channel_map.hpp>

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

static constexpr std::uint8_t all_channel_map[] = { 0xff, 0xff, 0xff, 0xff, 0x1f };
static constexpr std::uint8_t all_but_one_map[] = { 0xff, 0xff, 0xff, 0xff, 0xf };

template < unsigned Hop, const std::uint8_t*  Map >
struct fixture : bluetoe::link_layer::channel_map
{
    fixture()
    {
        BOOST_REQUIRE( reset( Map, Hop ) );
    }
};

template < unsigned Hop >
using all_channel = fixture< Hop, all_channel_map >;

template < unsigned Hop >
using all_but_one_channel = fixture< Hop, all_but_one_map >;

using all_channel_5 = all_channel< 5 >;

BOOST_FIXTURE_TEST_CASE( all_channels_hop_5, all_channel_5 )
{
    BOOST_CHECK_EQUAL( data_channel( 0 ), 5u );
    BOOST_CHECK_EQUAL( data_channel( 1 ), 10u );
    //...
    BOOST_CHECK_EQUAL( data_channel( 17 ), 16u );
    //...
    BOOST_CHECK_EQUAL( data_channel( 35 ), 32u );
    BOOST_CHECK_EQUAL( data_channel( 36 ), 0u );
}

using all_channel_16 = all_channel< 16 >;

BOOST_FIXTURE_TEST_CASE( all_channels_hop_16, all_channel_16 )
{
    BOOST_CHECK_EQUAL( data_channel( 0 ), 16u );
    BOOST_CHECK_EQUAL( data_channel( 1 ), 32u );
    //...
    BOOST_CHECK_EQUAL( data_channel( 11 ), 7u );
    //...
    BOOST_CHECK_EQUAL( data_channel( 35 ), 21u );
    BOOST_CHECK_EQUAL( data_channel( 36 ), 0u );
}

using all_channel_10 = all_channel< 10 >;

BOOST_FIXTURE_TEST_CASE( all_channels_hop_10, all_channel_10 )
{
    BOOST_CHECK_EQUAL( data_channel( 0 ), 10u );
    BOOST_CHECK_EQUAL( data_channel( 1 ), 20u );
    // ...
    BOOST_CHECK_EQUAL( data_channel( 7 ), 6u );
    BOOST_CHECK_EQUAL( data_channel( 10 ), 36u );
    // ...
    BOOST_CHECK_EQUAL( data_channel( 35 ), unsigned{ ( 36 * 10 ) % 37 } );
    BOOST_CHECK_EQUAL( data_channel( 36 ), 0u );
}

using all_but_one_channel_10 = all_but_one_channel< 10 >;

/*
 * if the last channel (channel 36) is not within the map, it have to be remaped to 36 mod 36 == 0
 */
BOOST_FIXTURE_TEST_CASE( all_but_one_channels_hop_10, all_but_one_channel_10 )
{
    BOOST_CHECK_EQUAL( data_channel( 0 ), 10u );
    BOOST_CHECK_EQUAL( data_channel( 1 ), 20u );
    // ...
    BOOST_CHECK_EQUAL( data_channel( 10 ), 0u );
    // ...
    BOOST_CHECK_EQUAL( data_channel( 35 ), unsigned{ ( 36 * 10 ) % 37 } );
    BOOST_CHECK_EQUAL( data_channel( 36 ), 0u );
}

static constexpr std::uint8_t a_few_channels_map[] = { 0x17, 0x44, 0x00, 0xff, 0x05 };

using only_a_few_channels_7 = fixture< 7, a_few_channels_map >;

/* the list of channels in the map is:
   0, 1, 2, 4, 10, 14, 24, 25, 26, 27, 28, 29, 30, 31, 32, 34
   #Channels = 16
 */
BOOST_FIXTURE_TEST_CASE( only_a_few_channels_hop_7, only_a_few_channels_7 )
{
    // we check the whole sequence
    BOOST_CHECK_EQUAL( data_channel( 0  ), 25u ); // 7 % 16
    BOOST_CHECK_EQUAL( data_channel( 1  ), 14u );
    BOOST_CHECK_EQUAL( data_channel( 2  ), 14u ); // 21 % 16
    BOOST_CHECK_EQUAL( data_channel( 3  ), 28u );
    BOOST_CHECK_EQUAL( data_channel( 4  ), 4u  ); // 35 % 16
    BOOST_CHECK_EQUAL( data_channel( 5  ), 14u ); // 5 % 16
    BOOST_CHECK_EQUAL( data_channel( 6  ), 30u ); // 12 % 16
    BOOST_CHECK_EQUAL( data_channel( 7  ), 4u  ); // 19 % 16
    BOOST_CHECK_EQUAL( data_channel( 8  ), 26u );
    BOOST_CHECK_EQUAL( data_channel( 9  ), 1u  ); // 33 % 16
    BOOST_CHECK_EQUAL( data_channel( 10 ), 4u  ); // 3 % 16
    BOOST_CHECK_EQUAL( data_channel( 11 ), 10u );
    BOOST_CHECK_EQUAL( data_channel( 12 ), 1u  ); // 17 % 16
    BOOST_CHECK_EQUAL( data_channel( 13 ), 24u );
    BOOST_CHECK_EQUAL( data_channel( 14 ), 31u );
    BOOST_CHECK_EQUAL( data_channel( 15 ), 1u  );
    BOOST_CHECK_EQUAL( data_channel( 16 ), 26u ); // 8 % 16
    BOOST_CHECK_EQUAL( data_channel( 17 ), 34u ); // 15 % 16
    BOOST_CHECK_EQUAL( data_channel( 18 ), 24u ); // 22 % 16
    BOOST_CHECK_EQUAL( data_channel( 19 ), 29u );
    BOOST_CHECK_EQUAL( data_channel( 20 ), 10u ); // 36 % 16
    BOOST_CHECK_EQUAL( data_channel( 21 ), 24u ); // 6 % 16
    BOOST_CHECK_EQUAL( data_channel( 22 ), 31u ); // 13 % 16
    BOOST_CHECK_EQUAL( data_channel( 23 ), 10u ); // 20 % 16
    BOOST_CHECK_EQUAL( data_channel( 24 ), 27u );
    BOOST_CHECK_EQUAL( data_channel( 25 ), 34u );
    BOOST_CHECK_EQUAL( data_channel( 26 ), 4u  );
    BOOST_CHECK_EQUAL( data_channel( 27 ), 29u ); // 11 % 16
    BOOST_CHECK_EQUAL( data_channel( 28 ), 2u  ); // 18 % 16
    BOOST_CHECK_EQUAL( data_channel( 29 ), 25u );
    BOOST_CHECK_EQUAL( data_channel( 30 ), 32u );
    BOOST_CHECK_EQUAL( data_channel( 31 ), 2u  );
    BOOST_CHECK_EQUAL( data_channel( 32 ), 27u ); // 9 % 16
    BOOST_CHECK_EQUAL( data_channel( 33 ), 0u  ); // 16 % 16
    BOOST_CHECK_EQUAL( data_channel( 34 ), 25u ); // 23 % 16
    BOOST_CHECK_EQUAL( data_channel( 35 ), 30u );
    BOOST_CHECK_EQUAL( data_channel( 36 ), 0u  );
}

/*
 * A map just containing channel 0 and channel 36
 */
static constexpr std::uint8_t only_two_channels_map[] = { 0x01, 0x00, 0x00, 0x00, 0x10 };

using only_two_channels_8 = fixture< 8, only_two_channels_map >;

BOOST_FIXTURE_TEST_CASE( having_odd_hop_increment_with_two_channels_leeds_to_long_periods_on_the_same_channel, only_two_channels_8 )
{
    BOOST_CHECK_EQUAL( data_channel( 0  ), 0u );
    BOOST_CHECK_EQUAL( data_channel( 1  ), 0u );
    BOOST_CHECK_EQUAL( data_channel( 2  ), 0u );
    // ...
    BOOST_CHECK_EQUAL( data_channel( 34  ), 36u );
    BOOST_CHECK_EQUAL( data_channel( 35  ), 36u );
    BOOST_CHECK_EQUAL( data_channel( 36  ), 0u );
}

/*
 * A map just containing one channel
 */
static const std::uint8_t only_one_channel_map[] = { 0x00, 0x04, 0x00, 0x00, 0x00 };
static const std::uint8_t only_one_channel_and_some_rfu_bits_map[] = { 0x00, 0x04, 0x00, 0x00, 0x60 };

using only_two_channels_8 = fixture< 8, only_two_channels_map >;

// 4.5.8.1 The minimum number of used channels shall be 2.
BOOST_FIXTURE_TEST_CASE( channel_map_shall_contain_at_least_two_bits, bluetoe::link_layer::channel_map )
{
    BOOST_CHECK( !reset( only_one_channel_map, 5u ) );
    BOOST_CHECK( !reset( only_one_channel_map, 11u ) );
    BOOST_CHECK( !reset( only_one_channel_and_some_rfu_bits_map, 5u ) );
    BOOST_CHECK( !reset( only_one_channel_and_some_rfu_bits_map, 11u ) );
}

BOOST_FIXTURE_TEST_CASE( invalid_hops_are_recognized, bluetoe::link_layer::channel_map )
{
    BOOST_CHECK( !reset( all_channel_map, 0 ) );
    BOOST_CHECK( !reset( a_few_channels_map, 4 ) );
    BOOST_CHECK( !reset( all_but_one_map, 17 ) );
    BOOST_CHECK( !reset( all_channel_map, 99 ) );
}

BOOST_FIXTURE_TEST_CASE( valid_hops_are_recognized, bluetoe::link_layer::channel_map )
{
    BOOST_CHECK( reset( all_channel_map, 5 ) );
    BOOST_CHECK( reset( a_few_channels_map, 7 ) );
    BOOST_CHECK( reset( all_but_one_map, 10 ) );
    BOOST_CHECK( reset( all_channel_map, 16 ) );
}

BOOST_FIXTURE_TEST_CASE( stores_the_right_hop, all_channel_5 )
{
    BOOST_CHECK( reset( only_two_channels_map ) );

    BOOST_CHECK_EQUAL( data_channel( 0 ), 36u );
    BOOST_CHECK_EQUAL( data_channel( 1 ), 0u );
}

static constexpr std::uint8_t all_but_25_map[] = { 0xff, 0xff, 0xff, 0xfd, 0x1f };

using all_channels_but_25 = all_channel< 6 >;

BOOST_FIXTURE_TEST_CASE( real_life_example, all_channels_but_25 )
{
    BOOST_CHECK( reset( all_but_25_map ) );
    BOOST_CHECK_EQUAL( data_channel( 28 ), 26u );
    BOOST_CHECK_EQUAL( data_channel( 29 ), 32u );
    BOOST_CHECK_EQUAL( data_channel( 30 ), 1u );
    BOOST_CHECK_EQUAL( data_channel( 35 ), 31u );
}


/**
 * @file Projector.hpp
 * @author Takashi Michikawa <michikawa@acm.org>
 */

#ifndef MI_PROJECTOR_HPP
#define MI_PROJECTOR_HPP 1
#include <array>

#include <Eigen/Dense>
#include <mi/NonCopyable.hpp>

namespace mi
{
        class Projector : public NonCopyable
        {
        public:
                explicit Projector ( double modelview[16], double projection[16], int viewport[4] );
		~Projector(void);
                Eigen::Vector3d project ( const Eigen::Vector3d& p, int *error = NULL );
                int project ( const double x, const double y, const double z, double &wx, double &wy, double &wz );
                Eigen::Vector3d unproject ( const Eigen::Vector3d& wp, int *error = NULL );
                int unproject ( const double wx, const double wy, const double wz, double &x, double &y, double &z );
        private:
		class Impl;
		std::unique_ptr<Impl> _impl;
        };
}
#endif// MI_PROJECTOR_HPP


#include <ros/ros.h>
#include <iostream>
#include <string>
#include <Eigen/Geometry>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <lightweight_vicon_bridge/MocapState.h>

// These values are tuned by hand as a "best match" to the real robot, since it's hard to measure directly
#define LEFT_FOREARM_X_OFFSET -0.235
#define LEFT_FOREARM_Y_OFFSET 0.0
#define LEFT_FOREARM_Z_OFFSET 0.02
#define RIGHT_FOREARM_X_OFFSET -0.230
#define RIGHT_FOREARM_Y_OFFSET 0.0
#define RIGHT_FOREARM_Z_OFFSET 0.02

typedef Eigen::Affine3d Pose;

std::unique_ptr<tf::TransformListener> g_transform_listener;
std::unique_ptr<tf::TransformBroadcaster> g_transform_broadcaster;

ros::Publisher g_left_arm_pose_pub;
ros::Publisher g_right_arm_pose_pub;
std::string g_robot_object_name;
std::string g_robot_segment_name;
std::string g_left_arm_object;
std::string g_right_arm_object;
std::string g_left_arm_segment;
std::string g_right_arm_segment;

void PublishLeftArmPose(tf::StampedTransform& torso_transform, tf::StampedTransform& forearm_to_wrist_transform,  geometry_msgs::Transform& mocap_base_pose, geometry_msgs::Transform& mocap_left_arm_pose)
{
    // Make the Eigen transform for the torso
    Eigen::Translation3d torso_to_base_translation(torso_transform.getOrigin().x(), torso_transform.getOrigin().y(), torso_transform.getOrigin().z());
    Eigen::Quaterniond torso_to_base_rotation(torso_transform.getRotation().w(), torso_transform.getRotation().x(), torso_transform.getRotation().y(), torso_transform.getRotation().z());
    Pose torso_to_base = torso_to_base_translation * torso_to_base_rotation;
    // Make the Eigen transforms for the base
    Eigen::Translation3d world_to_base_translation(mocap_base_pose.translation.x, mocap_base_pose.translation.y, mocap_base_pose.translation.z);
    Eigen::Quaterniond world_to_base_rotation(mocap_base_pose.rotation.w, mocap_base_pose.rotation.x, mocap_base_pose.rotation.y, mocap_base_pose.rotation.z);
    Pose world_to_base = world_to_base_translation * world_to_base_rotation;
    Pose world_to_base_inv = world_to_base.inverse();
    // Make the Eigen transform for the mocap forearm
    Eigen::Translation3d world_to_left_mocap_forearm_translation(mocap_left_arm_pose.translation.x, mocap_left_arm_pose.translation.y, mocap_left_arm_pose.translation.z);
    Eigen::Quaterniond world_to_left_mocap_forearm_rotation(mocap_left_arm_pose.rotation.w, mocap_left_arm_pose.rotation.x, mocap_left_arm_pose.rotation.y, mocap_left_arm_pose.rotation.z);
    Pose world_to_left_mocap_forearm = world_to_left_mocap_forearm_translation * world_to_left_mocap_forearm_rotation;
    // Make the fixed mocap left forearm->real left forearm transform
    Eigen::Translation3d mocap_left_forearm_to_left_forearm_translation(LEFT_FOREARM_X_OFFSET, LEFT_FOREARM_Y_OFFSET, LEFT_FOREARM_Z_OFFSET);
    Eigen::Quaterniond mocap_left_forearm_to_left_forearm_rotation(1.0, 0.0, 0.0, 0.0);
    Pose mocap_left_forearm_to_left_forearm = mocap_left_forearm_to_left_forearm_translation * mocap_left_forearm_to_left_forearm_rotation;
    // Make the Eigen transform for the left forearm->left wrist
    Eigen::Translation3d left_forearm_to_left_wrist_translation(forearm_to_wrist_transform.getOrigin().x(), forearm_to_wrist_transform.getOrigin().y(), forearm_to_wrist_transform.getOrigin().z());
    Eigen::Quaterniond left_forearm_to_left_wrist_rotation(forearm_to_wrist_transform.getRotation().w(), forearm_to_wrist_transform.getRotation().x(), forearm_to_wrist_transform.getRotation().y(), forearm_to_wrist_transform.getRotation().z());
    Pose left_forearm_to_left_wrist = left_forearm_to_left_wrist_translation * left_forearm_to_left_wrist_rotation;
    // Compose the transforms together
    Pose torso_to_left_wrist = torso_to_base * world_to_base_inv * world_to_left_mocap_forearm * mocap_left_forearm_to_left_forearm * left_forearm_to_left_wrist;
    // Extract from Eigen and publish
    Eigen::Quaterniond torso_to_left_wrist_rotation(torso_to_left_wrist.rotation());
    geometry_msgs::PoseStamped left_arm_pose;
    left_arm_pose.header.frame_id = "/torso_lift_link";
    left_arm_pose.pose.position.x = torso_to_left_wrist.translation().x();
    left_arm_pose.pose.position.y = torso_to_left_wrist.translation().y();
    left_arm_pose.pose.position.z = torso_to_left_wrist.translation().z();
    left_arm_pose.pose.orientation.x = torso_to_left_wrist_rotation.x();
    left_arm_pose.pose.orientation.y = torso_to_left_wrist_rotation.y();
    left_arm_pose.pose.orientation.z = torso_to_left_wrist_rotation.z();
    left_arm_pose.pose.orientation.w = torso_to_left_wrist_rotation.w();
    g_left_arm_pose_pub.publish(left_arm_pose);
    // Publish as a TF frame
    /* Convert to a TF transform and publish */
    tf::Transform left_arm_tf;
    left_arm_tf.setOrigin(tf::Vector3(left_arm_pose.pose.position.x, left_arm_pose.pose.position.y, left_arm_pose.pose.position.z));
    left_arm_tf.setRotation(tf::Quaternion(left_arm_pose.pose.orientation.x, left_arm_pose.pose.orientation.y, left_arm_pose.pose.orientation.z, left_arm_pose.pose.orientation.w));
    /* Publish TF */
    tf::StampedTransform left_arm_tf_msg(left_arm_tf, torso_transform.stamp_, std::string("/torso_lift_link"), std::string("/mocap_left_wrist"));
    g_transform_broadcaster->sendTransform(left_arm_tf_msg);
}

void PublishRightArmPose(tf::StampedTransform& torso_transform, tf::StampedTransform& forearm_to_wrist_transform, geometry_msgs::Transform& mocap_base_pose, geometry_msgs::Transform& mocap_right_arm_pose)
{
    // Make the Eigen transform for the torso
    Eigen::Translation3d torso_to_base_translation(torso_transform.getOrigin().x(), torso_transform.getOrigin().y(), torso_transform.getOrigin().z());
    Eigen::Quaterniond torso_to_base_rotation(torso_transform.getRotation().w(), torso_transform.getRotation().x(), torso_transform.getRotation().y(), torso_transform.getRotation().z());
    Pose torso_to_base = torso_to_base_translation * torso_to_base_rotation;
    // Make the Eigen transforms for the base
    Eigen::Translation3d world_to_base_translation(mocap_base_pose.translation.x, mocap_base_pose.translation.y, mocap_base_pose.translation.z);
    Eigen::Quaterniond world_to_base_rotation(mocap_base_pose.rotation.w, mocap_base_pose.rotation.x, mocap_base_pose.rotation.y, mocap_base_pose.rotation.z);
    Pose world_to_base = world_to_base_translation * world_to_base_rotation;
    Pose world_to_base_inv = world_to_base.inverse();
    // Make the Eigen transform for the mocap forearm
    Eigen::Translation3d world_to_right_mocap_forearm_translation(mocap_right_arm_pose.translation.x, mocap_right_arm_pose.translation.y, mocap_right_arm_pose.translation.z);
    Eigen::Quaterniond world_to_right_mocap_forearm_rotation(mocap_right_arm_pose.rotation.w, mocap_right_arm_pose.rotation.x, mocap_right_arm_pose.rotation.y, mocap_right_arm_pose.rotation.z);
    Pose world_to_right_mocap_forearm = world_to_right_mocap_forearm_translation * world_to_right_mocap_forearm_rotation;
    // Make the fixed mocap right forearm->real right forearm transform
    Eigen::Translation3d mocap_right_forearm_to_right_forearm_translation(RIGHT_FOREARM_X_OFFSET, RIGHT_FOREARM_Y_OFFSET, RIGHT_FOREARM_Z_OFFSET);
    Eigen::Quaterniond mocap_right_forearm_to_right_forearm_rotation(1.0, 0.0, 0.0, 0.0);
    Pose mocap_right_forearm_to_right_forearm = mocap_right_forearm_to_right_forearm_translation * mocap_right_forearm_to_right_forearm_rotation;
    // Make the Eigen transform for the right forearm->right wrist
    Eigen::Translation3d right_forearm_to_right_wrist_translation(forearm_to_wrist_transform.getOrigin().x(), forearm_to_wrist_transform.getOrigin().y(), forearm_to_wrist_transform.getOrigin().z());
    Eigen::Quaterniond right_forearm_to_right_wrist_rotation(forearm_to_wrist_transform.getRotation().w(), forearm_to_wrist_transform.getRotation().x(), forearm_to_wrist_transform.getRotation().y(), forearm_to_wrist_transform.getRotation().z());
    Pose right_forearm_to_right_wrist = right_forearm_to_right_wrist_translation * right_forearm_to_right_wrist_rotation;
    // Compose the transforms together
    Pose torso_to_right_wrist = torso_to_base * world_to_base_inv * world_to_right_mocap_forearm * mocap_right_forearm_to_right_forearm * right_forearm_to_right_wrist;
    // Extract from Eigen and publish
    Eigen::Quaterniond torso_to_right_wrist_rotation(torso_to_right_wrist.rotation());
    geometry_msgs::PoseStamped right_arm_pose;
    right_arm_pose.header.frame_id = "/torso_lift_link";
    right_arm_pose.pose.position.x = torso_to_right_wrist.translation().x();
    right_arm_pose.pose.position.y = torso_to_right_wrist.translation().y();
    right_arm_pose.pose.position.z = torso_to_right_wrist.translation().z();
    right_arm_pose.pose.orientation.x = torso_to_right_wrist_rotation.x();
    right_arm_pose.pose.orientation.y = torso_to_right_wrist_rotation.y();
    right_arm_pose.pose.orientation.z = torso_to_right_wrist_rotation.z();
    right_arm_pose.pose.orientation.w = torso_to_right_wrist_rotation.w();
    g_right_arm_pose_pub.publish(right_arm_pose);
    // Publish as a TF frame
    /* Convert to a TF transform and publish */
    tf::Transform right_arm_tf;
    right_arm_tf.setOrigin(tf::Vector3(right_arm_pose.pose.position.x, right_arm_pose.pose.position.y, right_arm_pose.pose.position.z));
    right_arm_tf.setRotation(tf::Quaternion(right_arm_pose.pose.orientation.x, right_arm_pose.pose.orientation.y, right_arm_pose.pose.orientation.z, right_arm_pose.pose.orientation.w));
    /* Publish TF */
    tf::StampedTransform right_arm_tf_msg(right_arm_tf, torso_transform.stamp_, std::string("/torso_lift_link"), std::string("/mocap_right_wrist"));
    g_transform_broadcaster->sendTransform(right_arm_tf_msg);
}

void MocapMsgCB(lightweight_vicon_bridge::MocapState msg)
{
    bool got_base = false;
    bool got_left_arm = false;
    bool got_right_arm = false;
    geometry_msgs::Transform base_transform;
    geometry_msgs::Transform left_arm_transform;
    geometry_msgs::Transform right_arm_transform;
    for (size_t idx_o = 0; idx_o < msg.tracked_objects.size(); idx_o++)
    {
        lightweight_vicon_bridge::MocapObject current_object = msg.tracked_objects[idx_o];
        if (current_object.name == g_robot_object_name)
        {
            for (size_t idx_s = 0; idx_s < current_object.segments.size(); idx_s++)
            {
                lightweight_vicon_bridge::MocapSegment current_segment = current_object.segments[idx_s];
                if (current_segment.name == g_robot_segment_name)
                {
                    if (!current_segment.occluded)
                    {
                        base_transform = current_segment.transform;
                        got_base = true;
                    }
                    else
                    {
                        ROS_WARN("Tracking robot base, but base is occluded");
                    }
                }
            }
        }
        else if (current_object.name == g_left_arm_object)
        {
            for (size_t idx_s = 0; idx_s < current_object.segments.size(); idx_s++)
            {
                lightweight_vicon_bridge::MocapSegment current_segment = current_object.segments[idx_s];
                if (current_segment.name == g_left_arm_segment)
                {
                    if (!current_segment.occluded)
                    {
                        left_arm_transform = current_segment.transform;
                        got_left_arm = true;
                    }
                    else
                    {
                        ROS_WARN("Tracking left arm, but left arm is occluded");
                    }
                }
            }
        }
        else if (current_object.name == g_right_arm_object)
        {
            for (size_t idx_s = 0; idx_s < current_object.segments.size(); idx_s++)
            {
                lightweight_vicon_bridge::MocapSegment current_segment = current_object.segments[idx_s];
                if (current_segment.name == g_right_arm_segment)
                {
                    if (!current_segment.occluded)
                    {
                        right_arm_transform = current_segment.transform;
                        got_right_arm = true;
                    }
                    else
                    {
                        ROS_WARN("Tracking right arm, but right arm is occluded");
                    }
                }
            }
        }
    }
    // Query TF to get the transform from /torso_lift_link to /base_footprint
    tf::StampedTransform torso_transform;
    bool got_torso = false;
    try
    {
        g_transform_listener->lookupTransform("/torso_lift_link", "/base_footprint", ros::Time(0), torso_transform);
        got_torso = true;
    }
    catch (tf::TransformException ex)
    {
        ROS_WARN("Unable to lookup transform from /torso_lift_link to /base_footprint");
    }
    // Query TF to get the transform from /l_forearm_link to /l_wrist_roll_link
    tf::StampedTransform left_forearm_to_wrist_transform;
    bool got_left_forearm = false;
    try
    {
        g_transform_listener->lookupTransform("/l_forearm_link", "/l_wrist_roll_link", ros::Time(0), left_forearm_to_wrist_transform);
        got_left_forearm = true;
    }
    catch (tf::TransformException ex)
    {
        ROS_WARN("Unable to lookup transform from /l_forearm_link to /l_wrist_roll_link");
    }
    // Query TF to get the transform from /r_forearm_link to /r_wrist_roll_link
    tf::StampedTransform right_forearm_to_wrist_transform;
    bool got_right_forearm = false;
    try
    {
        g_transform_listener->lookupTransform("/r_forearm_link", "/r_wrist_roll_link", ros::Time(0), right_forearm_to_wrist_transform);
        got_right_forearm = true;
    }
    catch (tf::TransformException ex)
    {
        ROS_WARN("Unable to lookup transform from /r_forearm_link to /r_wrist_roll_link");
    }
    // Publish what we can, depending on which transforms we've received
    if (got_base)
    {
        if (got_left_arm && got_left_forearm)
        {
            PublishLeftArmPose(torso_transform, left_forearm_to_wrist_transform, base_transform, left_arm_transform);
        }
        else
        {
            ROS_WARN("Unable to publish left arm pose");
        }
        if (got_right_arm && got_right_forearm)
        {
            PublishRightArmPose(torso_transform, right_forearm_to_wrist_transform, base_transform, right_arm_transform);
        }
        else
        {
            ROS_WARN("Unable to publish right arm pose");
        }
    }
    else
    {
        ROS_WARN("No base pose - unable to publish either left or right arm pose");
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "mocap_robot_arm_pub");
    ROS_INFO("Starting mocap robot arm pub...");
    ros::NodeHandle nh;
    ros::NodeHandle nhp("~");
    std::string tracker_topic;
    std::string left_arm_pose_topic;
    std::string right_arm_pose_topic;
    nhp.param(std::string("tracker_topic"), tracker_topic, std::string("mocap_tracking"));
    nhp.param(std::string("left_arm_pose_topic"), left_arm_pose_topic, std::string("/l_arm_pose_controller/pose"));
    nhp.param(std::string("right_arm_pose_topic"), right_arm_pose_topic, std::string("/r_arm_pose_controller/pose"));
    nhp.param(std::string("robot_object_name"), g_robot_object_name, std::string("ArchieBase"));
    nhp.param(std::string("robot_segment_name"), g_robot_segment_name, std::string("ArchieBase"));
    nhp.param(std::string("left_arm_object"), g_left_arm_object, std::string("ArchieLeftForearm"));
    nhp.param(std::string("right_arm_object"), g_right_arm_object, std::string("ArchieRightForearm"));
    nhp.param(std::string("left_arm_segment"), g_left_arm_segment, std::string("ArchieLeftForearm"));
    nhp.param(std::string("right_arm_segment"), g_right_arm_segment, std::string("ArchieRightForearm"));
    // Create the pose publishers
    g_left_arm_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(left_arm_pose_topic, 1, false);
    g_right_arm_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(right_arm_pose_topic, 1, false);
    // Create the TF broadcaster
    g_transform_broadcaster = std::unique_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
    // Create the TF listener
    g_transform_listener = std::unique_ptr<tf::TransformListener>(new tf::TransformListener(nh, ros::Duration(20.0)));
    ROS_INFO("Waiting for a few seconds to fill the TF cache...");
    ros::Duration(2.5).sleep();
    // Create the mocap data subscriber
    ros::Subscriber mocap_sub = nh.subscribe(tracker_topic, 1, MocapMsgCB);
    // Start streaming data
    ROS_INFO("Streaming data...");
    ros::Rate spin_rate(200.0);
    while (ros::ok())
    {
        // Handle ROS stuff
        ros::spinOnce();
        // Sleep briefly
        spin_rate.sleep();
    }
    return 0;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_UNITS_DETAIL_HETEROGENEOUS_CONVERSION_HPP
#define BOOST_UNITS_DETAIL_HETEROGENEOUS_CONVERSION_HPP

#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>

#include <boost/units/static_rational.hpp>
#include <boost/units/homogeneous_system.hpp>
#include <boost/units/detail/linear_algebra.hpp>

namespace boost {

namespace units {

namespace detail {

struct solve_end {
    template<class Begin, class Y>
    struct apply {
        typedef dimensionless_type type;
    };
};

struct no_solution {};

template<class X1, class X2, class Next>
struct solve_normal {
    template<class Begin, class Y>
    struct apply {
        typedef typename Begin::next next;
        typedef list<
            typename mpl::minus<
                typename mpl::times<X1, Y>::type,
                typename mpl::times<X2, typename Begin::item>::type
            >::type,
            typename Next::template apply<next, Y>::type
        > type;
    };
};

template<class Next>
struct solve_leading_zeroes {
    template<class Begin>
    struct apply {
        typedef list<
            typename Begin::item,
            typename Next::template apply<typename Begin::next>::type
        > type;
    };
    typedef solve_leading_zeroes type;
};

template<>
struct solve_leading_zeroes<no_solution> {
    typedef no_solution type;
};

template<class Next>
struct solve_first_non_zero {
    template<class Begin>
    struct apply {
        typedef typename Next::template apply<
            typename Begin::next,
            typename Begin::item
        >::type type;
    };
};

template<class Next>
struct solve_internal_zero {
    template<class Begin, class Y>
    struct apply {
        typedef list<
            typename Begin::item,
            typename Next::template apply<typename Begin::next, Y>::type
        > type;
    };
};

template<class T>
struct make_solve_list_internal_zero {
    template<class Next, class X>
    struct apply {
        typedef solve_normal<T, X, Next> type;
    };
};

template<>
struct make_solve_list_internal_zero<static_rational<0> > {
    template<class Next, class X>
    struct apply {
        typedef solve_internal_zero<Next> type;
    };
};

template<int N>
struct make_solve_list_normal {
    template<class Begin, class X>
    struct apply {
        typedef typename make_solve_list_internal_zero<
            typename Begin::item
        >::template apply<
            typename make_solve_list_normal<N-1>::template apply<typename Begin::next, X>::type,
            X
        >::type type;
    };
};

template<>
struct make_solve_list_normal<0> {
    template<class Begin, class X>
    struct apply {
        typedef solve_end type;
    };
};

template<int N>
struct make_solve_list_leading_zeroes;

template<class T>
struct make_solve_list_first_non_zero {
    template<class Begin, int N>
    struct apply {
        typedef solve_first_non_zero<
            typename make_solve_list_normal<N-1>::template apply<
                typename Begin::next,
                typename Begin::item
            >::type
        > type;
    };
};

template<>
struct make_solve_list_first_non_zero<static_rational<0> > {
    template<class Begin, int N>
    struct apply {
        typedef typename solve_leading_zeroes<
            typename make_solve_list_leading_zeroes<N-1>::template apply<
                typename Begin::next
            >::type
        >::type type;
    };
};

template<int N>
struct make_solve_list_leading_zeroes {
    template<class Begin>
    struct apply {
        typedef typename make_solve_list_first_non_zero<typename Begin::item>::template apply<Begin, N>::type type;
    };
};

template<>
struct make_solve_list_leading_zeroes<0> {
    template<class Begin>
    struct apply {
        typedef no_solution type;
    };
};

template<int N>
struct try_add_unit_impl {
    template<class Begin, class L>
    struct apply {
        typedef typename try_add_unit_impl<N-1>::template apply<typename Begin::next, L>::type next;
        typedef typename Begin::item::template apply<next>::type type;
        BOOST_STATIC_ASSERT((next::size::value - 1 == type::size::value));
    };
};

template<>
struct try_add_unit_impl<0> {
    template<class Begin, class L>
    struct apply {
        typedef L type;
    };
};

template<int N>
struct make_homogeneous_system_impl;

template<class T, bool is_done>
struct make_homogeneous_system_func;

template<class T>
struct make_homogeneous_system_func<T, false> {
    template<class Begin, class Current, class Units, class Dimensions, int N>
    struct apply {
        typedef typename make_homogeneous_system_impl<N-1>::template apply<
            typename Begin::next,
            list<T, Current>,
            list<typename Begin::item, Units>,
            Dimensions
        >::type type;
    };
};

template<class T>
struct make_homogeneous_system_func<T, true> {
    template<class Begin, class Current, class Units, class Dimensions, int N>
    struct apply {
        typedef list<typename Begin::item, Units> type;
    };
};

template<>
struct make_homogeneous_system_func<no_solution, false> {
    template<class Begin, class Current, class Units, class Dimensions, int N>
    struct apply {
        typedef typename make_homogeneous_system_impl<N-1>::template apply<
            typename Begin::next,
            Current,
            Units,
            Dimensions
        >::type type;
    };
};

template<>
struct make_homogeneous_system_func<no_solution, true> {
    template<class Begin, class Current, class Units, class Dimensions, int N>
    struct apply {
        typedef typename make_homogeneous_system_impl<N-1>::template apply<
            typename Begin::next,
            Current,
            Units,
            Dimensions
        >::type type;
    };
};

template<int N>
struct make_homogeneous_system_impl {
    template<class Begin, class Current, class Units, class Dimensions>
    struct apply {
        typedef typename expand_dimensions<Dimensions::size::value>::template apply<
            Dimensions,
            typename Begin::item::dimension_type
        >::type dimensions;
        typedef typename try_add_unit_impl<Current::size::value>::template apply<Current, dimensions>::type new_element;
        typedef typename make_solve_list_leading_zeroes<new_element::size::value>::template apply<new_element>::type new_func;
        typedef typename make_homogeneous_system_func<
            new_func,
            ((Current::size::value)+1) == (Dimensions::size::value)
        >::template apply<Begin, Current, Units, Dimensions, N>::type type;
    };
};

template<>
struct make_homogeneous_system_impl<0> {
    template<class Begin, class Current, class Units, class Dimensions>
    struct apply {
        typedef Units type;
    };
};

template<class Units>
struct make_homogeneous_system {
    typedef typename find_base_dimensions<Units>::type base_dimensions;
    typedef homogeneous_system<
        typename insertion_sort<
            typename make_homogeneous_system_impl<
                Units::size::value
            >::template apply<
                Units,
                dimensionless_type,
                dimensionless_type,
                base_dimensions
            >::type
        >::type
    > type;
};

template<int N>
struct extract_base_units {
    template<class Begin, class T>
    struct apply {
        typedef list<
            typename Begin::item::tag_type,
            typename extract_base_units<N-1>::template apply<typename Begin::next, T>::type
        > type;
    };
};

template<>
struct extract_base_units<0> {
    template<class Begin, class T>
    struct apply {
        typedef T type;
    };
};

}

}

}

#endif


/**
 * @file methods/ann/rnn_impl.hpp
 * @author Marcus Edel
 *
 * Definition of the RNN class, which implements recurrent neural networks.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#ifndef MLPACK_METHODS_ANN_RNN_IMPL_HPP
#define MLPACK_METHODS_ANN_RNN_IMPL_HPP

// In case it hasn't been included yet.
#include "rnn.hpp"

#include "visitor/load_output_parameter_visitor.hpp"
#include "visitor/save_output_parameter_visitor.hpp"
#include "visitor/forward_visitor.hpp"
#include "visitor/backward_visitor.hpp"
#include "visitor/reset_cell_visitor.hpp"
#include "visitor/deterministic_set_visitor.hpp"
#include "visitor/gradient_set_visitor.hpp"
#include "visitor/gradient_visitor.hpp"
#include "visitor/weight_set_visitor.hpp"

#include <boost/serialization/variant.hpp>

namespace mlpack {
namespace ann /** Artificial Neural Network. */ {

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::RNN(
    const size_t rho,
    const bool single,
    OutputLayerType outputLayer,
    InitializationRuleType initializeRule) :
    rho(rho),
    outputLayer(std::move(outputLayer)),
    initializeRule(std::move(initializeRule)),
    inputSize(0),
    outputSize(0),
    targetSize(0),
    reset(false),
    single(single),
    numFunctions(0),
    deterministic(true)
{
  /* Nothing to do here */
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::~RNN()
{
  for (LayerTypes<CustomLayers...>& layer : network)
  {
    boost::apply_visitor(deleteVisitor, layer);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType>
typename std::enable_if<
      HasMaxIterations<OptimizerType, size_t&(OptimizerType::*)()>
      ::value, void>::type
RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::
WarnMessageMaxIterations(OptimizerType& optimizer, size_t samples) const
{
  if (optimizer.MaxIterations() < samples &&
      optimizer.MaxIterations() != 0)
  {
    Log::Warn << "The optimizer's maximum number of iterations "
              << "is less than the size of the dataset; the "
              << "optimizer will not pass over the entire "
              << "dataset. To fix this, modify the maximum "
              << "number of iterations to be at least equal "
              << "to the number of points of your dataset "
              << "(" << samples << ")." << std::endl;
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType>
typename std::enable_if<
      !HasMaxIterations<OptimizerType, size_t&(OptimizerType::*)()>
      ::value, void>::type
RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::
WarnMessageMaxIterations(OptimizerType& /* optimizer */,
                         size_t /* samples */) const
{
  return;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType, typename... CallbackTypes>
double RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Train(
    arma::cube predictors,
    arma::cube responses,
    OptimizerType& optimizer,
    CallbackTypes&&... callbacks)
{
  numFunctions = responses.n_cols;

  this->predictors = std::move(predictors);
  this->responses = std::move(responses);

  this->deterministic = true;
  ResetDeterministic();

  if (!reset)
  {
    ResetParameters();
  }

  WarnMessageMaxIterations<OptimizerType>(optimizer, this->predictors.n_cols);

  // Train the model.
  Timer::Start("rnn_optimization");
  const double out = optimizer.Optimize(*this, parameter, callbacks...);
  Timer::Stop("rnn_optimization");

  Log::Info << "RNN::RNN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetCells()
{
  for (size_t i = 1; i < network.size(); ++i)
  {
    boost::apply_visitor(ResetCellVisitor(rho), network[i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename OptimizerType, typename... CallbackTypes>
double RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Train(
    arma::cube predictors,
    arma::cube responses,
    CallbackTypes&&... callbacks)
{
  numFunctions = responses.n_cols;

  this->predictors = std::move(predictors);
  this->responses = std::move(responses);

  this->deterministic = true;
  ResetDeterministic();

  if (!reset)
  {
    ResetParameters();
  }

  OptimizerType optimizer;

  WarnMessageMaxIterations<OptimizerType>(optimizer, this->predictors.n_cols);

  // Train the model.
  Timer::Start("rnn_optimization");
  const double out = optimizer.Optimize(*this, parameter, callbacks...);
  Timer::Stop("rnn_optimization");

  Log::Info << "RNN::RNN(): final objective of trained model is " << out
      << "." << std::endl;
  return out;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Predict(
    arma::cube predictors, arma::cube& results, const size_t batchSize)
{
  ResetCells();

  if (parameter.is_empty())
  {
    ResetParameters();
  }

  if (!deterministic)
  {
    deterministic = true;
    ResetDeterministic();
  }

  const size_t effectiveBatchSize = std::min(batchSize,
      size_t(predictors.n_cols));

  Forward(arma::mat(predictors.slice(0).colptr(0), predictors.n_rows,
      effectiveBatchSize, false, true));
  arma::mat resultsTemp = boost::apply_visitor(outputParameterVisitor,
      network.back());

  outputSize = resultsTemp.n_rows;
  results = arma::zeros<arma::cube>(outputSize, predictors.n_cols, rho);
  results.slice(0).submat(0, 0, results.n_rows - 1,
      effectiveBatchSize - 1) = resultsTemp;

  // Process in accordance with the given batch size.
  for (size_t begin = 0; begin < predictors.n_cols; begin += batchSize)
  {
    const size_t effectiveBatchSize = std::min(batchSize,
        size_t(predictors.n_cols - begin));
    for (size_t seqNum = !begin; seqNum < rho; ++seqNum)
    {
      Forward(arma::mat(predictors.slice(seqNum).colptr(begin),
          predictors.n_rows, effectiveBatchSize, false, true));

      results.slice(seqNum).submat(0, begin, results.n_rows - 1, begin +
          effectiveBatchSize - 1) = boost::apply_visitor(outputParameterVisitor,
          network.back());
    }
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& /* parameters */,
    const size_t begin,
    const size_t batchSize,
    const bool deterministic)
{
  if (parameter.is_empty())
  {
    ResetParameters();
  }

  if (deterministic != this->deterministic)
  {
    this->deterministic = deterministic;
    ResetDeterministic();
  }

  if (!inputSize)
  {
    inputSize = predictors.n_rows;
    targetSize = responses.n_rows;
  }
  else if (targetSize == 0)
  {
    targetSize = responses.n_rows;
  }

  ResetCells();

  double performance = 0;
  size_t responseSeq = 0;

  for (size_t seqNum = 0; seqNum < rho; ++seqNum)
  {
    // Wrap a matrix around our data to avoid a copy.
    arma::mat stepData(predictors.slice(seqNum).colptr(begin),
        predictors.n_rows, batchSize, false, true);
    Forward(stepData);
    if (!single)
    {
      responseSeq = seqNum;
    }

    performance += outputLayer.Forward(boost::apply_visitor(
        outputParameterVisitor, network.back()),
        arma::mat(responses.slice(responseSeq).colptr(begin),
            responses.n_rows, batchSize, false, true));
  }

  if (outputSize == 0)
  {
    outputSize = boost::apply_visitor(outputParameterVisitor,
        network.back()).n_elem / batchSize;
  }

  return performance;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
double RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Evaluate(
    const arma::mat& parameters,
    const size_t begin,
    const size_t batchSize)
{
  return Evaluate(parameters, begin, batchSize, true);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename GradType>
double RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::
EvaluateWithGradient(const arma::mat& /* parameters */,
                     const size_t begin,
                     GradType& gradient,
                     const size_t batchSize)
{
  // Initialize passed gradient.
  if (gradient.is_empty())
  {
    if (parameter.is_empty())
    {
      ResetParameters();
    }

    gradient = arma::zeros<arma::mat>(parameter.n_rows, parameter.n_cols);
  }
  else
  {
    gradient.zeros();
  }

  if (this->deterministic)
  {
    this->deterministic = false;
    ResetDeterministic();
  }

  if (!inputSize)
  {
    inputSize = predictors.n_rows;
    targetSize = responses.n_rows;
  }
  else if (targetSize == 0)
  {
    targetSize = responses.n_rows;
  }

  ResetCells();

  double performance = 0;
  size_t responseSeq = 0;
  const size_t effectiveRho = std::min(rho, size_t(responses.size()));

  for (size_t seqNum = 0; seqNum < effectiveRho; ++seqNum)
  {
    // Wrap a matrix around our data to avoid a copy.
    arma::mat stepData(predictors.slice(seqNum).colptr(begin),
        predictors.n_rows, batchSize, false, true);
    Forward(stepData);
    if (!single)
    {
      responseSeq = seqNum;
    }

    for (size_t l = 0; l < network.size(); ++l)
    {
      boost::apply_visitor(SaveOutputParameterVisitor(moduleOutputParameter),
          network[l]);
    }

    performance += outputLayer.Forward(boost::apply_visitor(
        outputParameterVisitor, network.back()),
        arma::mat(responses.slice(responseSeq).colptr(begin),
            responses.n_rows, batchSize, false, true));
  }

  if (outputSize == 0)
  {
    outputSize = boost::apply_visitor(outputParameterVisitor,
        network.back()).n_elem / batchSize;
  }

  // Initialize current/working gradient.
  if (currentGradient.is_empty())
  {
    currentGradient = arma::zeros<arma::mat>(parameter.n_rows,
        parameter.n_cols);
  }

  ResetGradients(currentGradient);

  for (size_t seqNum = 0; seqNum < effectiveRho; ++seqNum)
  {
    currentGradient.zeros();
    for (size_t l = 0; l < network.size(); ++l)
    {
      boost::apply_visitor(LoadOutputParameterVisitor(moduleOutputParameter),
          network[network.size() - 1 - l]);
    }

    if (single && seqNum > 0)
    {
      error.zeros();
    }
    else if (single && seqNum == 0)
    {
      outputLayer.Backward(boost::apply_visitor(
          outputParameterVisitor, network.back()),
          arma::mat(responses.slice(0).colptr(begin),
          responses.n_rows, batchSize, false, true), error);
    }
    else
    {
      outputLayer.Backward(boost::apply_visitor(
          outputParameterVisitor, network.back()),
          arma::mat(responses.slice(effectiveRho - seqNum - 1).colptr(begin),
          responses.n_rows, batchSize, false, true), error);
    }

    Backward();
    Gradient(
        arma::mat(predictors.slice(effectiveRho - seqNum - 1).colptr(begin),
        predictors.n_rows, batchSize, false, true));
    gradient += currentGradient;
  }

  return performance;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Gradient(
    const arma::mat& parameters,
    const size_t begin,
    arma::mat& gradient,
    const size_t batchSize)
{
  this->EvaluateWithGradient(parameters, begin, gradient, batchSize);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Shuffle()
{
  arma::cube newPredictors, newResponses;
  math::ShuffleData(predictors, responses, newPredictors, newResponses);

  predictors = std::move(newPredictors);
  responses = std::move(newResponses);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetParameters()
{
  ResetDeterministic();

  // Reset the network parameter with the given initialization rule.
  NetworkInitialization<InitializationRuleType,
                        CustomLayers...> networkInit(initializeRule);
  networkInit.Initialize(network, parameter);

  reset = true;
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Reset()
{
  ResetParameters();
  ResetCells();
  currentGradient.zeros();
  ResetGradients(currentGradient);
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetDeterministic()
{
  DeterministicSetVisitor deterministicSetVisitor(deterministic);
  std::for_each(network.begin(), network.end(),
      boost::apply_visitor(deterministicSetVisitor));
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::ResetGradients(
    arma::mat& gradient)
{
  size_t offset = 0;
  for (LayerTypes<CustomLayers...>& layer : network)
  {
    offset += boost::apply_visitor(GradientSetVisitor(gradient, offset), layer);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename InputType>
void RNN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Forward(const InputType& input)
{
  boost::apply_visitor(ForwardVisitor(input,
      boost::apply_visitor(outputParameterVisitor, network.front())),
      network.front());

  for (size_t i = 1; i < network.size(); ++i)
  {
    boost::apply_visitor(ForwardVisitor(
        boost::apply_visitor(outputParameterVisitor, network[i - 1]),
        boost::apply_visitor(outputParameterVisitor, network[i])),
        network[i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
void RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::Backward()
{
  boost::apply_visitor(BackwardVisitor(
        boost::apply_visitor(outputParameterVisitor, network.back()),
        error, boost::apply_visitor(deltaVisitor,
        network.back())), network.back());

  for (size_t i = 2; i < network.size(); ++i)
  {
    boost::apply_visitor(BackwardVisitor(
        boost::apply_visitor(outputParameterVisitor,
        network[network.size() - i]), boost::apply_visitor(
        deltaVisitor, network[network.size() - i + 1]),
        boost::apply_visitor(deltaVisitor, network[network.size() - i])),
        network[network.size() - i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename InputType>
void RNN<OutputLayerType, InitializationRuleType,
         CustomLayers...>::Gradient(const InputType& input)
{
  boost::apply_visitor(GradientVisitor(input,
      boost::apply_visitor(deltaVisitor, network[1])), network.front());

  for (size_t i = 1; i < network.size() - 1; ++i)
  {
    boost::apply_visitor(GradientVisitor(
        boost::apply_visitor(outputParameterVisitor, network[i - 1]),
        boost::apply_visitor(deltaVisitor, network[i + 1])),
        network[i]);
  }
}

template<typename OutputLayerType, typename InitializationRuleType,
         typename... CustomLayers>
template<typename Archive>
void RNN<OutputLayerType, InitializationRuleType, CustomLayers...>::serialize(
    Archive& ar, const unsigned int version)
{
  ar & BOOST_SERIALIZATION_NVP(parameter);
  ar & BOOST_SERIALIZATION_NVP(rho);
  ar & BOOST_SERIALIZATION_NVP(single);
  ar & BOOST_SERIALIZATION_NVP(inputSize);
  ar & BOOST_SERIALIZATION_NVP(outputSize);
  ar & BOOST_SERIALIZATION_NVP(targetSize);

  // Earlier versions of the RNN code did not serialize the 'reset' variable.
  if (version > 0)
  {
    ar & BOOST_SERIALIZATION_NVP(reset);
  }

  if (Archive::is_loading::value)
  {
    std::for_each(network.begin(), network.end(),
        boost::apply_visitor(deleteVisitor));
    network.clear();
  }

  ar & BOOST_SERIALIZATION_NVP(network);

  // If we are loading, we need to initialize the weights.
  if (Archive::is_loading::value)
  {
    // Earlier versions of the RNN code assumed that the weights needed to be
    // reset on load.
    if (version == 0)
      reset = false;

    size_t offset = 0;
    for (LayerTypes<CustomLayers...>& layer : network)
    {
      offset += boost::apply_visitor(WeightSetVisitor(parameter, offset),
          layer);

      boost::apply_visitor(resetVisitor, layer);
    }

    deterministic = true;
    ResetDeterministic();
  }
}

} // namespace ann
} // namespace mlpack

#endif


/**
 * @file test_constrained_ik.cpp
 * @brief Test Fixtures
 *
 * Consolidate variable-definitions and init functions for use by multiple tests.
 *
 * @author dsolomon
 * @date Sep 23, 2013
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2013, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <gtest/gtest.h>
#include <ros/ros.h>
#include "constrained_ik/constrained_ik.h"
#include "constrained_ik/constraints/goal_pose.h"
#include "constrained_ik/constraints/goal_position.h"
#include "constrained_ik/constraints/goal_orientation.h"
#include "constrained_ik/constraints/avoid_obstacles.h"
#include "constrained_ik/constrained_ik_utils.h"
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/joint_model_group.h>
#include <moveit/collision_plugin_loader/collision_plugin_loader.h>
#include <boost/random/uniform_int_distribution.hpp>

using constrained_ik::Constrained_IK;
using constrained_ik::basic_kin::BasicKin;
using Eigen::Affine3d;
using Eigen::VectorXd;
using Eigen::MatrixXd;
using Eigen::JacobiSVD;

const std::string GROUP_NAME = "manipulator"; /**< Default group name for tests */
const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; /**< Default ROS parameter for robot description */

/** @brief This test the Constrained_IK null space project calculation */
TEST(constrained_ik, nullspaceprojection)
{
  int rows = 6;
  int cols = 8;
  MatrixXd A = MatrixXd::Random(rows, cols);

  JacobiSVD<MatrixXd> svd(A,Eigen::ComputeFullV | Eigen::ComputeFullU);
  MatrixXd V(svd.matrixV());
  MatrixXd U(svd.matrixU());
  Eigen::MatrixXd S(rows,cols);
  // TODO learn how to initialize eigen matrices with zero()
  for(int i=0; i<rows; i++)
  {
    for(int j=0; j<cols; j++) S(i,j) = 0.0;
  }
  VectorXd s = svd.singularValues();
  for(int i=0; i<rows-3; i++)
  {
    S(i,i) = s(i);
  }

  MatrixXd A2 = U * S * V.transpose();

  Constrained_IK CIK;

  MatrixXd P1 = CIK.calcNullspaceProjectionTheRightWay(A2);
  EXPECT_EQ(P1.rows(), cols);
  EXPECT_EQ(P1.cols(), cols);
  MatrixXd P2 = CIK.calcNullspaceProjection(A2);
  EXPECT_EQ(P2.rows(), cols);
  EXPECT_EQ(P2.cols(), cols);
  VectorXd testv1 = VectorXd::Random(cols);
  VectorXd P1v = P1*testv1;
  VectorXd P2v = P2*testv1;
  VectorXd AP1v = A2*P1v;
  VectorXd AP2v = A2*P2v;
  double norm1 = AP1v.norm();
  double norm2 = AP2v.norm();
  EXPECT_LT(norm1, .00000001);
  EXPECT_LT(norm2, .00000001);
}

/**
 * @brief Constrained_IK Test Fixtures
 * Consolidate variable-definitions and init functions for use by multiple tests.
 */
class BasicIKTest : public ::testing::Test
{
protected:

  robot_model_loader::RobotModelLoaderPtr loader_;  /**< Used to load the robot model */
  moveit::core::RobotModelPtr robot_model_; /**< Robot model */
  planning_scene::PlanningScenePtr planning_scene_; /**< Planning scene for the current robot model */
  BasicKin kin; /**< Basic Kinematic Model of the robot.  */
  Constrained_IK ik; /**< The Constrained IK Solver */
  Affine3d homePose; /**< Cartesian home position */
  constrained_ik::ConstrainedIKConfiguration config; /**< Constrained IK configuration parameters */

  /** @brief See base class for documention */
  virtual void SetUp()
  {
    loader_.reset(new robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_PARAM));
    robot_model_ = loader_->getModel();

    ASSERT_TRUE(robot_model_ != nullptr);
    ASSERT_TRUE(kin.init(robot_model_->getJointModelGroup(GROUP_NAME)));
    ASSERT_TRUE(kin.calcFwdKin(VectorXd::Zero(6), homePose));
    ASSERT_NO_THROW(ik.init(kin));
    ASSERT_NO_THROW(planning_scene_.reset(new planning_scene::PlanningScene(robot_model_)));
    ASSERT_NO_THROW(config = ik.getSolverConfiguration());

  }
};

/** @brief This tests the Constrained_IK init functions */
TEST_F(BasicIKTest, inputValidation)
{
  EXPECT_ANY_THROW(Constrained_IK().init(BasicKin()));
  EXPECT_NO_THROW(Constrained_IK().init(kin));

  EXPECT_EQ(Constrained_IK().checkInitialized(), constrained_ik::initialization_state::NothingInitialized);
  constrained_ik::Constraint *goal_ptr = new  constrained_ik::constraints::GoalPosition();
  ik.addConstraint(goal_ptr, constrained_ik::constraint_types::Primary);
  EXPECT_NE(ik.checkInitialized(), constrained_ik::initialization_state::NothingInitialized);
}

/** @brief This performs input validation test for the Constrained_IK calcInvKin function */
TEST_F(BasicIKTest, calcInvKinInputValidation)
{
  VectorXd seed = VectorXd::Zero(6);
  VectorXd joints;
  constrained_ik::Constraint *goal_ptr = new  constrained_ik::constraints::GoalPosition();
  ik.addConstraint(goal_ptr, constrained_ik::constraint_types::Primary);
  EXPECT_NE(ik.checkInitialized(), constrained_ik::initialization_state::NothingInitialized);
  EXPECT_ANY_THROW(Constrained_IK().calcInvKin(Affine3d::Identity(), seed, joints));      // un-init Constrained_IK
  EXPECT_ANY_THROW(ik.calcInvKin(Affine3d(Eigen::Matrix4d::Zero()), seed, joints)); // empty Pose (zeros in matrix because unitary rotation matrix is often in memory)
  EXPECT_ANY_THROW(ik.calcInvKin(homePose, VectorXd(), joints));                    // un-init Seed
  EXPECT_ANY_THROW(ik.calcInvKin(homePose, VectorXd::Zero(99), joints));            // wrong-sized Seed

  // valid input
  //   - GTest doesn't print exception info.  This method allows us to print that info, for easier troubleshooting
  try
  {
    ik.calcInvKin(homePose, seed, joints);
    SUCCEED();
  } catch (const std::exception &ex)
  {
    ADD_FAILURE() << ex.what();
  }
}

/** @brief This tests the Constrained_IK calcInvKin function against known poses using only primary constraint */
TEST_F(BasicIKTest, knownPoses)
{
  Affine3d pose, rslt_pose;
  VectorXd seed, expected(6), joints;
  ik.loadDefaultSolverConfiguration();
  config = ik.getSolverConfiguration();

  // seed position *at* expected solution
  expected << M_PI_2, -M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, -M_PI_2;
  seed = expected;
  EXPECT_TRUE(kin.calcFwdKin(expected, pose)); // expect the pose to remain unchanged

  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
    "*at* from expected: " << expected.transpose() << std::endl;

  // adding primary constraints to move to a pose
  ik.clearConstraintList();
  constrained_ik::Constraint *goal_pose_ptr = new  constrained_ik::constraints::GoalPose();
  ik.addConstraint(goal_pose_ptr, constrained_ik::constraint_types::Primary);

  // seed = expected, pose = fwdK(expected)
  EXPECT_NO_THROW(ik.calcInvKin(pose, seed, joints));
  std::cout << "joint values returned" << joints << std::endl;
  EXPECT_TRUE(joints.isApprox(expected, 1e-10));

  // seed position *near* expected solution
  seed = expected + 0.01 * VectorXd::Random(expected.size());
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*near* from expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *near* #2
  seed = expected + 0.05 * VectorXd::Random(expected.size());
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*near* from expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // seed position *near* expected solution
  seed = expected + 0.1 * VectorXd::Random(expected.size());
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*near* from expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // test with seed far from expected position
  expected << M_PI_2, 0, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*far1* from expected: " << expected.transpose() << std::endl;
  config.primary_gain = 0.5;
  config.limit_primary_motion = false;
  ik.setSolverConfiguration(config);
  EXPECT_TRUE(ik.calcInvKin(pose, VectorXd::Zero(expected.size()), joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *far* #2
  expected << 0, -M_PI_2, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*far2* from expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, VectorXd::Zero(expected.size()), joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *farther*
  expected << M_PI_4, -M_PI_4, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*farther* from expected: " << expected.transpose() << std::endl;
  seed = VectorXd::Zero(expected.size());
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *very far*
  expected << M_PI_2, -M_PI_2, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*very far* from expected: " << expected.transpose() << std::endl;
  seed = VectorXd::Zero(expected.size());
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));
}

/** @brief This tests the Constrained_IK calcInvKin function null space motion */
TEST_F(BasicIKTest, NullMotion)
{
  Affine3d pose, rslt_pose;
  VectorXd seed, expected(6), joints;
  boost::random::mt19937 rng;
  ik.loadDefaultSolverConfiguration();
  config = ik.getSolverConfiguration();
  config.limit_auxiliary_interations = false;
  config.limit_auxiliary_motion = false;
  config.limit_primary_motion = false;
  ik.setSolverConfiguration(config);

  // adding primary constraints to move to a position, and auxillary constraints to move to pose
  ik.clearConstraintList();
  constrained_ik::Constraint *goal_position_ptr = new  constrained_ik::constraints::GoalPosition();
  ik.addConstraint(goal_position_ptr, constrained_ik::constraint_types::Primary);
  constrained_ik::Constraint *goal_orientation_ptr = new  constrained_ik::constraints::GoalOrientation();
  ik.addConstraint(goal_orientation_ptr, constrained_ik::constraint_types::Auxiliary);

  // seed position *at* expected solution
  expected << M_PI_2, -M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, -M_PI_2;

  kin.calcFwdKin(expected, pose);
  config.primary_gain = 0.5;
  config.auxiliary_gain = 0.5;
  ik.setSolverConfiguration(config);
  seed = expected;
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*at* from expected: " << expected.transpose() << std::endl;
  ik.calcInvKin(pose, seed, joints);
  EXPECT_TRUE(joints.isApprox(expected, 1e-10));

  // seed position *near* expected solution
  expected << M_PI_2, -M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, -M_PI_2;
  kin.calcFwdKin(expected, pose);
  seed = expected + 0.01 * VectorXd::Random(expected.size());
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*near* from expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *near* #2
  for(int j=0; j<3; j++) // do 3 random examples of near starts
  {
    for(int i=0; i<6; i++)  // find a random pose
    {
      boost::random::uniform_int_distribution<int> angle_degrees(-177, 177) ;
      boost::random::uniform_int_distribution<int> small_angle_degrees(-3, 3) ;
      expected[i] = angle_degrees(rng)* 3.14/180.0;
      seed[i] = expected[i] + small_angle_degrees(rng)*3.14/180.0;
    }
    kin.calcFwdKin(expected, pose);
    config.primary_gain = 0.15;
    config.auxiliary_gain = 0.05;
    ik.setSolverConfiguration(config);
    EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
    EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
    EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
    EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));
  }

  // *far*
  int num_success=0;
  for(int j=0; j<20; j++) // do 3 random examples of far starts
  {
    for(int i=0; i<6; i++)  // find a random pose
    {
      boost::random::uniform_int_distribution<int> angle_degrees(-180+50, 180-50) ;
      boost::random::uniform_int_distribution<int> small_angle_degrees(-50, 50) ;
      expected[i] = angle_degrees(rng)* 3.14/180.0;
      seed[i] = expected[i] + small_angle_degrees(rng)*3.14/180.0;
    }
    kin.calcFwdKin(expected, pose);
    config.primary_gain = 0.15;
    config.auxiliary_gain = 0.05;
    ik.setSolverConfiguration(config);
    ik.calcInvKin(pose, seed, joints);
    kin.calcFwdKin(joints, rslt_pose);
    if(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3) && rslt_pose.translation().isApprox(pose.translation(), 1e-3)) num_success++;
  }
  EXPECT_GE(num_success, 15);

  // *farther*
  num_success=0;
  for(int j=0; j<20; j++) // do 20 random examples of farther starts
  {
    for(int i=0; i<6; i++)  // find a random pose
    {
      boost::random::uniform_int_distribution<int> angle_degrees(-180+90, 180-90) ;
      boost::random::uniform_int_distribution<int> small_angle_degrees(-90, 90) ;
      expected[i] = angle_degrees(rng)* 3.14/180.0;
      seed[i] = expected[i] + small_angle_degrees(rng)*3.14/180.0;
    }
    kin.calcFwdKin(expected, pose);
    config.primary_gain = 0.15;
    config.auxiliary_gain = 0.05;
    ik.setSolverConfiguration(config);
    ik.calcInvKin(pose, seed, joints);
    kin.calcFwdKin(joints, rslt_pose);
    if(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3) && rslt_pose.translation().isApprox(pose.translation(), 1e-3)) num_success++;
  }
  EXPECT_GE(num_success, 10);
}

/** @brief This tests the Constrained_IK calcInvKin function null space motion convergence for known poses*/
TEST_F(BasicIKTest, NullMotionPose)
{
  Affine3d pose, rslt_pose;
  VectorXd seed, expected(6), joints;
  ik.loadDefaultSolverConfiguration();
  config = ik.getSolverConfiguration();

  // adding primary constraints to move to an orientation, and auxillary constraints to move to position
  ik.clearConstraintList();
  constrained_ik::Constraint *goal_orientation_ptr = new  constrained_ik::constraints::GoalOrientation();
  ik.addConstraint(goal_orientation_ptr, constrained_ik::constraint_types::Primary);
  constrained_ik::Constraint *goal_position_ptr = new  constrained_ik::constraints::GoalPosition();
  ik.addConstraint(goal_position_ptr, constrained_ik::constraint_types::Auxiliary);

  // seed position *at* expected solution
  expected << M_PI_2, -M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, -M_PI_2;
  kin.calcFwdKin(expected, pose);
  seed = expected;
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*at* from expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // seed position *near* expected solution
  seed = expected + 0.01 * VectorXd::Random(expected.size());
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*near* expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));
  // *near* #2
  seed = expected + 0.05 * VectorXd::Random(expected.size());
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*near* expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // seed position *near* expected solution
  seed = expected + 0.1 * VectorXd::Random(expected.size());
  std::cout << "Testing seed vector " << seed.transpose() << std::endl <<
               "*near* from expected: " << expected.transpose() << std::endl;
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // test with seed far from expected position
  expected << M_PI_2, 0, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*far1* from expected: " << expected.transpose() << std::endl;
  config.primary_gain = 0.1;
  config.auxiliary_gain = 0.1;
  config.limit_auxiliary_interations = false;
  config.limit_auxiliary_motion = false;
  config.limit_primary_motion = false;
  ik.setSolverConfiguration(config);
  seed = VectorXd::Zero(expected.size());
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *far* #2
  expected << 0, -M_PI_2, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*far2* from expected: " << expected.transpose() << std::endl;
  seed = VectorXd::Zero(expected.size());
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *farther*
  expected << M_PI_4, -M_PI_4, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*farther* from expected: " << expected.transpose() << std::endl;
  seed = VectorXd::Zero(expected.size());
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // *very far*
  expected << M_PI_2, -M_PI_2, 0, 0, 0, 0;
  kin.calcFwdKin(expected, pose);
  std::cout << "Testing seed vector " << VectorXd::Zero(expected.size()).transpose() << std::endl <<
               "*very far* from expected: " << expected.transpose() << std::endl;
  config.primary_gain = 0.1;
  config.auxiliary_gain = 0.001;
  config.solver_max_iterations = 10000;
  ik.setSolverConfiguration(config);
  seed = VectorXd::Zero(expected.size());
  EXPECT_TRUE(ik.calcInvKin(pose, seed, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints, rslt_pose));
  EXPECT_TRUE(rslt_pose.rotation().isApprox(pose.rotation(), 9e-3));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));
}

/**
 * @brief This test the AvoidObstacles constraint
 * The solver is setup with a primary GoalPosition constraint and
 * a axuiliary AvoidObstacles constraint. I then seeds the solver
 * with it current pose with a goal position as the current position
 * and the axuiliary constraint should move the robot in joint space
 * away from its current joint positions while maintaining the starting
 * position of the tcp.
 */
TEST_F(BasicIKTest, obstacleAvoidanceAuxiliaryConstraint)
{
  Affine3d pose, rslt_pose;
  VectorXd seed, expected(6), joints;
  ik.loadDefaultSolverConfiguration();
  config = ik.getSolverConfiguration();
  config.solver_min_iterations = 1;
  config.limit_auxiliary_interations = true;
  ik.setSolverConfiguration(config);

  // adding primary constraints to move to an orientation, and Auxiliary constraints to move to position
  ik.clearConstraintList();
  constrained_ik::Constraint *goal_position_ptr = new  constrained_ik::constraints::GoalPosition();

  std::vector<std::string> link_names;
  ik.getLinkNames(link_names);
  int link_id = link_names.size()/2;
  std::vector<std::string> obstacle_avoidance_link_name;
  constrained_ik::constraints::AvoidObstacles *avoid_obstacles_ptr =  new constrained_ik::constraints::AvoidObstacles();

  obstacle_avoidance_link_name.push_back(link_names[link_id]);
  avoid_obstacles_ptr->setAvoidanceLinks(obstacle_avoidance_link_name);
  avoid_obstacles_ptr->setMinDistance(link_names[link_id], 0.01);
  avoid_obstacles_ptr->setAmplitude(link_names[link_id], 1.0);
  avoid_obstacles_ptr->setAvoidanceDistance(link_names[link_id], 1.0);

  // add the constraints
  ik.addConstraint(goal_position_ptr, constrained_ik::constraint_types::Primary);
  ik.addConstraint(avoid_obstacles_ptr, constrained_ik::constraint_types::Auxiliary);

  // perform inverse kinematics but the position should not change, just the orientation
  expected << M_PI_2, -M_PI_2, -M_PI_2, -M_PI_2, M_PI_2, -M_PI_2;
  kin.calcFwdKin(expected, pose);
  seed = expected;
  config.auxiliary_max_iterations = 1;
  ik.setSolverConfiguration(config);
  EXPECT_TRUE(ik.calcInvKin(pose, seed, planning_scene_, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints,rslt_pose));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));

  // do it again, but his time allow 100 iterations on the auxiliary constriants
  // this allows more obstacle avoidance after the position has converged
  config.auxiliary_max_iterations = 100;
  ik.setSolverConfiguration(config);
  EXPECT_TRUE(ik.calcInvKin(pose, seed, planning_scene_, joints));
  EXPECT_TRUE(kin.calcFwdKin(joints,rslt_pose));
  EXPECT_TRUE(rslt_pose.translation().isApprox(pose.translation(), 1e-3));
}

/**
 * @brief This test checks the consistancy of the axisAngle calculations from a quaternion value
 * Namely does it always return an angle in +/-pi range? (The answer should be yes)
 */
TEST_F(BasicIKTest, consistancy)
{
    Eigen::Vector3d k(Eigen::Vector3d::UnitZ());
    for (double theta=-7.0; theta<7.0; theta+=1.0)
    {
        Eigen::Vector4d expected; expected << k, ik.rangedAngle(theta);

        //create angle-axis representation of theta @ k
        Eigen::AngleAxisd aa(theta, k);
        Eigen::Quaterniond q(aa);
        Eigen::AngleAxisd aa_q(q);

        //stuff results into vectorrd for direct comparison
        Eigen::Vector4d rslt; rslt << aa_q.axis(), aa_q.angle();
        rslt(3) = ik.rangedAngle(aa_q.angle());

        //if theta ~= 0, angleAxis/Quaternion operations will give [1,0,0] as vector
        if (fabs(theta) < 1e-6)
        {
            expected(0) = 1;
            expected(1) = 0;
            expected(2) = 0;
        }

        //rslt may be reverse vector & angle, which is still a valid representation
        EXPECT_TRUE(rslt.isApprox(expected, 1e-5) or rslt.isApprox(-expected, 1e-5));
    }
}
/** @brief This executes all tests for the Constraine_IK Class and its constraints */
int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::init(argc,argv,"test_constrained_ik");
  ros::NodeHandle nh;
  return RUN_ALL_TESTS();
}


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Roshan <thisisroshansmail@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_RANDOM_LINEAR_CONGRUENTIAL_ENGINE_HPP
#define BOOST_COMPUTE_RANDOM_LINEAR_CONGRUENTIAL_ENGINE_HPP

#include <algorithm>

#include <boost/compute/types.hpp>
#include <boost/compute/buffer.hpp>
#include <boost/compute/kernel.hpp>
#include <boost/compute/context.hpp>
#include <boost/compute/program.hpp>
#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/iterator/discard_iterator.hpp>
#include <boost/compute/utility/program_cache.hpp>

namespace boost {
namespace compute {

///
/// \class linear_congruential_engine
/// \brief 'Quick and Dirty' linear congruential engine
///
/// Quick and dirty linear congruential engine to generate low quality
/// random numbers very quickly. For uses in which good quality of random
/// numbers is required(Monte-Carlo Simulations), use other engines like
/// Mersenne Twister instead.
///
template<class T = uint_>
class linear_congruential_engine
{
public:
    typedef T result_type;
    static const T default_seed = 1;
    static const T a = 1099087573;
    static const size_t threads = 1024;

    /// Creates a new linear_congruential_engine and seeds it with \p value.
    explicit linear_congruential_engine(command_queue &queue,
                                        result_type value = default_seed)
        : m_context(queue.get_context()),
          m_multiplicands(m_context, threads * sizeof(result_type))
    {
        // setup program
        load_program();

        // seed state
        seed(value, queue);

        // generate multiplicands
        generate_multiplicands(queue);
    }

    /// Creates a new linear_congruential_engine object as a copy of \p other.
    linear_congruential_engine(const linear_congruential_engine<T> &other)
        : m_context(other.m_context),
          m_program(other.m_program),
          m_seed(other.m_seed),
          m_multiplicands(other.m_multiplicands)
    {
    }

    /// Copies \p other to \c *this.
    linear_congruential_engine<T>&
    operator=(const linear_congruential_engine<T> &other)
    {
        if(this != &other){
            m_context = other.m_context;
            m_program = other.m_program;
            m_seed = other.m_seed;
            m_multiplicands = other.m_multiplicands;
        }

        return *this;
    }

    /// Destroys the linear_congruential_engine object.
    ~linear_congruential_engine()
    {
    }

    /// Seeds the random number generator with \p value.
    ///
    /// \param value seed value for the random-number generator
    /// \param queue command queue to perform the operation
    ///
    /// If no seed value is provided, \c default_seed is used.
    void seed(result_type value, command_queue &queue)
    {
        (void) queue;

        m_seed = value;
    }

    /// \overload
    void seed(command_queue &queue)
    {
        seed(default_seed, queue);
    }

    /// Generates random numbers and stores them to the range [\p first, \p last).
    template<class OutputIterator>
    void generate(OutputIterator first, OutputIterator last, command_queue &queue)
    {
        size_t size = detail::iterator_range_size(first, last);

        kernel fill_kernel(m_program, "fill");
        fill_kernel.set_arg(1, m_multiplicands);
        fill_kernel.set_arg(2, first.get_buffer());

        size_t offset = 0;

        for(;;){
            size_t count = 0;
            if(size > threads){
                count = (std::min)(static_cast<size_t>(threads), size - offset);
            }
            else {
                count = size;
            }
            fill_kernel.set_arg(0, static_cast<const uint_>(m_seed));
            fill_kernel.set_arg(3, static_cast<const uint_>(offset));
            queue.enqueue_1d_range_kernel(fill_kernel, 0, count, 0);

            offset += count;

            if(offset >= size){
                break;
            }

            update_seed(queue);
        }
    }

    /// \internal_
    void generate(discard_iterator first, discard_iterator last, command_queue &queue)
    {
        (void) queue;

        size_t size = detail::iterator_range_size(first, last);
        uint_ max_mult =
            detail::read_single_value<T>(m_multiplicands, threads-1, queue);
        while(size >= threads) {
            m_seed *= max_mult;
            size -= threads;
        }
        m_seed *=
            detail::read_single_value<T>(m_multiplicands, size-1, queue);
    }

    /// Generates random numbers, transforms them with \p op, and then stores
    /// them to the range [\p first, \p last).
    template<class OutputIterator, class Function>
    void generate(OutputIterator first, OutputIterator last, Function op, command_queue &queue)
    {
        vector<T> tmp(std::distance(first, last), queue.get_context());
        generate(tmp.begin(), tmp.end(), queue);
        transform(tmp.begin(), tmp.end(), first, op, queue);
    }

    /// Generates \p z random numbers and discards them.
    void discard(size_t z, command_queue &queue)
    {
        generate(discard_iterator(0), discard_iterator(z), queue);
    }

private:
    /// \internal_
    /// Generates the multiplicands for each thread
    void generate_multiplicands(command_queue &queue)
    {
        kernel multiplicand_kernel =
            m_program.create_kernel("multiplicand");
        multiplicand_kernel.set_arg(0, m_multiplicands);

        queue.enqueue_task(multiplicand_kernel);
    }

    /// \internal_
    void update_seed(command_queue &queue)
    {
        m_seed *=
            detail::read_single_value<T>(m_multiplicands, threads-1, queue);
    }

    /// \internal_
    void load_program()
    {
        boost::shared_ptr<program_cache> cache =
            program_cache::get_global_cache(m_context);

        std::string cache_key =
            std::string("__boost_linear_congruential_engine_") + type_name<T>();

        const char source[] =
            "__kernel void multiplicand(__global uint *multiplicands)\n"
            "{\n"
            "    uint a = 1099087573;\n"
            "    multiplicands[0] = a;\n"
            "    for(uint i = 1; i < 1024; i++){\n"
            "        multiplicands[i] = a * multiplicands[i-1];\n"
            "    }\n"
            "}\n"

            "__kernel void fill(const uint seed,\n"
            "                   __global uint *multiplicands,\n"
            "                   __global uint *result,"
            "                   const uint offset)\n"
            "{\n"
            "    const uint i = get_global_id(0);\n"
            "    result[offset+i] = seed * multiplicands[i];\n"
            "}\n";

        m_program = cache->get_or_build(cache_key, std::string(), source, m_context);
    }

private:
    context m_context;
    program m_program;
    T m_seed;
    buffer m_multiplicands;
};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_RANDOM_LINEAR_CONGRUENTIAL_ENGINE_HPP


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "arap_linear_block.h"
#include "verbose.h"
#include "cotmatrix_entries.h"
#include <Eigen/Dense>

template <typename MatV, typename MatF, typename Scalar>
IGL_INLINE void igl::arap_linear_block(
  const MatV & V,
  const MatF & F,
  const int d,
  const igl::ARAPEnergyType energy,
  Eigen::SparseMatrix<Scalar> & Kd)
{
  switch(energy)
  {
    case ARAP_ENERGY_TYPE_SPOKES:
      return igl::arap_linear_block_spokes(V,F,d,Kd);
      break;
    case ARAP_ENERGY_TYPE_SPOKES_AND_RIMS:
      return igl::arap_linear_block_spokes_and_rims(V,F,d,Kd);
      break;
    case ARAP_ENERGY_TYPE_ELEMENTS:
      return igl::arap_linear_block_elements(V,F,d,Kd);
      break;
    default:
      verbose("Unsupported energy type: %d\n",energy);
      assert(false);
  }
}

template <typename MatV, typename MatF, typename Scalar>
IGL_INLINE void igl::arap_linear_block_spokes(
  const MatV & V,
  const MatF & F,
  const int d,
  Eigen::SparseMatrix<Scalar> & Kd)
{
  using namespace std;
  using namespace Eigen;
  // simplex size (3: triangles, 4: tetrahedra)
  int simplex_size = F.cols();
  // Number of elements
  int m = F.rows();
  // Temporary output
  Matrix<int,Dynamic,2> edges;
  Kd.resize(V.rows(), V.rows());
  vector<Triplet<Scalar> > Kd_IJV;
  if(simplex_size == 3)
  {
    // triangles
    Kd.reserve(7*V.rows());
    Kd_IJV.reserve(7*V.rows());
    edges.resize(3,2);
    edges <<
      1,2,
      2,0,
      0,1;
  }else if(simplex_size == 4)
  {
    // tets
    Kd.reserve(17*V.rows());
    Kd_IJV.reserve(17*V.rows());
    edges.resize(6,2);
    edges <<
      1,2,
      2,0,
      0,1,
      3,0,
      3,1,
      3,2;
  }
  // gather cotangent weights
  Matrix<Scalar,Dynamic,Dynamic> C;
  cotmatrix_entries(V,F,C);
  // should have weights for each edge
  assert(C.cols() == edges.rows());
  // loop over elements
  for(int i = 0;i<m;i++)
  {
    // loop over edges of element
    for(int e = 0;e<edges.rows();e++)
    {
      int source = F(i,edges(e,0));
      int dest = F(i,edges(e,1));
      double v = 0.5*C(i,e)*(V(source,d)-V(dest,d));
      Kd_IJV.push_back(Triplet<Scalar>(source,dest,v));
      Kd_IJV.push_back(Triplet<Scalar>(dest,source,-v));
      Kd_IJV.push_back(Triplet<Scalar>(source,source,v));
      Kd_IJV.push_back(Triplet<Scalar>(dest,dest,-v));
    }
  }
  Kd.setFromTriplets(Kd_IJV.begin(),Kd_IJV.end());
  Kd.makeCompressed();
}

template <typename MatV, typename MatF, typename Scalar>
IGL_INLINE void igl::arap_linear_block_spokes_and_rims(
  const MatV & V,
  const MatF & F,
  const int d,
  Eigen::SparseMatrix<Scalar> & Kd)
{
  using namespace std;
  using namespace Eigen;
  // simplex size (3: triangles, 4: tetrahedra)
  int simplex_size = F.cols();
  // Number of elements
  int m = F.rows();
  // Temporary output
  Kd.resize(V.rows(), V.rows());
  vector<Triplet<Scalar> > Kd_IJV;
  Matrix<int,Dynamic,2> edges;
  if(simplex_size == 3)
  {
    // triangles
    Kd.reserve(7*V.rows());
    Kd_IJV.reserve(7*V.rows());
    edges.resize(3,2);
    edges <<
      1,2,
      2,0,
      0,1;
  }else if(simplex_size == 4)
  {
    // tets
    Kd.reserve(17*V.rows());
    Kd_IJV.reserve(17*V.rows());
    edges.resize(6,2);
    edges <<
      1,2,
      2,0,
      0,1,
      3,0,
      3,1,
      3,2;
    // Not implemented yet for tets
    assert(false);
  }
  // gather cotangent weights
  Matrix<Scalar,Dynamic,Dynamic> C;
  cotmatrix_entries(V,F,C);
  // should have weights for each edge
  assert(C.cols() == edges.rows());
  // loop over elements
  for(int i = 0;i<m;i++)
  {
    // loop over edges of element
    for(int e = 0;e<edges.rows();e++)
    {
      int source = F(i,edges(e,0));
      int dest = F(i,edges(e,1));
      double v = C(i,e)*(V(source,d)-V(dest,d))/3.0;
      // loop over edges again
      for(int f = 0;f<edges.rows();f++)
      {
        int Rs = F(i,edges(f,0));
        int Rd = F(i,edges(f,1));
        if(Rs == source && Rd == dest)
        {
          Kd_IJV.push_back(Triplet<Scalar>(Rs,Rd,v));
          Kd_IJV.push_back(Triplet<Scalar>(Rd,Rs,-v));
        }else if(Rd == source)
        {
          Kd_IJV.push_back(Triplet<Scalar>(Rd,Rs,v));
        }else if(Rs == dest)
        {
          Kd_IJV.push_back(Triplet<Scalar>(Rs,Rd,-v));
        }
      }
      Kd_IJV.push_back(Triplet<Scalar>(source,source,v));
      Kd_IJV.push_back(Triplet<Scalar>(dest,dest,-v));
    }
  }
  Kd.setFromTriplets(Kd_IJV.begin(),Kd_IJV.end());
  Kd.makeCompressed();
}

template <typename MatV, typename MatF, typename Scalar>
IGL_INLINE void igl::arap_linear_block_elements(
  const MatV & V,
  const MatF & F,
  const int d,
  Eigen::SparseMatrix<Scalar> & Kd)
{
  using namespace std;
  using namespace Eigen;
  // simplex size (3: triangles, 4: tetrahedra)
  int simplex_size = F.cols();
  // Number of elements
  int m = F.rows();
  // Temporary output
  Kd.resize(V.rows(), F.rows());
  vector<Triplet<Scalar> > Kd_IJV;
  Matrix<int,Dynamic,2> edges;
  if(simplex_size == 3)
  {
    // triangles
    Kd.reserve(7*V.rows());
    Kd_IJV.reserve(7*V.rows());
    edges.resize(3,2);
    edges <<
      1,2,
      2,0,
      0,1;
  }else if(simplex_size == 4)
  {
    // tets
    Kd.reserve(17*V.rows());
    Kd_IJV.reserve(17*V.rows());
    edges.resize(6,2);
    edges <<
      1,2,
      2,0,
      0,1,
      3,0,
      3,1,
      3,2;
  }
  // gather cotangent weights
  Matrix<Scalar,Dynamic,Dynamic> C;
  cotmatrix_entries(V,F,C);
  // should have weights for each edge
  assert(C.cols() == edges.rows());
  // loop over elements
  for(int i = 0;i<m;i++)
  {
    // loop over edges of element
    for(int e = 0;e<edges.rows();e++)
    {
      int source = F(i,edges(e,0));
      int dest = F(i,edges(e,1));
      double v = C(i,e)*(V(source,d)-V(dest,d));
      Kd_IJV.push_back(Triplet<Scalar>(source,i,v));
      Kd_IJV.push_back(Triplet<Scalar>(dest,i,-v));
    }
  }
  Kd.setFromTriplets(Kd_IJV.begin(),Kd_IJV.end());
  Kd.makeCompressed();
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template IGL_INLINE void igl::arap_linear_block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, double>(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<int, -1, -1, 0, -1, -1> const&, int, igl::ARAPEnergyType, Eigen::SparseMatrix<double, 0, int>&);
#endif