// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Base implementations for math library.
 * @author Jakob Bornecrantz <jakob@collabora.com>
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @author Moses Turner <mosesturner@protonmail.com>
 * @ingroup aux_math
 */

#include "math/m_api.h"
#include "math/m_eigen_interop.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <assert.h>

using namespace xrt::auxiliary::math;

/*
 *
 * Copy helpers.
 *
 */

static inline Eigen::Quaternionf
copy(const struct xrt_quat &q)
{
	// Eigen constructor order is different from XRT, OpenHMD and OpenXR!
	//  Eigen: `float w, x, y, z`.
	// OpenXR: `float x, y, z, w`.
	return Eigen::Quaternionf(q.w, q.x, q.y, q.z);
}

static inline Eigen::Quaternionf
copy(const struct xrt_quat *q)
{
	return copy(*q);
}

XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
copyd(const struct xrt_quat &q)
{
	// Eigen constructor order is different from XRT, OpenHMD and OpenXR!
	//  Eigen: `float w, x, y, z`.
	// OpenXR: `float x, y, z, w`.
	return Eigen::Quaterniond(q.w, q.x, q.y, q.z);
}

XRT_MAYBE_UNUSED static inline Eigen::Quaterniond
copyd(const struct xrt_quat *q)
{
	return copyd(*q);
}

static inline Eigen::Vector3f
copy(const struct xrt_vec3 &v)
{
	return Eigen::Vector3f(v.x, v.y, v.z);
}

static inline Eigen::Vector3f
copy(const struct xrt_vec3 *v)
{
	return copy(*v);
}

XRT_MAYBE_UNUSED static inline Eigen::Vector3d
copyd(const struct xrt_vec3 &v)
{
	return Eigen::Vector3d(v.x, v.y, v.z);
}

XRT_MAYBE_UNUSED static inline Eigen::Vector3d
copyd(const struct xrt_vec3 *v)
{
	return copyd(*v);
}

static inline Eigen::Matrix3f
copy(const struct xrt_matrix_3x3 *m)
{
	Eigen::Matrix3f res;
	// clang-format off
	res << m->v[0], m->v[3], m->v[6],
	       m->v[1], m->v[4], m->v[7],
	       m->v[2], m->v[5], m->v[8];
	// clang-format on
	return res;
}

static inline Eigen::Matrix4f
copy(const struct xrt_matrix_4x4 *m)
{
	Eigen::Matrix4f res;
	// clang-format off
	res << m->v[0], m->v[4], m->v[8],  m->v[12],
	       m->v[1], m->v[5], m->v[9],  m->v[13],
	       m->v[2], m->v[6], m->v[10], m->v[14],
	       m->v[3], m->v[7], m->v[11], m->v[15];
	// clang-format on
	return res;
}

/*
 *
 * Exported vector functions.
 *
 */

extern "C" bool
math_vec3_validate(const struct xrt_vec3 *vec3)
{
	assert(vec3 != NULL);

	return map_vec3(*vec3).allFinite();
}

extern "C" void
math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut)
{
	assert(additional != NULL);
	assert(inAndOut != NULL);

	map_vec3(*inAndOut) += map_vec3(*additional);
}

extern "C" void
math_vec3_subtract(const struct xrt_vec3 *subtrahend, struct xrt_vec3 *inAndOut)
{
	assert(subtrahend != NULL);
	assert(inAndOut != NULL);

	map_vec3(*inAndOut) -= map_vec3(*subtrahend);
}

extern "C" void
math_vec3_scalar_mul(float scalar, struct xrt_vec3 *inAndOut)
{
	assert(inAndOut != NULL);

	map_vec3(*inAndOut) *= scalar;
}

extern "C" void
math_vec3_cross(const struct xrt_vec3 *l, const struct xrt_vec3 *r, struct xrt_vec3 *result)
{
	map_vec3(*result) = map_vec3(*l).cross(map_vec3(*r));
}

extern "C" void
math_vec3_normalize(struct xrt_vec3 *in)
{
	map_vec3(*in) = map_vec3(*in).normalized();
}

/*
 *
 * Exported quaternion functions.
 *
 */

extern "C" void
math_quat_from_angle_vector(float angle_rads, const struct xrt_vec3 *vector, struct xrt_quat *result)
{
	map_quat(*result) = Eigen::AngleAxisf(angle_rads, copy(vector));
}

extern "C" void
math_quat_from_matrix_3x3(const struct xrt_matrix_3x3 *mat, struct xrt_quat *result)
{
	Eigen::Matrix3f m;
	m << mat->v[0], mat->v[1], mat->v[2], mat->v[3], mat->v[4], mat->v[5], mat->v[6], mat->v[7], mat->v[8];

	Eigen::Quaternionf q(m);
	map_quat(*result) = q;
}

extern "C" void
math_quat_from_plus_x_z(const struct xrt_vec3 *plus_x, const struct xrt_vec3 *plus_z, struct xrt_quat *result)
{
	xrt_vec3 plus_y;
	math_vec3_cross(plus_z, plus_x, &plus_y);

	xrt_matrix_3x3 m = {{
	    plus_x->x,
	    plus_y.x,
	    plus_z->x,
	    plus_x->y,
	    plus_y.y,
	    plus_z->y,
	    plus_x->z,
	    plus_y.z,
	    plus_z->z,
	}};

	math_quat_from_matrix_3x3(&m, result);
}

static bool
quat_validate(const float precision, const struct xrt_quat *quat)
{
	assert(quat != NULL);
	auto rot = copy(*quat);

	/*
	 * This was originally squaredNorm, but that could result in a norm
	 * value that was further from 1.0f then FLOAT_EPSILON (two).
	 *
	 * Our tracking system would produce such orientations and looping those
	 * back into say a quad layer would cause this to fail. And even
	 * normalizing the quat would not fix this as normalizations uses
	 * non-squared "length" which does fall into the range and doesn't
	 * change the elements of the quat.
	 */
	auto norm = rot.norm();
	if (norm > 1.0f + precision || norm < 1.0f - precision) {
		return false;
	}

	// Technically not yet a required check, but easier to stop problems
	// now than once denormalized numbers pollute the rest of our state.
	// see https://gitlab.khronos.org/openxr/openxr/issues/922
	if (!rot.coeffs().allFinite()) {
		return false;
	}

	return true;
}

extern "C" bool
math_quat_validate(const struct xrt_quat *quat)
{
	const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
	return quat_validate(FLOAT_EPSILON, quat);
}

extern "C" bool
math_quat_validate_within_1_percent(const struct xrt_quat *quat)
{
	return quat_validate(0.01, quat);
}

extern "C" void
math_quat_invert(const struct xrt_quat *quat, struct xrt_quat *out_quat)
{
	map_quat(*out_quat) = map_quat(*quat).conjugate();
}

extern "C" void
math_quat_normalize(struct xrt_quat *inout)
{
	assert(inout != NULL);
	map_quat(*inout).normalize();
}

extern "C" bool
math_quat_ensure_normalized(struct xrt_quat *inout)
{
	assert(inout != NULL);

	if (math_quat_validate(inout))
		return true;

	const float FLOAT_EPSILON = Eigen::NumTraits<float>::epsilon();
	const float TOLERANCE = FLOAT_EPSILON * 5;

	auto rot = copy(*inout);
	auto norm = rot.norm();
	if (norm > 1.0f + TOLERANCE || norm < 1.0f - TOLERANCE) {
		return false;
	}

	map_quat(*inout).normalize();
	return true;
}

extern "C" void
math_quat_rotate(const struct xrt_quat *left, const struct xrt_quat *right, struct xrt_quat *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	auto q = l * r;

	map_quat(*result) = q;
}

extern "C" void
math_quat_rotate_vec3(const struct xrt_quat *left, const struct xrt_vec3 *right, struct xrt_vec3 *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	auto v = l * r;

	map_vec3(*result) = v;
}

extern "C" void
math_quat_rotate_derivative(const struct xrt_quat *quat, const struct xrt_vec3 *deriv, struct xrt_vec3 *result)
{
	assert(quat != NULL);
	assert(deriv != NULL);
	assert(result != NULL);

	auto l = copy(quat);
	auto m = Eigen::Quaternionf(0.0f, deriv->x, deriv->y, deriv->z);
	auto r = l.conjugate();

	auto v = l * m * r;

	struct xrt_vec3 ret = {v.x(), v.y(), v.z()};
	*result = ret;
}

extern "C" void
math_quat_slerp(const struct xrt_quat *left, const struct xrt_quat *right, float t, struct xrt_quat *result)
{
	assert(left != NULL);
	assert(right != NULL);
	assert(result != NULL);

	auto l = copy(left);
	auto r = copy(right);

	map_quat(*result) = l.slerp(t, r);
}

/*
 *
 * Exported matrix functions.
 *
 */

void
math_matrix_2x2_multiply(const struct xrt_matrix_2x2 *left,
                         const struct xrt_matrix_2x2 *right,
                         struct xrt_matrix_2x2 *result)
{
	result->v[0] = left->v[0] * right->v[0] + left->v[1] * right->v[2];
	result->v[1] = left->v[0] * right->v[1] + left->v[1] * right->v[3];
	result->v[2] = left->v[2] * right->v[0] + left->v[3] * right->v[2];
	result->v[3] = left->v[2] * right->v[1] + left->v[3] * right->v[3];
}

extern "C" void
math_matrix_3x3_transform_vec3(const struct xrt_matrix_3x3 *left, const struct xrt_vec3 *right, struct xrt_vec3 *result)
{
	Eigen::Matrix3f m;
	m << left->v[0], left->v[1], left->v[2], // 1
	    left->v[3], left->v[4], left->v[5],  // 2
	    left->v[6], left->v[7], left->v[8];  // 3

	map_vec3(*result) = m * copy(right);
}

extern "C" void
math_matrix_3x3_multiply(const struct xrt_matrix_3x3 *left,
                         const struct xrt_matrix_3x3 *right,
                         struct xrt_matrix_3x3 *result)
{
	result->v[0] = left->v[0] * right->v[0] + left->v[1] * right->v[3] + left->v[2] * right->v[6];
	result->v[1] = left->v[0] * right->v[1] + left->v[1] * right->v[4] + left->v[2] * right->v[7];
	result->v[2] = left->v[0] * right->v[2] + left->v[1] * right->v[5] + left->v[2] * right->v[8];

	result->v[3] = left->v[3] * right->v[0] + left->v[4] * right->v[3] + left->v[5] * right->v[6];
	result->v[4] = left->v[3] * right->v[1] + left->v[4] * right->v[4] + left->v[5] * right->v[7];
	result->v[5] = left->v[3] * right->v[2] + left->v[4] * right->v[5] + left->v[5] * right->v[8];

	result->v[6] = left->v[6] * right->v[0] + left->v[7] * right->v[3] + left->v[8] * right->v[6];
	result->v[7] = left->v[6] * right->v[1] + left->v[7] * right->v[4] + left->v[8] * right->v[7];
	result->v[8] = left->v[6] * right->v[2] + left->v[7] * right->v[5] + left->v[8] * right->v[8];
}

extern "C" void
math_matrix_3x3_inverse(const struct xrt_matrix_3x3 *in, struct xrt_matrix_3x3 *result)
{
	Eigen::Matrix3f m = copy(in);
	map_matrix_3x3(*result) = m.inverse();
}

void
math_matrix_4x4_identity(struct xrt_matrix_4x4 *result)
{
	map_matrix_4x4(*result) = Eigen::Matrix4f::Identity();
}

void
math_matrix_4x4_multiply(const struct xrt_matrix_4x4 *left,
                         const struct xrt_matrix_4x4 *right,
                         struct xrt_matrix_4x4 *result)
{
	map_matrix_4x4(*result) = copy(left) * copy(right);
}

void
math_matrix_4x4_view_from_pose(const struct xrt_pose *pose, struct xrt_matrix_4x4 *result)
{
	Eigen::Vector3f position = copy(&pose->position);
	Eigen::Quaternionf orientation = copy(&pose->orientation);

	Eigen::Translation3f translation(position);
	Eigen::Affine3f transformation = translation * orientation;

	map_matrix_4x4(*result) = transformation.matrix().inverse();
}

void
math_matrix_4x4_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4 *result)
{
	Eigen::Vector3f position = copy(&pose->position);
	Eigen::Quaternionf orientation = copy(&pose->orientation);

	auto scale = Eigen::Scaling(size->x, size->y, size->z);

	Eigen::Translation3f translation(position);
	Eigen::Affine3f transformation = translation * orientation * scale;

	map_matrix_4x4(*result) = transformation.matrix();
}

void
math_matrix_4x4_inverse_view_projection(const struct xrt_matrix_4x4 *view,
                                        const struct xrt_matrix_4x4 *projection,
                                        struct xrt_matrix_4x4 *result)
{
	Eigen::Matrix4f v = copy(view);
	Eigen::Matrix4f v3 = Eigen::Matrix4f::Identity();
	v3.block<3, 3>(0, 0) = v.block<3, 3>(0, 0);
	Eigen::Matrix4f vp = copy(projection) * v3;
	map_matrix_4x4(*result) = vp.inverse();
}

/*
 *
 * Exported Matrix 4x4 functions.
 *
 */

extern "C" void
m_mat4_f64_identity(struct xrt_matrix_4x4_f64 *result)
{
	map_matrix_4x4_f64(*result) = Eigen::Matrix4d::Identity();
}

extern "C" void
m_mat4_f64_invert(const struct xrt_matrix_4x4_f64 *matrix, struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Matrix4d m = map_matrix_4x4_f64(*matrix);
	map_matrix_4x4_f64(*result) = m.inverse();
}

extern "C" void
m_mat4_f64_multiply(const struct xrt_matrix_4x4_f64 *left,
                    const struct xrt_matrix_4x4_f64 *right,
                    struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Matrix4d l = map_matrix_4x4_f64(*left);
	Eigen::Matrix4d r = map_matrix_4x4_f64(*right);

	map_matrix_4x4_f64(*result) = l * r;
}

extern "C" void
m_mat4_f64_orientation(const struct xrt_quat *quat, struct xrt_matrix_4x4_f64 *result)
{
	map_matrix_4x4_f64(*result) = Eigen::Affine3d(copyd(*quat)).matrix();
}

extern "C" void
m_mat4_f64_model(const struct xrt_pose *pose, const struct xrt_vec3 *size, struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Vector3d position = copyd(pose->position);
	Eigen::Quaterniond orientation = copyd(pose->orientation);

	auto scale = Eigen::Scaling(copyd(size));

	Eigen::Translation3d translation(position);
	Eigen::Affine3d transformation = translation * orientation * scale;

	map_matrix_4x4_f64(*result) = transformation.matrix();
}

extern "C" void
m_mat4_f64_view(const struct xrt_pose *pose, struct xrt_matrix_4x4_f64 *result)
{
	Eigen::Vector3d position = copyd(pose->position);
	Eigen::Quaterniond orientation = copyd(pose->orientation);

	Eigen::Translation3d translation(position);
	Eigen::Affine3d transformation = translation * orientation;

	map_matrix_4x4_f64(*result) = transformation.matrix().inverse();
}

/*
 *
 * Exported pose functions.
 *
 */

extern "C" bool
math_pose_validate(const struct xrt_pose *pose)
{
	assert(pose != NULL);

	return math_vec3_validate(&pose->position) && math_quat_validate(&pose->orientation);
}

extern "C" void
math_pose_invert(const struct xrt_pose *pose, struct xrt_pose *outPose)
{
	assert(pose != NULL);
	assert(outPose != NULL);

	// Store results to temporary locals so we can do this "in-place"
	// (pose == outPose) if desired. Pure copies here.
	Eigen::Vector3f newPosition = position(*pose);
	Eigen::Quaternionf newOrientation = orientation(*pose);

	// Conjugate legal here since pose must be normalized/unit length.
	newOrientation = newOrientation.conjugate();
	// Use the newly inverted rotation, to rotate position.
	newPosition = -(newOrientation * newPosition);

	position(*outPose) = newPosition;
	orientation(*outPose) = newOrientation;
}

extern "C" void
math_pose_identity(struct xrt_pose *pose)
{
	pose->position.x = 0.0;
	pose->position.y = 0.0;
	pose->position.z = 0.0;
	pose->orientation.x = 0.0;
	pose->orientation.y = 0.0;
	pose->orientation.z = 0.0;
	pose->orientation.w = 1.0;
}

/*!
 * Return the result of transforming a point by a pose/transform.
 */
static inline Eigen::Vector3f
transform_point(const xrt_pose &transform, const xrt_vec3 &point)
{
	return orientation(transform) * map_vec3(point) + position(transform);
}

/*!
 * Return the result of transforming a pose by a pose/transform.
 */
static inline xrt_pose
transform_pose(const xrt_pose &transform, const xrt_pose &pose)
{
	xrt_pose ret;
	position(ret) = transform_point(transform, pose.position);
	orientation(ret) = orientation(transform) * orientation(pose);
	return ret;
}

extern "C" void
math_pose_transform(const struct xrt_pose *transform, const struct xrt_pose *pose, struct xrt_pose *outPose)
{
	assert(pose != NULL);
	assert(transform != NULL);
	assert(outPose != NULL);

	xrt_pose newPose = transform_pose(*transform, *pose);
	memcpy(outPose, &newPose, sizeof(xrt_pose));
}

extern "C" void
math_pose_transform_point(const struct xrt_pose *transform, const struct xrt_vec3 *point, struct xrt_vec3 *out_point)
{
	assert(transform != NULL);
	assert(point != NULL);
	assert(out_point != NULL);

	map_vec3(*out_point) = transform_point(*transform, *point);
}


/**
 *****************************************************************************
 * @author     This file is part of libsnark, developed by SCIPR Lab
 *             and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/
#include <boost/program_options.hpp>
#include <libff/common/profiling.hpp>

#include <libsnark/common/default_types/ram_zksnark_pp.hpp>
#include <libsnark/relations/ram_computations/memory/examples/memory_contents_examples.hpp>
#include <libsnark/relations/ram_computations/rams/examples/ram_examples.hpp>
#include <libsnark/relations/ram_computations/rams/tinyram/tinyram_params.hpp>
#include <libsnark/zk_proof_systems/zksnark/ram_zksnark/examples/run_ram_zksnark.hpp>
#include <libsnark/zk_proof_systems/zksnark/ram_zksnark/ram_zksnark.hpp>

using namespace libsnark;

template<typename FieldT>
void simulate_random_memory_contents(const tinyram_architecture_params &ap, const size_t input_size, const size_t program_size)
{
    const size_t num_addresses = 1ul<<ap.dwaddr_len();
    const size_t value_size = 2 * ap.w;
    memory_contents init_random = random_memory_contents(num_addresses, value_size, program_size + (input_size + 1)/2);

    libff::enter_block("Initialize random delegated memory");
    delegated_ra_memory<FieldT> dm_random(num_addresses, value_size, init_random);
    libff::leave_block("Initialize random delegated memory");
}

template<typename ppT>
void profile_ram_zksnark_verifier(const tinyram_architecture_params &ap, const size_t input_size, const size_t program_size)
{
    typedef ram_zksnark_machine_pp<ppT> ramT;
    const size_t time_bound  = 10;

    const size_t boot_trace_size_bound = program_size + input_size;
    const ram_example<ramT> example = gen_ram_example_complex<ramT>(ap, boot_trace_size_bound, time_bound, true);

    ram_zksnark_proof<ppT> pi;
    ram_zksnark_verification_key<ppT> vk = ram_zksnark_verification_key<ppT>::dummy_verification_key(ap);

    libff::enter_block("Verify fake proof");
    ram_zksnark_verifier<ppT>(vk, example.boot_trace, time_bound, pi);
    libff::leave_block("Verify fake proof");
}

template<typename ppT>
void print_ram_zksnark_verifier_profiling()
{
    libff::inhibit_profiling_info = true;
    for (size_t w : { 16, 32 })
    {
        const size_t k = 16;

        for (size_t input_size : { 0, 10, 100 })
        {
            for (size_t program_size = 10; program_size <= 10000; program_size *= 10)
            {
                const tinyram_architecture_params ap(w, k);

                profile_ram_zksnark_verifier<ppT>(ap, input_size, program_size);

                const double input_map = libff::last_times["Call to ram_zksnark_verifier_input_map"];
                const double preprocessing = libff::last_times["Call to r1cs_ppzksnark_verifier_process_vk"];
                const double accumulate = libff::last_times["Call to r1cs_ppzksnark_IC_query::accumulate"];
                const double pairings = libff::last_times["Online pairing computations"];
                const double total = libff::last_times["Call to ram_zksnark_verifier"];
                const double rest = total - (input_map + preprocessing + accumulate + pairings);

                const double delegated_ra_memory_init = libff::last_times["Construct delegated_ra_memory from memory map"];
                simulate_random_memory_contents<libff::Fr<typename ppT::curve_A_pp> >(ap, input_size, program_size);
                const double delegated_ra_memory_init_random = libff::last_times["Initialize random delegated memory"];
                const double input_map_random = input_map - delegated_ra_memory_init + delegated_ra_memory_init_random;
                const double total_random = total - delegated_ra_memory_init + delegated_ra_memory_init_random;

                printf("w = %zu, k = %zu, program_size = %zu, input_size = %zu, input_map = %0.2fms, preprocessing = %0.2fms, accumulate = %0.2fms, pairings = %0.2fms, rest = %0.2fms, total = %0.2fms (input_map_random = %0.2fms, total_random = %0.2fms)\n",
                       w, k, program_size, input_size, input_map * 1e-6, preprocessing * 1e-6, accumulate * 1e-6, pairings * 1e-6, rest * 1e-6, total * 1e-6, input_map_random * 1e-6, total_random * 1e-6);
            }
        }
    }
}

template<typename ppT>
void profile_ram_zksnark(const tinyram_architecture_params &ap, const size_t program_size, const size_t input_size, const size_t time_bound)
{
    typedef ram_zksnark_machine_pp<ppT> ramT;

    const size_t boot_trace_size_bound = program_size + input_size;
    const ram_example<ramT> example = gen_ram_example_complex<ramT>(ap, boot_trace_size_bound, time_bound, true);
    const bool test_serialization = true;
    const bool bit = run_ram_zksnark<ppT>(example, test_serialization);
    assert(bit);
}

namespace po = boost::program_options;

bool process_command_line(const int argc, const char** argv,
                          bool &profile_gp,
                          size_t &w,
                          size_t &k,
                          bool &profile_v,
                          size_t &l)
{
    try
    {
        po::options_description desc("Usage");
        desc.add_options()
            ("help", "print this help message")
            ("profile_gp", "profile generator and prover")
            ("w", po::value<size_t>(&w)->default_value(16), "word size")
            ("k", po::value<size_t>(&k)->default_value(16), "register count")
            ("profile_v", "profile verifier")
            ("v", "print version info")
            ("l", po::value<size_t>(&l)->default_value(10), "program length");

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("v"))
        {
            libff::print_compilation_info();
            exit(0);
        }

        if (vm.count("help"))
        {
            std::cout << desc << "\n";
            return false;
        }

        profile_gp = vm.count("profile_gp");
        profile_v = vm.count("profile_v");

        if (!(vm.count("profile_gp") ^ vm.count("profile_v")))
        {
            std::cout << "Must choose between profiling generator/prover and profiling verifier (see --help)\n";
            return false;
        }

        po::notify(vm);
    }
    catch(std::exception& e)
    {
        std::cerr << "Error: " << e.what() << "\n";
        return false;
    }

    return true;
}

int main(int argc, const char* argv[])
{
    libff::start_profiling();
    ram_zksnark_PCD_pp<default_ram_zksnark_pp>::init_public_params();

    bool profile_gp;
    size_t w;
    size_t k;
    bool profile_v;
    size_t l;

    if (!process_command_line(argc, argv, profile_gp, w, k, profile_v, l))
    {
        return 1;
    }

    tinyram_architecture_params ap(w, k);

    if (profile_gp)
    {
        profile_ram_zksnark<default_ram_zksnark_pp>(ap, 100, 100, 10); // w, k, l, n, T
    }

    if (profile_v)
    {
        profile_ram_zksnark_verifier<default_ram_zksnark_pp>(ap, l/2, l/2);
    }
}


/*******************************************************************************
 * Copyright (c) 2014, 2016  IBM Corporation and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *******************************************************************************/
#include "LatLngUtil.hpp"

#include <boost/geometry/algorithms/detail/vincenty_direct.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/core/srs.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/geometries/point.hpp>

namespace bg = boost::geometry;

namespace loc {
    const double d2r = M_PI / 180.0;
    const double r2d = 180.0 / M_PI;

    const double gda_a = 6378.1370;
    const double gda_f = 1.0 / 298.25722210;
    const double gda_b = gda_a * ( 1.0 - gda_f );

    template <typename P>
    bool LatLngUtil::non_precise_ct()
    {
        typedef typename bg::coordinate_type<P>::type ct;
        return boost::is_integral<ct>::value || boost::is_float<ct>::value;
    }

    template <typename P, typename Spheroid>
     Point2D LatLngUtil::transform(LatLng latlng, Anchor anchor, Spheroid spheroid) {
        typedef typename bg::promote_floating_point
        <typename bg::select_calculation_type<P, P, void>::type>::type calc_t;

        typedef bg::detail::vincenty_inverse<calc_t, true, true> inverse_formula;
        typename inverse_formula::result_type
        result_i = inverse_formula::apply(anchor.latlng.lng * d2r,
                                          anchor.latlng.lat * d2r,
                                          latlng.lng * d2r,
                                          latlng.lat * d2r,
                                          spheroid);
        calc_t dist = result_i.distance;
        calc_t azimuth = ((result_i.azimuth * r2d) - anchor.rotate) * d2r;

        Point2D p;
        p.x = sin(azimuth) * dist;
        p.y = cos(azimuth) * dist;
        return p;
    }

    template <typename P, typename Spheroid>
     LatLng LatLngUtil::transform(LatLng latlng, double dist, double angle, Spheroid spheroid){
        typedef typename bg::promote_floating_point
        <typename bg::select_calculation_type<P, P, void>::type>::type calc_t;

        typedef bg::detail::vincenty_direct<calc_t> direct_formula;
        typename direct_formula::result_type
        result = direct_formula::apply(latlng.lng * d2r,
                                       latlng.lat * d2r,
                                       dist,
                                       angle * d2r,
                                       spheroid);

        double lat = result.lat2 * r2d;
        double lng = result.lon2 * r2d;
        LatLng ll;
        ll.lat = lat;
        ll.lng = lng;

        return ll;
    }

     LatLng LatLngUtil::localToGlobal(const Point2D p, const Anchor anchor) {
        bg::srs::spheroid<double> const gda_spheroid(gda_a, gda_b);
        // usually atan2 takes (y, x) order but rotation is opposite in spherical coordinate
        double r = (atan2(p.x, p.y) * r2d)+anchor.rotate;
        double d = sqrt(pow(p.x,2)+pow(p.y,2)) / 1000; // in kilometer

        typedef typename bg::model::point<double, 2, bg::cs::geographic<bg::degree>> point_t;
        return transform<point_t>(anchor.latlng, d, r, gda_spheroid);
    }

    Point2D LatLngUtil::globalToLocal(const LatLng latlng, const Anchor anchor) {
        bg::srs::spheroid<double> const gda_spheroid(gda_a, gda_b);
        typedef typename bg::model::point<double, 2, bg::cs::geographic<bg::degree>> point_t;
        Point2D p = transform<point_t>(latlng, anchor, gda_spheroid);
        p.x *= 1000;
        p.y *= 1000;
        return p;
    }

};


/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

============================================================================*/

#include <mitkGIFVolumetricDensityStatistics.h>

// MITK
#include <mitkITKImageImport.h>
#include <mitkImageCast.h>
#include <mitkImageAccessByItk.h>
#include <mitkPixelTypeMultiplex.h>
#include <mitkImagePixelReadAccessor.h>

// ITK
#include <itkLabelStatisticsImageFilter.h>
#include <itkNeighborhoodIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkLabelGeometryImageFilter.h>

// VTK
#include <vtkSmartPointer.h>
#include <vtkImageMarchingCubes.h>
#include <vtkMassProperties.h>
#include <vtkDelaunay3D.h>
#include <vtkGeometryFilter.h>
#include <vtkDoubleArray.h>
#include <vtkPCAStatistics.h>
#include <vtkTable.h>

// STL
#include <limits>
#include <vnl/vnl_math.h>

// Eigen
#include <Eigen/Dense>

struct GIFVolumetricDensityStatisticsParameters
{
  double volume;
  std::string prefix;
};

template<typename TPixel, unsigned int VImageDimension>
void
CalculateVolumeDensityStatistic(itk::Image<TPixel, VImageDimension>* itkImage, mitk::Image::Pointer mask, GIFVolumetricDensityStatisticsParameters params, mitk::GIFVolumetricDensityStatistics::FeatureListType & featureList)
{
  typedef itk::Image<TPixel, VImageDimension> ImageType;
  typedef itk::Image<unsigned short, VImageDimension> MaskType;

  double volume = params.volume;
  std::string prefix = params.prefix;

  typename MaskType::Pointer maskImage = MaskType::New();
  mitk::CastToItkImage(mask, maskImage);

  itk::ImageRegionConstIteratorWithIndex<ImageType> imgA(itkImage, itkImage->GetLargestPossibleRegion());
  itk::ImageRegionConstIteratorWithIndex<ImageType> imgB(itkImage, itkImage->GetLargestPossibleRegion());
  itk::ImageRegionConstIteratorWithIndex<MaskType> maskA(maskImage, maskImage->GetLargestPossibleRegion());
  itk::ImageRegionConstIteratorWithIndex<MaskType> maskB(maskImage, maskImage->GetLargestPossibleRegion());

  double moranA = 0;
  double moranB = 0;
  double geary = 0;
  double Nv = 0;
  double w_ij = 0;
  double mean = 0;

  typename ImageType::PointType pointA;
  typename ImageType::PointType pointB;

  while (!imgA.IsAtEnd())
  {
    if (maskA.Get() > 0)
    {
      Nv += 1;
      mean += imgA.Get();
    }
    ++imgA;
    ++maskA;
  }
  mean /= Nv;
  imgA.GoToBegin();
  maskA.GoToBegin();

  while (!imgA.IsAtEnd())
  {
    if (maskA.Get() > 0)
    {
      imgB.GoToBegin();
      maskB.GoToBegin();
      while (!imgB.IsAtEnd())
      {
        if ((imgA.GetIndex() == imgB.GetIndex()) ||
          (maskB.Get() < 1))
        {
          ++imgB;
          ++maskB;
          continue;
        }
        itkImage->TransformIndexToPhysicalPoint(maskA.GetIndex(), pointA);
        itkImage->TransformIndexToPhysicalPoint(maskB.GetIndex(), pointB);

        double w = 1 / pointA.EuclideanDistanceTo(pointB);
        moranA += w*(imgA.Get() - mean)* (imgB.Get() - mean);
        geary += w * (imgA.Get() - imgB.Get()) * (imgA.Get() - imgB.Get());

        w_ij += w;

        ++imgB;
        ++maskB;
      }
      moranB += (imgA.Get() - mean)* (imgA.Get() - mean);
    }
    ++imgA;
    ++maskA;
  }

  MITK_INFO << "Volume: " << volume;
  MITK_INFO << " Mean: " << mean;
  featureList.push_back(std::make_pair(prefix + "Volume integrated intensity", volume* mean));
  featureList.push_back(std::make_pair(prefix + "Volume Moran's I index", Nv / w_ij * moranA / moranB));
  featureList.push_back(std::make_pair(prefix + "Volume Geary's C measure", ( Nv -1 ) / 2 / w_ij * geary/ moranB));
}

void calculateMOBB(vtkPointSet *pointset, double &volume, double &surface)
{
  volume = std::numeric_limits<double>::max();

  for (int cellID = 0; cellID < pointset->GetNumberOfCells(); ++cellID)
  {
    auto cell = pointset->GetCell(cellID);

    for (int edgeID = 0; edgeID < 3; ++edgeID)
    {
      auto edge = cell->GetEdge(edgeID);

      double pA[3], pB[3];
      double pAA[3], pBB[3];

      vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
      transform->PostMultiply();
      pointset->GetPoint(edge->GetPointId(0), pA);
      pointset->GetPoint(edge->GetPointId(1), pB);

      double angleZ = std::atan2((- pA[2] + pB[2]) ,(pA[1] - pB[1]));
      angleZ *= 180 / vnl_math::pi;
      if (pA[2] == pB[2])
        angleZ = 0;

      transform->RotateX(angleZ);
      transform->TransformPoint(pA, pAA);
      transform->TransformPoint(pB, pBB);

      double angleY = std::atan2((pAA[1] -pBB[1]) ,-(pAA[0] - pBB[0]));
      angleY *= 180 / vnl_math::pi;
      if (pAA[1] == pBB[1])
        angleY = 0;
      transform->RotateZ(angleY);

      double p0[3];
      pointset->GetPoint(edge->GetPointId(0), p0);

      double curMinX = std::numeric_limits<double>::max();
      double curMaxX = std::numeric_limits<double>::lowest();
      double curMinY = std::numeric_limits<double>::max();
      double curMaxY = std::numeric_limits<double>::lowest();
      double curMinZ = std::numeric_limits<double>::max();
      double curMaxZ = std::numeric_limits<double>::lowest();
      for (int pointID = 0; pointID < pointset->GetNumberOfPoints(); ++pointID)
      {
        double p[3];
        double p2[3];
        pointset->GetPoint(pointID, p);
        p[0] -= p0[0]; p[1] -= p0[1]; p[2] -= p0[2];
        transform->TransformPoint(p, p2);

        curMinX = std::min<double>(p2[0], curMinX);
        curMaxX = std::max<double>(p2[0], curMaxX);
        curMinY = std::min<double>(p2[1], curMinY);
        curMaxY = std::max<double>(p2[1], curMaxY);
        curMinZ = std::min<double>(p2[2], curMinZ);
        curMaxZ = std::max<double>(p2[2], curMaxZ);
      }

      if ((curMaxX - curMinX)*(curMaxY - curMinY)*(curMaxZ - curMinZ) < volume)
      {
        volume = (curMaxX - curMinX)*(curMaxY - curMinY)*(curMaxZ - curMinZ);
        surface = (curMaxX - curMinX)*(curMaxX - curMinX) + (curMaxY - curMinY)*(curMaxY - curMinY) + (curMaxZ - curMinZ)*(curMaxZ - curMinZ);
        surface *= 2;
      }

    }
  }
}

void calculateMEE(vtkPointSet *pointset, double &vol, double &surf, double tolerance=0.0001)
{
  // Inspired by https://github.com/smdabdoub/ProkaryMetrics/blob/master/calc/fitting.py

  int numberOfPoints = pointset->GetNumberOfPoints();
  int dimension = 3;
  Eigen::MatrixXd points(3, numberOfPoints);
  Eigen::MatrixXd Q(3+1, numberOfPoints);
  double p[3];

  std::cout << "Initialize Q " << std::endl;
  for (int i = 0; i < numberOfPoints; ++i)
  {
    pointset->GetPoint(i, p);
    points(0, i) = p[0];
    points(1, i) = p[1];
    points(2, i) = p[2];
    Q(0, i) = p[0];
    Q(1, i) = p[1];
    Q(2, i) = p[2];
    Q(3, i) = 1.0;
  }

  int count = 1;
  double error = 1;
  Eigen::VectorXd u_vector(numberOfPoints);
  u_vector.fill(1.0 / numberOfPoints);
  Eigen::DiagonalMatrix<double, Eigen::Dynamic> u = u_vector.asDiagonal();
  Eigen::VectorXd ones(dimension + 1);
  ones.fill(1);
  Eigen::MatrixXd Ones = ones.asDiagonal();

  // Khachiyan Algorithm
  while (error > tolerance)
  {
    auto Qt = Q.transpose();
    Eigen::MatrixXd X = Q*u*Qt;
    Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(X);
    Eigen::MatrixXd Xi = qr.solve(Ones);

    Eigen::MatrixXd M = Qt * Xi * Q;

    double maximumValue = M(0, 0);
    int maximumPosition = 0;
    for (int i = 0; i < numberOfPoints; ++i)
    {
      if (maximumValue < M(i, i))
      {
        maximumValue = M(i, i);
        maximumPosition = i;
      }
    }
    double stepsize = (maximumValue - dimension - 1) / ((dimension + 1) * (maximumValue - 1));
    Eigen::DiagonalMatrix<double, Eigen::Dynamic> new_u = (1.0 - stepsize) * u;
    new_u.diagonal()[maximumPosition] = (new_u.diagonal())(maximumPosition) + stepsize;
    ++count;
    error = (new_u.diagonal() - u.diagonal()).norm();
    u.diagonal() = new_u.diagonal();
  }

   // U = u

  Eigen::MatrixXd Ai = points * u * points.transpose() - points * u *(points * u).transpose();
  Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(Ai);
  Eigen::VectorXd ones2(dimension);
  ones2.fill(1);
  Eigen::MatrixXd Ones2 = ones2.asDiagonal();
  Eigen::MatrixXd A = qr.solve(Ones2)*1.0/dimension;

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A);
  double c = 1 / sqrt(svd.singularValues()[0]);
  double b = 1 / sqrt(svd.singularValues()[1]);
  double a = 1 / sqrt(svd.singularValues()[2]);
  double V = 4 * vnl_math::pi*a*b*c / 3;

  double ad_mvee= 0;
  double alpha = std::sqrt(1 - b*b / a / a);
  double beta = std::sqrt(1 - c*c / a / a);
  for (int i = 0; i < 20; ++i)
  {
    ad_mvee += 4 * vnl_math::pi*a*b*(alpha*alpha + beta*beta) / (2 * alpha*beta) * (std::pow(alpha*beta, i)) / (1 - 4 * i*i);
  }
  vol = V;
  surf = ad_mvee;
}

mitk::GIFVolumetricDensityStatistics::FeatureListType mitk::GIFVolumetricDensityStatistics::CalculateFeatures(const Image::Pointer & image, const Image::Pointer &mask)
{
  FeatureListType featureList;
  if (image->GetDimension() < 3)
  {
    return featureList;
  }

  std::string prefix = FeatureDescriptionPrefix();

  vtkSmartPointer<vtkImageMarchingCubes> mesher = vtkSmartPointer<vtkImageMarchingCubes>::New();
  vtkSmartPointer<vtkMassProperties> stats = vtkSmartPointer<vtkMassProperties>::New();
  vtkSmartPointer<vtkMassProperties> stats2 = vtkSmartPointer<vtkMassProperties>::New();
  mesher->SetInputData(mask->GetVtkImageData());
  mesher->SetValue(0, 0.5);
  stats->SetInputConnection(mesher->GetOutputPort());
  stats->Update();

  vtkSmartPointer<vtkDelaunay3D> delaunay =
    vtkSmartPointer< vtkDelaunay3D >::New();
  delaunay->SetInputConnection(mesher->GetOutputPort());
  delaunay->SetAlpha(0);
  delaunay->Update();
  vtkSmartPointer<vtkGeometryFilter> geometryFilter =
    vtkSmartPointer<vtkGeometryFilter>::New();
  geometryFilter->SetInputConnection(delaunay->GetOutputPort());
  geometryFilter->Update();
  stats2->SetInputConnection(geometryFilter->GetOutputPort());
  stats2->Update();

  double vol_mvee;
  double surf_mvee;
  calculateMEE(mesher->GetOutput(), vol_mvee, surf_mvee);

  double vol_mobb;
  double surf_mobb;
  calculateMOBB(geometryFilter->GetOutput(), vol_mobb, surf_mobb);

  double pi = vnl_math::pi;

  double meshVolume = stats->GetVolume();
  double meshSurf = stats->GetSurfaceArea();

  GIFVolumetricDensityStatisticsParameters params;
  params.volume = meshVolume;
  params.prefix = prefix;
  AccessByItk_3(image, CalculateVolumeDensityStatistic, mask, params, featureList);

  //Calculate center of mass shift
  int xx = mask->GetDimensions()[0];
  int yy = mask->GetDimensions()[1];
  int zz = mask->GetDimensions()[2];

  double xd = mask->GetGeometry()->GetSpacing()[0];
  double yd = mask->GetGeometry()->GetSpacing()[1];
  double zd = mask->GetGeometry()->GetSpacing()[2];

  int minimumX=xx;
  int maximumX=0;
  int minimumY=yy;
  int maximumY=0;
  int minimumZ=zz;
  int maximumZ=0;

  vtkSmartPointer<vtkDoubleArray> dataset1Arr = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset2Arr = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset3Arr = vtkSmartPointer<vtkDoubleArray>::New();
  dataset1Arr->SetNumberOfComponents(1);
  dataset2Arr->SetNumberOfComponents(1);
  dataset3Arr->SetNumberOfComponents(1);
  dataset1Arr->SetName("M1");
  dataset2Arr->SetName("M2");
  dataset3Arr->SetName("M3");

  vtkSmartPointer<vtkDoubleArray> dataset1ArrU = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset2ArrU = vtkSmartPointer<vtkDoubleArray>::New();
  vtkSmartPointer<vtkDoubleArray> dataset3ArrU = vtkSmartPointer<vtkDoubleArray>::New();
  dataset1ArrU->SetNumberOfComponents(1);
  dataset2ArrU->SetNumberOfComponents(1);
  dataset3ArrU->SetNumberOfComponents(1);
  dataset1ArrU->SetName("M1");
  dataset2ArrU->SetName("M2");
  dataset3ArrU->SetName("M3");

  vtkSmartPointer<vtkPoints> points =
    vtkSmartPointer< vtkPoints >::New();

  for (int x = 0; x < xx; x++)
  {
    for (int y = 0; y < yy; y++)
    {
      for (int z = 0; z < zz; z++)
      {
        itk::Image<int,3>::IndexType index;

        index[0] = x;
        index[1] = y;
        index[2] = z;

        mitk::ScalarType pxImage;
        mitk::ScalarType pxMask;

        mitkPixelTypeMultiplex5(
              mitk::FastSinglePixelAccess,
              image->GetChannelDescriptor().GetPixelType(),
              image,
              image->GetVolumeData(),
              index,
              pxImage,
              0);

        mitkPixelTypeMultiplex5(
              mitk::FastSinglePixelAccess,
              mask->GetChannelDescriptor().GetPixelType(),
              mask,
              mask->GetVolumeData(),
              index,
              pxMask,
              0);

        //Check if voxel is contained in segmentation
        if (pxMask > 0)
        {
          minimumX = std::min<int>(x, minimumX);
          minimumY = std::min<int>(y, minimumY);
          minimumZ = std::min<int>(z, minimumZ);
          maximumX = std::max<int>(x, maximumX);
          maximumY = std::max<int>(y, maximumY);
          maximumZ = std::max<int>(z, maximumZ);
          points->InsertNextPoint(x*xd, y*yd, z*zd);

          if (pxImage == pxImage)
          {
            dataset1Arr->InsertNextValue(x*xd);
            dataset2Arr->InsertNextValue(y*yd);
            dataset3Arr->InsertNextValue(z*zd);
          }
        }
      }
    }
  }

  vtkSmartPointer<vtkTable> datasetTable = vtkSmartPointer<vtkTable>::New();
  datasetTable->AddColumn(dataset1Arr);
  datasetTable->AddColumn(dataset2Arr);
  datasetTable->AddColumn(dataset3Arr);

  vtkSmartPointer<vtkPCAStatistics> pcaStatistics = vtkSmartPointer<vtkPCAStatistics>::New();
  pcaStatistics->SetInputData(vtkStatisticsAlgorithm::INPUT_DATA, datasetTable);
  pcaStatistics->SetColumnStatus("M1", 1);
  pcaStatistics->SetColumnStatus("M2", 1);
  pcaStatistics->SetColumnStatus("M3", 1);
  pcaStatistics->RequestSelectedColumns();
  pcaStatistics->SetDeriveOption(true);
  pcaStatistics->Update();

  vtkSmartPointer<vtkDoubleArray> eigenvalues = vtkSmartPointer<vtkDoubleArray>::New();
  pcaStatistics->GetEigenvalues(eigenvalues);

  std::vector<double> eigen_val(3);
  eigen_val[2] = eigenvalues->GetValue(0);
  eigen_val[1] = eigenvalues->GetValue(1);
  eigen_val[0] = eigenvalues->GetValue(2);

  double major = 2*sqrt(eigen_val[2]);
  double minor = 2*sqrt(eigen_val[1]);
  double least = 2*sqrt(eigen_val[0]);

  double alpha = std::sqrt(1 - minor*minor / major / major);
  double beta = std::sqrt(1 - least*least / major / major);

  double a = (maximumX - minimumX+1) * xd;
  double b = (maximumY - minimumY+1) * yd;
  double c = (maximumZ - minimumZ+1) * zd;

  double vd_aabb = meshVolume / (a*b*c);
  double ad_aabb = meshSurf / (2 * a*b + 2 * a*c + 2 * b*c);

  double vd_aee = 3 * meshVolume / (4.0*pi*major*minor*least);
  double ad_aee = 0;
  for (int i = 0; i < 20; ++i)
  {
    ad_aee += 4 * pi*major*minor*(alpha*alpha + beta*beta) / (2 * alpha*beta) * (std::pow(alpha*beta, i)) / (1 - 4 * i*i);
  }
  ad_aee = meshSurf / ad_aee;

  double vd_ch = meshVolume / stats2->GetVolume();
  double ad_ch = meshSurf / stats2->GetSurfaceArea();

  featureList.push_back(std::make_pair(prefix + "Volume density axis-aligned bounding box", vd_aabb));
  featureList.push_back(std::make_pair(prefix + "Surface density axis-aligned bounding box", ad_aabb));
  featureList.push_back(std::make_pair(prefix + "Volume density oriented minimum bounding box", meshVolume / vol_mobb));
  featureList.push_back(std::make_pair(prefix + "Surface density oriented minimum bounding box", meshSurf / surf_mobb));
  featureList.push_back(std::make_pair(prefix + "Volume density approx. enclosing ellipsoid", vd_aee));
  featureList.push_back(std::make_pair(prefix + "Surface density approx. enclosing ellipsoid", ad_aee));
  featureList.push_back(std::make_pair(prefix + "Volume density approx. minimum volume enclosing ellipsoid", meshVolume / vol_mvee));
  featureList.push_back(std::make_pair(prefix + "Surface density approx. minimum volume enclosing ellipsoid", meshSurf / surf_mvee));
  featureList.push_back(std::make_pair(prefix + "Volume density convex hull", vd_ch));
  featureList.push_back(std::make_pair(prefix + "Surface density convex hull", ad_ch));

  return featureList;
}

mitk::GIFVolumetricDensityStatistics::GIFVolumetricDensityStatistics()
{
  SetLongName("volume-density");
  SetShortName("volden");
  SetFeatureClassName("Morphological Density");
}

mitk::GIFVolumetricDensityStatistics::FeatureNameListType mitk::GIFVolumetricDensityStatistics::GetFeatureNames()
{
  FeatureNameListType featureList;
  return featureList;
}

void mitk::GIFVolumetricDensityStatistics::AddArguments(mitkCommandLineParser &parser)
{
  std::string name = GetOptionPrefix();

  parser.addArgument(GetLongName(), name, mitkCommandLineParser::Bool, "Use Volume-Density Statistic", "calculates volume density based features", us::Any());
}

void
mitk::GIFVolumetricDensityStatistics::CalculateFeaturesUsingParameters(const Image::Pointer & feature, const Image::Pointer &mask, const Image::Pointer &, FeatureListType &featureList)
{
  auto parsedArgs = GetParameter();
  if (parsedArgs.count(GetLongName()))
  {
    MITK_INFO << "Start calculating volumetric density features ....";
    auto localResults = this->CalculateFeatures(feature, mask);
    featureList.insert(featureList.end(), localResults.begin(), localResults.end());
    MITK_INFO << "Finished calculating volumetric density features....";
  }
}


#include <NTL/ZZX.h>

#include <NTL/new.h>

NTL_START_IMPL

const ZZX& ZZX::zero()
{
   static ZZX z;
   return z;
}

void conv(ZZ_pX& x, const ZZX& a)
{
   conv(x.rep, a.rep);
   x.normalize();
}

void conv(ZZX& x, const ZZ_pX& a)
{
   conv(x.rep, a.rep);
   x.normalize();
}

istream& operator>>(istream& s, ZZX& x)
{
   s >> x.rep;
   x.normalize();
   return s;
}

ostream& operator<<(ostream& s, const ZZX& a)
{
   return s << a.rep;
}

void ZZX::normalize()
{
   long n;
   const ZZ* p;

   n = rep.length();
   if (n == 0) return;
   p = rep.elts() + n;
   while (n > 0 && IsZero(*--p)) {
      n--;
   }
   rep.SetLength(n);
}

long IsZero(const ZZX& a)
{
   return a.rep.length() == 0;
}

long IsOne(const ZZX& a)
{
    return a.rep.length() == 1 && IsOne(a.rep[0]);
}

long operator==(const ZZX& a, const ZZX& b)
{
   long i, n;
   const ZZ *ap, *bp;

   n = a.rep.length();
   if (n != b.rep.length()) return 0;

   ap = a.rep.elts();
   bp = b.rep.elts();

   for (i = 0; i < n; i++)
      if (ap[i] != bp[i]) return 0;

   return 1;
}

long operator==(const ZZX& a, long b)
{
   if (b == 0)
      return IsZero(a);

   if (deg(a) != 0)
      return 0;

   return a.rep[0] == b;
}

long operator==(const ZZX& a, const ZZ& b)
{
   if (IsZero(b))
      return IsZero(a);

   if (deg(a) != 0)
      return 0;

   return a.rep[0] == b;
}

void GetCoeff(ZZ& x, const ZZX& a, long i)
{
   if (i < 0 || i > deg(a))
      clear(x);
   else
      x = a.rep[i];
}

void SetCoeff(ZZX& x, long i, const ZZ& a)
{
   long j, m;

   if (i < 0)
      Error("SetCoeff: negative index");

   if (NTL_OVERFLOW(i, 1, 0))
      Error("overflow in SetCoeff");

   m = deg(x);

   if (i > m && IsZero(a)) return;

   if (i > m) {
      /* careful: a may alias a coefficient of x */

      long alloc = x.rep.allocated();

      if (alloc > 0 && i >= alloc) {
         ZZ aa = a;
         x.rep.SetLength(i+1);
         x.rep[i] = aa;
      }
      else {
         x.rep.SetLength(i+1);
         x.rep[i] = a;
      }

      for (j = m+1; j < i; j++)
         clear(x.rep[j]);
   }
   else
      x.rep[i] = a;

   x.normalize();
}

void SetCoeff(ZZX& x, long i)
{
   long j, m;

   if (i < 0)
      Error("coefficient index out of range");

   if (NTL_OVERFLOW(i, 1, 0))
      Error("overflow in SetCoeff");

   m = deg(x);

   if (i > m) {
      x.rep.SetLength(i+1);
      for (j = m+1; j < i; j++)
         clear(x.rep[j]);
   }
   set(x.rep[i]);
   x.normalize();
}

void SetX(ZZX& x)
{
   clear(x);
   SetCoeff(x, 1);
}

long IsX(const ZZX& a)
{
   return deg(a) == 1 && IsOne(LeadCoeff(a)) && IsZero(ConstTerm(a));
}

const ZZ& coeff(const ZZX& a, long i)
{
   if (i < 0 || i > deg(a))
      return ZZ::zero();
   else
      return a.rep[i];
}

const ZZ& LeadCoeff(const ZZX& a)
{
   if (IsZero(a))
      return ZZ::zero();
   else
      return a.rep[deg(a)];
}

const ZZ& ConstTerm(const ZZX& a)
{
   if (IsZero(a))
      return ZZ::zero();
   else
      return a.rep[0];
}

void conv(ZZX& x, const ZZ& a)
{
   if (IsZero(a))
      x.rep.SetLength(0);
   else {
      x.rep.SetLength(1);
      x.rep[0] = a;
   }
}

void conv(ZZX& x, long a)
{
   if (a == 0)
      x.rep.SetLength(0);
   else {
      x.rep.SetLength(1);
      conv(x.rep[0], a);
   }
}

void conv(ZZX& x, const vec_ZZ& a)
{
   x.rep = a;
   x.normalize();
}

void add(ZZX& x, const ZZX& a, const ZZX& b)
{
   long da = deg(a);
   long db = deg(b);
   long minab = min(da, db);
   long maxab = max(da, db);
   x.rep.SetLength(maxab+1);

   long i;
   const ZZ *ap, *bp;
   ZZ* xp;

   for (i = minab+1, ap = a.rep.elts(), bp = b.rep.elts(), xp = x.rep.elts();
        i; i--, ap++, bp++, xp++)
      add(*xp, (*ap), (*bp));

   if (da > minab && &x != &a)
      for (i = da-minab; i; i--, xp++, ap++)
         *xp = *ap;
   else if (db > minab && &x != &b)
      for (i = db-minab; i; i--, xp++, bp++)
         *xp = *bp;
   else
      x.normalize();
}

void add(ZZX& x, const ZZX& a, const ZZ& b)
{
   long n = a.rep.length();
   if (n == 0) {
      conv(x, b);
   }
   else if (&x == &a) {
      add(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else if (x.rep.MaxLength() == 0) {
      x = a;
      add(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else {
      // ugly...b could alias a coeff of x

      ZZ *xp = x.rep.elts();
      add(xp[0], a.rep[0], b);
      x.rep.SetLength(n);
      xp = x.rep.elts();
      const ZZ *ap = a.rep.elts();
      long i;
      for (i = 1; i < n; i++)
         xp[i] = ap[i];
      x.normalize();
   }
}

void add(ZZX& x, const ZZX& a, long b)
{
   if (a.rep.length() == 0) {
      conv(x, b);
   }
   else {
      if (&x != &a) x = a;
      add(x.rep[0], x.rep[0], b);
      x.normalize();
   }
}

void sub(ZZX& x, const ZZX& a, const ZZX& b)
{
   long da = deg(a);
   long db = deg(b);
   long minab = min(da, db);
   long maxab = max(da, db);
   x.rep.SetLength(maxab+1);

   long i;
   const ZZ *ap, *bp;
   ZZ* xp;

   for (i = minab+1, ap = a.rep.elts(), bp = b.rep.elts(), xp = x.rep.elts();
        i; i--, ap++, bp++, xp++)
      sub(*xp, (*ap), (*bp));

   if (da > minab && &x != &a)
      for (i = da-minab; i; i--, xp++, ap++)
         *xp = *ap;
   else if (db > minab)
      for (i = db-minab; i; i--, xp++, bp++)
         negate(*xp, *bp);
   else
      x.normalize();

}

void sub(ZZX& x, const ZZX& a, const ZZ& b)
{
   long n = a.rep.length();
   if (n == 0) {
      conv(x, b);
      negate(x, x);
   }
   else if (&x == &a) {
      sub(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else if (x.rep.MaxLength() == 0) {
      x = a;
      sub(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else {
      // ugly...b could alias a coeff of x

      ZZ *xp = x.rep.elts();
      sub(xp[0], a.rep[0], b);
      x.rep.SetLength(n);
      xp = x.rep.elts();
      const ZZ *ap = a.rep.elts();
      long i;
      for (i = 1; i < n; i++)
         xp[i] = ap[i];
      x.normalize();
   }
}

void sub(ZZX& x, const ZZX& a, long b)
{
   if (b == 0) {
      x = a;
      return;
   }

   if (a.rep.length() == 0) {
      x.rep.SetLength(1);
      conv(x.rep[0], b);
      negate(x.rep[0], x.rep[0]);
   }
   else {
      if (&x != &a) x = a;
      sub(x.rep[0], x.rep[0], b);
   }
   x.normalize();
}

void sub(ZZX& x, long a, const ZZX& b)
{
   negate(x, b);
   add(x, x, a);
}

void sub(ZZX& x, const ZZ& b, const ZZX& a)
{
   long n = a.rep.length();
   if (n == 0) {
      conv(x, b);
   }
   else if (x.rep.MaxLength() == 0) {
      negate(x, a);
      add(x.rep[0], a.rep[0], b);
      x.normalize();
   }
   else {
      // ugly...b could alias a coeff of x

      ZZ *xp = x.rep.elts();
      sub(xp[0], b, a.rep[0]);
      x.rep.SetLength(n);
      xp = x.rep.elts();
      const ZZ *ap = a.rep.elts();
      long i;
      for (i = 1; i < n; i++)
         negate(xp[i], ap[i]);
      x.normalize();
   }
}

void negate(ZZX& x, const ZZX& a)
{
   long n = a.rep.length();
   x.rep.SetLength(n);

   const ZZ* ap = a.rep.elts();
   ZZ* xp = x.rep.elts();
   long i;

   for (i = n; i; i--, ap++, xp++)
      negate((*xp), (*ap));

}

long MaxBits(const ZZX& f)
{
   long i, m;
   m = 0;

   for (i = 0; i <= deg(f); i++) {
      m = max(m, NumBits(f.rep[i]));
   }

   return m;
}

void PlainMul(ZZX& x, const ZZX& a, const ZZX& b)
{
   if (&a == &b) {
      PlainSqr(x, a);
      return;
   }

   long da = deg(a);
   long db = deg(b);

   if (da < 0 || db < 0) {
      clear(x);
      return;
   }

   long d = da+db;

   const ZZ *ap, *bp;
   ZZ *xp;

   ZZX la, lb;

   if (&x == &a) {
      la = a;
      ap = la.rep.elts();
   }
   else
      ap = a.rep.elts();

   if (&x == &b) {
      lb = b;
      bp = lb.rep.elts();
   }
   else
      bp = b.rep.elts();

   x.rep.SetLength(d+1);

   xp = x.rep.elts();

   long i, j, jmin, jmax;
   ZZ t, accum;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-db);
      jmax = min(da, i);
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
	 mul(t, ap[j], bp[i-j]);
	 add(accum, accum, t);
      }
      xp[i] = accum;
   }
   x.normalize();
}

void PlainSqr(ZZX& x, const ZZX& a)
{
   long da = deg(a);

   if (da < 0) {
      clear(x);
      return;
   }

   long d = 2*da;

   const ZZ *ap;
   ZZ *xp;

   ZZX la;

   if (&x == &a) {
      la = a;
      ap = la.rep.elts();
   }
   else
      ap = a.rep.elts();

   x.rep.SetLength(d+1);

   xp = x.rep.elts();

   long i, j, jmin, jmax;
   long m, m2;
   ZZ t, accum;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-da);
      jmax = min(da, i);
      m = jmax - jmin + 1;
      m2 = m >> 1;
      jmax = jmin + m2 - 1;
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
	 mul(t, ap[j], ap[i-j]);
	 add(accum, accum, t);
      }
      add(accum, accum, accum);
      if (m & 1) {
	 sqr(t, ap[jmax + 1]);
	 add(accum, accum, t);
      }

      xp[i] = accum;
   }

   x.normalize();
}

static
void PlainMul(ZZ *xp, const ZZ *ap, long sa, const ZZ *bp, long sb)
{
   if (sa == 0 || sb == 0) return;

   long sx = sa+sb-1;

   long i, j, jmin, jmax;
   static ZZ t, accum;

   for (i = 0; i < sx; i++) {
      jmin = max(0, i-sb+1);
      jmax = min(sa-1, i);
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
         mul(t, ap[j], bp[i-j]);
         add(accum, accum, t);
      }
      xp[i] = accum;
   }
}

static
void KarFold(ZZ *T, const ZZ *b, long sb, long hsa)
{
   long m = sb - hsa;
   long i;

   for (i = 0; i < m; i++)
      add(T[i], b[i], b[hsa+i]);

   for (i = m; i < hsa; i++)
      T[i] = b[i];
}

static
void KarSub(ZZ *T, const ZZ *b, long sb)
{
   long i;

   for (i = 0; i < sb; i++)
      sub(T[i], T[i], b[i]);
}

static
void KarAdd(ZZ *T, const ZZ *b, long sb)
{
   long i;

   for (i = 0; i < sb; i++)
      add(T[i], T[i], b[i]);
}

static
void KarFix(ZZ *c, const ZZ *b, long sb, long hsa)
{
   long i;

   for (i = 0; i < hsa; i++)
      c[i] = b[i];

   for (i = hsa; i < sb; i++)
      add(c[i], c[i], b[i]);
}

static void PlainMul1(ZZ *xp, const ZZ *ap, long sa, const ZZ& b)
{
   long i;

   for (i = 0; i < sa; i++)
      mul(xp[i], ap[i], b);
}

static
void KarMul(ZZ *c, const ZZ *a,
            long sa, const ZZ *b, long sb, ZZ *stk)
{
   if (sa < sb) {
      { long t = sa; sa = sb; sb = t; }
      { const ZZ *t = a; a = b; b = t; }
   }

   if (sb == 1) {
      if (sa == 1)
         mul(*c, *a, *b);
      else
         PlainMul1(c, a, sa, *b);

      return;
   }

   if (sb == 2 && sa == 2) {
      mul(c[0], a[0], b[0]);
      mul(c[2], a[1], b[1]);
      add(stk[0], a[0], a[1]);
      add(stk[1], b[0], b[1]);
      mul(c[1], stk[0], stk[1]);
      sub(c[1], c[1], c[0]);
      sub(c[1], c[1], c[2]);

      return;

   }

   long hsa = (sa + 1) >> 1;

   if (hsa < sb) {
      /* normal case */

      long hsa2 = hsa << 1;

      ZZ *T1, *T2, *T3;

      T1 = stk; stk += hsa;
      T2 = stk; stk += hsa;
      T3 = stk; stk += hsa2 - 1;

      /* compute T1 = a_lo + a_hi */

      KarFold(T1, a, sa, hsa);

      /* compute T2 = b_lo + b_hi */

      KarFold(T2, b, sb, hsa);

      /* recursively compute T3 = T1 * T2 */

      KarMul(T3, T1, hsa, T2, hsa, stk);

      /* recursively compute a_hi * b_hi into high part of c */
      /* and subtract from T3 */

      KarMul(c + hsa2, a+hsa, sa-hsa, b+hsa, sb-hsa, stk);
      KarSub(T3, c + hsa2, sa + sb - hsa2 - 1);

      /* recursively compute a_lo*b_lo into low part of c */
      /* and subtract from T3 */

      KarMul(c, a, hsa, b, hsa, stk);
      KarSub(T3, c, hsa2 - 1);

      clear(c[hsa2 - 1]);

      /* finally, add T3 * X^{hsa} to c */

      KarAdd(c+hsa, T3, hsa2-1);
   }
   else {
      /* degenerate case */

      ZZ *T;

      T = stk; stk += hsa + sb - 1;

      /* recursively compute b*a_hi into high part of c */

      KarMul(c + hsa, a + hsa, sa - hsa, b, sb, stk);

      /* recursively compute b*a_lo into T */

      KarMul(T, a, hsa, b, sb, stk);

      KarFix(c, T, hsa + sb - 1, hsa);
   }
}

void KarMul(ZZX& c, const ZZX& a, const ZZX& b)
{
   if (IsZero(a) || IsZero(b)) {
      clear(c);
      return;
   }

   if (&a == &b) {
      KarSqr(c, a);
      return;
   }

   vec_ZZ mem;

   const ZZ *ap, *bp;
   ZZ *cp;

   long sa = a.rep.length();
   long sb = b.rep.length();

   if (&a == &c) {
      mem = a.rep;
      ap = mem.elts();
   }
   else
      ap = a.rep.elts();

   if (&b == &c) {
      mem = b.rep;
      bp = mem.elts();
   }
   else
      bp = b.rep.elts();

   c.rep.SetLength(sa+sb-1);
   cp = c.rep.elts();

   long maxa, maxb, xover;

   maxa = MaxBits(a);
   maxb = MaxBits(b);
   xover = 2;

   if (sa < xover || sb < xover)
      PlainMul(cp, ap, sa, bp, sb);
   else {
      /* karatsuba */

      long n, hn, sp, depth;

      n = max(sa, sb);
      sp = 0;
      depth = 0;
      do {
         hn = (n+1) >> 1;
         sp += (hn << 2) - 1;
         n = hn;
         depth++;
      } while (n >= xover);

      ZZVec stk;
      stk.SetSize(sp,
         ((maxa + maxb + NumBits(min(sa, sb)) + 2*depth + 10)
          + NTL_ZZ_NBITS-1)/NTL_ZZ_NBITS);

      KarMul(cp, ap, sa, bp, sb, stk.elts());
   }

   c.normalize();
}

void PlainSqr(ZZ* xp, const ZZ* ap, long sa)
{
   if (sa == 0) return;

   long da = sa-1;
   long d = 2*da;

   long i, j, jmin, jmax;
   long m, m2;
   static ZZ t, accum;

   for (i = 0; i <= d; i++) {
      jmin = max(0, i-da);
      jmax = min(da, i);
      m = jmax - jmin + 1;
      m2 = m >> 1;
      jmax = jmin + m2 - 1;
      clear(accum);
      for (j = jmin; j <= jmax; j++) {
	 mul(t, ap[j], ap[i-j]);
	 add(accum, accum, t);
      }
      add(accum, accum, accum);
      if (m & 1) {
	 sqr(t, ap[jmax + 1]);
	 add(accum, accum, t);
      }

      xp[i] = accum;
   }
}

static
void KarSqr(ZZ *c, const ZZ *a, long sa, ZZ *stk)
{
   if (sa == 1) {
      sqr(*c, *a);
      return;
   }

   if (sa == 2) {
      sqr(c[0], a[0]);
      sqr(c[2], a[1]);
      mul(c[1], a[0], a[1]);
      add(c[1], c[1], c[1]);

      return;
   }

   if (sa == 3) {
      sqr(c[0], a[0]);
      mul(c[1], a[0], a[1]);
      add(c[1], c[1], c[1]);
      sqr(stk[0], a[1]);
      mul(c[2], a[0], a[2]);
      add(c[2], c[2], c[2]);
      add(c[2], c[2], stk[0]);
      mul(c[3], a[1], a[2]);
      add(c[3], c[3], c[3]);
      sqr(c[4], a[2]);

      return;

   }

   long hsa = (sa + 1) >> 1;
   long hsa2 = hsa << 1;

   ZZ *T1, *T2;

   T1 = stk; stk += hsa;
   T2 = stk; stk += hsa2-1;

   KarFold(T1, a, sa, hsa);
   KarSqr(T2, T1, hsa, stk);

   KarSqr(c + hsa2, a+hsa, sa-hsa, stk);
   KarSub(T2, c + hsa2, sa + sa - hsa2 - 1);

   KarSqr(c, a, hsa, stk);
   KarSub(T2, c, hsa2 - 1);

   clear(c[hsa2 - 1]);

   KarAdd(c+hsa, T2, hsa2-1);
}

void KarSqr(ZZX& c, const ZZX& a)
{
   if (IsZero(a)) {
      clear(c);
      return;
   }

   vec_ZZ mem;

   const ZZ *ap;
   ZZ *cp;

   long sa = a.rep.length();

   if (&a == &c) {
      mem = a.rep;
      ap = mem.elts();
   }
   else
      ap = a.rep.elts();

   c.rep.SetLength(sa+sa-1);
   cp = c.rep.elts();

   long maxa, xover;

   maxa = MaxBits(a);

   xover = 2;

   if (sa < xover)
      PlainSqr(cp, ap, sa);
   else {
      /* karatsuba */

      long n, hn, sp, depth;

      n = sa;
      sp = 0;
      depth = 0;
      do {
         hn = (n+1) >> 1;
         sp += hn+hn+hn - 1;
         n = hn;
         depth++;
      } while (n >= xover);

      ZZVec stk;
      stk.SetSize(sp,
         ((2*maxa + NumBits(sa) + 2*depth + 10)
          + NTL_ZZ_NBITS-1)/NTL_ZZ_NBITS);

      KarSqr(cp, ap, sa, stk.elts());
   }

   c.normalize();
}

NTL_END_IMPL


#pragma once
#include "sico/types/orientations.hpp"

#ifdef SICO_USE_EIGEN
#pragma warning(push)
#pragma warning(disable : 4365)
#pragma warning(disable : 4464)
#pragma warning(disable : 4514)
#pragma warning(disable : 4710)
#pragma warning(disable : 4625)
#pragma warning(disable : 4626)
#pragma warning(disable : 4820)
#pragma warning(disable : 5026)
#pragma warning(disable : 5027)
#pragma warning(disable : 5045)
#define EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
#include <Eigen/Dense>
#pragma warning(pop)

namespace sico {

using vector3d   = Eigen::Vector3d;
using matrix3d   = Eigen::Matrix3d;
using quaternion = Eigen::Quaterniond;

} // namespace sico

#else
#include "sico/types/angles.hpp"

#include <array>
#include <type_traits>

#pragma warning(push)
#pragma warning(disable : 5045) // spectre
namespace sico {

using scalar_t = double;

template<size_t S>
using arrayd = std::array<scalar_t, S>;

// Basic linear algebra based on arrays of doubles
using vector3d   = arrayd<3>;
using matrix3d   = arrayd<9>;
using quaternion = arrayd<4>;

// Beneric operations
template<size_t S>
arrayd<S> operator+(arrayd<S> const& s1, arrayd<S> const& s2)
{
    arrayd<S> s;
    for (size_t i = 0; i < S; i++)
        s[i] = s1[i] + s2[i];
    return s;
}
template<size_t S>
arrayd<S>& operator+=(arrayd<S>& s1, arrayd<S> const& s2)
{
    for (size_t i = 0; i < S; i++)
        s1[i] += s2[i];
    return s1;
}
template<size_t S>
arrayd<S>& operator+=(arrayd<S>& s1, scalar_t const v)
{
    for (size_t i = 0; i < S; i++)
        s1[i] += v;
    return s1;
}
template<size_t S>
arrayd<S> operator-(arrayd<S> const& s1)
{
    arrayd<S> s;
    for (size_t i = 0; i < S; i++)
        s[i] = -s1[i];
    return s;
}
template<size_t S>
arrayd<S> operator*(arrayd<S> const& s1, scalar_t const t)
{
    arrayd<S> s;
    for (size_t i = 0; i < S; i++)
        s[i] = s1[i] * t;
    return s;
}
template<size_t S>
arrayd<S>& operator*=(arrayd<S>& s, scalar_t const t)
{
    for (size_t i = 0; i < S; i++)
        s[i] *= t;
    return s;
}
template<size_t S>
bool operator==(arrayd<S> const& s1, arrayd<S> const& s2)
{
    bool r = true;
    for (size_t i = 0; i < S; i++)
        r = r && s1[i] == s2[i];
    return r;
}
template<size_t S>
scalar_t dot(arrayd<S> const& s1, arrayd<S> const& s2)
{
    scalar_t d = 0;
    for (size_t i = 0; i < S; i++) {
        d += s1[i] * s2[i];
    }
    return d;
}

// generic operations, based on previously defined ones
template<size_t S>
arrayd<S> operator-(arrayd<S> const& s1, arrayd<S> const& s2)
{
    return s1 + -s2;
}
template<size_t S>
arrayd<S>& operator-=(arrayd<S>& s1, arrayd<S> const& s2)
{
    s1 += -s2;
    return s1;
}
template<size_t S>
arrayd<S>& operator-=(arrayd<S>& s1, scalar_t const v)
{
    s1 += -v;
    return s1;
}
template<size_t S>
arrayd<S> operator/(arrayd<S> const& s1, scalar_t const d)
{
    return s1 * (1.0 / d);
}
template<size_t S>
arrayd<S>& operator/=(arrayd<S>& s1, scalar_t const d)
{
    s1 *= 1.0 / d;
    return s1;
}
template<size_t S>
bool operator!=(arrayd<S> const& s1, arrayd<S> const& s2)
{
    return !(s1 == s2);
}
template<size_t S>
scalar_t length_squared(arrayd<S> const& s)
{
    return dot(s, s);
}
template<size_t S>
scalar_t length(arrayd<S> const& s)
{
    return sqrt(length_squared(s));
}
template<size_t S>
arrayd<S>& normalize(arrayd<S>& s)
{
    auto const l = length(s);
    if (l < 0.0001)
        return s;
    s /= l;
    return s;
}
template<size_t S>
arrayd<S> normalized(arrayd<S> s)
{
    normalize(s);
    return s;
}

// quaternion operations
inline quaternion operator*(quaternion const& q1, quaternion const& q2)
{
    return { q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
             q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
             q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
             q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] };
}
inline quaternion conj(quaternion const& q)
{
    return { -q[0], -q[1], -q[2], q[3] };
}
inline vector3d operator*(quaternion const& rot, vector3d const& v)
{
    quaternion const pure { v[0], v[1], v[2], 0.0 };
    quaternion const rot_conj = conj(rot);
    quaternion const temp { pure[3] * rot_conj[0] + pure[0] * rot_conj[3] + pure[1] * rot_conj[2]
                                - pure[2] * rot_conj[1],
                            pure[3] * rot_conj[1] + pure[1] * rot_conj[3] + pure[2] * rot_conj[0]
                                - pure[0] * rot_conj[2],
                            pure[3] * rot_conj[2] + pure[2] * rot_conj[3] + pure[0] * rot_conj[1]
                                - pure[1] * rot_conj[0],
                            pure[3] * rot_conj[3] - pure[0] * rot_conj[0] - pure[1] * rot_conj[1]
                                - pure[2] * rot_conj[2] };

    return { rot[3] * temp[0] + rot[0] * temp[3] + rot[1] * temp[2] - rot[2] * temp[1],
             rot[3] * temp[1] + rot[1] * temp[3] + rot[2] * temp[0] - rot[0] * temp[2],
             rot[3] * temp[2] + rot[2] * temp[3] + rot[0] * temp[1] - rot[1] * temp[0] };
}
inline quaternion from_euler(scalar_t xRot, scalar_t yRot, scalar_t zRot)
{
    using std::cos;
    using std::sin;
    auto const       hX = xRot * 0.5;
    auto const       hY = yRot * 0.5;
    auto const       hZ = zRot * 0.5;
    quaternion const qx { sin(hX), 0.0, 0.0, cos(hX) };
    quaternion const qy { 0.0, sin(hY), 0.0, cos(hY) };
    quaternion const qz { 0.0, 0.0, sin(hZ), cos(hZ) };
    quaternion       result = qx * qy * qz;
    normalize(result);
    return result;
}

// matrix operations
inline double& mat(matrix3d& m, size_t row, size_t column)
{
    return m[row * 3 + column];
}
inline double mat(matrix3d const& m, size_t row, size_t column)
{
    return m[row * 3 + column];
}
inline vector3d operator*(matrix3d const& m, vector3d const& v)
{
    vector3d res;
    res.fill(0);
    for (unsigned r = 0; r < 3; ++r)
        for (unsigned c = 0; c < 3; ++c)
            res[r] += mat(m, r, c) * v[c];

    return res;
}
inline matrix3d rot_mat(quaternion const& q)
{
    auto const xs = q[0] + q[0];
    auto const ys = q[1] + q[1];
    auto const zs = q[2] + q[2];
    auto const xx = q[0] * xs;
    auto const xy = q[0] * ys;
    auto const xz = q[0] * zs;
    auto const yy = q[1] * ys;
    auto const yz = q[1] * zs;
    auto const zz = q[2] * zs;
    auto const wx = q[3] * xs;
    auto const wy = q[3] * ys;
    auto const wz = q[3] * zs;

    matrix3d m;
    mat(m, 0, 0) = double(1.0) - (yy + zz);
    mat(m, 1, 0) = xy + wz;
    mat(m, 2, 0) = xz - wy;
    mat(m, 0, 1) = xy - wz;
    mat(m, 1, 1) = double(1.0) - (xx + zz);
    mat(m, 2, 1) = yz + wx;
    mat(m, 0, 2) = xz + wy;
    mat(m, 1, 2) = yz - wx;
    mat(m, 2, 2) = double(1.0) - (xx + yy);
    return m;
}
inline void to_euler(matrix3d const& m, scalar_t& x, scalar_t& y, scalar_t& z)
{
    y = asin(mat(m, 0, 2));
    if (y < SICO_PI2) {
        if (y > -SICO_PI2) {
            x = atan2(-mat(m, 1, 2), mat(m, 2, 2));
            z = atan2(-mat(m, 0, 1), mat(m, 0, 0));
        }
        else {
            x = -atan2(mat(m, 1, 0), mat(m, 1, 1));
            z = 0;
        }
    }
    else {
        x = atan2(mat(m, 1, 0), mat(m, 1, 1));
        z = 0;
    }
}

} // namespace sico
#pragma warning(pop)
#endif

namespace sico {
template<typename Ref>
inline quaternion make_quat(quat<Ref> const& q)
{
#ifdef SICO_USE_EIGEN
    return quaternion(q.w, q.x, q.y, q.z);
#else
    return quaternion { q.x, q.y, q.z, q.w };
#endif
}

template<typename Ref>
inline quat<Ref> make_quat(quaternion const& q)
{
#ifdef SICO_USE_EIGEN
    return quat<Ref> { q.x(), q.y(), q.z(), q.w() };
#else
    return quat<Ref> { q[0], q[1], q[2], q[3] };
#endif
}
} // namespace sico

//
// Simulation-Coordinates library
// Author F.Jacomme
// MIT Licensed
//


#ifndef MATHTOOLBOX_KERNEL_FUNCTIONS_HPP
#define MATHTOOLBOX_KERNEL_FUNCTIONS_HPP

#include <Eigen/Core>
#include <functional>

namespace mathtoolbox
{
    using Kernel = std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&)>;
    using KernelThetaDerivative =
        std::function<Eigen::VectorXd(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&)>;
    using KernelThetaIDerivative =
        std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&, const int)>;
    using KernelFirstArgDerivative =
        std::function<Eigen::VectorXd(const Eigen::VectorXd&, const Eigen::VectorXd&, const Eigen::VectorXd&)>;

    double GetArdSquaredExpKernel(const Eigen::VectorXd& x_a, const Eigen::VectorXd& x_b, const Eigen::VectorXd& theta);

    Eigen::VectorXd GetArdSquaredExpKernelThetaDerivative(const Eigen::VectorXd& x_a,
                                                          const Eigen::VectorXd& x_b,
                                                          const Eigen::VectorXd& theta);

    double GetArdSquaredExpKernelThetaIDerivative(const Eigen::VectorXd& x_a,
                                                  const Eigen::VectorXd& x_b,
                                                  const Eigen::VectorXd& theta,
                                                  const int              index);

    Eigen::VectorXd GetArdSquaredExpKernelFirstArgDerivative(const Eigen::VectorXd& x_a,
                                                             const Eigen::VectorXd& x_b,
                                                             const Eigen::VectorXd& theta);

    double GetArdMatern52Kernel(const Eigen::VectorXd& x_a, const Eigen::VectorXd& x_b, const Eigen::VectorXd& theta);

    Eigen::VectorXd GetArdMatern52KernelThetaDerivative(const Eigen::VectorXd& x_a,
                                                        const Eigen::VectorXd& x_b,
                                                        const Eigen::VectorXd& theta);

    double GetArdMatern52KernelThetaIDerivative(const Eigen::VectorXd& x_a,
                                                const Eigen::VectorXd& x_b,
                                                const Eigen::VectorXd& theta,
                                                const int              index);

    Eigen::VectorXd GetArdMatern52KernelFirstArgDerivative(const Eigen::VectorXd& x_a,
                                                           const Eigen::VectorXd& x_b,
                                                           const Eigen::VectorXd& theta);
} // namespace mathtoolbox

#endif // MATHTOOLBOX_KERNEL_FUNCTIONS_HPP