//------------------------------------------------------------------------------
/// \file BinaryTrees_tests.cpp
/// \date 20201023 03:44
//------------------------------------------------------------------------------
#include "DataStructures/BinaryTrees.h"

#include <boost/test/unit_test.hpp>
#include <string>
#include <vector>

using DataStructures::BinaryTrees::TreeNode;
using DataStructures::BinaryTrees::balance_max_height_recursive;
using DataStructures::BinaryTrees::inorder_traversal_iterative;
using DataStructures::BinaryTrees::inorder_traversal_recursive;
using DataStructures::BinaryTrees::is_same_recursive;
using DataStructures::BinaryTrees::level_order_traversal;
using DataStructures::BinaryTrees::max_depth;
using DataStructures::BinaryTrees::postorder_traversal_iterative_simple;
using DataStructures::BinaryTrees::postorder_traversal_recursive;
using DataStructures::BinaryTrees::preorder_traversal;
using DataStructures::BinaryTrees::preorder_traversal_recursive;
using DataStructures::BinaryTrees::serialize;
using std::string;
using std::vector;

BOOST_AUTO_TEST_SUITE(DataStructures)
BOOST_AUTO_TEST_SUITE(BinaryTrees_tests)

// cf. https://leetcode.com/explore/learn/card/data-structure-tree/134/traverse-a-tree/992/
TreeNode example_root {6};
TreeNode d11 {2};
TreeNode d12 {7};
TreeNode d21 {1};
TreeNode d22 {4};
TreeNode d23 {9};
TreeNode d31 {3};
TreeNode d32 {5};
TreeNode d33 {8};

TreeNode example_root_A {1};
TreeNode d11_A {2};
TreeNode d12_A {3};
TreeNode d21_A {4};
TreeNode d22_A {5};
TreeNode d23_A {6};

TreeNode example_root_B {3};
TreeNode d11_B {9};
TreeNode d12_B {20};
TreeNode d21_B {15};
TreeNode d22_B {7};

TreeNode leaf {42};

TreeNode base_case_1_root {1};
TreeNode base_case_1_d11 {2};
TreeNode base_case_1_d12 {3};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(PreorderTraversalTraversesFirstEncounters)
{
  example_root.left_ = &d11;
  example_root.right_ = &d12;
  d11.left_ = &d21;
  d11.right_ = &d22;
  d22.left_ = &d31;
  d22.right_ = &d32;
  d12.right_ = &d23;
  d23.right_ = &d33;

  TreeNode* example_root_ptr {&example_root};

  vector<int> result {preorder_traversal(example_root_ptr)};

  vector<int> expected {6, 2, 1, 4, 3, 5, 7, 9, 8};

  BOOST_TEST(result == expected);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(PreorderTraversalRecursiveTraversesFirstEncounters)
{
  example_root.left_ = &d11;
  example_root.right_ = &d12;
  d11.left_ = &d21;
  d11.right_ = &d22;
  d22.left_ = &d31;
  d22.right_ = &d32;
  d12.right_ = &d23;
  d23.right_ = &d33;

  TreeNode* example_root_ptr {&example_root};

  vector<int> result {preorder_traversal_recursive(example_root_ptr)};

  vector<int> expected {6, 2, 1, 4, 3, 5, 7, 9, 8};

  BOOST_TEST(result == expected);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InorderTraversalIterativeTraversesAfterLeftSubtree)
{
  example_root_A.left_ = &d11_A;
  example_root_A.right_ = &d12_A;
  d11_A.left_ = &d21_A;
  d11_A.right_ = &d22_A;
  d12_A.left_ = &d23_A;

  TreeNode* example_root_ptr {&example_root_A};

  vector<int> result {inorder_traversal_iterative(example_root_ptr)};

  vector<int> expected {4, 2, 5, 1, 6, 3};

  BOOST_TEST(result == expected);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(InorderTraversalRecursiveTraversesAfterLeftSubtree)
{
  example_root.left_ = &d11;
  example_root.right_ = &d12;
  d11.left_ = &d21;
  d11.right_ = &d22;
  d22.left_ = &d31;
  d22.right_ = &d32;
  d12.right_ = &d23;
  d23.right_ = &d33;

  TreeNode* example_root_ptr {&example_root};

  vector<int> result {inorder_traversal_recursive(example_root_ptr)};

  vector<int> expected {1, 2, 3, 4, 5, 6, 7, 9, 8};

  BOOST_TEST(result == expected);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(PostorderTraversalIterativeTraversesAfterRightSubtree)
{
  example_root_A.left_ = &d11_A;
  example_root_A.right_ = &d12_A;
  d11_A.left_ = &d21_A;
  d11_A.right_ = &d22_A;
  d12_A.left_ = &d23_A;

  TreeNode* example_root_ptr {&example_root_A};

  vector<int> result {postorder_traversal_iterative(example_root_ptr)};

  vector<int> expected {4, 5, 2, 6, 3, 1};

  BOOST_TEST(result == expected);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(PostorderTraversalRecursiveTraversesAfterRightSubtree)
{
  example_root.left_ = &d11;
  example_root.right_ = &d12;
  d11.left_ = &d21;
  d11.right_ = &d22;
  d22.left_ = &d31;
  d22.right_ = &d32;
  d12.right_ = &d23;
  d23.right_ = &d33;

  TreeNode* example_root_ptr {&example_root};

  vector<int> result {postorder_traversal_recursive(example_root_ptr)};

  vector<int> expected {1, 3, 5, 4, 2, 8, 9, 7, 6};

  BOOST_TEST(result == expected);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(
  PostorderTraversalIterativeSimpleTraversesAfterRightSubtree)
{
  example_root_A.left_ = &d11_A;
  example_root_A.right_ = &d12_A;
  d11_A.left_ = &d21_A;
  d11_A.right_ = &d22_A;
  d12_A.left_ = &d23_A;

  TreeNode* example_root_ptr {&example_root_A};

  vector<int> result {postorder_traversal_iterative_simple(example_root_ptr)};

  vector<int> expected {4, 5, 2, 6, 3, 1};

  BOOST_TEST(result == expected);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(LevelOrderTraversalReturnsNodesInByLevels)
{
  example_root_B.left_ = &d11_B;
  example_root_B.right_ = &d12_B;
  d12_B.left_ = &d21_B;
  d12_B.right_ = &d22_B;

  TreeNode* example_root_ptr {&example_root_B};

  vector<vector<int>> result {level_order_traversal(example_root_ptr)};

  BOOST_TEST(result.at(0) == vector<int>{3});
  BOOST_TEST(result.at(1) == vector<int>({9, 20}));
  BOOST_TEST(result.at(2) == vector<int>({15, 7}));
  BOOST_TEST(result.size() == 3);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(MaxDepthFindsMaximumDepth)
{
  example_root_B.left_ = &d11_B;
  example_root_B.right_ = &d12_B;
  d12_B.left_ = &d21_B;
  d12_B.right_ = &d22_B;

  TreeNode* example_root_ptr {&example_root_B};

  const int result {max_depth(example_root_ptr)};

  BOOST_TEST(result == 3);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BalanceMaxHeightRecursiveWorksOnSimpleBaseCases)
{
  {
    TreeNode* example_ptr {nullptr};

    const auto result = balance_max_height_recursive(example_ptr);
    BOOST_TEST(result.first == true);
    BOOST_TEST(result.second == -1);
  }
  {
    // Check if we have the test setup initial conditions we want.
    TreeNode* example_root_ptr {&leaf};
    BOOST_TEST(example_root_ptr->left_ == nullptr);
    BOOST_TEST(example_root_ptr->right_ == nullptr);

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == true);
    BOOST_TEST(result.second == 0);
  }
  {
    base_case_1_root.left_ = &base_case_1_d11;
    base_case_1_root.right_ = &base_case_1_d12;
    TreeNode* example_root_ptr {&base_case_1_root};

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == true);
    BOOST_TEST(result.second == 1);
  }
  {
    TreeNode* example_root_ptr {&example_root_B};
    BOOST_TEST(example_root_ptr->left_ != nullptr);
    BOOST_TEST(example_root_ptr->right_ != nullptr);

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == true);
    BOOST_TEST(result.second == 2);
  }
}

TreeNode height_example_1_root {3};
TreeNode height_example_1_d11 {9};
TreeNode height_example_1_d12 {20};
TreeNode height_example_1_d21 {15};
TreeNode height_example_1_d22 {7};

TreeNode height_example_2_root {1};
TreeNode height_example_2_d11 {2};
TreeNode height_example_2_d12 {2};
TreeNode height_example_2_d21 {3};
TreeNode height_example_2_d22 {3};
TreeNode height_example_2_d31 {4};
TreeNode height_example_2_d32 {4};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BalanceMaxHeightRecursiveDeterminesBalanceProperty)
{
  {
    TreeNode* example_root_ptr {&example_root_B};
    BOOST_TEST(example_root_ptr->left_ != nullptr);
    BOOST_TEST(example_root_ptr->right_ != nullptr);

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == true);
    BOOST_TEST(result.second == 2);
  }
  {
    TreeNode* example_root_ptr {&example_root_A};
    BOOST_TEST(example_root_ptr->left_ != nullptr);
    BOOST_TEST(example_root_ptr->right_ != nullptr);

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == true);
    BOOST_TEST(result.second == 2);
  }
  {
    TreeNode* example_root_ptr {&example_root};
    BOOST_TEST(example_root_ptr->left_ != nullptr);
    BOOST_TEST(example_root_ptr->right_ != nullptr);

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == false);
    BOOST_TEST(result.second == 3);
  }
  {
    height_example_1_root.left_ = &height_example_1_d11;
    height_example_1_root.right_ = &height_example_1_d12;
    height_example_1_d12.left_ = &height_example_1_d21;
    height_example_1_d12.right_ = &height_example_1_d22;

    TreeNode* example_root_ptr {&height_example_1_root};
    BOOST_TEST(example_root_ptr->left_ != nullptr);
    BOOST_TEST(example_root_ptr->right_ != nullptr);

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == true);
    BOOST_TEST(result.second == 2);
  }
  {
    height_example_2_root.left_ = &height_example_2_d11;
    height_example_2_root.right_ = &height_example_2_d12;
    height_example_2_d11.left_ = &height_example_2_d21;
    height_example_2_d11.right_ = &height_example_2_d22;
    height_example_2_d21.left_ = &height_example_2_d31;
    height_example_2_d21.right_ = &height_example_2_d32;

    TreeNode* example_root_ptr {&height_example_2_root};
    BOOST_TEST(example_root_ptr->left_ != nullptr);
    BOOST_TEST(example_root_ptr->right_ != nullptr);

    const auto result = balance_max_height_recursive(example_root_ptr);
    BOOST_TEST(result.first == false);
    BOOST_TEST(result.second == 3);
  }
}

/// cf. https://www.techiedelight.com/check-if-two-binary-trees-are-identical-not-iterative-recursive/
/// cf. https://medium.com/techie-delight/binary-tree-interview-questions-and-practice-problems-439df7e5ea1f
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IsSameRecursiveReturnsTrueForSameTrees)
{
  {
    TreeNode x {15};
    TreeNode x_d11 {10};
    TreeNode x_d12 {20};
    TreeNode x_d21 {8};
    TreeNode x_d22 {12};
    TreeNode x_d23 {16};
    TreeNode x_d24 {25};

    TreeNode* x_ptr {&x};
    x_ptr->left_ = &x_d11;
    x_ptr->right_ = &x_d12;
    x_ptr->left_->left_ = &x_d21;
    x_ptr->left_->right_ = &x_d22;
    x_ptr->right_->left_ = &x_d23;
    x_ptr->right_->left_ = &x_d24;

    TreeNode y {15};
    TreeNode y_d11 {10};
    TreeNode y_d12 {20};
    TreeNode y_d21 {8};
    TreeNode y_d22 {12};
    TreeNode y_d23 {16};
    TreeNode y_d24 {25};

    TreeNode* y_ptr {&y};
    y_ptr->left_ = &y_d11;
    y_ptr->right_ = &y_d12;
    y_ptr->left_->left_ = &y_d21;
    y_ptr->left_->right_ = &y_d22;
    y_ptr->right_->left_ = &y_d23;
    y_ptr->right_->left_ = &y_d24;

    BOOST_TEST(is_same_recursive(x_ptr, y_ptr));
  }
  {
    TreeNode* example_root_ptr {&example_root};
    TreeNode* example_root_A_ptr {&example_root_A};
    BOOST_TEST(!is_same_recursive(example_root_ptr, example_root_A_ptr));
  }
}

//------------------------------------------------------------------------------
/// cf. https://leetcode.com/problems/serialize-and-deserialize-binary-tree/
//------------------------------------------------------------------------------

TreeNode serialize_example_1_root {1};
TreeNode serialize_example_1_d11 {2};
TreeNode serialize_example_1_d12 {3};
TreeNode serialize_example_1_d21 {4};
TreeNode serialize_example_1_d22 {5};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SerializeSerializesBinaryTree)
{
  serialize_example_1_root.left_ = &serialize_example_1_d11;
  serialize_example_1_root.right_ = &serialize_example_1_d12;
  serialize_example_1_d12.left_ = &serialize_example_1_d21;
  serialize_example_1_d12.right_ = &serialize_example_1_d22;

  TreeNode* example_root_ptr {&serialize_example_1_root};

  const string result {serialize(example_root_ptr)};

  BOOST_TEST(result == "1,2,null,null,3,4,null,null,5,null,null");
}

BOOST_AUTO_TEST_SUITE_END() // BinaryTrees_tests
BOOST_AUTO_TEST_SUITE_END() // DataStructures


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

#include <boost/lexical_cast.hpp>

#include <am_driver/WheelEncoder.h>

namespace gazebo
{
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboRosWheelEncoder);

#define RADIANS_PER_TICK		(M_PI*2.0/1093.0)
#define WHEEL_METER_PER_TICK	(0.000704)

////////////////////////////////////////////////////////////////////////////////
// Constructor
GazeboRosWheelEncoder::GazeboRosWheelEncoder()
{
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosWheelEncoder::~GazeboRosWheelEncoder()
{
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosWheelEncoder::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
	// Make sure the ROS node for Gazebo has already been initalized
	if (!ros::isInitialized())
	{
		ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. Load the Gazebo system plugin 'libam_gazebo_wheelencoder.so' in the gazebo_ros package)");
		return;
	}

	ROS_INFO("WheelEncoder: am_gazebo_wheelencoder loaded...");

	parent = _parent;
	world = _parent->GetWorld();

	gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "WheelEncoder" ) );
	// Make sure the ROS node for Gazebo has already been initialized
	gazebo_ros_->isInitialized();

	// Publisher
	encoderPub = gazebo_ros_->node()->advertise<am_driver::WheelEncoder>("wheel_encoder", 100);

	// Get the links/joints required...

	std::string name = "back_left_wheel";
	leftWheelLink = _parent->GetLink(name);

	if (!leftWheelLink)
	{
		ROS_FATAL("WheelEncoder plugin error: link %s does not exist\n", name.c_str());
		return;
	}
	name = "back_left_wheel_joint";
	leftWheelJoint = _parent->GetJoint(name);

	if (!leftWheelJoint)
	{
		ROS_FATAL("WheelEncoder plugin error: joint %s does not exist\n", name.c_str());
		return;
	}

	name = "back_right_wheel";
	rightWheelLink = _parent->GetLink(name);

	if (!rightWheelLink)
	{
		ROS_FATAL("WheelEncoder plugin error: link %s does not exist\n", name.c_str());
		return;
	}
	name = "back_right_wheel_joint";
	rightWheelJoint = _parent->GetJoint(name);

	if (!rightWheelJoint)
	{
		ROS_FATAL("WheelEncoder plugin error: joint %s does not exist\n", name.c_str());
		return;
	}

	lastLeftAngle = leftWheelJoint->GetAngle(0).Radian();
	lastRightAngle = rightWheelJoint->GetAngle(0).Radian();

 	// listen to the update event (broadcast every simulation iteration)
	//updateConnection = event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosWheelEncoder::Update, this ) );

	// connect Update function
	updateTimer.setUpdateRate(50.0);
	updateTimer.Load(world, _sdf);
	updateConnection = updateTimer.Connect(boost::bind(&GazeboRosWheelEncoder::Update, this));

}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosWheelEncoder::Update()
{

	// Get the actual angle of the Joints

	// LEFT
	double leftAngle = leftWheelJoint->GetAngle(0).Radian();
	double dLeftAngle = leftAngle - lastLeftAngle;
	lastLeftAngle = leftAngle;
	//std::cout << "LeftAngle: " <<  leftAngle*180/M_PI << std::endl;

	double leftPulses = dLeftAngle / RADIANS_PER_TICK;
	double leftDist = leftPulses * WHEEL_METER_PER_TICK;

	// RIGHT
	double rightAngle = rightWheelJoint->GetAngle(0).Radian();
	double dRightAngle = rightAngle - lastRightAngle;
	lastRightAngle = rightAngle;
	//std::cout << "RightAngle: " <<  rightAngle*180/M_PI << std::endl;

	double rightPulses = dRightAngle / RADIANS_PER_TICK;
	double rightDist = rightPulses * WHEEL_METER_PER_TICK;

	// ACCUM
	double leftAccum = -leftAngle/RADIANS_PER_TICK;
	double rightAccum = rightAngle/RADIANS_PER_TICK;

	// PUBLISH
	//std::cout << "LeftDist: " << leftDist << " RightDist: " << rightDist;
	//std::cout << " LeftAccum: " << leftAccum << " RightAccum: " << rightAccum << std::endl;

	// Limit if small values...
	if (fabs(leftDist) < 0.0001)
	{
		leftDist = 0.0;
	}
	if (fabs(rightDist) < 0.0001)
	{
		rightDist = 0.0;
	}

	am_driver::WheelEncoder encoder;

	encoder.header.frame_id = "wheel_encoder";
	encoder.header.stamp = ros::Time::now();
	encoder.lwheel = leftDist;
	encoder.rwheel = rightDist;
	encoder.lwheelAccum = leftAccum;
	encoder.rwheelAccum = rightAccum;

	encoderPub.publish(encoder);
}

}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef SNPTEST_CASE_CONTROL_MULTINOMIAL_LOG_LIKELIHOOD_HPP
#define SNPTEST_CASE_CONTROL_MULTINOMIAL_LOG_LIKELIHOOD_HPP

#include <vector>
#include <memory>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "metro/regression/Design.hpp"
#include "metro/regression/LogLikelihood.hpp"

namespace metro {
	namespace regression {
		/*
		* This class implements a log-likelihood for multinomial logistic regression, allowing predictors
		* to take one of a finite set of values with associated probabilities. (A "missing data log-likelihood" ).
		* The outcome variables must form a contiguous set of numbers in the range 0,1,...,M
		* for some positive integer M > 0.
		*
		* Notation:
		* N - number of samples
		* L - number of predictor levels
		* M - number of outcome levels not counting the baseline.
		*
		*/
		struct MultinomialRegressionLogLikelihood: public LogLikelihood
		{
		public:
			typedef regression::Design::Vector Vector ;
			typedef regression::Design::RowVector RowVector ;
			typedef regression::Design::Matrix Matrix ;
			typedef Eigen::Block< Matrix > MatrixBlock ;
			typedef Eigen::Block< Matrix const > ConstMatrixBlock ;
			typedef Eigen::PermutationMatrix< Eigen::Dynamic, Eigen::Dynamic > PermutationMatrix ;
			typedef boost::function< std::string( std::string const& predictor_name, int outcome_level ) > GetParameterName ;
		public:
			typedef std::auto_ptr< MultinomialRegressionLogLikelihood > UniquePtr ;
			static UniquePtr create( regression::Design::UniquePtr ) ;

		public:
			MultinomialRegressionLogLikelihood( regression::Design::UniquePtr ) ;

			regression::Design& design() const { return *m_design ; }
			void set_predictor_levels( Matrix const& levels, Matrix const& probabilities, std::vector< metro::SampleRange > const& included_samples ) ;

			void set_parameter_naming_scheme( GetParameterName ) ;
			int number_of_parameters() const ;
			int number_of_outcomes() const ;
			std::string get_parameter_name( std::size_t i ) const ;
			IntegerMatrix identify_parameters() const ;

			void evaluate_at( Vector const& parameters, int const numberOfDerivatives = 2 ) ;
			void evaluate( int const numberOfDerivatives = 2 ) ;

			Vector const& parameters() const ;
			double get_value_of_function() const ;
			Vector get_value_of_first_derivative() const ;
			Matrix get_value_of_second_derivative() const ;

			std::string get_summary() const ;

		private:
			regression::Design::UniquePtr m_design ;
			int const m_number_of_samples ;
			GetParameterName m_get_parameter_name ;
			Vector m_parameter_vector ;
			Matrix m_parameter_matrix ;
			IntegerMatrix m_parameter_identity ;
			std::size_t m_numberOfDerivativesComputed ;

			// rearranger.  This matrix rearranges columns of an NxLM matrix from L blocks of NxM to M blocks of NxL.
			PermutationMatrix m_outcome_wise_to_predictor_wise_rearranger ;
			// Psi matrix, (NxLM).  This represents phenotype data as indicator 1's in each row.
			Matrix m_psi ;
			// F matrix (Nx(L(M+1))).  Stores outcome probabilities.
			Matrix m_F ;
			// A matrix (NxL).  Stores outcome probabilities times predictor probability, renormalised.
			Matrix m_A ;
			// B matrix (Nx(LM)).  Used in the computation of 1st and 2nd derivatives.
			Matrix m_B ;
			// C matrix (Nx(LM^2)).  Used in the computation of 2nd derivative.
			Matrix m_C ;
			// temp matrix.  Stores the terms that are summed to make the 1st derivative
			Matrix m_first_derivative_terms ;
			// temp matrix.  Stores rows of the design matrix tensor squares.
			mutable Matrix m_design_matrix_tensor_square_rows ;

			double m_value_of_function ;
			Vector m_value_of_first_derivative ;
			Matrix m_value_of_second_derivative ;

		private:

			void compute_psi(
				Matrix const& outcome,
				int const number_of_levels,
				std::vector< metro::SampleRange > const& included_samples,
				Matrix* result
			) const ;
			void compute_rearranger( int const number_of_predictor_levels, PermutationMatrix* result ) const ;
			IntegerMatrix compute_parameter_identity() const ;

			void evaluate_impl( int const numberOfDerivatives ) ;
			void compute_F( Matrix const& parameters, Matrix* result ) const ;
			void rearrange_F( Matrix* result ) const ;
			void compute_A_and_function_value(
				Matrix const& F,
				Matrix const& predictor_probabilities,
				std::vector< metro::SampleRange > const& included_samples,
				Matrix* result,
				double* value_of_function
			) const ;
			void compute_B(
				Matrix const& A,
				Matrix const& F,
				Matrix const& psi,
				Matrix* result
			) const ;
			void compute_value_of_first_derivative(
				Matrix const& B,
				std::vector< metro::SampleRange > const& included_samples,
				Vector* result,
				Matrix* terms
			) const ;
			// void compute_C( Matrix const& B, Matrix const& F, Matrix const& Gamma, Matrix* result ) const ;
			void compute_C(
				Matrix const& B,
				Matrix const& F,
				Matrix const& Gamma,
				std::vector< metro::SampleRange > const& included_samples,
				Matrix* result
			) const ;
			void compute_value_of_second_derivative(
				Matrix const& B,
				Matrix const& C,
				std::vector< metro::SampleRange > const& included_samples,
				Matrix* result
			) const ;
		} ;
	}
}

#endif


#include <vector>

#include <glm/glm.hpp>

#include <Eigen/Sparse>

#include "Types.h"
#include "Viewer.h"
#include "controls.h"
#include "Dynamic.h"

extern int shadFlag;
extern View::Viewer *p_viewer;

namespace BalloonFEM
{
    Engine::Engine(TetraMesh* tetra,
            ElasticModel*   volume_model,
            AirModel*       air_model,
            FilmModel*      film_model,
            BendingModel*   bend_model)
    {
        m_tetra = tetra;
        m_size = tetra->vertices.size();
        m_volume_model = volume_model;
        m_air_model = air_model;
		m_film_model = film_model;
        m_bend_model = bend_model;

		f_ext.assign(m_size, Vec3(0));

		this->inputData();
    }

	void Engine::inputData()
	{
        cur_state.input(m_tetra);

		/* load data */
		for (size_t i = 0; i < m_size; i++)
		{
			Vertex &v = m_tetra->vertices[i];
			f_ext[i] = v.m_f_ext;
		}
	}

    void Engine::outputData()
    {
        cur_state.project();
        cur_state.output();
    }

    void Engine::stepToNext()
    {
        std::swap(cur_state, next_state);
    }

    void Engine::computeForceAndGradient(ObjState &state, SpVec &f, SpMat &A)
    {
        Vvec3 f_sum;
        f_sum.assign( m_size, Vec3(0.0));

        /* compute elastic forces by tetrahedrons */
        computeElasticForces(state, f_sum);

		/* compute film forces by pieces */
		computeFilmForces(state, f_sum);

        /* compute forces by air pressure */
		computeAirForces(state, f_sum);

        for(size_t i = 0; i < m_size; i++)
            f_sum[i] += f_ext[i];

		/* compute forces diff by air pressure */
        SpMat K = computeAirDiffMat(state);

		/* compute film forces by pieces */
		K += computeFilmDiffMat(state);

		/* compute elastic forces diff by tetrahedrons */
		K += computeElasticDiffMat(state);

        /* compute bending force and gradient */
        K -= bendingForceAndGradient(state, f_sum);

        /* convert force to SpVec */
        SpVec f_real = SpVec::Zero(3 * m_size);
        for (size_t i = 0; i < f_sum.size(); i++)
        {
            f_real( 3 * i ) = f_sum[i].x;
            f_real( 3 * i + 1) = f_sum[i].y;
            f_real( 3 * i + 2) = f_sum[i].z;
        }

        /* convert K to \tilde K with W transfer. The restricted vertices
         * has all 0 colume and raw so we add 1 to its diagnal */
        A = - state.projectMat().transpose() * K * state.projectMat() + state.restrictedMat();

        f = state.projectMat().transpose() * f_real;

    }

    #define CONVERGE_ERROR_RATE 1e-4
    void Engine::solveStaticPos()
    {
      /* initialize next_state */
        next_state = cur_state;
		next_state.project();
        SpVec f_sum = SpVec::Zero(next_state.freedomDegree());

        /* initialize temp variable for iterative implicit solving */
        /* f is total force on each vertex */
        /* K = - df/dr, here Force Diff Mat compute df/dr */
        SpMat K;
        computeForceAndGradient(next_state, f_sum, K);

        SpVec dstate = SpVec::Zero(next_state.freedomDegree());

        /* solver */
        Eigen::SimplicialLDLT<SpMat> solver;
        SpVec &b = f_sum;

		double err_f = f_sum.dot(f_sum);
		double err_begin = err_f;
		int count_iter = 0;		/* K dx = f iter count */
        /* while not converge f == 0, iterate */
		while ((err_f > CONVERGE_ERROR_RATE * err_begin) && (err_f > 1e-10))
        {
            /* debug use */
			count_iter++;
            printf("%d iter of K dv = f , err_felas = %.4e \n", count_iter, err_f);

            /* r0 = b - Ax0 */
            SpVec r = b - K * dstate;

            printf("building solver\n");
            solver.compute(K);
            if (solver.info() != Eigen::Success)
            {
                printf("decomposition failed!\n");
				printf("Number of non zeros: %d \n", K.nonZeros());
                return;
            }
			printf("solve delta_x \n");
            SpVec dstate = solver.solve(r);

            /* update v_pos_next and f_sum */
            next_state.update(dstate);
            dstate.setZero();

			/* debug watch use*/
			next_state.output();
			shadFlag = 1;
			p_viewer->refresh();
			//Control::mOutput();

            /* update K and f*/
            computeForceAndGradient(next_state, f_sum, K);

			err_f = f_sum.dot(f_sum);
        }

		printf("f_sum error %f \n", err_f);
		printf("finish solving \n");
    }

  //  void Engine::forceTest(Vvec3 &f_sum, Vvec3 &f_loc)
  //  {
  //      f_sum.assign( m_size, Vec3(0) );
		//f_loc = cur_state.world_space_pos;

		//SpVec f = SpVec::Zero(cur_state.freedomDegree());
  //      /* compute nodal force for each vertex */
  //      return computeForceAndGradient(cur_state, f_sum);
  //  }

}


#include <gtest/gtest.h>
#include <boost/optional.hpp>
#include <snark/actuators/wheels/wheel_command.h>

using namespace snark::wheels;

class wheels_test : public ::testing::Test
{
    protected:
        virtual void SetUp()
        {
            rear_left_pose
                << -1,  0,  0, -1,
                    0,  0, -1,  1,
                    0, -1,  0,  0,
                    0,  0,  0,  1;
            front_left_pose
                << -1,  0,  0,  1,
                    0,  0, -1,  1,
                    0, -1,  0,  0,
                    0,  0,  0,  1;
            front_right_pose
                <<  1,  0,  0,  1,
                    0,  0,  1, -1,
                    0, -1,  0,  0,
                    0,  0,  0,  1;
            rear_right_pose
                <<  1,  0,  0, -1,
                    0,  0,  1, -1,
                    0, -1,  0,  0,
                    0,  0,  0,  1;
            wheel_offset = 0;
            angle_limit = limit( M_PI*0.5 );
        }

        virtual void TearDown()
        {
        }

    public:
        Eigen::Matrix4d rear_left_pose;
        Eigen::Matrix4d front_left_pose;
        Eigen::Matrix4d rear_right_pose;
        Eigen::Matrix4d front_right_pose;
        double wheel_offset;
        boost::optional< limit > angle_limit;
};

class wheels_test_180 : public wheels_test
{
    protected:
        virtual void SetUp()
        {
            wheels_test::SetUp();
            angle_limit = limit( M_PI );
        }
};

class wheels_test_540 : public wheels_test
{
    protected:
        virtual void SetUp()
        {
            wheels_test::SetUp();
            angle_limit = limit( M_PI*3 );
        }
};

class wheels_test_720 : public wheels_test
{
    protected:
        virtual void SetUp()
        {
            wheels_test::SetUp();
            angle_limit = limit( M_PI*4 );
        }
};

class wheels_test_no_limit : public wheels_test
{
    protected:
        virtual void SetUp()
        {
            wheels_test::SetUp();
            angle_limit = boost::optional< limit >();
        }
};

TEST_F( wheels_test, crab_north_west )
{
    steer_command steer( 1, 1, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.25, front_left.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.25, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test, crab_north )
{
    steer_command steer( 1, 0, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_EQ(  0, rear_left.turnrate );
    EXPECT_EQ( -1, rear_left.velocity );
    EXPECT_EQ(  0, front_left.turnrate );
    EXPECT_EQ( -1, front_left.velocity );
    EXPECT_EQ(  0, front_right.turnrate );
    EXPECT_EQ(  1, front_right.velocity );
    EXPECT_EQ(  0, rear_right.turnrate );
    EXPECT_EQ(  1, rear_right.velocity );
}

TEST_F( wheels_test, crab_north_east )
{
    steer_command steer( 1, -1, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_NEAR( -M_PI*0.25, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, front_right.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test, crab_east )
{
    steer_command steer( 0, -1, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_NEAR(  M_PI*0.5, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.5, front_left.turnrate, 1e-9 );
    EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.5, front_right.turnrate, 1e-9 );
    EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.5, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test, crab_south_east )
{
    steer_command steer( -1, -1, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.25, front_left.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.25, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test, crab_south )
{
    steer_command steer( -1, 0, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_EQ(  0, rear_left.turnrate );
    EXPECT_EQ(  1, rear_left.velocity );
    EXPECT_EQ(  0, front_left.turnrate );
    EXPECT_EQ(  1, front_left.velocity );
    EXPECT_EQ(  0, front_right.turnrate );
    EXPECT_EQ( -1, front_right.velocity );
    EXPECT_EQ(  0, rear_right.turnrate );
    EXPECT_EQ( -1, rear_right.velocity );
}

TEST_F( wheels_test, crab_south_west )
{
    steer_command steer( -1,  1, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_NEAR( -M_PI*0.25, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, front_right.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test, crab_west )
{
    steer_command steer( 0, 1, 0 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_NEAR( -M_PI*0.5, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.5, front_left.turnrate, 1e-9 );
    EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.5, front_right.turnrate, 1e-9 );
    EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.5, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test, spot_turn )
{
    steer_command steer( 0, 0, 1 );
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit );
    EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
    EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
    EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test_180, crab_nearest_north )
{
    steer_command steer( 1, 0, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  0, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  0, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_180, crab_nearest_south )
{
    steer_command steer( -1, 0, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  0, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  0, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_180, crab_nearest_east )
{
    steer_command steer( 0, -1, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*0.25 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_180, crab_nearest_west )
{
    steer_command steer( 0, 1, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*0.25 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_180, crab_nearest_south_west )
{
    steer_command steer( -1, 1, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_180, spot_turn )
{
    steer_command steer( 0, 0, 1 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*0.5 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*0.5 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_720, crab_nearest_north )
{
    steer_command steer( 1, 0, 0 );
    {
        boost::optional< double > current_angle( 350/180.0*M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_540, crab_nearest_north )
{
    steer_command steer( 1, 0, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  0, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  0, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*3, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*3, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*3, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*3, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_540, crab_nearest_south )
{
    steer_command steer( -1, 0, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  0, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  0, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  0, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*3, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*3, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*3, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*3, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_540, crab_nearest_east )
{
    steer_command steer( 0, -1, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*0.25 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_540, crab_nearest_west )
{
    steer_command steer( 0, 1, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*0.25 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2.5, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.5, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_540, crab_nearest_south_west )
{
    steer_command steer( -1, 1, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*1.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*1.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*2.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*2.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_540, spot_turn )
{
    steer_command steer( 0, 0, 1 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*0.5 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*0.5 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*2 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*1.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*2.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*2.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI*3 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*2.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*2.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_no_limit, crab_north )
{
    steer_command steer( 1, 0, 0 );
    boost::optional< double > current_angle;
    wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
    wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
    wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
    wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
    EXPECT_NEAR(  0, rear_left.turnrate, 1e-9 );
    EXPECT_NEAR( -1, rear_left.velocity, 1e-9 );
    EXPECT_NEAR(  0, front_left.turnrate, 1e-9 );
    EXPECT_NEAR( -1, front_left.velocity, 1e-9 );
    EXPECT_NEAR(  0, front_right.turnrate, 1e-9 );
    EXPECT_NEAR(  1, front_right.velocity, 1e-9 );
    EXPECT_NEAR(  0, rear_right.turnrate, 1e-9 );
    EXPECT_NEAR(  1, rear_right.velocity, 1e-9 );
}

TEST_F( wheels_test_no_limit, crab_nearest_south_west )
{
    steer_command steer( -1, 1, 0 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*1.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
}

TEST_F( wheels_test_no_limit, spot_turn_nearest )
{
    steer_command steer( 0, 0, 1 );
    {
        boost::optional< double > current_angle( 0 );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*0.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR(  std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR(  M_PI*1.25, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*1.25, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR(  M_PI*0.75, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
    {
        boost::optional< double > current_angle( -M_PI );
        wheel_command rear_left = compute_wheel_command( steer, rear_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_left = compute_wheel_command( steer, front_left_pose, wheel_offset, angle_limit, current_angle );
        wheel_command front_right = compute_wheel_command( steer, front_right_pose, wheel_offset, angle_limit, current_angle );
        wheel_command rear_right = compute_wheel_command( steer, rear_right_pose, wheel_offset, angle_limit, current_angle );
        EXPECT_NEAR( -M_PI*0.75, rear_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, front_left.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_left.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*0.75, front_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), front_right.velocity, 1e-9 );
        EXPECT_NEAR( -M_PI*1.25, rear_right.turnrate, 1e-9 );
        EXPECT_NEAR( -std::sqrt(2), rear_right.velocity, 1e-9 );
    }
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE threecenter_gwbse_test

// Third party includes
#include <boost/test/unit_test.hpp>

// VOTCA inlcudes
#include <votca/tools/eigenio_matrixmarket.h>
#include <votca/tools/tokenizer.h>

// Local VOTCA includes
#include "votca/xtp/aobasis.h"
#include "votca/xtp/qmmolecule.h"
#include "votca/xtp/threecenter.h"

using namespace votca::xtp;
using namespace std;

BOOST_AUTO_TEST_SUITE(threecenter_gwbse_test)
BOOST_AUTO_TEST_CASE(threecenter_gwbse) {

  QMMolecule mol(" ", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/threecenter_gwbse/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) +
             "/threecenter_gwbse/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, mol);

  Eigen::MatrixXd MOs = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_gwbse/MOs.mm");

  Logger log;
  TCMatrix_gwbse tc{log};
  tc.Initialize(aobasis.AOBasisSize(), 0, 5, 0, 7);
  tc.Fill(aobasis, aobasis, MOs);

  Eigen::MatrixXd ref0b = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_gwbse/ref0b.mm");

  bool check0_before = ref0b.isApprox(tc[0], 1e-5);
  if (!check0_before) {
    cout << "tc0" << endl;
    cout << tc[0] << endl;
    cout << "tc0_ref" << endl;
    cout << ref0b << endl;
  }
  BOOST_CHECK_EQUAL(check0_before, true);

  Eigen::MatrixXd ref2b = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_gwbse/ref2b.mm");

  bool check2_before = ref2b.isApprox(tc[2], 1e-5);
  if (!check2_before) {
    cout << "tc2" << endl;
    cout << tc[2] << endl;
    cout << "tc2_ref" << endl;
    cout << ref2b << endl;
  }

  BOOST_CHECK_EQUAL(check2_before, true);

  Eigen::MatrixXd ref4b = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_gwbse/ref4b.mm");

  bool check4_before = ref4b.isApprox(tc[4], 1e-5);
  if (!check4_before) {
    cout << "tc4" << endl;
    cout << tc[4] << endl;
    cout << "tc4_ref" << endl;
    cout << ref4b << endl;
  }

  BOOST_CHECK_EQUAL(check4_before, true);

  Eigen::MatrixXd auxmatrix =
      Eigen::MatrixXd::Identity(aobasis.AOBasisSize(), aobasis.AOBasisSize());
  tc.MultiplyRightWithAuxMatrix(auxmatrix);

  bool check0_after = ref0b.isApprox(tc[0], 1e-5);
  if (!check0_after) {
    cout << "tc0" << endl;
    cout << tc[0] << endl;
    cout << "tc0_ref" << endl;
    cout << ref0b << endl;
  }
  BOOST_CHECK_EQUAL(check0_after, true);

  bool check2_after = ref2b.isApprox(tc[2], 1e-5);
  if (!check2_after) {
    cout << "tc2" << endl;
    cout << tc[2] << endl;
    cout << "tc2_ref" << endl;
    cout << ref2b << endl;
  }

  BOOST_CHECK_EQUAL(check2_after, true);

  bool check4_after = ref4b.isApprox(tc[4], 1e-5);
  if (!check4_after) {
    cout << "tc4" << endl;
    cout << tc[4] << endl;
    cout << "tc4_ref" << endl;
    cout << ref4b << endl;
  }

  BOOST_CHECK_EQUAL(check4_after, true);
}
BOOST_AUTO_TEST_SUITE_END()