//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Pavel Kharitonov <ipavrus@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE tiger_test

#include <boost/crypto3/hash/algorithm/hash.hpp>
#include <boost/crypto3/hash/adaptor/hashed.hpp>

#include <boost/crypto3/hash/tiger.hpp>

#include <boost/filesystem/path.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <boost/static_assert.hpp>

#include <iostream>
#include <string>
#include <unordered_map>

#include <cstdio>

using namespace boost::crypto3;
using namespace boost::crypto3::accumulators;

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost

BOOST_TEST_DONT_PRINT_LOG_VALUE(hashes::tiger<192>::digest_type)

template<std::size_t Size>
class fixture {
public:
    accumulator_set<hashes::tiger<Size>> acc;
    typedef hashes::tiger<Size> hash_t;
    virtual ~fixture() {
    }
};

#ifndef CRYPTO3_CI_DATA_DRIVEN_TESTS_DISABLED

const char *construct_file(const char *path) {
    return (boost::filesystem::path(path).parent_path() / "data" / "tiger.json").c_str();
}

boost::property_tree::ptree string_data(const char *child_name) {
    boost::property_tree::ptree root_data;
    boost::property_tree::read_json(construct_file(BOOST_PP_STRINGIZE(__FILE__)), root_data);

    return root_data.get_child(child_name);
}

BOOST_AUTO_TEST_SUITE(tiger_test_suite1)

BOOST_DATA_TEST_CASE(tiger_algorithm_test, string_data("data_192"), array_element) {
    std::string out = hash<hashes::tiger<192>>(array_element.first);

    BOOST_CHECK_EQUAL(out, array_element.second.data());
}

BOOST_DATA_TEST_CASE(tiger_adaptor_test, string_data("data_192"), array_element) {
    std::string out = array_element.first | adaptors::hashed<hashes::tiger<192>>;

    BOOST_CHECK_EQUAL(out, array_element.second.data());
}

BOOST_AUTO_TEST_SUITE_END()

#endif

BOOST_AUTO_TEST_SUITE(tiger_test_suite2)

BOOST_AUTO_TEST_CASE(tiger_iterator_hash1) {

    std::string input = "a";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("77befbef2e7ef8ab2ec8f93bf587a7fc613e247f5f247809", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash2) {

    std::string input = "abc";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("2aab1484e8c158f2bfb8c5ff41b57a525129131c957b5f93", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash3) {

    std::string input = "message digest";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("d981f8cb78201a950dcf3048751e441c517fca1aa55a29f6", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash4) {

    std::string input = "abcdefghijklmnopqrstuvwxyz";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("1714a472eee57d30040412bfcc55032a0b11602ff37beee9", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash5) {

    std::string input = "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("0f7bf9a19b9c58f2b7610df7e84f0ac3a71c631e7b53f78e", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash6) {

    std::string input = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("8dcea680a17583ee502ba38a3c368651890ffbccdc49a8cc", out);
}

BOOST_AUTO_TEST_CASE(tiger_iterator_hash7) {

    std::string input = "12345678901234567890123456789012345678901234567890123456789012345678901234567890";
    std::string out = hash<hashes::tiger<192>>(input.begin(), input.end());

    BOOST_CHECK_EQUAL("1c14795529fd9f207a958f84c52f11e887fa0cabdfd91bfd", out);
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file stomp_3dof.cpp
 * @brief This contains gtest code for stomp
 *
 * @author Jorge Nicho
 * @date March 25, 2016
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2016, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <iostream>
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <stomp/stomp.h>
#include <stomp/task.h>

using Trajectory = Eigen::MatrixXd; /**< Assign Type Trajectory to Eigen::MatrixXd Type */

const std::size_t NUM_DIMENSIONS = 3;                               /**< Number of parameters to optimize */
const std::size_t NUM_TIMESTEPS = 20;                               /**< Number of timesteps */
const double DELTA_T = 0.1;                                         /**< Timestep in seconds */
const std::vector<double> START_POS = { 1.4, 1.4, 0.5 };            /**< Trajectory starting position */
const std::vector<double> END_POS = { -1.25, 1.0, -0.26 };          /**< Trajectory ending posiiton */
const std::vector<double> BIAS_THRESHOLD = { 0.050, 0.050, 0.050 }; /**< Threshold to determine whether two trajectories
                                                                       are equal */
const std::vector<double> STD_DEV = { 1.0, 1.0, 1.0 }; /**< Standard deviation used for generating noisy parameters */

using namespace stomp;

/** @brief A dummy task for testing STOMP */
class DummyTask : public Task
{
public:
  /**
   * @brief A dummy task for testing Stomp
   * @param parameters_bias default parameter bias used for computing cost for the test
   * @param bias_thresholds threshold to determine whether two trajectories are equal
   * @param std_dev standard deviation used for generating noisy parameters
   */
  DummyTask(const Trajectory& parameters_bias,
            const std::vector<double>& bias_thresholds,
            const std::vector<double>& std_dev)
    : parameters_bias_(parameters_bias), bias_thresholds_(bias_thresholds), std_dev_(std_dev)
  {
    // generate smoothing matrix
    int num_timesteps = parameters_bias.cols();
    generateSmoothingMatrix(num_timesteps, 1.0, smoothing_M_);
    srand(1);
  }

  /** @brief See base clase for documentation */
  bool generateNoisyParameters(const Eigen::MatrixXd& parameters,
                               std::size_t start_timestep,
                               std::size_t num_timesteps,
                               int iteration_number,
                               int rollout_number,
                               Eigen::MatrixXd& parameters_noise,
                               Eigen::MatrixXd& noise) override
  {
    double rand_noise;
    for (std::size_t d = 0; d < parameters.rows(); d++)
    {
      for (std::size_t t = 0; t < parameters.cols(); t++)
      {
        rand_noise = static_cast<double>(rand() % RAND_MAX) / static_cast<double>(RAND_MAX - 1);  // 0 to 1
        rand_noise = 2 * (0.5 - rand_noise);
        noise(d, t) = rand_noise * std_dev_[d];
      }
    }

    parameters_noise = parameters + noise;

    return true;
  }

  bool computeCosts(const Trajectory& parameters,
                    std::size_t start_timestep,
                    std::size_t num_timesteps,
                    int iteration_number,
                    Eigen::VectorXd& costs,
                    bool& validity) override
  {
    return computeNoisyCosts(parameters, start_timestep, num_timesteps, iteration_number, -1, costs, validity);
  }

  bool computeNoisyCosts(const Trajectory& parameters,
                         std::size_t start_timestep,
                         std::size_t num_timesteps,
                         int iteration_number,
                         int rollout_number,
                         Eigen::VectorXd& costs,
                         bool& validity) override
  {
    costs.setZero(num_timesteps);
    double diff;
    double cost = 0.0;
    validity = true;

    for (std::size_t t = 0u; t < num_timesteps; t++)
    {
      cost = 0;
      for (std::size_t d = 0u; d < parameters.rows(); d++)
      {
        diff = std::abs(parameters(d, t) - parameters_bias_(d, t));
        if (diff > std::abs(bias_thresholds_[d]))
        {
          cost += diff;
          validity = false;
        }
      }

      costs(t) = cost;
    }

    return true;
  }

  bool filterParameterUpdates(std::size_t start_timestep,
                              std::size_t num_timesteps,
                              int iteration_number,
                              const Eigen::MatrixXd& parameters,
                              Eigen::MatrixXd& updates) override
  {
    return smoothParameterUpdates(start_timestep, num_timesteps, iteration_number, updates);
  }

protected:
  /**
   * @brief Perform a smooth update given a noisy update
   * @param start_timestep starting timestep
   * @param num_timesteps number of timesteps
   * @param iteration_number number of interations allowed
   * @param updates returned smooth update
   * @return True if successful, otherwise false
   */
  bool smoothParameterUpdates(std::size_t start_timestep,
                              std::size_t num_timesteps,
                              int iteration_number,
                              Eigen::MatrixXd& updates)
  {
    for (auto d = 0u; d < updates.rows(); d++)
    {
      updates.row(d).transpose() = smoothing_M_ * (updates.row(d).transpose());
    }

    return true;
  }

protected:
  Trajectory parameters_bias_;          /**< Parameter bias used for computing cost for the test */
  std::vector<double> bias_thresholds_; /**< Threshold to determine whether two trajectories are equal */
  std::vector<double> std_dev_;         /**< Standard deviation used for generating noisy parameters */
  Eigen::MatrixXd smoothing_M_;         /**< Matrix used for smoothing the trajectory */
};

/**
 * @brief Compares whether two trajectories are close to each other within a threshold.
 * @param optimized optimized trajectory
 * @param desired desired trajectory
 * @param thresholds used to determine if two values are equal
 * @return True if the difference between the two is less than the threshold, otherwise false
 */
bool compareDiff(const Trajectory& optimized, const Trajectory& desired, const std::vector<double>& thresholds)
{
  auto num_dimensions = optimized.rows();
  Trajectory diff = Trajectory::Zero(num_dimensions, optimized.cols());
  for (auto d = 0u; d < num_dimensions; d++)
  {
    diff.row(d) = optimized.row(d) - desired.row(d);
    diff.row(d).cwiseAbs();
    if ((diff.row(d).array() > thresholds[d]).any())
    {
      return false;
    }
  }

  return true;
}

/**
 * @brief Create the STOMP configuration for the 3 DOF problem.
 * @return  StompConfiguration
 */
StompConfiguration create3DOFConfiguration()
{
  StompConfiguration c;
  c.num_timesteps = NUM_TIMESTEPS;
  c.num_iterations = 40;
  c.num_dimensions = NUM_DIMENSIONS;
  c.delta_t = DELTA_T;
  c.control_cost_weight = 0.0;
  c.initialization_method = TrajectoryInitializations::LINEAR_INTERPOLATION;
  c.num_iterations_after_valid = 0;
  c.num_rollouts = 20;
  c.max_rollouts = 20;

  return c;
}

/**
 * @brief Compute a linear interpolated trajectory given a start and end state
 * @param start start position
 * @param end last position
 * @param num_timesteps number of timesteps
 * @param traj returned linear interpolated trajectory
 */
void interpolate(const std::vector<double>& start,
                 const std::vector<double>& end,
                 std::size_t num_timesteps,
                 Trajectory& traj)
{
  auto dimensions = start.size();
  traj = Eigen::MatrixXd::Zero(dimensions, num_timesteps);
  for (auto d = 0u; d < dimensions; d++)
  {
    double delta = (end[d] - start[d]) / (num_timesteps - 1);
    for (auto t = 0u; t < num_timesteps; t++)
    {
      traj(d, t) = start[d] + t * delta;
    }
  }
}

/** @brief This tests the Stomp constructor */
TEST(Stomp3DOF, construction)
{
  Trajectory trajectory_bias;
  interpolate(START_POS, END_POS, NUM_TIMESTEPS, trajectory_bias);
  TaskPtr task(new DummyTask(trajectory_bias, BIAS_THRESHOLD, STD_DEV));

  Stomp stomp(create3DOFConfiguration(), task);
}

/** @brief This tests the Stomp solve method for a default case */
TEST(Stomp3DOF, solve_default)
{
  Trajectory trajectory_bias;
  interpolate(START_POS, END_POS, NUM_TIMESTEPS, trajectory_bias);
  TaskPtr task(new DummyTask(trajectory_bias, BIAS_THRESHOLD, STD_DEV));

  StompConfiguration config = create3DOFConfiguration();
  Stomp stomp(config, task);

  Trajectory optimized;
  stomp.solve(START_POS, END_POS, optimized);

  EXPECT_EQ(optimized.rows(), NUM_DIMENSIONS);
  EXPECT_EQ(optimized.cols(), NUM_TIMESTEPS);
  EXPECT_TRUE(compareDiff(optimized, trajectory_bias, BIAS_THRESHOLD));

  // calculate difference
  Trajectory diff;
  diff = trajectory_bias - optimized;

  std::string line_separator = "\n------------------------------------------------------\n";
  std::cout << line_separator;
  std::cout << stomp::toString(trajectory_bias);
  std::cout << line_separator;
  std::cout << toString(optimized) << "\n";
  std::cout << "Differences"
            << "\n"
            << toString(diff) << line_separator;
}

/** @brief This tests the Stomp solve method using a linear interpolated inital trajectory */
TEST(Stomp3DOF, solve_interpolated_initial)
{
  Trajectory trajectory_bias;
  interpolate(START_POS, END_POS, NUM_TIMESTEPS, trajectory_bias);
  TaskPtr task(new DummyTask(trajectory_bias, BIAS_THRESHOLD, STD_DEV));

  StompConfiguration config = create3DOFConfiguration();
  config.initialization_method = TrajectoryInitializations::LINEAR_INTERPOLATION;
  Stomp stomp(config, task);

  Trajectory optimized;
  stomp.solve(trajectory_bias, optimized);

  EXPECT_EQ(optimized.rows(), NUM_DIMENSIONS);
  EXPECT_EQ(optimized.cols(), NUM_TIMESTEPS);
  EXPECT_TRUE(compareDiff(optimized, trajectory_bias, BIAS_THRESHOLD));

  // calculate difference
  Trajectory diff;
  diff = trajectory_bias - optimized;

  std::string line_separator = "\n------------------------------------------------------\n";
  std::cout << line_separator;
  std::cout << stomp::toString(trajectory_bias);
  std::cout << line_separator;
  std::cout << toString(optimized) << "\n";
  std::cout << "Differences"
            << "\n"
            << toString(diff) << line_separator;
}

/** @brief This tests the Stomp solve method using a cubic polynomial interpolated inital trajectory */
TEST(Stomp3DOF, solve_cubic_polynomial_initial)
{
  Trajectory trajectory_bias;
  interpolate(START_POS, END_POS, NUM_TIMESTEPS, trajectory_bias);
  TaskPtr task(new DummyTask(trajectory_bias, BIAS_THRESHOLD, STD_DEV));

  StompConfiguration config = create3DOFConfiguration();
  config.initialization_method = TrajectoryInitializations::CUBIC_POLYNOMIAL_INTERPOLATION;
  Stomp stomp(config, task);

  Trajectory optimized;
  stomp.solve(trajectory_bias, optimized);

  EXPECT_EQ(optimized.rows(), NUM_DIMENSIONS);
  EXPECT_EQ(optimized.cols(), NUM_TIMESTEPS);
  EXPECT_TRUE(compareDiff(optimized, trajectory_bias, BIAS_THRESHOLD));

  // calculate difference
  Trajectory diff;
  diff = trajectory_bias - optimized;

  std::string line_separator = "\n------------------------------------------------------\n";
  std::cout << line_separator;
  std::cout << stomp::toString(trajectory_bias);
  std::cout << line_separator;
  std::cout << toString(optimized) << "\n";
  std::cout << "Differences"
            << "\n"
            << toString(diff) << line_separator;
}

/** @brief This tests the Stomp solve method using a minimum cost inital trajectory */
TEST(Stomp3DOF, solve_min_control_cost_initial)
{
  Trajectory trajectory_bias;
  interpolate(START_POS, END_POS, NUM_TIMESTEPS, trajectory_bias);
  TaskPtr task(new DummyTask(trajectory_bias, BIAS_THRESHOLD, STD_DEV));

  StompConfiguration config = create3DOFConfiguration();
  config.initialization_method = TrajectoryInitializations::MININUM_CONTROL_COST;
  Stomp stomp(config, task);

  Trajectory optimized;
  stomp.solve(trajectory_bias, optimized);

  EXPECT_EQ(optimized.rows(), NUM_DIMENSIONS);
  EXPECT_EQ(optimized.cols(), NUM_TIMESTEPS);
  EXPECT_TRUE(compareDiff(optimized, trajectory_bias, BIAS_THRESHOLD));

  // calculate difference
  Trajectory diff;
  diff = trajectory_bias - optimized;

  std::string line_separator = "\n------------------------------------------------------\n";
  std::cout << line_separator;
  std::cout << stomp::toString(trajectory_bias);
  std::cout << line_separator;
  std::cout << toString(optimized) << "\n";
  std::cout << "Differences"
            << "\n"
            << toString(diff) << line_separator;
}

/** @brief This tests Stomp solve method given 40 timesteps */
TEST(Stomp3DOF, solve_40_timesteps)
{
  Trajectory trajectory_bias;
  int num_timesteps = 40;
  interpolate(START_POS, END_POS, num_timesteps, trajectory_bias);
  TaskPtr task(new DummyTask(trajectory_bias, BIAS_THRESHOLD, STD_DEV));

  StompConfiguration config = create3DOFConfiguration();
  config.initialization_method = TrajectoryInitializations::LINEAR_INTERPOLATION;
  config.num_iterations = 100;
  config.num_timesteps = num_timesteps;
  Stomp stomp(config, task);

  Trajectory optimized;
  stomp.solve(START_POS, END_POS, optimized);

  EXPECT_EQ(optimized.rows(), NUM_DIMENSIONS);
  EXPECT_EQ(optimized.cols(), num_timesteps);
  EXPECT_TRUE(compareDiff(optimized, trajectory_bias, BIAS_THRESHOLD));

  // calculate difference
  Trajectory diff;
  diff = trajectory_bias - optimized;

  std::string line_separator = "\n------------------------------------------------------\n";
  std::cout << line_separator;
  std::cout << stomp::toString(trajectory_bias);
  std::cout << line_separator;
  std::cout << toString(optimized) << "\n";
  std::cout << "Differences"
            << "\n"
            << toString(diff) << line_separator;
}

/** @brief This tests Stomp solve method given 60 timesteps */
TEST(Stomp3DOF, solve_60_timesteps)
{
  Trajectory trajectory_bias;
  int num_timesteps = 60;
  interpolate(START_POS, END_POS, num_timesteps, trajectory_bias);
  TaskPtr task(new DummyTask(trajectory_bias, BIAS_THRESHOLD, STD_DEV));

  StompConfiguration config = create3DOFConfiguration();
  // config.initialization_method = TrajectoryInitializations::MININUM_CONTROL_COST; // this initialization method is
  // currently behaving oddly
  config.num_iterations = 100;
  config.num_timesteps = num_timesteps;
  Stomp stomp(config, task);

  Trajectory optimized;
  stomp.solve(START_POS, END_POS, optimized);

  EXPECT_EQ(optimized.rows(), NUM_DIMENSIONS);
  EXPECT_EQ(optimized.cols(), num_timesteps);
  EXPECT_TRUE(compareDiff(optimized, trajectory_bias, BIAS_THRESHOLD));

  // calculate difference
  Trajectory diff;
  diff = trajectory_bias - optimized;

  std::string line_separator = "\n------------------------------------------------------\n";
  std::cout << line_separator;
  std::cout << stomp::toString(trajectory_bias);
  std::cout << line_separator;
  std::cout << toString(optimized) << "\n";
  std::cout << "Differences"
            << "\n"
            << toString(diff) << line_separator;
}


/*ckwg +29
 * Copyright 2014-2015 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief Implementation of \link maptk::similarity_ similarity_<T> \endlink
 *        for \c T = { \c float, \c double }
 */

#include "similarity.h"
#include "eigen_io.h"
#include <cmath>
#include <Eigen/LU>

namespace maptk
{

/// Constructor - from a matrix
template <typename T>
similarity_<T>
::similarity_(const Eigen::Matrix<T,4,4>& M)
{
  if (M(3,0) != T(0) || M(3,1) != T(0) || M(3,2) != T(0) || M(3,3) != T(1))
  {
    // not a similarity if bottom row is not [0,0,0,1]
    // TODO throw an exception here
    std::cerr << "third row of similarity matrix must be [0,0,0,1]"<<std::endl;
    return;
  }
  Eigen::Matrix<T,3,3> sr = M.template block<3,3>(0,0);
  this->scale_ = sr.determinant();
  if (this->scale_ <= T(0))
  {
    // similarity must have positive scale
    // TODO throw an exception
    std::cerr << "determinant in upper 3x3 of similarity matrix must be positive"<<std::endl;
    return;
  }
  // take the cube root
  this->scale_ = std::pow(this->scale_, static_cast<T>(1/3.0));
  // factor scale out of sr
  sr /= this->scale_;
  this->rot_ = rotation_<T>(sr);
  assert((Eigen::Matrix<T,3,3>(this->rot_)-sr).norm() < 1e-4);
  this->trans_ = M.template block<3,1>(0,3);
}

/// Convert to a 4x4 matrix
template <typename T>
similarity_<T>
::operator Eigen::Matrix<T,4,4>() const
{
  Eigen::Matrix<T,4,4> mat = Eigen::Matrix<T,4,4>::Zero();
  mat(3,3) = 1;
  mat.template block<3,3>(0,0) = this->scale_ * Eigen::Matrix<T,3,3>(this->rot_);
  mat.template block<3,1>(0,3) = this->trans_;
  return mat;
}

/// Compose two transforms
template <typename T>
similarity_<T>
similarity_<T>
::operator*(const similarity_<T>& rhs) const
{
  return similarity_<T>(this->scale_ * rhs.scale_,
                        this->rot_ * rhs.rot_,
                        *this * rhs.trans_);
}

/// Transform a vector
template <typename T>
Eigen::Matrix<T,3,1>
similarity_<T>
::operator*(const Eigen::Matrix<T,3,1>& rhs) const
{
  return this->scale_ * (this->rot_ * rhs) + this->trans_;
}

/// output stream operator for a similarity transformation
template <typename T>
std::ostream&  operator<<(std::ostream& s, const similarity_<T>& t)
{
  s << t.scale() << ' ' << t.rotation() << ' ' << t.translation();
  return s;
}

/// input stream operator for a similarity transformation
template <typename T>
std::istream&  operator>>(std::istream& s, similarity_<T>& t)
{
  T sc;
  rotation_<T> ro;
  Eigen::Matrix<T,3,1> tr;
  s >> sc >> ro >> tr;
  t = similarity_<T>(sc, ro, tr);
  return s;
}

/// \cond DoxygenSuppress
#define INSTANTIATE_SIMILARITY(T) \
template class MAPTK_LIB_EXPORT similarity_<T>; \
template MAPTK_LIB_EXPORT std::ostream&  operator<<(std::ostream& s, const similarity_<T>& t); \
template MAPTK_LIB_EXPORT std::istream&  operator>>(std::istream& s, similarity_<T>& t)

INSTANTIATE_SIMILARITY(double);
INSTANTIATE_SIMILARITY(float);

#undef INSTANTIATE_SIMILARITY
/// \endcond

} // end namespace maptk