#include <boost/property_tree/json_parser.hpp>

#include <fstream>

#include <gtest/gtest.h>

#include <rai/lib/interface.h>
#include <rai/node/common.hpp>
#include <rai/node/node.hpp>

#include <ed25519-donna/ed25519.h>

TEST (ed25519, signing)
{
	rai::uint256_union prv (0);
	rai::uint256_union pub;
	ed25519_publickey (prv.bytes.data (), pub.bytes.data ());
	rai::uint256_union message (0);
	rai::uint512_union signature;
	ed25519_sign (message.bytes.data (), sizeof (message.bytes), prv.bytes.data (), pub.bytes.data (), signature.bytes.data ());
	auto valid1 (ed25519_sign_open (message.bytes.data (), sizeof (message.bytes), pub.bytes.data (), signature.bytes.data ()));
	ASSERT_EQ (0, valid1);
	signature.bytes[32] ^= 0x1;
	auto valid2 (ed25519_sign_open (message.bytes.data (), sizeof (message.bytes), pub.bytes.data (), signature.bytes.data ()));
	ASSERT_NE (0, valid2);
}

TEST (transaction_block, empty)
{
	rai::keypair key1;
	rai::state_block block (key1.pub, 0, 0, rai::genesis_account, 13, 1, key1.prv, key1.pub, 2);
	rai::uint256_union hash (block.hash ());
	ASSERT_FALSE (rai::validate_message (key1.pub, hash, block.signature_get ()));
	auto signature (block.signature_get ());
	signature.bytes[32] ^= 0x1;
	block.signature_set (signature);
	ASSERT_TRUE (rai::validate_message (key1.pub, hash, block.signature_get ()));
}

TEST (uint512_union, parse_zero)
{
	rai::uint512_union input (rai::uint512_t (0));
	std::string text;
	input.encode_hex (text);
	rai::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_EQ (input, output);
	ASSERT_TRUE (output.number ().is_zero ());
}

TEST (uint512_union, parse_zero_short)
{
	std::string text ("0");
	rai::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_TRUE (output.number ().is_zero ());
}

TEST (uint512_union, parse_one)
{
	rai::uint512_union input (rai::uint512_t (1));
	std::string text;
	input.encode_hex (text);
	rai::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_EQ (input, output);
	ASSERT_EQ (1, output.number ());
}

TEST (uint512_union, parse_error_symbol)
{
	rai::uint512_union input (rai::uint512_t (1000));
	std::string text;
	input.encode_hex (text);
	text[5] = '!';
	rai::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_TRUE (error);
}

TEST (uint512_union, max)
{
	rai::uint512_union input (std::numeric_limits<rai::uint512_t>::max ());
	std::string text;
	input.encode_hex (text);
	rai::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_FALSE (error);
	ASSERT_EQ (input, output);
	ASSERT_EQ (rai::uint512_t ("0xffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff"), output.number ());
}

TEST (uint512_union, parse_error_overflow)
{
	rai::uint512_union input (std::numeric_limits<rai::uint512_t>::max ());
	std::string text;
	input.encode_hex (text);
	text.push_back (0);
	rai::uint512_union output;
	auto error (output.decode_hex (text));
	ASSERT_TRUE (error);
}

TEST (frontier_req, serialization)
{
	rai::frontier_req request1;
	request1.start = 1;
	request1.age = 2;
	request1.count = 3;
	std::vector<uint8_t> bytes;
	{
		rai::vectorstream stream (bytes);
		request1.serialize (stream);
	}
	auto error (false);
	rai::bufferstream stream (bytes.data (), bytes.size ());
	rai::message_header header (error, stream);
	ASSERT_FALSE (error);
	rai::frontier_req request2 (error, stream, header);
	ASSERT_FALSE (error);
	ASSERT_EQ (request1, request2);
}

TEST (block, publish_req_serialization)
{
	rai::keypair key1;
	rai::keypair key2;
	auto block (std::unique_ptr<rai::state_block> (new rai::state_block (2, 0, 0, rai::genesis_account, 200, key2.pub, rai::keypair ().prv, 2, 3)));
	rai::publish req (std::move (block));
	std::vector<uint8_t> bytes;
	{
		rai::vectorstream stream (bytes);
		req.serialize (stream);
	}
	auto error (false);
	rai::bufferstream stream2 (bytes.data (), bytes.size ());
	rai::message_header header (error, stream2);
	ASSERT_FALSE (error);
	rai::publish req2 (error, stream2, header);
	ASSERT_FALSE (error);
	ASSERT_EQ (req, req2);
	ASSERT_EQ (*req.block, *req2.block);
}

TEST (block, confirm_req_serialization)
{
	rai::keypair key1;
	rai::keypair key2;
	auto block (std::unique_ptr<rai::state_block> (new rai::state_block (2, 0, 0, rai::genesis_account, 200, key2.pub, rai::keypair ().prv, 2, 3)));
	rai::confirm_req req (std::move (block));
	std::vector<uint8_t> bytes;
	{
		rai::vectorstream stream (bytes);
		req.serialize (stream);
	}
	auto error (false);
	rai::bufferstream stream2 (bytes.data (), bytes.size ());
	rai::message_header header (error, stream2);
	rai::confirm_req req2 (error, stream2, header);
	ASSERT_FALSE (error);
	ASSERT_EQ (req, req2);
	ASSERT_EQ (*req.block, *req2.block);
}

TEST (state_block, serialization)
{
	ASSERT_EQ (212, rai::state_block::size);
	rai::keypair key1;
	rai::keypair key2;
	rai::state_block block1 (key1.pub, 1, 12345, key2.pub, 2, 4, key1.prv, key1.pub, 5);
	ASSERT_EQ (key1.pub, block1.account ());
	ASSERT_EQ (rai::block_hash (1), block1.previous ());
	ASSERT_EQ (rai::short_timestamp (12345).data.number (), block1.creation_time ().data.number ());
	ASSERT_EQ (key2.pub, block1.representative ());
	ASSERT_EQ (rai::amount (2), block1.balance ());
	ASSERT_EQ (rai::uint256_union (4), block1.link ());
	std::vector<uint8_t> bytes;
	{
		rai::vectorstream stream (bytes);
		block1.serialize (stream);
	}
	ASSERT_EQ (rai::state_block::size, bytes.size ());
	ASSERT_EQ (212, bytes.size ());
	ASSERT_EQ (32 + 4 + 32 + 32 + 8 + 32 + 64 + 8, bytes.size ());
	ASSERT_EQ (0x39, bytes[32 + 3]); // Ensure creation_time is serialized big-endian
	ASSERT_EQ (0x00, bytes[32 + 0]);
	ASSERT_EQ (0x02, bytes[32 + 4 + 32 + 32 + 7]); // Ensure amount is serialized big-endian
	ASSERT_EQ (0x00, bytes[32 + 4 + 32 + 32 + 0]);
	ASSERT_EQ (0x05, bytes[32 + 4 + 32 + 32 + 8 + 32 + 64 + 7]); // Ensure work is serialized big-endian
	ASSERT_EQ (0x00, bytes[32 + 4 + 32 + 32 + 8 + 32 + 64 + 0]);
	bool error1 (false);
	rai::bufferstream stream (bytes.data (), bytes.size ());
	rai::state_block block2 (error1, stream);
	ASSERT_FALSE (error1);
	ASSERT_EQ (key1.pub, block2.account ());
	ASSERT_EQ (rai::block_hash (1), block2.previous ());
	ASSERT_EQ (rai::short_timestamp (12345).data.number (), block2.creation_time ().data.number ());
	ASSERT_EQ (key2.pub, block2.representative ());
	ASSERT_EQ (rai::amount (2), block2.balance ());
	ASSERT_EQ (rai::uint256_union (4), block2.link ());
	ASSERT_EQ (block1, block2);
	block2.account_set (rai::account ());
	block2.previous_set (rai::block_hash ());
	block2.creation_time ().data.decode_dec ("0");
	block2.representative_set (rai::account ());
	block2.balance_set (rai::amount ());
	block2.hashables.link.clear ();
	block2.signature_set (rai::uint512_union (0));
	block2.work_set (0);
	rai::bufferstream stream2 (bytes.data (), bytes.size ());
	ASSERT_FALSE (block2.deserialize (stream2));
	ASSERT_EQ (block1, block2);
	std::string json;
	block1.serialize_json (json);
	std::stringstream body (json);
	boost::property_tree::ptree tree;
	boost::property_tree::read_json (body, tree);
	bool error2 (false);
	rai::state_block block3 (error2, tree);
	ASSERT_FALSE (error2);
	ASSERT_EQ (block1, block3);
	block3.account_set (rai::account ());
	block3.previous_set (rai::account ());
	block2.creation_time ().data.decode_dec ("0");
	block3.representative_set (rai::account ());
	block3.balance_set (rai::amount ());
	block3.hashables.link.clear ();
	block3.signature_set (rai::uint512_union (0));
	block3.work_set (0);
	ASSERT_FALSE (block3.deserialize_json (tree));
	ASSERT_EQ (block1, block3);
}

TEST (state_block, hashing)
{
	rai::keypair key;
	rai::state_block block (key.pub, 0, 12345, key.pub, 0, 0, key.prv, key.pub, 0);
	auto hash (block.hash ());
	auto account (block.account ());
	account.bytes[0] ^= 0x1;
	block.account_set (account);
	ASSERT_NE (hash, block.hash ());
	account.bytes[0] ^= 0x1;
	block.account_set (account);
	ASSERT_EQ (hash, block.hash ());
	auto previous (block.previous ());
	previous.bytes[0] ^= 0x1;
	block.previous_set (previous);
	ASSERT_NE (hash, block.hash ());
	previous.bytes[0] ^= 0x1;
	block.previous_set (previous);
	ASSERT_EQ (hash, block.hash ());
	auto representative (block.representative ());
	representative.bytes[0] ^= 0x1;
	block.representative_set (representative);
	ASSERT_NE (hash, block.hash ());
	representative.bytes[0] ^= 0x1;
	block.representative_set (representative);
	ASSERT_EQ (hash, block.hash ());
	auto balance (block.balance ());
	((uint8_t *)&balance.data)[0] ^= 0x1;
	block.balance_set (balance);
	ASSERT_NE (hash, block.hash ());
	((uint8_t *)&balance.data)[0] ^= 0x1;
	block.balance_set (balance);
	ASSERT_EQ (hash, block.hash ());
	block.hashables.link.bytes[0] ^= 0x1;
	ASSERT_NE (hash, block.hash ());
	block.hashables.link.bytes[0] ^= 0x1;
	ASSERT_EQ (hash, block.hash ());
}

TEST (state_block, subtype)
{
	bool init (false);
	rai::block_store store (init, rai::unique_path ());
	ASSERT_TRUE (!init);
	rai::stat stats;
	rai::ledger ledger (store, stats);
	rai::transaction transaction (store.environment, nullptr, true);
	rai::genesis genesis;
	genesis.initialize (transaction, store);
	rai::keypair key;
	rai::account_info info1;
	ASSERT_FALSE (store.account_get (transaction, rai::genesis_account, info1));
	rai::state_block send (rai::genesis_account, info1.head, 0, rai::genesis_account, rai::genesis_amount - 100, key.pub, rai::test_genesis_key.prv, rai::test_genesis_key.pub, 0);
	ASSERT_EQ (rai::state_block_subtype::send, send.get_subtype (rai::genesis_amount, 0));
	ASSERT_FALSE (send.is_valid_open_subtype ());
	ASSERT_TRUE (send.is_valid_send_or_receive_subtype ());
	ASSERT_FALSE (send.is_valid_change_subtype ());
	ASSERT_EQ (rai::process_result::progress, ledger.process (transaction, send).code);
	rai::state_block open (key.pub, 0, 0, key.pub, 100, send.hash (), key.prv, key.pub, 0);
	ASSERT_EQ (rai::state_block_subtype::open_receive, open.get_subtype (0, 0));
	ASSERT_TRUE (open.is_valid_open_subtype ());
	ASSERT_FALSE (open.is_valid_send_or_receive_subtype ());
	ASSERT_FALSE (open.is_valid_change_subtype ());
	ASSERT_EQ (rai::process_result::progress, ledger.process (transaction, open).code);
	rai::state_block send2 (rai::genesis_account, send.hash (), 0, rai::genesis_account, rai::genesis_amount - 210, key.pub, rai::test_genesis_key.prv, rai::test_genesis_key.pub, 0);
	ASSERT_EQ (rai::state_block_subtype::send, send2.get_subtype (rai::genesis_amount - 100, 0));
	ASSERT_FALSE (send2.is_valid_open_subtype ());
	ASSERT_TRUE (send2.is_valid_send_or_receive_subtype ());
	ASSERT_FALSE (send2.is_valid_change_subtype ());
	ASSERT_EQ (rai::process_result::progress, ledger.process (transaction, send2).code);
	rai::state_block receive (key.pub, open.hash (), 0, key.pub, 210, send2.hash(), key.prv, key.pub, 0);
	ASSERT_EQ (rai::state_block_subtype::receive, receive.get_subtype (100, 0));
	ASSERT_FALSE (receive.is_valid_open_subtype ());
	ASSERT_TRUE (receive.is_valid_send_or_receive_subtype ());
	ASSERT_FALSE (receive.is_valid_change_subtype ());
	ASSERT_EQ (rai::process_result::progress, ledger.process (transaction, receive).code);
	rai::state_block change (key.pub, receive.hash (), 0, rai::genesis_account, 210, 0, key.prv, key.pub, 0);
	ASSERT_EQ (rai::state_block_subtype::change, change.get_subtype (210, 0));
	ASSERT_FALSE (change.is_valid_open_subtype ());
	ASSERT_FALSE (change.is_valid_send_or_receive_subtype ());
	ASSERT_TRUE (change.is_valid_change_subtype ());
	ASSERT_EQ (rai::process_result::progress, ledger.process (transaction, change).code);
	// invalid block
	rai::state_block invalid (key.pub, receive.hash (), 0, rai::genesis_account, 210, send2.hash (), key.prv, key.pub, 0);
	ASSERT_EQ (rai::state_block_subtype::undefined, invalid.get_subtype (210, 0));
	ASSERT_FALSE (invalid.is_valid_open_subtype ());
	ASSERT_TRUE (invalid.is_valid_send_or_receive_subtype ());
	ASSERT_FALSE (invalid.is_valid_change_subtype ());
	ASSERT_EQ (rai::process_result::invalid_state_block, ledger.process (transaction, invalid).code);
}

TEST (comment_block, create)
{
	rai::keypair key;
	auto comment (rai::comment_block (1, 2, 0, 5, 6, rai::comment_block_subtype::account, "COMMENT1", key.prv, key.pub, 0));
	ASSERT_EQ (rai::comment_block_subtype::account, comment.subtype ());
	ASSERT_EQ ("COMMENT1", comment.comment ());
}

TEST (comment_block, raw_comment)
{
	auto comment1 ("COMMENT1");
	auto comment_raw1 (rai::comment_block::comment_string_to_raw (comment1));
	ASSERT_EQ (8, comment_raw1.length ());
	ASSERT_EQ ('C', comment_raw1.value ()[0]);
	ASSERT_EQ ('O', comment_raw1.value ()[1]);
	ASSERT_EQ ('M', comment_raw1.value ()[2]);
	ASSERT_EQ ('1', comment_raw1.value ()[7]);
	auto comment2 (rai::comment_block::comment_raw_to_string (comment_raw1));
	ASSERT_EQ (comment1, comment2);

	rai::keypair key;
	auto comment5 (rai::comment_block (1, 2, 0, 5, 6, rai::comment_block_subtype::account, comment1, key.prv, key.pub, 0));
	ASSERT_EQ (comment1, comment5.comment ());
	ASSERT_EQ ('O', comment5.comment_raw ().value ()[1]);
}

TEST (comment_block, long_comment)
{
	int maxlen = 160;
	ASSERT_EQ (maxlen, rai::comment_block::max_comment_length);
	std::string max_comment;
	for (auto i (0); i < maxlen; ++i)
	{
		max_comment.append (std::to_string (i % 10));
	}
	std::string toolong_comment (max_comment);
	toolong_comment.append ("EXTRA");

	rai::keypair key;
	// comment truncated
	auto comment5 (rai::comment_block (1, 2, 0, 5, 6, rai::comment_block_subtype::account, toolong_comment, key.prv, key.pub, 0));
	ASSERT_EQ (max_comment, comment5.comment ());
	ASSERT_EQ (maxlen, comment5.comment ().length ());
}

TEST (comment_block, utf_comment)
{
	// TODO UTF-8 conversion
	std::string comment1 ("MÍKRÓ+ÁÉÍÓŐÚŰX");
	ASSERT_EQ (23, comment1.length ()); // longer, UTF
	auto comment_raw1 (rai::comment_block::comment_string_to_raw (comment1));
	ASSERT_EQ (23, comment_raw1.length ());
	ASSERT_EQ ('M', comment_raw1.value ()[0]);
	ASSERT_EQ (195, comment_raw1.value ()[1]);
	ASSERT_EQ ('X', comment_raw1.value ()[22]);
	auto comment2 (rai::comment_block::comment_raw_to_string (comment_raw1));
	ASSERT_EQ (comment1, comment2);
	ASSERT_EQ (23, comment2.length ());
}

TEST (comment_block, serialization)
{
	ASSERT_EQ (186, rai::comment_block::size_base);
	std::string comment1 ("COMMENT_serialization_MÍKRÓ+ÁÉÍÓŐÚŰX");
	ASSERT_EQ (45, comment1.length ()); // longer, UTF
	rai::keypair key;
	auto comment_block (rai::comment_block (1, 2, 0, 5, 6, rai::comment_block_subtype::account, comment1, key.prv, key.pub, 0));
	// serialize
	std::vector<uint8_t> bytes;
	{
		rai::vectorstream stream (bytes);
		comment_block.serialize (stream);
	}
	ASSERT_EQ (186 + 45, bytes.size ());
	// deserialize
	rai::bufferstream stream (bytes.data (), bytes.size ());
	auto error (false);
	rai::comment_block comment_block2 (error, stream);
	ASSERT_FALSE (error);
	// compare
	ASSERT_EQ ("COMMENT_serialization_MÍKRÓ+ÁÉÍÓŐÚŰX", comment_block2.comment ());
	ASSERT_EQ (rai::comment_block_subtype::account, comment_block2.subtype ());
	ASSERT_EQ (comment_block.hash (), comment_block2.hash ());
}

TEST (comment_block, serialization_json)
{
	std::string comment1 ("COMMENT_serialization_json_MÍKRÓ+ÁÉÍÓŐÚŰX");
	ASSERT_EQ (50, comment1.length ()); // longer, UTF
	rai::keypair key;
	auto comment_block (rai::comment_block (1, 2, 0, 5, 6, rai::comment_block_subtype::account, comment1, key.prv, key.pub, 0));
	// serialize
	std::string json;
	comment_block.serialize_json (json);
	// deserialize
	std::stringstream body (json);
	boost::property_tree::ptree tree;
	boost::property_tree::read_json (body, tree);
	auto error (false);
	rai::comment_block comment_block2 (error, tree);
	ASSERT_FALSE (error);
	// compare
	ASSERT_EQ ("COMMENT_serialization_json_MÍKRÓ+ÁÉÍÓŐÚŰX", comment_block2.comment ());
	ASSERT_EQ (rai::comment_block_subtype::account, comment_block2.subtype ());
	ASSERT_EQ (comment_block.hash (), comment_block2.hash ());
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Core>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "rectify_map.h"

namespace snark { namespace imaging {

/// constructor from parameters
rectify_map::rectify_map ( const Eigen::Matrix3d& leftCamera, const Vector5d& leftDistortion, const Eigen::Matrix3d& rightCamera, const Vector5d& rightDistortion,
                           unsigned int imageWidth, unsigned int imageHeight, const Eigen::Matrix3d& rotation, const Eigen::Vector3d& translation, bool rectified )
{
    cv::eigen2cv( leftCamera, m_leftCamera );
    cv::eigen2cv( leftDistortion, m_leftDistortion );
    cv::eigen2cv( rightCamera, m_rightCamera );
    cv::eigen2cv( rightDistortion, m_rightDistortion );
    m_imageSize.width = imageWidth;
    m_imageSize.height = imageHeight;
    cv::eigen2cv( rotation, m_rotation );
    cv::eigen2cv( translation, m_translation );

    cv::stereoRectify( m_leftCamera, m_leftDistortion, m_rightCamera, m_rightDistortion, m_imageSize, m_rotation, m_translation,
                       m_R1, m_R2, m_P1, m_P2, m_Q );

    if ( !rectified && ( leftDistortion.norm() > 1e-5 || rightDistortion.norm() > 1e-5 || !rotation.isApprox( Eigen::Matrix3d::Identity() ) ) )
    {
        cv::initUndistortRectifyMap( m_leftCamera, m_leftDistortion, m_R1, m_P1, m_imageSize, CV_16SC2, m_map11, m_map12 );
        cv::initUndistortRectifyMap( m_rightCamera, m_rightDistortion, m_R2, m_P2, m_imageSize, CV_16SC2, m_map21, m_map22);
    }
    else
    {
//     no rectification is needed ( pre-rectified images ), only compute Q
    }
}

/// constructor from maps
rectify_map::rectify_map ( const Eigen::Matrix3d& leftCamera, const Eigen::Matrix3d& rightCamera, const Eigen::Vector3d& translation,
                           const cv::Mat& left_x, const cv::Mat& left_y, const cv::Mat& right_x, const cv::Mat& right_y, bool rectified ):
    m_map11( left_x ),
    m_map12( left_y ),
    m_map21( right_x ),
    m_map22( right_y )
{
    Vector5d distortion( Vector5d::Zero() );
    cv::eigen2cv( leftCamera, m_leftCamera );
    cv::eigen2cv( distortion, m_leftDistortion );
    cv::eigen2cv( rightCamera, m_rightCamera );
    cv::eigen2cv( distortion, m_rightDistortion );
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
    cv::eigen2cv( I, m_rotation );
    m_imageSize.width = m_map11.cols;
    m_imageSize.height = m_map11.rows;
    cv::eigen2cv( translation, m_translation );

    cv::stereoRectify( m_leftCamera, m_leftDistortion, m_rightCamera, m_rightDistortion, m_imageSize, m_rotation, m_translation,
                       m_R1, m_R2, m_P1, m_P2, m_Q );
}

/// remap left image
cv::Mat rectify_map::remap_left ( const cv::Mat& left ) const
{
    if( m_map11.cols != 0 )
    {
        cv::Mat result;
        cv::remap( left, result, m_map11, m_map12, cv::INTER_LINEAR );
        return result;
    }
    else
    {
        return left;
    }
}

/// remap right image
cv::Mat rectify_map::remap_right ( const cv::Mat& right ) const
{
    if( m_map21.cols != 0 )
    {
        cv::Mat result;
        cv::remap( right, result, m_map21, m_map22, cv::INTER_LINEAR );
        return result;
    }
    else
    {
        return right;
    }
}

} }


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_INTEROP_VTK_MATRIX4X4_HPP
#define BOOST_COMPUTE_INTEROP_VTK_MATRIX4X4_HPP

#include <vtkMatrix4x4.h>

#include <boost/compute/types/fundamental.hpp>

namespace boost {
namespace compute {

/// Converts a \c vtkMatrix4x4 to a \c float16_.
inline float16_ vtk_matrix4x4_to_float16(const vtkMatrix4x4 *matrix)
{
    float16_ result;

    for(int i = 0; i < 4; i++){
        for(int j = 0; j < 4; j++){
            result[i*4+j] = matrix->GetElement(i, j);
        }
    }

    return result;
}

/// Converts a \c vtkMatrix4x4 to a \c double16_;
inline double16_ vtk_matrix4x4_to_double16(const vtkMatrix4x4 *matrix)
{
    double16_ result;
    std::memcpy(&result, matrix->Element, 16 * sizeof(double));
    return result;
}

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

#endif // BOOST_COMPUTE_INTEROP_VTK_MATRIX4X4_HPP


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/SimulationSetup/EstimationSetup/createCartesianStatePartials.h"
#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/rotationMatrixPartial.h"
#include "Tudat/Astrodynamics/Ephemerides/fullPlanetaryRotationModel.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::ephemerides;
using namespace tudat::estimatable_parameters;
using namespace tudat::observation_partials;
using namespace tudat::simulation_setup;

BOOST_AUTO_TEST_SUITE( test_planetary_rotation_model_rotation_matrix_partials )

//! Test whether partial derivatives of rotation matrix computed by FullPlanetaryRotationMode works correctly
BOOST_AUTO_TEST_CASE( testPlanetaryRotationModelEphemerisPartials )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Create rotation model
    double initialTime = 0.0;
    double finalTime = 1.0E8;

    spice_interface::loadStandardSpiceKernels( );

    NamedBodyMap bodyMap;
    bodyMap[ "Mars" ] = std::make_shared< Body >( );

    std::shared_ptr< RotationModelSettings > defaultMarsRotationSettings =
            getHighAccuracyMarsRotationModel( initialTime, finalTime );

    std::shared_ptr< RotationalEphemeris > marsRotationModel =
            createRotationModel( defaultMarsRotationSettings, "Mars" );

    std::shared_ptr< PlanetaryRotationModelSettings > marsRotationModelSettings =
            std::dynamic_pointer_cast< PlanetaryRotationModelSettings >( defaultMarsRotationSettings );

    double coreFactor = marsRotationModelSettings->getCoreFactor( );

    double freeCoreNutationRate = marsRotationModelSettings->getFreeCoreNutationRate( );

    std::map< double, std::pair< double, double > > rotationRateCorrections =
            marsRotationModelSettings -> getRotationRateCorrections( );

    std::map< double, std::pair< double, double > > xPolarMotionCoefficients =
            marsRotationModelSettings -> getxPolarMotionCoefficients( );

    std::map< double, std::pair< double, double > > yPolarMotionCoefficients =
            marsRotationModelSettings -> getyPolarMotionCoefficients( );

    //Test for Rotation Matrix Partial wrt Periodic Spin Variations
    {

        // Create partial object.

        std::shared_ptr< RotationMatrixPartialWrtPeriodicSpinVariations > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtPeriodicSpinVariations >(
                    std::dynamic_pointer_cast< PlanetaryRotationModel >( marsRotationModel ) );

        // Compute partial analytically

        double testTime = 9.0E7;

        std::vector< Eigen::Matrix3d > rotationMatrixPartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter( testTime );

        std::vector< Eigen::Matrix3d > rotationMatrixDerivativePartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter( testTime );

        // Compute partial numerically
        {
            // Compute partial for Phi_cj numerically
            {
//                std::cout << std::endl<< std::endl << "Phi_cj:" << std::endl;

                double perturbation = 1.0E-4;

                std::map< double, std::pair< double, double > > upperturbateRotationRateCorrections;

                std::map< double, std::pair< double, double > > downperturbateRotationRateCorrections;

                for( std::map< double, std::pair< double, double > >::iterator correctionIterator = rotationRateCorrections.begin( );
                     correctionIterator != rotationRateCorrections.end( ); correctionIterator++ )
                {
                    upperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first + perturbation, correctionIterator->second.second );

                    downperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first - perturbation, correctionIterator->second.second );

                    if ( correctionIterator->first > 1.0 )
                    {
                        upperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second  );

                        downperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second);
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            upperturbateRotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d upperturbedRotationMatrix =
                        upperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d upperturbedRotationMatrixDerivative =
                        upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            downperturbateRotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d downperturbedRotationMatrix =
                        downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );

                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 0 ) - numericalRotationMatrixPartial;

//                std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at(0) << std::endl;

//                std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

//                std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

                // Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-9 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 0 ) - numericalRotationMatrixDerivativePartial;

//                std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
//                             rotationMatrixDerivativePartials.at(0) << std::endl;

//                std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
//                             numericalRotationMatrixDerivativePartial << std::endl;

//                std::cout << "rotation Matrix derivative Partial difference"<< std::endl <<
//                             matrixDifference << std::endl << std::endl<< std::endl;

                //Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-13 );
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );
            }

            //Compute partial for Phi_sj numerically
            {
//                std::cout << std::endl<< std::endl<< "Phi_sj:" << std::endl;

                double perturbation = 9.0E-5;

                std::map< double, std::pair< double, double > > upperturbateRotationRateCorrections;

                std::map< double, std::pair< double, double > > downperturbateRotationRateCorrections;

                for( std::map< double, std::pair< double, double > >::iterator correctionIterator = rotationRateCorrections.begin( );
                     correctionIterator != rotationRateCorrections.end( ); correctionIterator++ )
                {
                    upperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first, correctionIterator->second.second + perturbation );

                    downperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first, correctionIterator->second.second - perturbation );

                    if ( correctionIterator->first > 1.0 )
                    {
                        upperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second  );

                        downperturbateRotationRateCorrections[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second);
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            upperturbateRotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d upperturbedRotationMatrix =
                        upperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d upperturbedRotationMatrixDerivative =
                        upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            downperturbateRotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d downperturbedRotationMatrix =
                        downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );

                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 1 ) - numericalRotationMatrixPartial;

//                std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at( 1 ) << std::endl;

//                std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

//                std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

                // Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-9 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 1 ) - numericalRotationMatrixDerivativePartial;

//                std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
//                             rotationMatrixDerivativePartials.at( 1 ) << std::endl;

//                std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
//                             numericalRotationMatrixDerivativePartial << std::endl;

//                std::cout << "rotation Matrix derivative Partial difference"<< std::endl <<
//                             matrixDifference << std::endl << std::endl<< std::endl;

                //Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-13 );
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );
            }
        }
    }

    //Test for Rotation Matrix Partial Wrt Polar motion amplitude
    {
        // Create partial object.

        std::shared_ptr< RotationMatrixPartialWrtPolarMotionAmplitude > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtPolarMotionAmplitude >(
                    std::dynamic_pointer_cast< PlanetaryRotationModel >( marsRotationModel ) );

        // Compute partial analytically

        double testTime = 0.9E8;

        std::vector< Eigen::Matrix3d > rotationMatrixPartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter( testTime );

        std::vector< Eigen::Matrix3d > rotationMatrixDerivativePartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter( testTime );

        // Compute partial numerically
        {
            //Compute partial wrt X_cj numerically
            {
//                std::cout << std::endl<< std::endl << "X_cj:" << std::endl;

                double perturbation = 1.0E-5;

                std::map< double, std::pair< double, double > > upperturbatePolarMotionCoefficients;

                std::map< double, std::pair< double, double > > downperturbatePolarMotionCoefficients;

                for( std::map< double, std::pair< double, double > >::iterator correctionIterator = xPolarMotionCoefficients.begin( );
                     correctionIterator != xPolarMotionCoefficients.end( ); correctionIterator++ )
                {
                    upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first + perturbation , correctionIterator->second.second );

                    downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first - perturbation , correctionIterator->second.second );

                    if ( correctionIterator->first > 1.0 )
                    {
                        upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second  );

                        downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second);
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            upperturbatePolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d upperturbedRotationMatrix = upperturbatedMarsRotationModel->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );

                Eigen::Matrix3d upperturbedRotationMatrixDerivative = upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame(
                            testTime );

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            downperturbatePolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d downperturbedRotationMatrix =
                        downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );

                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 0 ) - numericalRotationMatrixPartial;

//                std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at( 0 ) << std::endl;

//                std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

//                std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

                // Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-10 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 0 ) - numericalRotationMatrixDerivativePartial;

//                std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
//                             rotationMatrixDerivativePartials.at( 0 ) << std::endl;

//                std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
//                             numericalRotationMatrixDerivativePartial << std::endl;

//                std::cout << "rotation Matrix derivative Partial difference"<< std::endl << matrixDifference << std::endl;

                //Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-15 );
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );
            }

            // Compute partial wrt X_sj numerically
            {
//                std::cout << std::endl<< std::endl << "X_sj:" << std::endl;

                double perturbation = 9.0E-5;

                std::map< double, std::pair< double, double > > upperturbatePolarMotionCoefficients;

                std::map< double, std::pair< double, double > > downperturbatePolarMotionCoefficients;

                for( std::map< double, std::pair< double, double > >::iterator correctionIterator = xPolarMotionCoefficients.begin( );
                     correctionIterator != xPolarMotionCoefficients.end( ); correctionIterator++ )
                {
                    upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first , correctionIterator->second.second + perturbation );

                    downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first , correctionIterator->second.second - perturbation );

                    if ( correctionIterator->first > 1.0 )
                    {
                        upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second  );

                        downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second);
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            upperturbatePolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d upperturbedRotationMatrix = upperturbatedMarsRotationModel->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );

                Eigen::Matrix3d upperturbedRotationMatrixDerivative = upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame(
                            testTime );

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            downperturbatePolarMotionCoefficients,
                            yPolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d downperturbedRotationMatrix =
                        downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );

                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 1 ) - numericalRotationMatrixPartial;

//                std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at( 1 ) << std::endl;

//                std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

//                std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

                // Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-10 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 1 ) - numericalRotationMatrixDerivativePartial;

//                std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
//                             rotationMatrixDerivativePartials.at( 1 ) << std::endl;

//                std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
//                             numericalRotationMatrixDerivativePartial << std::endl;

//                std::cout << "rotation Matrix derivative Partial difference"<< std::endl <<
//                             matrixDifference << std::endl;

                //Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-14 );
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );
            }

            // Compute partial wrt Y_cj numerically
            {
//                std::cout << std::endl<< std::endl << "Y_cj:" << std::endl;

                double perturbation = 9.0E-7;

                std::map< double, std::pair< double, double > > upperturbatePolarMotionCoefficients;

                std::map< double, std::pair< double, double > > downperturbatePolarMotionCoefficients;

                for( std::map< double, std::pair< double, double > >::iterator correctionIterator = yPolarMotionCoefficients.begin( );
                     correctionIterator != yPolarMotionCoefficients.end( ); correctionIterator++ )
                {
                    upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first + perturbation , correctionIterator->second.second );

                    downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first - perturbation , correctionIterator->second.second );

                    if ( correctionIterator->first > 1.0 )
                    {
                        upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second  );

                        downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second);
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            upperturbatePolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d upperturbedRotationMatrix =
                        upperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d upperturbedRotationMatrixDerivative =
                        upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            downperturbatePolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d downperturbedRotationMatrix =
                        downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );

                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 2 ) - numericalRotationMatrixPartial;

////                std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at( 2 ) << std::endl;

////                std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

////                std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

                // Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-10 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 2 ) - numericalRotationMatrixDerivativePartial;

////                std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
////                             rotationMatrixDerivativePartials.at( 2 ) << std::endl;

////                std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
////                             numericalRotationMatrixDerivativePartial << std::endl;

////                std::cout << "rotation Matrix derivative Partial difference"<< std::endl << matrixDifference << std::endl;

                //Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-14 );
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );
            }

            // Compute partial wrt Y_sj numerically
            {
//                std::cout << std::endl<< std::endl << "Y_sj:" << std::endl;

                double perturbation = 5.0E-5;

                std::map< double, std::pair< double, double > > upperturbatePolarMotionCoefficients;

                std::map< double, std::pair< double, double > > downperturbatePolarMotionCoefficients;

                for( std::map< double, std::pair< double, double > >::iterator correctionIterator = yPolarMotionCoefficients.begin( );
                     correctionIterator != yPolarMotionCoefficients.end( ); correctionIterator++ )
                {
                    upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first , correctionIterator->second.second + perturbation );

                    downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                correctionIterator->second.first , correctionIterator->second.second - perturbation );

                    if ( correctionIterator->first > 1.0 )
                    {
                        upperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second  );

                        downperturbatePolarMotionCoefficients[ correctionIterator->first ] = std::make_pair(
                                    correctionIterator->second.first, correctionIterator->second.second);
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            upperturbatePolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d upperturbedRotationMatrix =
                        upperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d upperturbedRotationMatrixDerivative =
                        upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            downperturbatePolarMotionCoefficients );

                std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                        createRotationModel( marsRotationModelSettings, "Mars" );

                Eigen::Matrix3d downperturbedRotationMatrix =
                        downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );

                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 3 ) - numericalRotationMatrixPartial;

//                std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at( 3 ) << std::endl;

//                std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

//                std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

                // Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-10 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 3 ) - numericalRotationMatrixDerivativePartial;

//                std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
//                             rotationMatrixDerivativePartials.at( 3 ) << std::endl;

//                std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
//                             numericalRotationMatrixDerivativePartial << std::endl;

//                std::cout << "rotation Matrix derivative Partial difference"<< std::endl << matrixDifference << std::endl;

                //Compare analytical and numerical result.

                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-14 );
                    }
                }

                marsRotationModelSettings->setPeriodTerms(
                            marsRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                            marsRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                            rotationRateCorrections,
                            xPolarMotionCoefficients,
                            yPolarMotionCoefficients );
            }
        }
    }

    //Test for Rotation Matrix Partial wrt Core Factor
    {
//        std::cout << std::endl << std::endl << "Core Factor" << std::endl;

        // Create partial object.

        std::shared_ptr< RotationMatrixPartialWrtCoreFactor > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtCoreFactor >(
                    std::dynamic_pointer_cast< PlanetaryRotationModel >( marsRotationModel ) );

        // Compute partial analytically

        double testTime = 9.0E7;

        std::vector< Eigen::Matrix3d > rotationMatrixPartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter( testTime );

        std::vector< Eigen::Matrix3d > rotationMatrixDerivativePartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter( testTime );

        // Compute partial numerically
        {
            double perturbation = 9.0E-2;

            double upperturbateCoreFactor = coreFactor + perturbation ;

            double downperturbateCoreFactor = coreFactor - perturbation ;

            marsRotationModelSettings->setCoreFactorAndFreeCoreNutation(
                        upperturbateCoreFactor, freeCoreNutationRate );

            std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                    createRotationModel( marsRotationModelSettings, "Mars" );

            Eigen::Matrix3d upperturbedRotationMatrix =
                    upperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

            Eigen::Matrix3d upperturbedRotationMatrixDerivative =
                    upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame(  testTime );

            marsRotationModelSettings->setCoreFactorAndFreeCoreNutation(
                        downperturbateCoreFactor, freeCoreNutationRate );

            std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                    createRotationModel( marsRotationModelSettings, "Mars" );

            Eigen::Matrix3d downperturbedRotationMatrix =
                    downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

            Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                    downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

            Eigen::Matrix3d numericalRotationMatrixPartial =
                    ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );

            Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                    ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                    ( 2.0 * perturbation );

            Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 0 ) - numericalRotationMatrixPartial;

//            std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at( 0 ) << std::endl;

//            std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

//            std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

            // Compare analytical and numerical result.

            for( unsigned int i = 0; i < 3; i++ )
            {
                for( unsigned int j = 0; j < 3; j++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-12 );
                }
            }

            matrixDifference = rotationMatrixDerivativePartials.at( 0 ) - numericalRotationMatrixDerivativePartial;

//            std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
//                         rotationMatrixDerivativePartials.at( 0 ) << std::endl;

//            std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
//                         numericalRotationMatrixDerivativePartial << std::endl;

//            std::cout << "rotation Matrix derivative Partial difference"<< std::endl << matrixDifference << std::endl;

            //Compare analytical and numerical result.

            for( unsigned int i = 0; i < 3; i++ )
            {
                for( unsigned int j = 0; j < 3; j++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-16 );
                }
            }
        }
    }

    //Test for Rotation Matrix Partial wrt Free Core Nutation
    {
//        std::cout << std::endl << std::endl << "Free Core Nutation" << std::endl;

        // Create partial object.

        std::shared_ptr< RotationMatrixPartialWrtFreeCoreNutationRate > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtFreeCoreNutationRate >(
                    std::dynamic_pointer_cast< PlanetaryRotationModel >( marsRotationModel ) );

        // Compute partial analytically

        double testTime = 9.0E7;

        std::vector< Eigen::Matrix3d > rotationMatrixPartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter( testTime );

        std::vector< Eigen::Matrix3d > rotationMatrixDerivativePartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter( testTime );

        // Compute partial numerically
        {
            double perturbation = 5.0E-11;

            double upperturbateFreeCoreNutationRate = freeCoreNutationRate + perturbation ;

            double downperturbateFreeCoreNutationRate = freeCoreNutationRate - perturbation ;

            marsRotationModelSettings->setCoreFactorAndFreeCoreNutation( coreFactor , upperturbateFreeCoreNutationRate );

            std::shared_ptr< RotationalEphemeris > upperturbatedMarsRotationModel =
                    createRotationModel( marsRotationModelSettings, "Mars" );

            Eigen::Matrix3d upperturbedRotationMatrix =
                    upperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

            Eigen::Matrix3d upperturbedRotationMatrixDerivative =
                    upperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

            marsRotationModelSettings->setCoreFactorAndFreeCoreNutation( coreFactor, downperturbateFreeCoreNutationRate );

            std::shared_ptr< RotationalEphemeris > downperturbatedMarsRotationModel =
                    createRotationModel( marsRotationModelSettings, "Mars" );

            Eigen::Matrix3d downperturbedRotationMatrix =
                    downperturbatedMarsRotationModel->getRotationToBaseFrame( testTime ).toRotationMatrix( );

            Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                    downperturbatedMarsRotationModel->getDerivativeOfRotationToBaseFrame( testTime );

            Eigen::Matrix3d numericalRotationMatrixPartial =
                    ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );
            Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                    ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                    ( 2.0 * perturbation );

            Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 0 ) - numericalRotationMatrixPartial;

//            std::cout << "analitical Rotation Matrix Partial"<< std::endl << rotationMatrixPartials.at( 0 ) << std::endl;

//            std::cout << "numerical Rotation Matrix Partial"<< std::endl << numericalRotationMatrixPartial << std::endl;

//            std::cout << "rotation Matrix Partial difference"<< std::endl << matrixDifference << std::endl;

            // Compare analytical and numerical result.

            for( unsigned int i = 0; i < 3; i++ )
            {
                for( unsigned int j = 0; j < 3; j++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-3 );
                }
            }

            matrixDifference = rotationMatrixDerivativePartials.at( 0 ) - numericalRotationMatrixDerivativePartial;

//            std::cout << "analitical Rotation Matrix derivative Partial"<< std::endl <<
//                         rotationMatrixDerivativePartials.at( 0 ) << std::endl;

//            std::cout << "numerical Rotation Matrix derivative Partial"<< std::endl <<
//                         numericalRotationMatrixDerivativePartial << std::endl;

//            std::cout << "rotation Matrix derivative Partial difference"<< std::endl << matrixDifference << std::endl;

            //Compare analytical and numerical result.

            for( unsigned int i = 0; i < 3; i++ )
            {
                for( unsigned int j = 0; j < 3; j++ )
                {
                    BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-7 );
                }
            }
        }
    }

}
BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/python.hpp>
#include <pupiltracker_pythonwrapper/conversion.h>
#include <pupiltracker/PupilTracker.h>
#include <pupiltracker/cvx.h>

namespace py = boost::python;

py::list findPupil(PyObject *mat, int radius_min = 3, int radius_max = 8, double canny_blur = 1,
		double canny_threshold = 20, double canny_threshold2 = 40, int starburst_points = 0,
		int percentage_inliers = 30, int inlier_iterations = 2, bool image_aware_support = true,
		int early_termination_percentage = 95, bool early_rejection = true, int seed = -1)
{
	//Convert ndarray image into cv::Mat
	NDArrayConverter cvt;
	cv::Mat m = cvt.toMat(mat);

	//Initialize pupiltracker
	pupiltracker::TrackerParams params;
        params.Radius_Min = radius_min; //int(param_vec[0]); //Default = 3
        params.Radius_Max = radius_max; //int(param_vec[1]); //Default = 8

        params.CannyBlur = canny_blur; //param_vec[2]; //Default = 1
        params.CannyThreshold1 = canny_threshold; //param_vec[3]; //Default = 20
        params.CannyThreshold2 = canny_threshold2; //param_vec[4]; //Default = 40
        params.StarburstPoints = starburst_points; //int(param_vec[5]); //Default = 0

        params.PercentageInliers = percentage_inliers; //int(param_vec[6]); //Default = 30
        params.InlierIterations = inlier_iterations; //int(param_vec[7]); //Default = 2
        params.ImageAwareSupport = image_aware_support;
        params.EarlyTerminationPercentage = early_termination_percentage; //int(param_vec[8]); //Default = 95
        params.EarlyRejection = early_rejection;
        params.Seed = seed; //int(param_vec[9]); //Default = -1

        pupiltracker::findPupilEllipse_out out;
        pupiltracker::tracker_log log;
        pupiltracker::findPupilEllipse(params, m, out, log);

	py::list to_ret;
	to_ret.append(out.elPupil.center.x);
	to_ret.append(out.elPupil.center.y);
	to_ret.append(out.elPupil.size.width);
	to_ret.append(out.elPupil.size.height);
	to_ret.append(out.elPupil.angle);

	return to_ret;
}

static void init()
{
	Py_Initialize();
	import_array();
}

BOOST_PYTHON_MODULE(PupilTrackerPythonWrapper)
{
	init();
	py::def("findPupil", findPupil, (
		py::arg("mat"),
		py::arg("radius_min")=3,
		py::arg("radius_max")=8,
		py::arg("canny_blur")=1,
		py::arg("canny_threshold1")=20,
		py::arg("canny_threshold2")=40,
		py::arg("starburst_points")=0,
		py::arg("percentage_inliers")=30,
		py::arg("inlier_iterations")=2,
		py::arg("image_aware_support")=true,
		py::arg("early_termination_percentage")=95,
		py::arg("early_rejection")=true,
		py::arg("seed")=-1),
		"Find the pupil in an image. Returns center xy, width, height, and angle");
}


//  minus.hpp
//
//  (C) Copyright 2011 Vicente J. Botet Escriba
//  Use, modification and distribution are subject to the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt).
//

#ifndef BOOST_RATIO_MPL_MINUS_HPP
#define BOOST_RATIO_MPL_MINUS_HPP

#include <boost/ratio/ratio.hpp>
#include <boost/ratio/mpl/numeric_cast.hpp>
#include <boost/mpl/minus.hpp>

namespace boost {
namespace mpl {

template<>
struct minus_impl< rational_c_tag,rational_c_tag >
{
    template< typename R1, typename R2 > struct apply
        : ratio_subtract<R1, R2>
    {
    };
};
}
}

#endif  // BOOST_RATIO_MPL_MINUS_HPP


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <test/minimal_product.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>
#include <laws/comparable.hpp>
#include <laws/foldable.hpp>
#include <laws/orderable.hpp>
#include <laws/product.hpp>
using namespace boost::hana;

using test::ct_eq;
using test::ct_ord;

int main() {
    // make sure `first` and `second` are "accessors"
    auto prod = test::minimal_product(test::Tracked{1}, test::Tracked{2});
    first(std::move(prod));
    second(std::move(prod));

    //////////////////////////////////////////////////////////////////////////
    // Comparable, Orderable, Foldable, Product
    //////////////////////////////////////////////////////////////////////////
    auto eq_elems = make<Tuple>(ct_eq<3>{}, ct_eq<4>{});

    auto eqs = make<Tuple>(
          test::minimal_product(ct_eq<3>{}, ct_eq<3>{})
        , test::minimal_product(ct_eq<3>{}, ct_eq<4>{})
        , test::minimal_product(ct_eq<4>{}, ct_eq<3>{})
        , test::minimal_product(ct_eq<4>{}, ct_eq<4>{})
    );

    auto ords = make<Tuple>(
          test::minimal_product(ct_ord<3>{}, ct_ord<3>{})
        , test::minimal_product(ct_ord<3>{}, ct_ord<4>{})
        , test::minimal_product(ct_ord<4>{}, ct_ord<3>{})
        , test::minimal_product(ct_ord<4>{}, ct_ord<4>{})
    );

    test::TestComparable<test::MinimalProduct>{eqs};
    test::TestOrderable<test::MinimalProduct>{ords};
    test::TestFoldable<test::MinimalProduct>{eqs};
    test::TestProduct<test::MinimalProduct>{eq_elems};
}


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE nn_osc

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cmath>
#include <algorithm>
#include "nn.hpp"

BOOST_AUTO_TEST_CASE(nn_osc)
{
  using namespace nn;
  typedef std::pair<float, float> weight_t;
  typedef PfIjspeert<params::Vectorf<3> > pf_t;
  typedef AfDirectT<weight_t> af_t;
  typedef Neuron<pf_t, af_t, weight_t> neuron_t;
  typedef Connection<weight_t, weight_t> connection_t;
  typedef NN<neuron_t, connection_t> nn_t;
  typedef nn_t::vertex_desc_t vertex_desc_t;
  nn_t nn;
  std::vector<vertex_desc_t> vs;

  float omega = 0.6;
  float x = 0;
  float r = 0.59;
  float phi_ij = 0.81;
  for (size_t i = 0; i < 5; ++i)
    {
      vertex_desc_t v = nn.add_neuron(boost::lexical_cast<std::string>(i));
      nn.get_neuron_by_vertex(v).get_pf().set_omega(omega);
      nn.get_neuron_by_vertex(v).get_pf().set_x(x);
      nn.get_neuron_by_vertex(v).get_pf().set_r(r);
      vs.push_back(v);
    }

  for (size_t i = 0; i < 4; ++i)
    {
      nn.add_connection(vs[i], vs[i + 1], std::make_pair(5, phi_ij));
      nn.add_connection(vs[i + 1], vs[i], std::make_pair(5, -phi_ij));
    }

  nn.init();
  for (size_t s = 0; s < 1000; ++s)
    {
      std::vector<weight_t> in;
      nn.step(in);
      for (size_t i = 0; i < vs.size(); ++i)
	std::cout<< i << " "
		 << nn.get_neuron_by_vertex(vs[i]).get_pf().get_theta_i()
		 << std::endl;
    }
  // you should have beautiful oscillations

}


#include <Functions/FunctionFactory.h>
#include <Functions/geometryConverters.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <common/logger_useful.h>

#include <Columns/ColumnArray.h>
#include <Columns/ColumnTuple.h>
#include <Columns/ColumnsNumber.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypeCustomGeo.h>

#include <memory>
#include <string>

namespace DB
{

namespace ErrorCodes
{
    extern const int BAD_ARGUMENTS;
}

template <typename Point>
class FunctionPolygonConvexHull : public IFunction
{
public:
    static const char * name;

    explicit FunctionPolygonConvexHull() = default;

    static FunctionPtr create(ContextConstPtr)
    {
        return std::make_shared<FunctionPolygonConvexHull>();
    }

    String getName() const override
    {
        return name;
    }

    bool isVariadic() const override
    {
        return false;
    }

    size_t getNumberOfArguments() const override
    {
        return 1;
    }

    DataTypePtr getReturnTypeImpl(const DataTypes &) const override
    {
        return DataTypeFactory::instance().get("Polygon");
    }

    ColumnPtr executeImpl(const ColumnsWithTypeAndName & arguments, const DataTypePtr & /*result_type*/, size_t input_rows_count) const override
    {
        PolygonSerializer<Point> serializer;

        callOnGeometryDataType<Point>(arguments[0].type, [&] (const auto & type)
        {
            using TypeConverter = std::decay_t<decltype(type)>;
            using Converter = typename TypeConverter::Type;

            if constexpr (std::is_same_v<Converter, ColumnToPointsConverter<Point>>)
                throw Exception(fmt::format("The argument of function {} must not be a Point", getName()), ErrorCodes::BAD_ARGUMENTS);
            else
            {
                auto geometries = Converter::convert(arguments[0].column->convertToFullColumnIfConst());

                for (size_t i = 0; i < input_rows_count; i++)
                {
                    Polygon<Point> convex_hull{};
                    boost::geometry::convex_hull(geometries[i], convex_hull);
                    serializer.add(convex_hull);
                }
            }
        }
        );

        return serializer.finalize();
    }

    bool useDefaultImplementationForConstants() const override
    {
        return true;
    }
};

template <>
const char * FunctionPolygonConvexHull<CartesianPoint>::name = "polygonConvexHullCartesian";

void registerFunctionPolygonConvexHull(FunctionFactory & factory)
{
    factory.registerFunction<FunctionPolygonConvexHull<CartesianPoint>>();
}

}


//
// \author Raphael Hoheneder
// \date 19.04.21
// \Copyright ARTI - Autonomous Robot Technology GmbH. All rights reserved.
//

#include <arti_nav_core_utils/nurbs_trajectory_converter.h>
#include <angles/angles.h>
#include <cmath>
#include <ros/console.h>
#include <tinynurbs/tinynurbs.h>
#include <boost/optional.hpp>

namespace arti_nav_core_utils
{

NurbsTrajectoryConverter::NurbsTrajectoryConverter(double step_distance, double min_vel, double
max_lateral_acceleration, double max_speed)
: step_distance_(step_distance), min_vel_(min_vel) , max_lateral_acceleration_(max_lateral_acceleration),
  max_speed_(max_speed)
{
}

NurbsTrajectoryConverter::~NurbsTrajectoryConverter() = default;

arti_nav_core_msgs::Trajectory2DWithLimits NurbsTrajectoryConverter::convert()
{
  arti_nav_core_msgs::Trajectory2DWithLimits trajectories;
  //arti_nav_core_msgs::Movement2DWithLimits
  // speed ramp

  int speedup = 0;
  int speeddown = 0;
  if (max_speed_ > 0.0)
  {
    double curr_speed = max_speed_;

    double break_dist_start = getBrakingDistance(speed_prev_, curr_speed, acceleration_);
    ROS_DEBUG_STREAM("acc distance: " << break_dist_start);
    speedup = std::ceil(break_dist_start / step_distance_);
    ROS_DEBUG_STREAM("speedstep up: " << speedup << " s_ac: " << break_dist_start << " step_dist: " <<
                                      step_distance_);
    if (speed_prev_ - curr_speed >= 0.0)
    {
      speedup = 0;
    }

    double break_dist_end = getBrakingDistance(curr_speed, speed_next_, deceleration_);
    ROS_DEBUG_STREAM("breakdistance: " << break_dist_end);
    //if(break_dist_end > distance_)
    //  break_dist_end = distance_;
    speeddown = std::ceil(break_dist_end / step_distance_);

    if (speed_next_ - curr_speed >= 0.0)
    {
      speeddown = 0;
    }
    ROS_DEBUG_STREAM("speedstep down: " << speeddown << " s_de: " << break_dist_end << " step_dist: " <<
                                        step_distance_);
  }
  double nr_interval = std::floor(distance_ / step_distance_); //(0.1m)

  boost::optional<double> old_yaw;

  // iterate over curve with nr_interval steps
  for(int i = 0; i <= nr_interval; i++)
  {
    double val = double(i)/double(nr_interval);
    if(i == nr_interval)
    {
      val = 1.0;
    }
    glm::vec<3, double_t> point = tinynurbs::curvePoint(crv_, val);
    glm::vec<3, double_t> orientation = tinynurbs::curveTangent(crv_, val);
    ROS_DEBUG_STREAM("Curve Point u: " << val << " x: " << point.x << " y: " << point.y << " z: " << point.z <<
                                       " | Orientation x: " << orientation.x << " y:" << orientation.y << " z:" <<
                                       orientation.z << " b:" << orientation.b << " length:" << orientation.length());
    std::vector<glm::vec<3, double_t>> deriv = tinynurbs::curveDerivatives(crv_, 2, val);

    double yaw = std::atan2(orientation.y, orientation.x);

    ROS_DEBUG_STREAM("yaw: " << yaw);

    arti_nav_core_msgs::Movement2DWithLimits move;
    move.pose.point.x.value = point.x;
    move.pose.point.x.has_limits = false;

    move.pose.point.y.value = point.y;
    move.pose.point.y.has_limits = false;

    // TODO Add thresholds xy, theta
    //if(threshold_xy_ )

    move.pose.theta.value = yaw;
    if (orientation_ != 0.0)
    {
      move.pose.theta.value = angles::normalize_angle(yaw - orientation_);
    }
    move.pose.theta.has_limits = false;

    if (max_speed_ > 0.0)
    {
      move.twist.x.has_limits = true;
      double speed = max_speed_;

      // Speed ramp up
      if ((speed_prev_ <= speed) && (i < speedup) && (speedup != 0)) // omit zero value at start
      {
        double new_speed = getVelFromAcceleration( i * step_distance_, speed_prev_, acceleration_);
        //if(i == 0)
        //  new_speed = getVelFromDecelaration((speedup - i+1) * step_distance_, max_speed_, acceleration_);
        ROS_DEBUG_STREAM("speedup: " << new_speed << " distance: " << i * step_distance_);
        speed = (speed < new_speed) ? speed : new_speed;
      }
      //Speed ramp down
      if ((speed_next_ <= speed) && ((i) >= (nr_interval - speeddown)) && (speeddown != 0))
      {
        int s = nr_interval - i;
        double new_speed = getVelFromAcceleration(step_distance_ * s, speed_next_, deceleration_);
        ROS_DEBUG_STREAM("nr_interval: " << nr_interval << " i: " << i << " speed_down: " << speeddown
                                         << "calc_speed: " << new_speed
                                         << " soll: " << speed << "  s: " << s);
        ROS_DEBUG_STREAM("new_speed: " << new_speed); //<< "  speed form interpolation: " << speed_new);
        speed = (speed < new_speed) ? speed : new_speed;
      }

      if (speed < 0.0)
      {
        speed = 0.0;
      }
      if (speed < min_vel_)
      {
        speed = min_vel_;
      }

      ROS_DEBUG_STREAM("speed: " << speed);

      move.twist.x.value = speed;
      move.twist.x.upper_limit = 0.0; // relative limits to target speed aka x.value
      move.twist.x.lower_limit = -speed; // relative limits to target speed aka x.value
      if (std::abs(orientation_) >= M_PI_2) // reverse drive
      {
        move.twist.x.value = -speed;
        move.twist.x.upper_limit = speed; // relative limits to target speed aka x.value
        move.twist.x.lower_limit = 0.0;
      }
      //move.twist.theta.value = delta_theta?

    }

    glm::vec<3, double_t> t = deriv[0];

    //t = deriv[1];
    //t = glm::normalize(t);
    if (deriv[2].x != 0.0 && deriv[2].y != 0.0)
    {
      ROS_DEBUG_STREAM("Orientation x: " << orientation.x << " y:" << orientation.y);
      // 1st derivative is the tangent or Orientation of a evaluation point on the curve.
      ROS_DEBUG_STREAM(
        "derivative[1] x: " << deriv[1].x << " y:" << deriv[1].y );//<< " | norm x:" << t.x << " y:" << t.y);
      // 2nd derivative is the curvature -> use to calculate the radius at evaluation point on the curve.
      t = glm::normalize(deriv[2]);
      ROS_DEBUG_STREAM(
        "derivative[2] x: " << deriv[2].x << " y:" << deriv[2].y );//<< " | norm x:" << t.x << " y:" << t.y);
      //double k2 = std::hypot(deriv[2].x, deriv[2].y);

      double k = std::abs((deriv[1].x *deriv[2].y) - (deriv[2].x * deriv[1].y)) / std::pow((std::pow(deriv[1].x,2) +
        std::pow(deriv[1].y, 2)),(3.0/2.0));
      //if(std::abs(k - k2) > 0.02)
      //{
      //  ROS_WARN_STREAM("Radius missmatch k: " << k << " k2: " << k2);
      //}
      double r = k < 0.0001 ? 100000 : 1 / k; // avoid numerical instability
      ROS_DEBUG_STREAM("K: " << k << " | r: " << r);
      double v_theta = 0.0; //yaw * r; // angular velocity
      if(old_yaw)
      {
        double delta_t = step_distance_ / std::abs(move.twist.x.value);
        v_theta = angles::normalize_angle(*old_yaw - yaw) * r/delta_t;
      }
      old_yaw = boost::make_optional<double>(yaw);
      double v_curve_max = std::sqrt(r * max_lateral_acceleration_);
      v_curve_max = v_curve_max > min_vel_ ? v_curve_max : min_vel_;
      if(std::abs(move.twist.x.value) > std::abs(v_curve_max))
      {
        ROS_DEBUG_STREAM("Max curve speed reduction, v: " <<  move.twist.x.value << " | v_curve_max: " << v_curve_max);
        move.twist.x.value = std::copysign(v_curve_max,  move.twist.x.value);

      }
      ROS_DEBUG_STREAM("yaw: " << yaw << "  r: " << r << " v_theta: " << v_theta);
      move.twist.theta.value = v_theta;

      ROS_DEBUG_STREAM("\n #############################################################################");
    }
    trajectories.movements.push_back(move);
  }
  return trajectories;
}

void NurbsTrajectoryConverter::addControlPts(double x, double y, double z)
{
  control_points_.emplace_back(x, y, z);
  crv_.control_points.emplace_back(x, y, z);
}

unsigned int NurbsTrajectoryConverter::getControlPtsSize()
{
  return control_points_.size();
}

void NurbsTrajectoryConverter::clearControlPts()
{
  control_points_.clear();
  crv_.control_points.clear();
}

void NurbsTrajectoryConverter::addKnot(double knot)
{
  knots_.push_back(knot);
  crv_.knots.push_back(knot);
}

void NurbsTrajectoryConverter::clearKnots()
{
  knots_.clear();
  crv_.knots.clear();
}

unsigned int NurbsTrajectoryConverter::getKnotSize()
{
  return crv_.knots.size();
}

void NurbsTrajectoryConverter::setDegree(int degree)
{
  crv_.degree = degree;
}

int NurbsTrajectoryConverter::getDegree()
{
  return crv_.degree;
}

void NurbsTrajectoryConverter::setMaxSpeed(double max_speed)
{
  max_speed_ = max_speed;
}

void NurbsTrajectoryConverter::setSpeedPrevious(double speed)
{
  speed_prev_ = speed;
}

void NurbsTrajectoryConverter::setSpeedNext(double speed)
{
  speed_next_ = speed;
}

void NurbsTrajectoryConverter::setStepDistance(double step_distance)
{
  step_distance_ = step_distance;
}

bool NurbsTrajectoryConverter::checkValidity()
{
  return curveIsValid(crv_);
}

void NurbsTrajectoryConverter::setAcceleration(double acceleration)
{
  acceleration_ = acceleration;
}

void NurbsTrajectoryConverter::setDeceleration(double deceleration)
{
  deceleration_ = deceleration;
}

double NurbsTrajectoryConverter::getBrakingDistance(double v_prev, double v_next, double acceleration)
{
  if (acceleration < 0.001)
  {
    ROS_WARN_STREAM("acceleration limit 0.001, given value: " << acceleration << " overridden");
    acceleration = 0.001;
  }
  double breaking_distance = std::abs(std::pow(v_prev, 2) - std::pow(v_next, 2)) / (2 * acceleration);
  return breaking_distance;
}

double NurbsTrajectoryConverter::getVelFromAcceleration(double dist_traveled, double vel_curr, double acceleration)
{
  if (std::abs(acceleration) < 0.001)
  {
    ROS_WARN_STREAM("acceleration limit |0.001|, given value: " << acceleration << " overridden");
    acceleration = 0.001;
  }

  double resulting_vel = std::sqrt(std::pow(vel_curr, 2) + (2 * std::abs(acceleration) * dist_traveled));

  if ((resulting_vel < 0.0) || !std::isfinite(resulting_vel))
  {
    resulting_vel = 0.0;
    ROS_WARN_STREAM("getVelFromAcceleration: " << resulting_vel);
  }
  return resulting_vel;
}

double NurbsTrajectoryConverter::getVelFromDecelaration(double dist_traveled, double vel_curr, double decelaration)
{
  if (std::abs(decelaration) < 0.001)
  {
    ROS_WARN_STREAM("decelaration limit |0.001|, given value: " << decelaration << " overridden");
    decelaration = 0.001;
  }

  double resulting_vel = std::sqrt(std::pow(vel_curr, 2) - (2 * std::abs(decelaration) * dist_traveled));

  if ((resulting_vel < 0.0) || !std::isfinite(resulting_vel))
  {
    resulting_vel = 0.0;
    ROS_WARN_STREAM("getVelFromDecelaration: " << resulting_vel);
  }
  //ROS_DEBUG_STREAM("NURBS_TRAJECTORY: vel_curr: " << vel_curr << " diff: " << delta_vel << " s: " << dist_traveled);*/
  return resulting_vel;
}

void NurbsTrajectoryConverter::setDistance(double distance)
{
  distance_ = distance;
}

void NurbsTrajectoryConverter::setUseEuclideanDistance()
{
  ROS_WARN_STREAM("use only the euclidean distance between points");
  int s = control_points_.size() - 1;
  for (size_t i = 1; i < control_points_.size(); i++)
  {
    distance_ += std::sqrt(
      std::pow((control_points_[0].x - control_points_[s].x), 2) +
      std::pow((control_points_[0].y - control_points_[s].y), 2));
  }
}

void NurbsTrajectoryConverter::setOrientation(double orientation)
{
  orientation_ = orientation;
}

}