// Copyright (c) 2015-2016 The Bitcoin Core developers
// Copyright (c) 2020 Bitcoin Association
// Distributed under the Open BSV software license, see the accompanying file LICENSE.

#include "consensus/merkle.h"
#include "test/test_bitcoin.h"
#include "task_helpers.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(merkle_tests, TestingSetup)

// Older version of the merkle root computation code, for comparison.
static uint256 BlockBuildMerkleTree(const CBlock &block, bool *fMutated,
                                    std::vector<uint256> &vMerkleTree) {
    vMerkleTree.clear();
    // Safe upper bound for the number of total nodes.
    vMerkleTree.reserve(block.vtx.size() * 2 + 16);
    for (std::vector<CTransactionRef>::const_iterator it(block.vtx.begin());
         it != block.vtx.end(); ++it)
        vMerkleTree.push_back((*it)->GetId());
    int j = 0;
    bool mutated = false;
    for (int nSize = block.vtx.size(); nSize > 1; nSize = (nSize + 1) / 2) {
        for (int i = 0; i < nSize; i += 2) {
            int i2 = std::min(i + 1, nSize - 1);
            if (i2 == i + 1 && i2 + 1 == nSize &&
                vMerkleTree[j + i] == vMerkleTree[j + i2]) {
                // Two identical hashes at the end of the list at a particular
                // level.
                mutated = true;
            }
            vMerkleTree.push_back(
                Hash(vMerkleTree[j + i].begin(), vMerkleTree[j + i].end(),
                     vMerkleTree[j + i2].begin(), vMerkleTree[j + i2].end()));
        }
        j += nSize;
    }
    if (fMutated) {
        *fMutated = mutated;
    }
    return (vMerkleTree.empty() ? uint256() : vMerkleTree.back());
}

// Older version of the merkle branch computation code, for comparison.
static std::vector<uint256>
BlockGetMerkleBranch(const CBlock &block,
                     const std::vector<uint256> &vMerkleTree, int nIndex) {
    std::vector<uint256> vMerkleBranch;
    int j = 0;
    for (int nSize = block.vtx.size(); nSize > 1; nSize = (nSize + 1) / 2) {
        int i = std::min(nIndex ^ 1, nSize - 1);
        vMerkleBranch.push_back(vMerkleTree[j + i]);
        nIndex >>= 1;
        j += nSize;
    }
    return vMerkleBranch;
}

static inline int ctz(uint32_t i) {
    if (i == 0) return 0;
    int j = 0;
    while (!(i & 1)) {
        j++;
        i >>= 1;
    }
    return j;
}

BOOST_AUTO_TEST_CASE(merkle_test) {
    for (int i = 0; i < 32; i++) {
        // Try 32 block sizes: all sizes from 0 to 16 inclusive, and then 15
        // random sizes.
        int ntx = (i <= 16) ? i : 17 + (InsecureRandRange(4000));
        // Try up to 3 mutations.
        for (int mutate = 0; mutate <= 3; mutate++) {
            // The last how many transactions to duplicate first.
            int duplicate1 = mutate >= 1 ? 1 << ctz(ntx) : 0;
            if (duplicate1 >= ntx) {
                // Duplication of the entire tree results in a different root
                // (it adds a level).
                break;
            }

            // The resulting number of transactions after the first duplication.
            int ntx1 = ntx + duplicate1;
            // Likewise for the second mutation.
            int duplicate2 = mutate >= 2 ? 1 << ctz(ntx1) : 0;
            if (duplicate2 >= ntx1) break;
            int ntx2 = ntx1 + duplicate2;
            // And for the third mutation.
            int duplicate3 = mutate >= 3 ? 1 << ctz(ntx2) : 0;
            if (duplicate3 >= ntx2) break;
            int ntx3 = ntx2 + duplicate3;
            // Build a block with ntx different transactions.
            CBlock block;
            block.vtx.resize(ntx);
            for (int j = 0; j < ntx; j++) {
                CMutableTransaction mtx;
                mtx.nLockTime = j;
                block.vtx[j] = MakeTransactionRef(std::move(mtx));
            }
            // Compute the root of the block before mutating it.
            bool unmutatedMutated = false;
            uint256 unmutatedRoot = BlockMerkleRoot(block, &unmutatedMutated);
            BOOST_CHECK(unmutatedMutated == false);
            uint256 newestUnmutatedRoot;
            {
                CMerkleTree newestMerkleTree(block.vtx, uint256(), 0);
                newestUnmutatedRoot = newestMerkleTree.GetMerkleRoot();
            }
            // Optionally mutate by duplicating the last transactions, resulting
            // in the same merkle root.
            block.vtx.resize(ntx3);
            for (int j = 0; j < duplicate1; j++) {
                block.vtx[ntx + j] = block.vtx[ntx + j - duplicate1];
            }
            for (int j = 0; j < duplicate2; j++) {
                block.vtx[ntx1 + j] = block.vtx[ntx1 + j - duplicate2];
            }
            for (int j = 0; j < duplicate3; j++) {
                block.vtx[ntx2 + j] = block.vtx[ntx2 + j - duplicate3];
            }
            // Compute the merkle root and merkle tree using the old mechanism.
            bool oldMutated = false;
            std::vector<uint256> merkleTree;
            uint256 oldRoot =
                BlockBuildMerkleTree(block, &oldMutated, merkleTree);
            // Compute the merkle root using the new mechanism.
            bool newMutated = false;
            uint256 newRoot = BlockMerkleRoot(block, &newMutated);
            uint256 newestRoot;
            {
                CMerkleTree newestMerkleTree(block.vtx, uint256(), 0);
                newestRoot = newestMerkleTree.GetMerkleRoot();
            }
            BOOST_CHECK(oldRoot == newRoot);
            BOOST_CHECK(newRoot == unmutatedRoot);
            BOOST_CHECK((newRoot == uint256()) == (ntx == 0));
            BOOST_CHECK(oldMutated == newMutated);
            BOOST_CHECK(newMutated == !!mutate);
            BOOST_CHECK(newestUnmutatedRoot == oldRoot);
            BOOST_CHECK(newestRoot == oldRoot);
            // If no mutation was done (once for every ntx value), try up to 16
            // branches.
            if (mutate == 0) {
                for (int loop = 0; loop < std::min(ntx, 16); loop++) {
                    // If ntx <= 16, try all branches. Otherise, try 16 random
                    // ones.
                    int mtx = loop;
                    if (ntx > 16) {
                        mtx = InsecureRandRange(ntx);
                    }
                    std::vector<uint256> newBranch =
                        BlockMerkleBranch(block, mtx);
                    std::vector<uint256> oldBranch =
                        BlockGetMerkleBranch(block, merkleTree, mtx);

                    CMerkleTree newestMerkleTree(block.vtx, uint256(), 0);
                    CMerkleTree::MerkleProof newestBranch = newestMerkleTree.GetMerkleProof(block.vtx[mtx]->GetId(), false);
                    BOOST_CHECK(newestBranch.transactionIndex == mtx);

                    BOOST_CHECK(oldBranch == newBranch);
                    BOOST_CHECK(oldBranch == newestBranch.merkleTreeHashes);
                    BOOST_CHECK(
                        ComputeMerkleRootFromBranch(block.vtx[mtx]->GetId(),
                                                    newBranch, mtx) == oldRoot);
                    BOOST_CHECK(
                        ComputeMerkleRootFromBranch(block.vtx[mtx]->GetId(),
                                                    newestBranch.merkleTreeHashes, mtx) == oldRoot);
                }
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(merkle_tree_test)
{
    /* Test blocks with different number of transactions
       Minimum CMerkleTree batch size is set to 4096 transaction ids. That means any merkle tree
       with more than 4096 transaction ids (leaves) will be split into subtrees, each calculated
       in parallel and merged together.
       In this test we use up to 9192 leaves, causing three subtrees to be calculated and merged with
       these 1000 combinations of leaves:
       4096 + 4096 + 1
       4096 + 4096 + 2
       4096 + 4096 + 3
       4096 + 4096 + 4
       ...
       4096 + 4096 + 1000
     */
    int minBatchSize = 4096;
    // Initialize thread pool using 3 threads
    std::unique_ptr<CThreadPool<CQueueAdaptor>> pMerkleTreeThreadPool = std::make_unique<CThreadPool<CQueueAdaptor>>("MerkleTreeThreadPoolTest", 3);
    for (int numberOfTransactions = 2*minBatchSize + 1; numberOfTransactions <= (2*minBatchSize + 1000); ++numberOfTransactions)
    {
        CBlock block;
        block.vtx.resize(numberOfTransactions);
        for (int txIndex = 0; txIndex < numberOfTransactions; ++txIndex)
        {
            CMutableTransaction mtx;
            mtx.nLockTime = txIndex;
            block.vtx[txIndex] = MakeTransactionRef(std::move(mtx));
        }
        // Constructor will create merkle tree by splitting it into two subtrees, parallel calculation and merging them
        CMerkleTree merkleTree(block.vtx, uint256(), 0, pMerkleTreeThreadPool.get());
        uint256 originalMerkleRoot = BlockMerkleRoot(block);
        uint256 newMerkleRoot = merkleTree.GetMerkleRoot();
        // root from CMerkleTree instance must be same as legacy merkle root
        BOOST_CHECK(originalMerkleRoot == newMerkleRoot);
    }
}

BOOST_AUTO_TEST_SUITE_END()


/// Gaze estimator based on 3D model of the eye

////////////////////
/// DEPENDENCIES ///
////////////////////

// ROS 2
#include <rclcpp/rclcpp.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

// ROS 2 interfaces
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <gaze_msgs/msg/pupil_centres_stamped.hpp>
#include <gaze_msgs/msg/gaze_stamped.hpp>
#include <gaze_msgs/msg/gaze_binocular_stamped.hpp>

// Eigen
#include <Eigen/Geometry>

/////////////////
/// CONSTANTS ///
/////////////////

/// The name of this node
const std::string NODE_NAME = "rgbd_gaze";
/// Size of the queue size used by the synchronizer in its policy
const uint8_t SYNCHRONIZER_QUEUE_SIZE = 50;

/// Index of the left eye
const uint8_t EYE_LEFT = 0;
/// Index of the right eye
const uint8_t EYE_RIGHT = 1;

const float VISUAL_EYEBALL_COLOR[] = {0, 0, 0.5, 0.75};

const float VISUAL_PUPIL_CENTRE_SCALE = 0.005;
const float VISUAL_PUPIL_CENTRE_COLOR[] = {0.25, 0.25, 1.0, 1};

const float VISUAL_CORNEA_CENTRE_SCALE = 0.0025;
const float VISUAL_CORNEA_CENTRE_COLOR[] = {0, 0, 0.75, 1};

const float VISUAL_OPTICAL_AXIS_LENGTH = 5.0;
const float VISUAL_OPTICAL_AXIS_WIDTH = 0.001;
const float VISUAL_OPTICAL_AXIS_COLOR[] = {1.0, 0, 0, 1};

const float VISUAL_VISUAL_AXIS_LENGTH = 5.0;
const float VISUAL_VISUAL_AXIS_WIDTH = 0.0015;
const float VISUAL_VISUAL_AXIS_COLOR[] = {0, 1.0, 0, 1};

const float VISUAL_COMPOUND_GAZE_LENGTH = 5.0;
const float VISUAL_COMPOUND_GAZE_WIDTH = 0.002;
const float VISUAL_COMPOUND_GAZE_COLOR[] = {0, 0, 1.0, 1};

/////////////
/// TYPES ///
/////////////

/// Policy of the synchronizer
typedef message_filters::sync_policies::ExactTime<geometry_msgs::msg::PoseStamped,
                                                  gaze_msgs::msg::PupilCentresStamped>
    synchronizer_policy;

////////////////////////
/// HELPER FUNCTIONS ///
////////////////////////

void transform_to_camera_frame(Eigen::ParametrizedLine<double, 3> &parametrized_line, const Eigen::Isometry3d &head_pose)
{
  parametrized_line.origin() = head_pose * parametrized_line.origin();
  parametrized_line.direction() = head_pose.rotation() * parametrized_line.direction();
}

namespace Eigen
{
  gaze_msgs::msg::Gaze toMsg(Eigen::ParametrizedLine<double, 3> &parametrized_line)
  {
    gaze_msgs::msg::Gaze gaze;
    gaze.origin = Eigen::toMsg(parametrized_line.origin());
    Eigen::Vector3d direction = parametrized_line.direction();
    gaze.direction.x = direction[0];
    gaze.direction.y = direction[1];
    gaze.direction.z = direction[2];
    return gaze;
  }
} // namespace Eigen

////////////////////
/// HELPER CLASS ///
////////////////////

class Eye3dModel
{
public:
  /// Position of the eyeball centre wrt. head
  Eigen::Vector3d eyeball_centre;
  /// Radius of the eyeball
  double eyeball_radius;
  /// Horizontal angular deviation between optical and visual axis, rotated around centre of corneal curvature - in radians
  double alpha;
  /// Vertical angular deviation between optical and visual axis, rotated around centre of corneal curvature - in radians
  double beta;
  /// Distance between eyebal centre and the entre of the corneal curvature
  double distance_eyeball_cornea;

  Eigen::ParametrizedLine<double, 3> determine_optical_axis(const Eigen::Isometry3d &head_pose, const Eigen::Vector3d &pupil_centre);
  Eigen::ParametrizedLine<double, 3> determine_visual_axis(const Eigen::ParametrizedLine<double, 3> &optical_axis);
};

Eigen::ParametrizedLine<double, 3> Eye3dModel::determine_optical_axis(const Eigen::Isometry3d &head_pose, const Eigen::Vector3d &pupil_centre)
{
  // Get pupil centre (wrt. head)
  Eigen::Vector3d pupil_centre_wrt_head = head_pose.inverse() * pupil_centre;

  // Determine optical axis unit vector (wrt. head)
  Eigen::Vector3d optical_axis_unit_vect = (pupil_centre_wrt_head - eyeball_centre).normalized();

  // Represent optical axis as parametrized line (wrt. head)
  return Eigen::ParametrizedLine<double, 3>(eyeball_centre, optical_axis_unit_vect);
}

Eigen::ParametrizedLine<double, 3> Eye3dModel::determine_visual_axis(const Eigen::ParametrizedLine<double, 3> &optical_axis)
{
  // Determine visual axis unit vector by the use of kappa (wrt. head)
  Eigen::Quaternion kappa = Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitX());

  // Rotate
  Eigen::Vector3d visual_axis_unit_vect = kappa * optical_axis.direction();

  // Represent visual axis as a parametrized line (wrt. head)
  return Eigen::ParametrizedLine<double, 3>(optical_axis.pointAt(distance_eyeball_cornea), visual_axis_unit_vect);
}

//////////////////
/// NODE CLASS ///
//////////////////

/// Class representation of this node
class RgbdGaze : public rclcpp::Node
{
public:
  /// Constructor
  RgbdGaze();

private:
  /// Subscriber to the head pose
  message_filters::Subscriber<geometry_msgs::msg::PoseStamped> sub_head_pose_;
  /// Subscriber to pupil centres
  message_filters::Subscriber<gaze_msgs::msg::PupilCentresStamped> sub_pupil_centres_;

  /// Synchronizer of the subscribers
  message_filters::Synchronizer<synchronizer_policy> synchronizer_;

  /// Eye model parameters based on specific user calibration
  Eye3dModel eye_models_[2];

  /// Publisher of the optical axes
  rclcpp::Publisher<gaze_msgs::msg::GazeBinocularStamped>::SharedPtr pub_optical_axes_;
  /// Publisher of the visual axes
  rclcpp::Publisher<gaze_msgs::msg::GazeBinocularStamped>::SharedPtr pub_visual_axes_;
  /// Publisher of compound gaze
  rclcpp::Publisher<gaze_msgs::msg::GazeStamped>::SharedPtr pub_compound_gaze_;
  /// Publisher of visualisation markers
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;

  /// Broadcaster of the TF frames
  tf2_ros::TransformBroadcaster tf2_broadcaster_;

  /// Callback called each time a message is received on all topics
  void synchronized_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg_head_pose,
                             const gaze_msgs::msg::PupilCentresStamped::SharedPtr msg_pupil_centres);
};

RgbdGaze::RgbdGaze() : Node(NODE_NAME),
                       sub_head_pose_(this, "head_pose"),
                       sub_pupil_centres_(this, "pupil_centres"),
                       synchronizer_(synchronizer_policy(SYNCHRONIZER_QUEUE_SIZE), sub_head_pose_, sub_pupil_centres_),
                       tf2_broadcaster_(this)
{
  // Synchronize the subscriptions under a single callback
  synchronizer_.registerCallback(&RgbdGaze::synchronized_callback, this);

  // Parameters of the element
  this->declare_parameter<bool>("optical_axis_origin_at_cornea_centre", true);
  this->declare_parameter<bool>("publish_compound_gaze", true);
  this->declare_parameter<bool>("broadcast_tf", false);
  this->declare_parameter<bool>("publish_markers", true);

  // Register publisher of optical axes
  rclcpp::QoS qos_optical_axes = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
  pub_optical_axes_ = this->create_publisher<gaze_msgs::msg::GazeBinocularStamped>("optical_axes", qos_optical_axes);

  // Register publisher of visual axes
  rclcpp::QoS qos_visual_axes = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
  pub_visual_axes_ = this->create_publisher<gaze_msgs::msg::GazeBinocularStamped>("visual_axes", qos_visual_axes);

  // Register publisher of compound gaze
  if (this->get_parameter("publish_compound_gaze").get_value<bool>())
  {
    rclcpp::QoS qos_compound_gaze = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
    pub_compound_gaze_ = this->create_publisher<gaze_msgs::msg::GazeStamped>("compound_gaze", qos_compound_gaze);
  }

  // Register publisher of visualisation markers
  if (this->get_parameter("publish_markers").get_value<bool>())
  {
    rclcpp::QoS qos_markers = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
    pub_markers_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("visualisation_markers", qos_markers);
  }

  // User specific parameters obtained via calibration
  this->declare_parameter<std::string>("user.id", "default");
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    std::string eye_side;
    int8_t side;
    if (eye == EYE_LEFT)
    {
      side = -1;
      eye_side = "left";
    }
    else
    {
      side = 1;
      eye_side = "right";
    }
    eye_models_[eye].eyeball_centre[0] = this->declare_parameter<double>("user.eyes." + eye_side + ".eyeball_centre.x", side * 0.033);
    eye_models_[eye].eyeball_centre[1] = this->declare_parameter<double>("user.eyes." + eye_side + ".eyeball_centre.y", -0.002);
    eye_models_[eye].eyeball_centre[2] = this->declare_parameter<double>("user.eyes." + eye_side + ".eyeball_centre.z", -0.005);
    eye_models_[eye].eyeball_radius = this->declare_parameter<double>("user.eyes." + eye_side + ".eyeball_radius", 0.012);
    eye_models_[eye].alpha = this->declare_parameter<double>("user.eyes." + eye_side + ".kappa.alpha", 0.0);
    eye_models_[eye].beta = this->declare_parameter<double>("user.eyes." + eye_side + ".kappa.beta", 0.0);
    eye_models_[eye].distance_eyeball_cornea = this->declare_parameter<double>("user.eyes." + eye_side + ".distance_eyeball_cornea", 0.0);
  }
  RCLCPP_INFO(this->get_logger(), "Loaded parameters for user \"%s\"", this->get_parameter("user.id").get_value<std::string>().c_str());

  RCLCPP_INFO(this->get_logger(), "Node initialised");
}

void RgbdGaze::synchronized_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg_head_pose,
                                     const gaze_msgs::msg::PupilCentresStamped::SharedPtr msg_pupil_centres)
{
  RCLCPP_DEBUG(this->get_logger(), "Received synchronized messages for processing");

  // Convert head pose ROS2 to Eigen (wrt. camera)
  Eigen::Isometry3d head_pose;
  tf2::fromMsg(msg_head_pose->pose, head_pose);

  // Create output msgs
  gaze_msgs::msg::GazeBinocularStamped optical_axes_msg, visual_axes_msg;
  optical_axes_msg.header = visual_axes_msg.header = msg_head_pose->header;

  Eigen::Vector3d pupil_centres[2];
  Eigen::ParametrizedLine<double, 3> optical_axis[2], visual_axis[2];
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    // Convert pupil centre ROS2 to Eigen (wrt. camera)
    tf2::fromMsg(msg_pupil_centres->pupils.centres[eye], pupil_centres[eye]);

    // Get optical and visual axis (wrt. head pose)
    optical_axis[eye] = eye_models_[eye].determine_optical_axis(head_pose, pupil_centres[eye]);
    visual_axis[eye] = eye_models_[eye].determine_visual_axis(optical_axis[eye]);

    // Transform optical and visual axes into camera coordinate system
    transform_to_camera_frame(optical_axis[eye], head_pose);
    transform_to_camera_frame(visual_axis[eye], head_pose);

    // Fill messages
    optical_axes_msg.gaze[eye] = Eigen::toMsg(optical_axis[eye]);
    visual_axes_msg.gaze[eye] = Eigen::toMsg(visual_axis[eye]);

    // Use cornea centre for optical axes instead of eyeball centre, if desired
    if (this->get_parameter("optical_axis_origin_at_cornea_centre").get_value<bool>())
    {
      optical_axes_msg.gaze[eye].origin = visual_axes_msg.gaze[eye].origin;
    }
  }

  // Publish messages
  pub_optical_axes_->publish(optical_axes_msg);
  pub_visual_axes_->publish(visual_axes_msg);

  Eigen::ParametrizedLine<double, 3> compound_gaze;
  if (this->get_parameter("publish_compound_gaze").get_value<bool>())
  {
    // Create a compound gaze as average of visual axes
    compound_gaze = Eigen::ParametrizedLine<double, 3>(head_pose.translation(), (visual_axis[EYE_LEFT].direction() + visual_axis[EYE_RIGHT].direction()) / 2.0);

    // Create output msg
    gaze_msgs::msg::GazeStamped compound_gaze_msg;
    compound_gaze_msg.header = msg_head_pose->header;

    // Fill message
    compound_gaze_msg.gaze = Eigen::toMsg(compound_gaze);

    // Publish message
    pub_compound_gaze_->publish(compound_gaze_msg);
  }

  if (this->get_parameter("broadcast_tf").get_value<bool>())
  {
    // Quat for flipping the eye gaze
    tf2::Quaternion flip_quat;
    flip_quat.setRPY(0, M_PI, M_PI);

    geometry_msgs::msg::TransformStamped transform;
    transform.header = msg_pupil_centres->header;
    for (uint8_t eye = 0; eye < 2; eye++)
    {
      std::string eye_side;
      if (eye == EYE_LEFT)
      {
        eye_side = "_left";
      }
      else
      {
        eye_side = "_right";
      }

      // Eyeball centre - optical axis
      transform.child_frame_id = "eyeball_centre_optical" + eye_side;
      transform.transform.translation.x = optical_axis[eye].origin()[0];
      transform.transform.translation.y = optical_axis[eye].origin()[1];
      transform.transform.translation.z = optical_axis[eye].origin()[2];
      tf2::Quaternion optical_axis_quat;
      optical_axis_quat.setValue(optical_axis[eye].direction()[0], optical_axis[eye].direction()[1], optical_axis[eye].direction()[2]);
      optical_axis_quat *= flip_quat;
      transform.transform.rotation = tf2::toMsg(optical_axis_quat);
      tf2_broadcaster_.sendTransform(transform);

      // Cornea centre - visual axis
      transform.child_frame_id = "cornea_centre_visual" + eye_side;
      transform.transform.translation.x = visual_axis[eye].origin()[0];
      transform.transform.translation.y = visual_axis[eye].origin()[1];
      transform.transform.translation.z = visual_axis[eye].origin()[2];
      tf2::Quaternion visual_axis_quat;
      visual_axis_quat.setValue(visual_axis[eye].direction()[0], visual_axis[eye].direction()[1], visual_axis[eye].direction()[2]);
      visual_axis_quat *= flip_quat;
      transform.transform.rotation = tf2::toMsg(visual_axis_quat);
      tf2_broadcaster_.sendTransform(transform);
    }
  }

  if (this->get_parameter("publish_markers").get_value<bool>())
  {
    visualization_msgs::msg::MarkerArray markers;
    visualization_msgs::msg::Marker default_marker;
    default_marker.header = msg_pupil_centres->header;
    default_marker.action = visualization_msgs::msg::Marker::ADD;

    for (uint8_t eye = 0; eye < 2; eye++)
    {
      std::string eye_side;
      if (eye == EYE_LEFT)
      {
        eye_side = "_left";
      }
      else
      {
        eye_side = "_right";
      }
      default_marker.ns = std::string(this->get_namespace()) + "gaze" + eye_side;

      // // Pupil centre
      // {
      //   visualization_msgs::msg::Marker pupil_centre_marker = default_marker;
      //   pupil_centre_marker.id = 0;
      //   pupil_centre_marker.type = visualization_msgs::msg::Marker::SPHERE;
      //   pupil_centre_marker.pose.position = Eigen::toMsg(pupil_centres[eye]);
      //   pupil_centre_marker.scale.x =
      //       pupil_centre_marker.scale.y =
      //           pupil_centre_marker.scale.z = VISUAL_PUPIL_CENTRE_SCALE;
      //   pupil_centre_marker.color.r = VISUAL_PUPIL_CENTRE_COLOR[0];
      //   pupil_centre_marker.color.g = VISUAL_PUPIL_CENTRE_COLOR[1];
      //   pupil_centre_marker.color.b = VISUAL_PUPIL_CENTRE_COLOR[2];
      //   pupil_centre_marker.color.a = VISUAL_PUPIL_CENTRE_COLOR[3];
      //   markers.markers.push_back(pupil_centre_marker);
      // }

      // // Eyeball centre
      // {
      //   visualization_msgs::msg::Marker eyeball_centremarker = default_marker;
      //   eyeball_centremarker.id = 1;
      //   eyeball_centremarker.type = visualization_msgs::msg::Marker::SPHERE;
      //   eyeball_centremarker.pose.position = Eigen::toMsg(optical_axis[eye].origin());
      //   eyeball_centremarker.scale.x =
      //       eyeball_centremarker.scale.y =
      //           eyeball_centremarker.scale.z = 2 * eye_models_[eye].eyeball_radius;
      //   eyeball_centremarker.color.r = VISUAL_EYEBALL_COLOR[0];
      //   eyeball_centremarker.color.g = VISUAL_EYEBALL_COLOR[1];
      //   eyeball_centremarker.color.b = VISUAL_EYEBALL_COLOR[2];
      //   eyeball_centremarker.color.a = VISUAL_EYEBALL_COLOR[3];
      //   markers.markers.push_back(eyeball_centremarker);
      // }

      // // Cornea centre
      // {
      //   visualization_msgs::msg::Marker cornea_centre_marker = default_marker;
      //   cornea_centre_marker.id = 2;
      //   cornea_centre_marker.type = visualization_msgs::msg::Marker::SPHERE;
      //   cornea_centre_marker.pose.position = Eigen::toMsg(visual_axis[eye].origin());
      //   cornea_centre_marker.scale.x =
      //       cornea_centre_marker.scale.y =
      //           cornea_centre_marker.scale.z = VISUAL_CORNEA_CENTRE_SCALE;
      //   cornea_centre_marker.color.r = VISUAL_CORNEA_CENTRE_COLOR[0];
      //   cornea_centre_marker.color.g = VISUAL_CORNEA_CENTRE_COLOR[1];
      //   cornea_centre_marker.color.b = VISUAL_CORNEA_CENTRE_COLOR[2];
      //   cornea_centre_marker.color.a = VISUAL_CORNEA_CENTRE_COLOR[3];
      //   markers.markers.push_back(cornea_centre_marker);
      // }

      // // Optical axis
      // {
      //   visualization_msgs::msg::Marker optical_axis_marker = default_marker;
      //   optical_axis_marker.id = 3;
      //   optical_axis_marker.type = visualization_msgs::msg::Marker::ARROW;
      //   geometry_msgs::msg::Point start, end;
      //   start = Eigen::toMsg(optical_axis[eye].origin());
      //   end = Eigen::toMsg(optical_axis[eye].pointAt(VISUAL_OPTICAL_AXIS_LENGTH));
      //   optical_axis_marker.points.push_back(start);
      //   optical_axis_marker.points.push_back(end);
      //   optical_axis_marker.scale.x =
      //       optical_axis_marker.scale.y = VISUAL_OPTICAL_AXIS_WIDTH;
      //   optical_axis_marker.scale.z = 0;
      //   optical_axis_marker.color.r = VISUAL_OPTICAL_AXIS_COLOR[0];
      //   optical_axis_marker.color.g = VISUAL_OPTICAL_AXIS_COLOR[1];
      //   optical_axis_marker.color.b = VISUAL_OPTICAL_AXIS_COLOR[2];
      //   optical_axis_marker.color.a = VISUAL_OPTICAL_AXIS_COLOR[3];
      //   markers.markers.push_back(optical_axis_marker);
      // }

      // Visual axis
      {
        visualization_msgs::msg::Marker visual_axis_marker = default_marker;
        visual_axis_marker.id = 4;
        visual_axis_marker.type = visualization_msgs::msg::Marker::ARROW;
        geometry_msgs::msg::Point start, end;
        start = Eigen::toMsg(visual_axis[eye].origin());
        end = Eigen::toMsg(visual_axis[eye].pointAt(VISUAL_VISUAL_AXIS_LENGTH));
        visual_axis_marker.points.push_back(start);
        visual_axis_marker.points.push_back(end);
        visual_axis_marker.scale.x =
            visual_axis_marker.scale.y = VISUAL_VISUAL_AXIS_WIDTH;
        visual_axis_marker.scale.z = 0;
        visual_axis_marker.color.r = VISUAL_VISUAL_AXIS_COLOR[0];
        visual_axis_marker.color.g = VISUAL_VISUAL_AXIS_COLOR[1];
        visual_axis_marker.color.b = VISUAL_VISUAL_AXIS_COLOR[2];
        visual_axis_marker.color.a = VISUAL_VISUAL_AXIS_COLOR[3];
        markers.markers.push_back(visual_axis_marker);
      }
    }

    // Compound gaze
    if (this->get_parameter("publish_compound_gaze").get_value<bool>())
    {
      visualization_msgs::msg::Marker compound_gaze_marker = default_marker;
      compound_gaze_marker.ns = std::string(this->get_namespace()) + "gaze_compound";
      compound_gaze_marker.id = 0;
      compound_gaze_marker.type = visualization_msgs::msg::Marker::ARROW;
      geometry_msgs::msg::Point start, end;
      start = Eigen::toMsg(compound_gaze.origin());
      end = Eigen::toMsg(compound_gaze.pointAt(VISUAL_COMPOUND_GAZE_LENGTH));
      compound_gaze_marker.points.push_back(start);
      compound_gaze_marker.points.push_back(end);
      compound_gaze_marker.scale.x =
          compound_gaze_marker.scale.y = VISUAL_COMPOUND_GAZE_WIDTH;
      compound_gaze_marker.scale.z = 0;
      compound_gaze_marker.color.r = VISUAL_COMPOUND_GAZE_COLOR[0];
      compound_gaze_marker.color.g = VISUAL_COMPOUND_GAZE_COLOR[1];
      compound_gaze_marker.color.b = VISUAL_COMPOUND_GAZE_COLOR[2];
      compound_gaze_marker.color.a = VISUAL_COMPOUND_GAZE_COLOR[3];
      markers.markers.push_back(compound_gaze_marker);
    }

    pub_markers_->publish(markers);
  }
}

////////////
/// MAIN ///
////////////

/// Main function that initiates an object of `RgbdGaze` class as the core of this node.
int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<RgbdGaze>());
  rclcpp::shutdown();
  return EXIT_SUCCESS;
}


#include <QTest>
#include <QDebug>
#include <Malachite/BlendMode>
#include <random>
#include <boost/range.hpp>

#include "test.h"

using namespace Malachite;

Test::Test(QObject *parent) :
	QObject(parent)
{
}

void Test::test_blend()
{
	constexpr int pixelCount = 10;

	std::random_device randomDevice;
	std::mt19937 randomEngine(randomDevice());
	std::uniform_real_distribution<float> unitDist(0.f, 1.f);

	auto makeRandomOpacity = [&]()
	{
		return unitDist(randomEngine);
	};

	auto makeRandomPixel = [&]()
	{
		float a = makeRandomOpacity();
		float r = makeRandomOpacity() * a;
		float g = makeRandomOpacity() * a;
		float b = makeRandomOpacity() * a;
		return Pixel(a, r, g, b);
	};

	auto makeRandomOpacities = [&]()
	{
		QVector<float> vec(pixelCount);

		for (int i = 0; i < pixelCount; ++i)
			vec[i] = makeRandomOpacity();

		return vec;
	};

	auto makeRandomPixels = [&]()
	{
		QVector<Pixel> vec(pixelCount);

		for (int i = 0; i < pixelCount; ++i)
			vec[i] = makeRandomPixel();

		return vec;
	};

	auto comparePixelVectors = [](const QVector<Pixel> &v1, const QVector<Pixel> &v2)
	{
		auto predicate = [](const Pixel &p1, const Pixel &p2)
		{
			return qFuzzyCompare(p1.a(), p2.a()) && qFuzzyCompare(p1.r(), p2.r()) && qFuzzyCompare(p1.g(), p2.g()) && qFuzzyCompare(p1.b(), p2.b());
		};

		return boost::range::equal(v1, v2, predicate);
	};

	auto verifyPixels = [&](const QVector<Pixel> &correct, const QVector<Pixel> &result)
	{
		auto compare = comparePixelVectors(correct, result);

		if (!compare)
		{
			qDebug() << "correct:";
			qDebug() << correct;
			qDebug() << "result:";
			qDebug() << result;
		}

		return compare;
	};

	auto correctBlend = [](const Pixel &dst, const Pixel &src)
	{
		return src.v() + (1.0f - src.a()) * dst.v();
	};

	auto blendOp = BlendMode(BlendMode::SourceOver).op();

	const auto dstPixels = makeRandomPixels();
	const auto srcPixels = makeRandomPixels();

	qDebug() << "dst:";
	qDebug() << dstPixels;
	qDebug() << "src:";
	qDebug() << srcPixels;

	// simple blending
	{
		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
			correctResultPixels[i] = correctBlend(correctResultPixels[i], srcPixels[i]);

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);

		blendOp->blend(pixelCount, dst, src);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with pixel masks
	{
		const auto maskPixels = makeRandomPixels();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[i].v() * maskPixels[i].aV();
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);
		auto mask = wrapPointer(maskPixels.data(), pixelCount);

		blendOp->blend(srcPixels.size(), dst, src, mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with float masks
	{
		const auto masks = makeRandomOpacities();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[i].v() * masks[i];
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);
		auto mask = wrapPointer(masks.data(), pixelCount);

		blendOp->blend(srcPixels.size(), dst, src, mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with one pixel mask
	{
		const auto mask = makeRandomPixel();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[i].v() * mask.aV();
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);

		blendOp->blend(srcPixels.size(), dst, src, mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with one float mask
	{
		const auto opacity = makeRandomOpacity();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[i].v() * opacity;
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);

		blendOp->blend(srcPixels.size(), dst, src, opacity);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending one pixel
	{
		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			correctResultPixels[i] = correctBlend(correctResultPixels[i], srcPixels[0]);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);

		blendOp->blend(srcPixels.size(), dst, src[0]);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending one pixel with pixel masks
	{
		const auto maskPixels = makeRandomPixels();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[0].v() * maskPixels[i].aV();
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);
		auto mask = wrapPointer(maskPixels.data(), pixelCount);

		blendOp->blend(srcPixels.size(), dst, src[0], mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending one pixel with float masks
	{
		const auto masks = makeRandomOpacities();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[0].v() * masks[i];
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);
		auto mask = wrapPointer(masks.data(), pixelCount);

		blendOp->blend(srcPixels.size(), dst, src[0], mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// reverse blending

	// simple blending
	{
		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
			correctResultPixels[i] = correctBlend(correctResultPixels[i], srcPixels[pixelCount - 1 - i]);

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);

		blendOp->blendReversed(srcPixels.size(), dst, src);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with pixel masks
	{
		const auto maskPixels = makeRandomPixels();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[pixelCount - 1 - i].v() * maskPixels[i].aV();
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);
		auto mask = wrapPointer(maskPixels.data(), pixelCount);

		blendOp->blendReversed(srcPixels.size(), dst, src, mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with float masks
	{
		const auto masks = makeRandomOpacities();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[pixelCount - 1 - i].v() * masks[i];
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);
		auto mask = wrapPointer(masks.data(), pixelCount);

		blendOp->blendReversed(srcPixels.size(), dst, src, mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with one pixel mask
	{
		const auto mask = makeRandomPixel();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[pixelCount - 1 - i].v() * mask.aV();
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);

		blendOp->blendReversed(srcPixels.size(), dst, src, mask);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}

	// blending with one float mask
	{
		const auto opacity = makeRandomOpacity();

		auto correctResultPixels = dstPixels;

		for (int i = 0; i < pixelCount; ++i)
		{
			PixelVec src = srcPixels[pixelCount - 1 - i].v() * opacity;
			correctResultPixels[i] = correctBlend(correctResultPixels[i], src);
		}

		auto resultPixels = dstPixels;
		auto dst = wrapPointer(resultPixels.data(), pixelCount);
		auto src = wrapPointer(srcPixels.data(), pixelCount);

		blendOp->blendReversed(srcPixels.size(), dst, src, opacity);

		QVERIFY(verifyPixels(correctResultPixels, resultPixels));
	}
}

QTEST_MAIN(Test)


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_python_trajectories_cubic_hermite_spline_hpp__
#define __multicontact_api_python_trajectories_cubic_hermite_spline_hpp__

#include <boost/python/tuple.hpp>
#include <string>

#include "multicontact-api/trajectories/cubic-hermite-spline.hpp"
#include "multicontact-api/bindings/python/serialization/archive.hpp"
#include "multicontact-api/bindings/python/utils/printable.hpp"

namespace multicontact_api
{
  namespace python
  {

    namespace bp = boost::python;

    template<typename CubicHermiteSpline>
    struct CubicHermiteSplinePythonVisitor
    : public boost::python::def_visitor< CubicHermiteSplinePythonVisitor<CubicHermiteSpline> >
    {
      typedef typename CubicHermiteSpline::Scalar Scalar;
      typedef typename CubicHermiteSpline::MatrixDx MatrixDx;
      typedef typename CubicHermiteSpline::VectorX VectorX;
      typedef typename CubicHermiteSpline::VectorD VectorD;
      typedef typename CubicHermiteSpline::Index Index;

      template<class PyClass>
      void visit(PyClass& cl) const
      {
        cl
        .def(bp::init<Index>(bp::arg("size"),"Init the hermite spline with a given size."))
          .def(bp::init<VectorX,MatrixDx,MatrixDx>(bp::args("abcissa", "points","derivatives"),"Init from a given set of points and derivatives, together with a vector of absicca."))
        .def(bp::init<CubicHermiteSpline>(bp::args("other"),"Copy contructor."))

        .add_property("absicca",
                      bp::make_function((const VectorX & (CubicHermiteSpline::*)(void)) &CubicHermiteSpline::absicca,
                                        bp::return_value_policy<bp::copy_const_reference>()),
                      &CubicHermiteSpline::setAbsicca,
                      "Abiscca of the spline.")
        .add_property("points",
                      bp::make_function((const MatrixDx & (CubicHermiteSpline::*)(void) const) &CubicHermiteSpline::points,
                                        bp::return_value_policy<bp::copy_const_reference>()),
                      &setPoints,
                      "Via points of the spline.")
        .add_property("derivatives",
                      bp::make_function((const MatrixDx & (CubicHermiteSpline::*)(void) const) &CubicHermiteSpline::derivatives,
                                        bp::return_value_policy<bp::copy_const_reference>()),
                      &setDerivatives,
                      "Derivative values of the via points of the spline.")

        .def("size",&CubicHermiteSpline::size,"Returns the number of points contained in the spline.")
        .def("resize",&CubicHermiteSpline::resize,bp::arg("size"),"Resize the spline.")
        .def("dimension",&CubicHermiteSpline::dimension,"Returns the dimension of the spline.")
        .def("numIntervals",&CubicHermiteSpline::numIntervals,"Returns the number of intervals contained in the spline.")

        .def("eval",&eval,bp::arg("t"),"Eval the spline at a given abscicca t.")
        .def(bp::self+bp::self)
        .def(bp::self-bp::self)
        .def(bp::self+=bp::self)
        .def(bp::self-=bp::self)
        .def(bp::self == bp::self)
        .def(bp::self != bp::self)
        .def("copy",&copy,"Returns a copy of *this.")

        .def("Constant",&CubicHermiteSpline::Constant,bp::arg("point"),"Returns a constant spline passing through the input point.")
        .staticmethod("Constant")
        ;
      }

      static void expose(const std::string & class_name)
      {
        std::string doc = "Cubic Hermite Spline";
        bp::class_<CubicHermiteSpline>(class_name.c_str(),
                                       doc.c_str(),
                                       bp::no_init)
        .def(CubicHermiteSplinePythonVisitor<CubicHermiteSpline>())
        .def(SerializableVisitor<CubicHermiteSpline>())
//        .def(PrintableVisitor<CubicHermiteSpline>())
        ;

      }

    protected:

      static void setPoints(CubicHermiteSpline & self, const MatrixDx & points)
      { self.points() = points; }
      static void setDerivatives(CubicHermiteSpline & self, const MatrixDx & derivatives)
      { self.derivatives() = derivatives; }

      static bp::tuple eval(const CubicHermiteSpline & self, const Scalar t)
      {
        VectorD point(VectorD::Zero(self.dimension())), derivative(VectorD::Zero(self.dimension()));
        self.eval(t,point,derivative);

        return bp::make_tuple(point,derivative);
      }

      static CubicHermiteSpline copy(const CubicHermiteSpline & self) { return CubicHermiteSpline(self); }

    };

    void exposeSplineAlgos()
    {
      bp::def("createHermiteSplineAtAbsicca",
              &multicontact_api::trajectories::createHermiteSplineAtAbsicca<double, 3>,
              bp::args("Spline", "New Abscissa"),
              "create and return a new spline at the points of the new abscissa."
              "The size of the new abscissa needs to be higher so that there is no information loss.");
    }

  }
}

#endif // ifndef __multicontact_api_python_trajectories_cubic_hermite_spline_hpp__


// Copyright (C) 2003  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <ctime>
#include <cstdlib>

#include <dlib/entropy_encoder.h>
#include <dlib/entropy_decoder.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace std;
    using namespace dlib;

    logger dlog("test.entropy_coder");

    namespace entropy_coder_kernel_test_helpers
    {
        template <
            typename encoder,
            typename decoder
            >
        std::string test (
            const std::string& input
        )
        /*!
            ensures
                - encodes the data from in and then tries to decode it and returns
                  "" if it was successfully decoded else it returns the decoded string
        !*/
        {
            ostringstream sout;
            istringstream sin;
            istringstream in;

            in.str(input);

            const unsigned long max_total = 65535;

            unsigned long counts[256];
            for (int i = 0; i < 256; ++i)
            {
                counts[i] = 1;
            }

            encoder e;

            DLIB_TEST(e.stream_is_set() == false);

            e.set_stream(sout);

            DLIB_TEST(e.stream_is_set() == true);
            DLIB_TEST(&e.get_stream() == &sout);

            unsigned char ch;

            unsigned long total = 256;

            while (in.read((char*)&ch,1))
            {
                if (total > max_total)
                {
                    total = 0;
                    for (int j = 0; j<256; ++j)
                    {
                        counts[j] >>= 1;
                        if (counts[j] == 0)
                            counts[j] = 1;
                        total += counts[j];

                    }
                }

                unsigned long low_count = 0;
                unsigned long high_count;
                for (int i = 0; i < ch; ++i)
                    low_count += counts[i];
                high_count = low_count + counts[ch];

                e.encode(low_count,high_count,total);

                ++total;
                counts[ch] += 1;
            }

            DLIB_TEST(e.stream_is_set() == true);
            DLIB_TEST(&e.get_stream() == &sout);

            e.clear();

            DLIB_TEST(e.stream_is_set() == false);

            // *****************************************

            decoder d;

            DLIB_TEST(d.stream_is_set() == false);
            DLIB_TEST(d.get_target_called() == false);

            sin.str(sout.str());
            sout.str("");

            d.set_stream(sin);

            DLIB_TEST(d.get_target_called() == false);

            DLIB_TEST(d.stream_is_set() == true);
            DLIB_TEST(&d.get_stream() == &sin);

            for (int i = 0; i < 256; ++i)
            {
                counts[i] = 1;
            }

            total = 256;

            for (string::size_type i = 0; i < input.size()  ; ++i)
            {
                if (total > max_total)
                {
                    total = 0;
                    for (int j = 0; j<256; ++j)
                    {
                        counts[j] >>= 1;
                        if (counts[j] == 0)
                            counts[j] = 1;
                        total += counts[j];

                    }
                }

                DLIB_TEST(d.get_target_called() == false);

                unsigned long target = d.get_target(total);

                DLIB_TEST(target < total);

                DLIB_TEST(d.get_target_called() == true);

                unsigned long low_count;
                unsigned long high_count = 0;

                unsigned long j;
                for (j = 0; high_count <= target; ++j)
                {
                    high_count += counts[j];
                }
                --j;
                low_count = high_count - counts[j];

                ch = static_cast<unsigned char>(j);

                sout.rdbuf()->sputn((char*)&ch,1);

                d.decode(low_count,high_count);
                DLIB_TEST(d.get_target_called() == false);
                ++total;
                counts[ch] += 1;

            }

            DLIB_TEST(d.stream_is_set() == true);
            DLIB_TEST(&d.get_stream() == &sin);

            d.clear();

            DLIB_TEST(d.stream_is_set() == false);
            DLIB_TEST_MSG(sout.str().size() == input.size(),"the test script is buggy");

            if (sout.str() == input)
                return "";
            else
                return sout.str();

        }

    }

    template <
        typename encoder,
        typename decoder
        >
    void entropy_coder_kernel_test (
    )
    /*!
        requires
            - encoder is an implementation of entropy_encoder/entropy_encoder_kernel_abstract.h
            - decoder is an implementation of entropy_decoder/entropy_decoder_kernel_abstract.h
        ensures
            - runs tests on encoder and decoder for compliance with the specs
    !*/
    {
        using namespace entropy_coder_kernel_test_helpers;

        dlog << LTRACE << 1;

        print_spinner();
        string temp, temp2;

        srand(static_cast<int>(time(0)));

        for (int k = 0; k < 10000; ++k)
        {
            string temp;
            istringstream sin;
            ostringstream sout;
            decoder d;
            encoder e;

            e.set_stream(sout);

            int num = ::rand() %200;
            unsigned long total[200];
            unsigned long high_count[200];
            unsigned long low_count[200];
            for (int i = 0; i < num; ++i)
            {
                total[i] = ::rand()%256 + 20;
                high_count[i] = ::rand()%total[i] + 1;
                low_count[i] = ::rand()%high_count[i];

                e.encode(low_count[i],high_count[i],total[i]);
            }

            e.clear();

            sout.rdbuf()->sputc('a');

            sin.str(sout.str());

            d.set_stream(sin);

            for (int i = 0; i < num; ++i)
            {
                unsigned long N = d.get_target(total[i]);
                DLIB_TEST(low_count[i] <= N && N < high_count[i]);
                d.decode(low_count[i],high_count[i]);
            }

            DLIB_TEST_MSG(sin.rdbuf()->sgetc() != EOF,"num: " << num);
            DLIB_TEST_MSG(sin.rdbuf()->sgetc() == 'a',
                         "sin.rdbuf()->sgetc() == " << (char)sin.rdbuf()->sgetc() <<
                         "\nnum: " << num
            );
            DLIB_TEST(sin.rdbuf()->sbumpc() == 'a');
            DLIB_TEST(sin.rdbuf()->sgetc() == EOF);

        } // for (int k = 0; k < 10000; ++k)

        dlog << LTRACE << 2;

        print_spinner();

        // the point of this block is to make sure that the return value
        // from decoder.get_target(total) is a always less than total
        for (int k = 0; k < 20; ++k)
        {
            string temp;
            temp.push_back(static_cast<char>(::rand()&0xff));
            istringstream sin(temp);
            decoder d;
            d.set_stream(sin);
            unsigned long total = ::rand()%256 + 20;
            unsigned long target = d.get_target(total);
            DLIB_TEST(target<total);

            for (int i = 0; i < 30; ++i)
            {
                unsigned long high_count = ::rand()%total + 1;
                unsigned long low_count = ::rand()%high_count;
                if (high_count <= target)
                    high_count = target+1;
                if (low_count > target)
                    low_count = target;

                d.decode(low_count,high_count);
                target = d.get_target(total);
                DLIB_TEST_MSG(target<total,"target: " << target << "    total: " << total);
            }
        }

        print_spinner();

        dlog << LTRACE << 3;

        for (int k = 0; k < 10; ++k)
        {
            unsigned long seed1 = 1064644658, seed2 = 1064543921;
            //unsigned long seed1 = 1064682621, seed2 = 1064543921;

            // make array be an array with each element in the range 0 to 255
            // and have the probability of seeing each number in the array
            // not be the same
            //seed1 = static_cast<unsigned long>(time(0));
            srand(seed1 );
            int array[65536];
            for (int i = 0; i < 65536; ++i)
            {
                array[i] = ::rand()%256;
            }
            for (int i = 0; i < 60; ++i)
            {
                int idx = ::rand()%65536;
                int radius = 35;
                if (idx > radius && idx <65536-radius)
                {
                    for (int j = idx-radius; j < idx+radius; ++j)
                        array[j] = array[idx];
                }
            }

            // test with 3 random strings of length 10000
            // but use the above array to bias the random numbers
            for (int j = 0; j < 3; ++j)
            {
                print_spinner();
                temp = "";
                //seed2 = static_cast<unsigned long>(time(0));
                srand(seed2 );
                for ( int i = 0; i < 10000; ++i)
                {
                    int a = array[::rand()%65536];
                    temp += (unsigned char)a;
                }
                string temp2;
                temp2 = test<encoder,decoder>(temp);
                if (temp2 != "")
                {

                    int k = 0;
                    DLIB_TEST(temp != temp2);
                    while (temp[k] == temp2[k])++k;
                }

                DLIB_TEST_MSG(temp2 == "","")
            }
        }

        print_spinner();

        dlog << LTRACE << 4;

        // test with a large string which contains all the same character
        temp = "eeeeeeeeee";
        for (int i = 0; i < 13; ++i)
        {
            temp = temp + temp;
        }
        temp = test<encoder,decoder>(temp);
        if (temp != "")
        {
            // crop off all the e's until we find the part that is messed up
            string::size_type pos = temp.find_first_not_of("e");
            temp = temp.substr(pos);
        }
        DLIB_TEST_MSG(temp == "","decoded string: \"" << temp << "\"") /**/

            dlog << LTRACE << 5;

        print_spinner();

        temp = "davis";
        temp = test<encoder,decoder>(temp);  DLIB_TEST_MSG(temp == "","decoded string: \"" << temp << "\"")

            temp = "";
        temp = test<encoder,decoder>(temp);  DLIB_TEST_MSG(temp == "","decoded string: \"" << temp << "\"")

            // test for each single character
            for ( int i = 0; i <= 255; ++i)
            {
                temp = (unsigned char)i;
                temp = test<encoder,decoder>(temp);  DLIB_TEST_MSG(temp == "","decoded string: \"" << temp << "\"")
            }

        dlog << LTRACE << 6;

        // test with a long string with the same thing repeated many times
        temp = "davis ";
        for (int i = 0; i < 10; ++i)
        {
            temp = temp + temp;
        }
        temp = test<encoder,decoder>(temp);  DLIB_TEST_MSG(temp == "","decoded string: \"" << temp << "\"")

            dlog << LTRACE << 7;

        // test with 10 random strings of length 1000
        for (int j = 0; j < 10; ++j)
        {
            temp = "";
            srand(static_cast<unsigned int>(time(0)));
            for ( int i = 0; i < 1000; ++i)
            {
                int a = ::rand()%256;
                temp += (unsigned char)a;
            }
            temp = test<encoder,decoder>(temp);  DLIB_TEST_MSG(temp == "","decoded string: \"" << temp << "\"")
        }

        dlog << LTRACE << 8;

        print_spinner();

        // test with 15 random strings of length 30000
        for (int j = 0; j < 15; ++j)
        {
            print_spinner();
            temp = "";
            unsigned long seed = static_cast<unsigned int>(time(0));
            srand(seed);
            for ( int i = 0; i < 30000; ++i)
            {
                int a = ::rand()%256;
                temp += (unsigned char)a;
            }
            temp = test<encoder,decoder>(temp);  DLIB_TEST_MSG(temp == "","seed: " << seed)
        }

        dlog << LTRACE << 9;

        print_spinner();

        // test with a large string which contains all the same character
        temp = "         ";
        for (int i = 0; i < 10; ++i)
        {
            temp = temp + temp;
        }
        temp = test<encoder,decoder>(temp);
        if (temp != "")
        {
            // crop off all the spacess until we find the part that is messed up
            string::size_type pos = temp.find_first_not_of(" ");
            temp = temp.substr(pos);
        }
        DLIB_TEST_MSG(temp == "","decoded string: \"" << temp << "\"")/**/

            dlog << LTRACE << 10;

        // test with a large string which contains a bunch of a's followed by a
        // bunch of z's
        temp = "aaaaaaaa";
        temp2 = "zzzzzzzz";
        for (int i = 0; i < 12; ++i)
        {
            temp = temp + temp;
            temp2 = temp2 + temp2;
        }
        temp += temp2;
        print_spinner();
        temp = test<encoder,decoder>(temp);
        DLIB_TEST(temp == "");

        dlog << LTRACE << 11;

    }

    class entropy_coder_tester : public tester
    {
    public:
        entropy_coder_tester (
        ) :
            tester ("test_entropy_coder",
                    "Runs tests on the entropy_coder component.")
        {}

        void perform_test (
        )
        {
            dlog << LINFO << "testing kernel_1a 1";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_1a,
                entropy_decoder::kernel_1a
                >();

            dlog << LINFO << "testing kernel_1a_c 2";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_1a_c,
                entropy_decoder::kernel_1a_c
                >();

            dlog << LINFO << "testing kernel_1a 3";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_2a,
                entropy_decoder::kernel_2a
                >();

            dlog << LINFO << "testing kernel_1a_c 4";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_2a_c,
                entropy_decoder::kernel_2a_c
                >();

            dlog << LINFO << "testing kernel_1a 5";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_1a,
                entropy_decoder::kernel_1a_c
                >();

            dlog << LINFO << "testing kernel_1a_c 6";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_1a_c,
                entropy_decoder::kernel_1a
                >();

            dlog << LINFO << "testing kernel_1a 7";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_2a,
                entropy_decoder::kernel_2a_c
                >();

            dlog << LINFO << "testing kernel_1a_c 8";
            entropy_coder_kernel_test<
                entropy_encoder::kernel_2a_c,
                entropy_decoder::kernel_2a
                >();

        }
    } a;

}


// This file is a part of the OpenSurgSim project.
// Copyright 2012-2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/// \file
/// Tests that exercise the functionality of our matrix typedefs, which come
/// straight from Eigen.

#include <math.h>

#include <Eigen/Geometry>  // SurgSim/Math/Matrix.h by itself does not provide cross()
#include <gtest/gtest.h>

#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Matrix.h"

// Define test fixture class templates.
// We don't really need fixtures as such, but the templatization encodes type.

template <class T>
class MatrixTestBase : public testing::Test
{
public:
	typedef T Scalar;
};

template <class T>
class Matrix22Tests : public MatrixTestBase<typename T::Scalar>
{
public:
	typedef T Matrix22;
};

// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
typedef ::testing::Types<SurgSim::Math::Matrix22d,
						 SurgSim::Math::Matrix22f> Matrix22Variants;
TYPED_TEST_CASE(Matrix22Tests, Matrix22Variants);

template <class T>
class Matrix33Tests : public MatrixTestBase<typename T::Scalar>
{
public:
	typedef T Matrix33;
};

// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
typedef ::testing::Types<SurgSim::Math::Matrix33d,
						 SurgSim::Math::Matrix33f> Matrix33Variants;
TYPED_TEST_CASE(Matrix33Tests, Matrix33Variants);

template <class T>
class Matrix44Tests : public MatrixTestBase<typename T::Scalar>
{
public:
	typedef T Matrix44;
};

// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
typedef ::testing::Types<SurgSim::Math::Matrix44d,
						 SurgSim::Math::Matrix44f> Matrix44Variants;
TYPED_TEST_CASE(Matrix44Tests, Matrix44Variants);

template <class T>
class AllMatrixTests : public MatrixTestBase<typename T::Scalar>
{
public:
	typedef T Matrix;
};

template <class T>
class AllDynamicMatrixTests : public MatrixTestBase<typename T::Scalar>
{
public:
	typedef T Matrix;
};

// This used to contain aligned (via Eigen::AutoAlign) matrix type aliases, but we got rid of those.
typedef ::testing::Types<SurgSim::Math::Matrix22d,
						 SurgSim::Math::Matrix22f,
						 SurgSim::Math::Matrix33d,
						 SurgSim::Math::Matrix33f,
						 SurgSim::Math::Matrix44d,
						 SurgSim::Math::Matrix44f> AllMatrixVariants;
TYPED_TEST_CASE(AllMatrixTests, AllMatrixVariants);

typedef ::testing::Types<Eigen::MatrixXd,
						 Eigen::MatrixXf,
						 SurgSim::Math::Matrix> AllDynamicMatrixVariants;
TYPED_TEST_CASE(AllDynamicMatrixTests, AllDynamicMatrixVariants);

template <class T>
class UnalignedMatrixTests : public MatrixTestBase<typename T::Scalar>
{
public:
	typedef T Matrix;
};

template <class T>
class UnalignedDynamicMatrixTests : public MatrixTestBase<typename T::Scalar>
{
public:
	typedef T Matrix;
};

typedef ::testing::Types<SurgSim::Math::Matrix22d,
						 SurgSim::Math::Matrix22f,
						 SurgSim::Math::Matrix33d,
						 SurgSim::Math::Matrix33f,
						 SurgSim::Math::Matrix44d,
						 SurgSim::Math::Matrix44f> UnalignedMatrixVariants;
TYPED_TEST_CASE(UnalignedMatrixTests, UnalignedMatrixVariants);

typedef ::testing::Types<Eigen::MatrixXd,
						 Eigen::MatrixXf,
						 SurgSim::Math::Matrix> UnalignedDynamicMatrixVariants;
TYPED_TEST_CASE(UnalignedDynamicMatrixTests, UnalignedDynamicMatrixVariants);

// Now we're ready to start testing...

// ==================== CONSTRUCTION & INITIALIZATION ====================

/// Test that matrices can be constructed.
TYPED_TEST(Matrix22Tests, CanConstruct)
{
	typedef typename TestFixture::Matrix22 Matrix22;

	// Warning: Eigen *does not* provide a 1-argument constructor that
	// initializes all elements to the same value!  If you do something like
	//    SurgSim::Math::Matrix22fu oneArg2fu(1.23f);
	// the argument is converted to an integral type, interpreted as a size,
	// and promptly ignored because the size is fixed.  Oops.
	// To generate a constant matrix, use Matrix22f::Constant(val).

	Matrix22 default2;
}

/// Test that matrices can be constructed.
TYPED_TEST(Matrix33Tests, CanConstruct)
{
	typedef typename TestFixture::Matrix33 Matrix33;

	// Warning: Eigen *does not* provide a 1-argument constructor that
	// initializes all elements to the same value!  If you do something like
	//    SurgSim::Math::Matrix22fu oneArg2fu(1.23f);
	// the argument is converted to an integral type, interpreted as a size,
	// and promptly ignored because the size is fixed.  Oops.
	// To generate a constant matrix, use Matrix22f::Constant(val).

	Matrix33 default3;
}

/// Test that matrices can be constructed.
TYPED_TEST(Matrix44Tests, CanConstruct)
{
	typedef typename TestFixture::Matrix44 Matrix44;

	// Warning: Eigen *does not* provide a 1-argument constructor that
	// initializes all elements to the same value!  If you do something like
	//    SurgSim::Math::Matrix22fu oneArg2fu(1.23f);
	// the argument is converted to an integral type, interpreted as a size,
	// and promptly ignored because the size is fixed.  Oops.
	// To generate a constant matrix, use Matrix22f::Constant(val).

	Matrix44 default4;
}

/// Test that the default constructor DOESN'T initialize matrices.
//
// Only test the non-vectorized versions.  Otherwise, we'd need to
// allocate memory in a way that guarantees Eigen-compatible alignment.
//
// TODO(bert): There is some Eigen flag that causes matrices and matrices to be
//   initialized after all!  We should check for that here.
TYPED_TEST(UnalignedMatrixTests, DefaultConstructorInitialization)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	EXPECT_TRUE(SIZE >= 2 && SIZE <= 4);
	EXPECT_EQ(SIZE, Matrix::ColsAtCompileTime);

	// Allocate a buffer for the matrix type on stack, based on the size
	// of the object we're testing.  The object will be allocated inside
	// the buffer using the placement syntax for the new() operator.
	// Eigen's new operatore will attempt to align returned value on word sized
	// boundaries, so add 64 bytes to guarantee enough size.
	unsigned char buffer[sizeof(Matrix) + 64];

	{
		// Please don't write production (non-test) code that looks like this. =)
		memset(&buffer, 0xF0, sizeof(buffer));
		Matrix* matrix = new(&buffer) Matrix;
		for (int row = 0;  row < SIZE;  ++row)
		{
			for (int col = 0;  col < SIZE;  ++col)
			{
				EXPECT_NE(0.0f, (*matrix)(row, col)) << row << "," << col << " was NOT supposed to be zeroed.";
			}
		}
		// Destroying the object is a good idea, even if unnecessary here:
		matrix->Matrix::~Matrix();
	}
}

/// Test setting the matrix using the << syntax.
TYPED_TEST(Matrix22Tests, ShiftCommaInitialization)
{
	typedef typename TestFixture::Matrix22 Matrix22;

	Matrix22 matrix;
	// Initialize elements in order.  Do NOT put parentheses around the list!
	matrix <<
		1.1f, 1.2f,
		1.3f, 1.4f;
	for (int row = 0;  row < 2;  ++row)
	{
		for (int col = 0;  col < 2;  ++col)
		{
			EXPECT_NEAR(1.1 + 0.2*row + 0.1*col, matrix(row, col), 1e-6) <<
				row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test setting the matrix using the << syntax.
TYPED_TEST(Matrix33Tests, ShiftCommaInitialization)
{
	typedef typename TestFixture::Matrix33 Matrix33;

	Matrix33 matrix;
	// Initialize elements in order.  Do NOT put parentheses around the list!
	matrix <<
		1.1f, 1.2f, 1.3f,
		1.4f, 1.5f, 1.6f,
		1.7f, 1.8f, 1.9f;
	for (int row = 0;  row < 3;  ++row)
	{
		for (int col = 0;  col < 3;  ++col)
		{
			EXPECT_NEAR(1.1 + 0.3*row + 0.1*col, matrix(row, col), 1e-6) <<
				row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test setting the matrix using the << syntax.
TYPED_TEST(Matrix44Tests, ShiftCommaInitialization)
{
	typedef typename TestFixture::Matrix44 Matrix44;

	Matrix44 matrix;
	// Initialize elements in order.  Do NOT put parentheses around the list!
	matrix <<
		1.1f, 1.2f, 1.3f, 1.4f,
		1.5f, 1.6f, 1.7f, 1.8f,
		1.9f, 2.0f, 2.1f, 2.2f,
		2.3f, 2.4f, 2.5f, 2.6f;
	for (int row = 0;  row < 4;  ++row)
	{
		for (int col = 0;  col < 4;  ++col)
		{
			EXPECT_NEAR(1.1 + 0.4*row + 0.1*col, matrix(row, col), 1e-6) <<
				row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test getting a zero value usable in expressions.
TYPED_TEST(AllMatrixTests, ZeroValue)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix matrix = 1000 * Matrix::Zero();
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(0.0, matrix(row, col), 1e-20) << row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test setting matrices to 0.
TYPED_TEST(AllMatrixTests, SetToZero)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix matrix;
	matrix.setZero();
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(0.0, matrix(row, col), 1e-20) << row << "," << col << " wasn't properly cleared.";
		}
	}
}

/// Test getting a constant value usable in expressions.
TYPED_TEST(AllMatrixTests, ConstantValue)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix matrix = 2 * Matrix::Constant(0.5f);
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(1.0, matrix(row, col), 1e-6) << row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test setting matrices to a constant.
TYPED_TEST(AllMatrixTests, SetToConstant)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix matrix;
	matrix.setConstant(7.2f);
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(7.2, matrix(row, col), 1e-6) << row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test initializing a row-major Eigen matrix from a float array.
TYPED_TEST(AllMatrixTests, InitializeRowMajorFromArray)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	typedef Eigen::Matrix<T, SIZE, SIZE, Eigen::AutoAlign | Eigen::RowMajor> RMatrix;

	// This array has more elements than we will need.
	// The element type must match the matrix!
	const T inputArray[18]  =
	{
		0.01f,  1.02f,  2.03f,  3.04f,  4.05f,  5.06f,  6.07f,  7.08f,  8.09f,
		9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
	};

	RMatrix matrix(inputArray);
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(0.01 + (row*SIZE + col) * 1.01, matrix(row, col), 1e-6) <<
				row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test initializing a column-major Eigen matrix from a float array.
TYPED_TEST(AllMatrixTests, InitializeColumnMajorFromArray)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	typedef Eigen::Matrix<T, SIZE, SIZE, Eigen::AutoAlign | Eigen::ColMajor> CMatrix;

	// This array has more elements than we will need.
	// The element type must match the matrix!
	const T inputArray[18]  =
	{
		0.01f,  1.02f,  2.03f,  3.04f,  4.05f,  5.06f,  6.07f,  7.08f,  8.09f,
		9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
	};

	CMatrix matrix(inputArray);
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(0.01 + (col*SIZE + row) * 1.01, matrix(row, col), 1e-6) <<
				row << "," << col << " wasn't properly initialized.";
		}
	}
}

/// Test initializing from a float array.
/// Among other things, tests that our matrices are row-major.
TYPED_TEST(AllMatrixTests, InitializeFromArray)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	// This array has more elements than we will need.
	// The element type must match the matrix!
	const T inputArray[18]  =
	{
		0.01f,  1.02f,  2.03f,  3.04f,  4.05f,  5.06f,  6.07f,  7.08f,  8.09f,
		9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
	};

	Matrix matrix(inputArray);
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(0.01 + (row*SIZE + col) * 1.01, matrix(row, col), 1e-6) <<
				row << "," << col << " wasn't properly initialized.";
		}
	}
}

// Test conversion to and from yaml node
TYPED_TEST(AllMatrixTests, YamlConvert)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;

	// This array has more elements than we will need.
	// The element type must match the matrix!
	const T inputArray[18]  =
	{
		0.01f,  1.02f,  2.03f,  3.04f,  4.05f,  5.06f,  6.07f,  7.08f,  8.09f,
		9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
	};

	Matrix matrix(inputArray);

	YAML::Node node;

	ASSERT_NO_THROW(node = matrix);

	EXPECT_TRUE(node.IsSequence());
	EXPECT_EQ(matrix.rows(), static_cast<typename Matrix::Index>(node.size()));

	ASSERT_NO_THROW({Matrix expected = node.as<Matrix>();});
	EXPECT_TRUE(matrix.isApprox(node.as<Matrix>()));
}

/// Test assignment.
TYPED_TEST(AllMatrixTests, Assign)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = Matrix::Constant(6.0f);
	EXPECT_NEAR(6.0f * SIZE * SIZE, a.sum(), 1e-6);
	const Matrix b = Matrix::Constant(7.0f);
	EXPECT_NEAR(7.0f * SIZE * SIZE, b.sum(), 1e-6);
	a = b;
	EXPECT_NEAR(7.0f * SIZE * SIZE, a.sum(), 1e-6);
}

// ==================== ACCESS ====================

/// Access by rows and columns.
TYPED_TEST(AllMatrixTests, RowsAndColumns)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	typedef Eigen::Matrix<T, SIZE, 1> Vector;

	Matrix a = Matrix::Zero();
	for (int i = 0;  i < SIZE;  ++i)
	{
		Vector rowVector = Vector::Constant(i + 1.f);
		a.row(i) = rowVector;
	}

	for (int i = 0;  i < SIZE;  ++i)
	{
		Vector rowVector = a.row(i);
		EXPECT_NEAR((i + 1.f) * SIZE, rowVector.sum(), 1e-6);
		Vector columnVector = a.col(i);
		EXPECT_NEAR(SIZE * (SIZE+1) / 2., columnVector.sum(), 1e-6);
	}

	for (int i = 0;  i < SIZE;  ++i)
	{
		Vector columnVector = Vector::Constant(i + 1.f);
		a.col(i) = columnVector;
	}

	for (int i = 0;  i < SIZE;  ++i)
	{
		Vector columnVector = a.col(i);
		EXPECT_NEAR((i + 1.f) * SIZE, columnVector.sum(), 1e-6);
		Vector rowVector = a.row(i);
		EXPECT_NEAR(SIZE * (SIZE+1) / 2., rowVector.sum(), 1e-6);
	}
}

/// Access to the diagonal.
TYPED_TEST(AllMatrixTests, Diagonal)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	typedef Eigen::Matrix<T, SIZE, 1> Vector;

	Matrix a = Matrix::Zero();
	{
		Vector diagonalVector = Vector::Constant(2.f);
		a.diagonal() = diagonalVector;
	}
	EXPECT_NEAR(2.f * SIZE, a.sum(), 1e-6);

	Matrix b = Matrix::Identity();
	{
		EXPECT_NEAR(1.f * SIZE, b.diagonal().sum(), 1e-6);
	}
}

// ==================== REPRESENTATION CONVERSIONS ====================

/// Test setting quaternions from an angle/axis rotation.
TYPED_TEST(Matrix33Tests, FromAngleAxis)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar   T;

	typedef Eigen::Matrix<T, 3, 1> Vector3;

	T angle = 0.1f;
	Vector3 axis = Vector3::UnitZ();

	Matrix33 expectedMatrix;
	T sinAngle = std::sin(angle);
	T cosAngle = std::cos(angle);
	expectedMatrix <<
		cosAngle, -sinAngle, 0,
		sinAngle, cosAngle, 0,
		0, 0, 1;

	using SurgSim::Math::makeRotationMatrix;

	Matrix33 matrix = makeRotationMatrix(angle, axis);
	EXPECT_NEAR(0, (matrix - expectedMatrix).norm(), 9e-6) << "The rotation matrix wasn't properly computed.";
}

/// Test extracting an angle/axis rotation from a quaternion.
TYPED_TEST(Matrix33Tests, ToAngleAxis)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar T;

	typedef Eigen::Matrix<T, 3, 1> Vector3;

	T angle = -0.1f;
	Vector3 axis = Vector3::UnitZ();

	Matrix33 matrix;
	T sinAngle = std::sin(angle);
	T cosAngle = std::cos(angle);
	matrix <<
		cosAngle, -sinAngle, 0,
		sinAngle, cosAngle, 0,
		0, 0, 1;

	using SurgSim::Math::computeAngleAndAxis;
	using SurgSim::Math::computeAngle;

	T angle2;
	Vector3 axis2;
	computeAngleAndAxis(matrix, &angle2, &axis2);
	EXPECT_NEAR(-angle,    angle2,    1e-6) << "angle wasn't properly computed.";
	EXPECT_NEAR(-axis.x(), axis2.x(), 1e-6) << "X wasn't properly computed.";
	EXPECT_NEAR(-axis.y(), axis2.y(), 1e-6) << "Y wasn't properly computed.";
	EXPECT_NEAR(-axis.z(), axis2.z(), 1e-6) << "Y wasn't properly computed.";

	EXPECT_NEAR(-angle, computeAngle(matrix), 1e-6) << "angle wasn't properly computed by computeAngle().";
}

/// Test building a skew symmetric matrix from a vector
TYPED_TEST(Matrix33Tests, MakeSkewSymmetricMatrixTest)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar T;
	typedef Eigen::Matrix<T, 3, 1> Vector3;

	Vector3 v(static_cast<T>(0.3), static_cast<T>(-1.4), static_cast<T>(8.3));
	Matrix33 matrix = SurgSim::Math::makeSkewSymmetricMatrix(v);
	EXPECT_TRUE(matrix.diagonal().isZero());
	EXPECT_NEAR(static_cast<T>(0.3) , matrix(2, 1), std::numeric_limits<T>::epsilon());
	EXPECT_NEAR(static_cast<T>(-0.3), matrix(1, 2), std::numeric_limits<T>::epsilon());
	EXPECT_NEAR(static_cast<T>(-1.4), matrix(0, 2), std::numeric_limits<T>::epsilon());
	EXPECT_NEAR(static_cast<T>(1.4) , matrix(2, 0), std::numeric_limits<T>::epsilon());
	EXPECT_NEAR(static_cast<T>(8.3) , matrix(1, 0), std::numeric_limits<T>::epsilon());
	EXPECT_NEAR(static_cast<T>(-8.3), matrix(0, 1), std::numeric_limits<T>::epsilon());
}

/// Test extracting a vector from a skew symmetric part of a matrix
TYPED_TEST(Matrix33Tests, SkewTest)
{
	typedef typename TestFixture::Scalar T;
	typedef Eigen::Matrix<T, 3, 1> Vector3;

	Vector3 vExpected(static_cast<T>(0.3), static_cast<T>(-1.4), static_cast<T>(8.3));
	Vector3 v = SurgSim::Math::skew(SurgSim::Math::makeSkewSymmetricMatrix(vExpected));
	EXPECT_TRUE(v.isApprox(vExpected));
}

// ==================== ARITHMETIC ====================

/// Negation (unary minus).
TYPED_TEST(AllMatrixTests, Negate)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = Matrix::Constant(0.1f);
	EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
	Matrix b = -a;
	EXPECT_NEAR(-0.1 * SIZE * SIZE, b.sum(), 1e-6);
}

/// Addition.
TYPED_TEST(AllMatrixTests, Add)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = Matrix::Constant(0.1f);
	EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
	Matrix b = a + Matrix::Ones() + a;
	EXPECT_NEAR(1.2 * SIZE * SIZE, b.sum(), 1e-6);
}

/// Subtraction.
TYPED_TEST(AllMatrixTests, Subtract)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = Matrix::Constant(0.1f);
	EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
	Matrix b = Matrix::Ones() - a;
	EXPECT_NEAR(0.9 * SIZE * SIZE, b.sum(), 1e-6);
}

/// Incrementing by a value.
TYPED_TEST(AllMatrixTests, AddTo)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = Matrix::Constant(0.1f);
	EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
	a += Matrix::Ones();
	EXPECT_NEAR(1.1 * SIZE * SIZE, a.sum(), 1e-6);
}

/// Decrementing by a value.
TYPED_TEST(AllMatrixTests, SubtractFrom)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = Matrix::Constant(1.1f);
	EXPECT_NEAR(1.1 * SIZE * SIZE, a.sum(), 1e-6);
	a -= Matrix::Ones();
	EXPECT_NEAR(0.1 * SIZE * SIZE, a.sum(), 1e-6);
}

/// Matrix-scalar multiplication.
TYPED_TEST(AllMatrixTests, MultiplyMatrixScalar)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix a = Matrix::Random();
	Matrix b = a * 1.23f;
	EXPECT_NEAR(1.23 * a.sum(), b.sum(), 1e-6);
}

/// Scalar-matrix multiplication.
TYPED_TEST(AllMatrixTests, MultiplyScalarMatrix)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix a = Matrix::Random();
	Matrix b = 1.23f * a;
	EXPECT_NEAR(1.23 * a.sum(), b.sum(), 1e-6);
}

/// Division by scalar.
TYPED_TEST(AllMatrixTests, DivideScalar)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix a = Matrix::Random();
	Matrix b = a / 1.23f;
	EXPECT_NEAR(a.sum() / 1.23, b.sum(), 1e-6);
}

/// Matrix-vector multiplication.
TYPED_TEST(AllMatrixTests, MultiplyMatrixVector)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	typedef Eigen::Matrix<T, SIZE, 1> Vector;

	Matrix a = Matrix::Random();
	Vector v = Vector::Zero();
	v[0] = 1.f;
	Vector w = a * v;
	EXPECT_NEAR(a.col(0).sum(), w.sum(), 1e-6);
}

/// Matrix-matrix multiplication.
TYPED_TEST(AllMatrixTests, MultiplyMatrixMatrix)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = Matrix::Random();
	Matrix I = Matrix::Identity();
	Matrix b = I * a * I;
	EXPECT_NEAR(0, (b - a).norm(), SIZE * SIZE * 1e-6);
}

/// Matrix inverse.
TYPED_TEST(AllMatrixTests, Inverse)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a = 2.0f * Matrix::Identity() + 0.5f * Matrix::Random();   // try to make an invertible matrix
	Matrix b = a.inverse();
	Matrix ab = a * b;
	EXPECT_NEAR(0, (ab - Matrix::Identity()).norm(), SIZE * SIZE * 1e-6);
	Matrix ba = b * a;
	EXPECT_NEAR(0, (ba - Matrix::Identity()).norm(), SIZE * SIZE * 1e-6);
}

/// Matrix transpose.
TYPED_TEST(AllMatrixTests, Transpose)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix a;
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			a(row, col) = 2.f * row - 3.f * col;
		}
	}

	// Note: DO NOT do things like "a = a.transpose()"; aliasing will result in an error.
	// You can use transposeInPlace(), or "a = a.transpose().eval()".

	Matrix b = a.transpose();
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			EXPECT_NEAR(2.f * col - 3.f * row, b(row, col), 1e-6);
		}
	}
}

/// Component-wise multiplication.
TYPED_TEST(AllMatrixTests, ComponentwiseMultiply)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix a = Matrix::Random();
	Matrix b = Matrix::Identity();
	Matrix c = a.cwiseProduct(b);
	EXPECT_NEAR(0, c.sum() - c.diagonal().sum(), 1e-6);
	EXPECT_NEAR(0, (a - c).diagonal().sum(), 1e-6);
}

/// Component-wise division.
TYPED_TEST(AllMatrixTests, ComponentwiseDivide)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix a = Matrix::Random();
	Matrix b = Matrix::Constant(0.5f);
	Matrix c = a.cwiseQuotient(b);
	EXPECT_NEAR(a.sum() * 2, c.sum(), 1e-6);
}

/// Frobenius norm and its square.
TYPED_TEST(AllMatrixTests, FrobeniusNorm)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix m;
	T sumSquares = 0;
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			T value = row + col + 11.0f;
			m(row, col) = value;
			sumSquares += value * value;
		}
	}

	EXPECT_NEAR(sumSquares, m.squaredNorm(), 1e-4);
	EXPECT_NEAR(sqrt(sumSquares), m.norm(), 1e-4);
}

/// L1 (Manhattan) norm and L_Infinity (largest absolute value) norm.
TYPED_TEST(AllMatrixTests, L1NormAndLInfNorm)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix m;
	T sumAbsolute = 0;
	for (int row = 0;  row < SIZE;  ++row)
	{
		for (int col = 0;  col < SIZE;  ++col)
		{
			T value = row + col - (SIZE + 0.6f);
			m(row, col) = value;
			// NOTE: DON'T use plain abs(), it may truncate to int!
			sumAbsolute += std::abs(value);
		}
	}
	// the function is set up so that the (0,0) corner will always have the biggest absolute value
	T maxAbsolute = (SIZE + 0.6f);

	Matrix n = -m;
	// Ugh, "template" is required to get this to parse properly.  This is
	// triggered because the test is a part of a template class; you don't
	// need to do this in a non-template context.
	EXPECT_NEAR(sumAbsolute, m.template lpNorm<1>(), 1e-4) << "m=" << m;
	EXPECT_NEAR(sumAbsolute, n.template lpNorm<1>(), 1e-4) << "n=" << n;
	EXPECT_NEAR(maxAbsolute, m.template lpNorm<Eigen::Infinity>(), 1e-4) << "m=" << m;
	EXPECT_NEAR(maxAbsolute, n.template lpNorm<Eigen::Infinity>(), 1e-4) << "n=" << m;
}

/// Minimum and maximum elements.
TYPED_TEST(AllMatrixTests, MinAndMax)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;

	const T inputArray[18]  =
	{
		0.01f,  1.02f,  2.03f,  3.04f,  4.05f,  5.06f,  6.07f,  7.08f,  8.09f,
		9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
	};

	Matrix m(inputArray);
	EXPECT_NEAR(inputArray[0], m.minCoeff(), 1e-6);
	EXPECT_NEAR(inputArray[SIZE*SIZE-1], m.maxCoeff(), 1e-6);
}

/// Trace.
TYPED_TEST(AllMatrixTests, Trace)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;

	Matrix a = Matrix::Random();
	T expectedTrace = a.diagonal().sum();
	EXPECT_NEAR(expectedTrace, a.trace(), 1e-6);
}

/// Determinant.
TYPED_TEST(AllMatrixTests, Determinant)
{
	typedef typename TestFixture::Matrix Matrix;
	const int SIZE = Matrix::RowsAtCompileTime;

	Matrix m = exp(1.f) * Matrix::Identity();
	EXPECT_NEAR(exp(1.*SIZE), m.determinant(), 1e-4);
}

/// Determinant (explicit 2x2 result).
TYPED_TEST(Matrix22Tests, Determinant22)
{
	typedef typename TestFixture::Matrix22 Matrix22;
	typedef typename TestFixture::Scalar   T;

	Matrix22 m = Matrix22::Random();
	T expectedDeterminant = m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0);
	EXPECT_NEAR(expectedDeterminant, m.determinant(), 1e-6);
}

/// Determinant (explicit 3x3 result).
TYPED_TEST(Matrix33Tests, Determinant33)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar   T;

	Matrix33 m = Matrix33::Random();
	T expectedDeterminant = m.row(0).cross(m.row(1)).dot(m.row(2));
	EXPECT_NEAR(expectedDeterminant, m.determinant(), 1e-6);
}

// ==================== SUBMATRICES (EXTENDING/SHRINKING) ====================

/// Extending matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix22Tests, Extend2to3)
{
	typedef typename TestFixture::Matrix22 Matrix22;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 3, 3> Matrix33;

	Matrix22 matrix2;
	matrix2 <<
		1.1f, 1.2f,
		1.3f, 1.4f;

	Matrix33 matrix3 = Matrix33::Identity();
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix3.template block<2, 2>(0, 0) = matrix2;

	EXPECT_NEAR(6.0, matrix3.sum(), 1e-6) << "extending was incorrect: " << matrix3;
}

/// Extending matrices using the block(i,j,r,c) syntax.
TYPED_TEST(Matrix22Tests, DynamicExtend2to3)
{
	typedef typename TestFixture::Matrix22 Matrix22;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 3, 3> Matrix33;

	Matrix22 matrix2;
	matrix2 <<
		1.1f, 1.2f,
		1.3f, 1.4f;

	Matrix33 matrix3 = Matrix33::Identity();
	matrix3.block(0, 0, 2, 2) = matrix2;

	EXPECT_NEAR(6.0, matrix3.sum(), 1e-6) << "extending was incorrect: " << matrix3;
}

/// Shrinking matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix22Tests, Shrink3to2)
{
	typedef typename TestFixture::Matrix22 Matrix22;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 3, 3> Matrix33;

	Matrix33 matrix3;
	matrix3 <<
		1.1f, 1.2f, 1.3f,
		1.4f, 1.5f, 1.6f,
		1.7f, 1.8f, 1.9f;

	Matrix22 matrix2;
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix2 = matrix3.template block<2, 2>(0, 0);

	EXPECT_NEAR(5.2, matrix2.sum(), 1e-6) << "shrinking was incorrect: " << matrix2;
}

/// Extending matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix33Tests, Extend2to3)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 2, 2> Matrix22;

	Matrix22 matrix2;
	matrix2 <<
		1.1f, 1.2f,
		1.3f, 1.4f;

	Matrix33 matrix3 = Matrix33::Identity();
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix3.template block<2, 2>(0, 0) = matrix2;

	EXPECT_NEAR(6.0, matrix3.sum(), 1e-6) << "extending was incorrect: " << matrix3;
}

/// Shrinking matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix33Tests, Shrink3to2)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 2, 2> Matrix22;

	Matrix33 matrix3;
	matrix3 <<
		1.1f, 1.2f, 1.3f,
		1.4f, 1.5f, 1.6f,
		1.7f, 1.8f, 1.9f;

	Matrix22 matrix2;
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix2 = matrix3.template block<2, 2>(0, 0);

	EXPECT_NEAR(5.2, matrix2.sum(), 1e-6) << "shrinking was incorrect: " << matrix2;
}

/// Extending matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix33Tests, Extend3to4)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 4, 4> Matrix44;

	Matrix33 matrix3;
	matrix3 <<
		1.1f, 1.2f, 1.3f,
		1.4f, 1.5f, 1.6f,
		1.7f, 1.8f, 1.9f;

	Matrix44 matrix4 = Matrix44::Identity();
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix4.template block<3, 3>(0, 0) = matrix3;

	EXPECT_NEAR(14.5, matrix4.sum(), 1e-6) << "extending was incorrect: " << matrix4;
}

/// Shrinking matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix33Tests, Shrink4to3)
{
	typedef typename TestFixture::Matrix33 Matrix33;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 4, 4> Matrix44;

	Matrix44 matrix4;
	matrix4 <<
		1.1f, 1.2f, 1.3f, 1.4f,
		1.5f, 1.6f, 1.7f, 1.8f,
		1.9f, 2.0f, 2.1f, 2.2f,
		2.3f, 2.4f, 2.5f, 2.6f;

	Matrix33 matrix3;
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix3 = matrix4.template block<3, 3>(0, 0);

	EXPECT_NEAR(14.4, matrix3.sum(), 1e-6) << "shrinking was incorrect: " << matrix3;
}

/// Extending matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix44Tests, Extend3to4)
{
	typedef typename TestFixture::Matrix44 Matrix44;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 3, 3> Matrix33;

	Matrix33 matrix3;
	matrix3 <<
		1.1f, 1.2f, 1.3f,
		1.4f, 1.5f, 1.6f,
		1.7f, 1.8f, 1.9f;

	Matrix44 matrix4 = Matrix44::Identity();
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix4.template block<3, 3>(0, 0) = matrix3;

	EXPECT_NEAR(14.5, matrix4.sum(), 1e-6) << "extending was incorrect: " << matrix4;
}

/// Shrinking matrices using the block<r,c>() syntax.
TYPED_TEST(Matrix44Tests, Shrink4to3)
{
	typedef typename TestFixture::Matrix44 Matrix44;
	typedef typename TestFixture::Scalar  T;

	typedef Eigen::Matrix<T, 3, 3> Matrix33;

	Matrix44 matrix4;
	matrix4 <<
		1.1f, 1.2f, 1.3f, 1.4f,
		1.5f, 1.6f, 1.7f, 1.8f,
		1.9f, 2.0f, 2.1f, 2.2f,
		2.3f, 2.4f, 2.5f, 2.6f;

	Matrix33 matrix3;
	// Ugh, this is efficient but "template" is required to get it to parse
	// properly.  This is triggered because the test is a part of a template
	// class; you don't need to do this in a non-template context.
	matrix3 = matrix4.template block<3, 3>(0, 0);

	EXPECT_NEAR(14.4, matrix3.sum(), 1e-6) << "shrinking was incorrect: " << matrix3;
}

// ==================== TYPE CONVERSION ====================

/// Typecasting matrices (double <-> float conversions).
TYPED_TEST(AllMatrixTests, TypeCasting)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int SIZE = Matrix::RowsAtCompileTime;
	typedef Eigen::Matrix<double, SIZE, SIZE> Matrixd;
	typedef Eigen::Matrix<float,  SIZE, SIZE> Matrixf;

	Matrix a = Matrix::Random();
	T expectedSum = a.sum();

	// Ugh, "template" is required to get this to parse properly.  This is
	// triggered because the test is a part of a template class; you don't
	// need to do this in a non-template context.
	Matrixd d = a.template cast<double>();
	EXPECT_NEAR(expectedSum, d.sum(), 1e-6);
	Matrixf f = a.template cast<float>();
	EXPECT_NEAR(expectedSum, f.sum(), 1e-6);
}

// ==================== MISCELLANEOUS ====================

/// Reading from and writing to arrays or blocks of double/float in memory.
TYPED_TEST(AllMatrixTests, ArrayReadWrite)
{
	typedef typename TestFixture::Matrix Matrix;
	typedef typename TestFixture::Scalar T;
	const int NUM_ELEMENTS = Matrix::SizeAtCompileTime;

	const T inputArray[18]  =
	{
		0.01f,  1.02f,  2.03f,  3.04f,  4.05f,  5.06f,  6.07f,  7.08f,  8.09f,
		9.10f, 10.11f, 11.12f, 12.13f, 13.14f, 14.15f, 15.16f, 16.17f, 17.18f
	};
	T outputArray[18];

	Eigen::Map<const Matrix> inputMatrix(inputArray);
	Matrix a = inputMatrix;

	Eigen::Map<Matrix> outputMatrix(outputArray);
	outputMatrix = a;

	for (int i = 0;  i < NUM_ELEMENTS;  ++i)
	{
		EXPECT_NEAR(inputArray[i], outputArray[i], 1e-6);
	}
}

// TO DO:
// non-checked access via coeff()
// testing numerical validity
// testing for denormalized numbers

namespace
{
	template <class T>
	void testScalar(T valueExpected, T value){}

	template <>
	void testScalar<double>(double valueExpected, double value)
	{
		EXPECT_DOUBLE_EQ(valueExpected, value);
	}

	template <>
	void testScalar<float>(float valueExpected, float value)
	{
		EXPECT_FLOAT_EQ(valueExpected, value);
	}
};

TYPED_TEST(AllDynamicMatrixTests, addSubMatrix)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix m, mInit, m2, m2Init;
	m.resize(18, 18);   m.setRandom();   mInit  = m;
	m2.resize(18, 18);  m2.setRandom();  m2Init = m2;

	ASSERT_NO_THROW(SurgSim::Math::addSubMatrix(m2.block(3,3, 3,3), 2,2, 3,3, &m););

	EXPECT_TRUE(m2.isApprox(m2Init));
	EXPECT_FALSE(m.isApprox(mInit));
	for (int rowId = 0; rowId < 6; rowId++)
	{
		EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
		EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
	}
	for (int rowId = 6; rowId < 9; rowId++)
	{
		for (int colId = 6; colId < 9; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-6), m(rowId, colId));
		}
	}
	for (int rowId = 9; rowId < 18; rowId++)
	{
		EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
		EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
	}
}

TYPED_TEST(AllDynamicMatrixTests, addSubMatrixBlocks)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix m, mInit, m2, m2Init;
	std::vector<size_t> nodeIds;
	m.resize(18, 18);   m.setRandom();   mInit = m;
	m2.resize(18, 18);  m2.setRandom();  m2Init = m2;
	nodeIds.push_back(1);
	nodeIds.push_back(3);
	nodeIds.push_back(5);

	ASSERT_NO_THROW(SurgSim::Math::addSubMatrix(m2.block(3,3, 9,9), nodeIds, 3, &m););
	EXPECT_TRUE(m2.isApprox(m2Init));
	EXPECT_FALSE(m.isApprox(mInit));
	for (int rowId = 0; rowId < 3; rowId++)
	{
		EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
		EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
	}
	for (int rowId = 3; rowId < 6; rowId++)
	{
		for (int colId = 3; colId < 6; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-3, 3 + colId-3), m(rowId, colId));
		}
		for (int colId = 9; colId < 12; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-3, 3 + colId-6), m(rowId, colId));
		}
		for (int colId = 15; colId < 18; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-3, 3 + colId-9), m(rowId, colId));
		}
	}
	for (int rowId = 6; rowId < 9; rowId++)
	{
		EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
		EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
	}
	for (int rowId = 9; rowId < 12; rowId++)
	{
		for (int colId = 3; colId < 6; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-3), m(rowId, colId));
		}
		for (int colId = 9; colId < 12; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-6), m(rowId, colId));
		}
		for (int colId = 15; colId < 18; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-6, 3 + colId-9), m(rowId, colId));
		}
	}
	for (int rowId = 12; rowId < 15; rowId++)
	{
		EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
		EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
	}
	for (int rowId = 15; rowId < 18; rowId++)
	{
		for (int colId = 3; colId < 6; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-9, 3 + colId-3), m(rowId, colId));
		}
		for (int colId = 9; colId < 12; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-9, 3 + colId-6), m(rowId, colId));
		}
		for (int colId = 15; colId < 18; colId++)
		{
			testScalar(mInit(rowId, colId) + m2Init(3 + rowId-9, 3 + colId-9), m(rowId, colId));
		}
	}
}

TYPED_TEST(AllDynamicMatrixTests, setSubMatrix)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix m, mInit, m2, m2Init;
	m.resize(18, 18);   m.setRandom();   mInit  = m;
	m2.resize(18, 18);  m2.setRandom();  m2Init = m2;

	ASSERT_NO_THROW(SurgSim::Math::setSubMatrix(m2.block(3,3, 3,3), 2,2, 3,3, &m););
	EXPECT_TRUE(m2.isApprox(m2Init));
	EXPECT_FALSE(m.isApprox(mInit));
	for (int rowId = 0; rowId < 6; rowId++)
	{
		EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
		EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
	}
	for (int rowId = 6; rowId < 6+3; rowId++)
	{
		for (int colId = 6; colId < 6+3; colId++)
		{
			testScalar(m2Init(3 + rowId-6, 3 + colId-6), m(rowId, colId));
		}
	}
	for (int rowId = 6+3; rowId < 18; rowId++)
	{
		EXPECT_TRUE(m.row(rowId).isApprox(mInit.row(rowId)));
		EXPECT_TRUE(m.col(rowId).isApprox(mInit.col(rowId)));
	}
}

TYPED_TEST(AllDynamicMatrixTests, getSubMatrix)
{
	typedef typename TestFixture::Matrix Matrix;

	Matrix m, mInit;
	m.resize(18, 18); m.setRandom(); mInit = m;

	Eigen::Block<Matrix> subMatrix = SurgSim::Math::getSubMatrix(m, 2,2, 3,3);
	EXPECT_TRUE(m.isApprox(mInit));
	for (int rowId = 0; rowId < 3; rowId++)
	{
		for (int colId = 0; colId < 3; colId++)
		{
			testScalar(m(2 * 3 + rowId, 2 * 3 + colId) , subMatrix(rowId, colId));
			// Also test that the returned value are pointing to the correct data
			EXPECT_EQ(&subMatrix(rowId, colId), &m(2 * 3 + rowId, 2 * 3 + colId));
		}
	}
}