/*
 * XSENSMODEL.hpp
 *
 *  Created on: 04.09.2018
 *      Author: tomlucas
 */

#ifndef ESTIMATORS_MODELS_XSENSMODELWITHBIAS_HPP_
#define ESTIMATORS_MODELS_XSENSMODELWITHBIAS_HPP_
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "../../Plugins/imu_plugin.hpp"
#include <Eigen_Utils.hpp>
#include "State.hpp"
#include "../StateBoxes/CartesianBox.hpp"
#include "../StateBoxes/OrientationSpeedDerivBox.hpp"
#include "../StateBoxes/AngularRateBiasBox.hpp"
#include "../StateBoxes/AccelerationBiasBox.hpp"
#include "../AlignType.hpp"
namespace zavi
::estimator::model {
	typedef State<zavi::estimator::state_boxes::OrientationSpeedDerivBox,state_boxes::AccelerationBiasBox, state_boxes::AngularRateBiasBox > CALIBRATION_MODEL_TYPE;
	/**
	 * Model for calibration
	 * Assumes that the bycycle only rolls around the ground x-axis
	 */
	class CalibrationModel: public CALIBRATION_MODEL_TYPE {
		//static_assert(std::is_base_of<StateEstimator,state_estimator_type>,"NoOrientationModel template fail state_estimator_type is no base class of StateEstimator");

	public:

		/**
		 * Function to integrate an input measurement
		 * @param state the current state
		 * @param alignment the calibration matrix from imu to body frame
		 * @param prior an arbitrary prior that can be passed to the function
		 * @return the estimated inputs at state
		 */
		struct input_measurement: public base_measurement<6> {
			static inline Eigen::Matrix<double,output_size,output_size> stiffnes;
			template<typename T>
			using MEASURE_TYPE=Eigen::Matrix<T,output_size,1>;
			template<typename T>
			inline MEASURE_TYPE<T> operator()(const OUTER_T<T> &state,const ALIGNMENT_TYPE<T> & alignment, void * prior) {
				MEASURE_TYPE<T> measurement = MEASURE_TYPE < T > ::Zero();
				measurement.template block<3,1>(0, 0) =acceleration_measurement(state,alignment, prior);
				measurement.template block<3,1>(3, 0) = orientation_change_measurement(state,alignment, prior);
				return measurement;
			}
		};
	public:
		CalibrationModel(std::shared_ptr<plugin::SensorPlugin> sensor);
		static constexpr int orientation_start=0;
		static constexpr int orientation_change_start=9;
		static constexpr int orientation_change_deriv_start=12;
		static constexpr int position_start=-1;
		static constexpr int velocity_start=-1;
		static constexpr int acceleration_start=-1;
		static constexpr int velocity_cov_start=-1;
		virtual ~CalibrationModel();

		/**
		 * Function to integrate an acceleration measurement
		 * @param state the current state
		 * @param alignment the calibration matrix from imu to body frame
		 * @param prior an arbitrary prior that can be passed to the function
		 * @return the estimated acceleration at state
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> acceleration_measurement(const OUTER_T<T> & state,const ALIGNMENT_TYPE<T> & alignment,
				void * prior) {
			Eigen::Matrix<T,3,3> Q_i_b=alignment.template block<3,3>(0,0);
			Eigen::Matrix<T,3,3> S= eigen_util::makeSkewSymmetric<T>(getOrientationChange(state));
			return Q_i_b*(eigen_util::makeSkewSymmetric<T>(getOrientationChangeDeriv(state))+S*S )*alignment.template block<3,1>(0,3)+state.template block<3,1>(15,0)+state.template block<3,1>(18,0)+Q_i_b*getOrientation(state).transpose()*plugin::IMU_Plugin::gravity;
		}

		/**
		 * Function to integrate an orientation measurement
		 * @param state the current state
		 * @param alignment the calibration matrix from imu to body frame
		 * @param prior an arbitrary prior that can be passed to the function
		 * @return the estimated orientation at state
		 */
		struct orientation_measurement {
			static inline Eigen::Matrix<double,3,3> stiffnes;
			static constexpr inline int output_size=3;
			template<typename T>
			inline Eigen::Matrix<T, 9, 1> operator()(const OUTER_T<T> &state,const ALIGNMENT_TYPE<T>& alignment, void * prior) const {
				return state.template block<9, 1>(orientation_start, 0);
			}
			template<typename T,typename T2>
			static auto boxminus(const Eigen::Matrix<T,9,1> &a,const Eigen::Matrix<T2,9,1> &b) {
				return zavi::eigen_util::boxMinusOrientation<T,T2>(Eigen::Map<const Eigen::Matrix<T,3,3> >(a.data()),Eigen::Map<const Eigen::Matrix<T2,3,3> >(b.data()));
			}
		};

		/**
		 * Function to integrate an acceleration measurement
		 * @param state the current state
		 * @param prior unused
		 * @return the estimated orientation change
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> orientation_change_measurement(
				const OUTER_T<T> & state,const Eigen::Matrix<T,4,4> & alignment, void * prior) {
			return alignment.template block<3,3>(0,0)*getOrientationChange(state)+state.template block<3,1>(21,0)+state.template block<3,1>(24,0);
		}

		/**
		 * get the orientation in state
		 * @param state the state
		 * @return the orientation part as 3x3 matrix
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 3> getOrientation(const OUTER_T<T> &state) {
			return Eigen::Map<const Eigen::Matrix<T, 3, 3> >(state.template block<9,1>(orientation_start, 0).data());
		}

		/**
		 * get the orientation change in state
		 * @param state the state
		 * @return the orientation change part as 3x3 matrix
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> getOrientationChange(const OUTER_T<T> &state) {
			return state.template block<3, 1>(orientation_change_start, 0);
		}
		/**
		 * get the orientation change in state
		 * @param state the state
		 * @return the orientation change part as 3x3 matrix
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> getOrientationChangeDeriv(const OUTER_T<T> &state) {
			return state.template block<3, 1>(orientation_change_deriv_start, 0);
		}
		/**
		 * Turn rotation matrix into 9x1 block
		 * @param rotation the rotation matrix
		 * @return a 9x1 block containing the rotation matrix
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 9, 1> toStateOrientation(const Eigen::Matrix<T, 3, 3> &rotation) {
			return Eigen::Map<Eigen::Matrix<T, 9, 1> >(rotation.data());
		}
		/**
		 * returns the Position
		 */
		template<typename T>
		static Eigen::Matrix<T, 3, 1> getPosition(const OUTER_T<T> &state) {
			return state.template block<3, 1>(position_start, 0);
		}
		/**
		 * returns the Velocity
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> getVelocity(const OUTER_T<T> &state) {
			return state.template block<3, 1>(velocity_start, 0);
		}
		/**
		 * returns the Acceleration
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 1> getAcceleration(const OUTER_T<T> &state) {
			return state.template block<3, 1>(acceleration_start, 0);
		}
		/**
		 * returns the OrientationCov
		 */
		template<typename T>
		static inline Eigen::Matrix<T, 3, 3> getVelocityCov(const Eigen::Matrix<T,inner_size,inner_size> &cov) {
			return cov.template block<3, 3>(velocity_cov_start,3);
		}
	};
}
//zavi::estimator::model

#endif /* ESTIMATORS_MODELS_XSENSMODELWITHBIAS_HPP_ */


//
// Copyright 2018 Mateusz Loskot <mateusz at loskot dot net>
//
// 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
//
#include <boost/gil/concepts.hpp>
#include <boost/gil/point.hpp>

#include <type_traits>

namespace gil = boost::gil;

template <typename Point>
void test_members()
{
    static_assert(Point::num_dimensions == 2U, "point is not 2D");

    using value_t = typename Point::value_type;
    using coord_t = typename Point::template axis<0>::coord_t;
    static_assert(std::is_same<value_t, coord_t>::value,
                  "point and axis type mismatch");
}

int main()
{
    boost::function_requires<gil::PointNDConcept<gil::point<int>>>();
    boost::function_requires<gil::PointNDConcept<gil::point_t>>();

    boost::function_requires<gil::Point2DConcept<gil::point<int>>>();
    boost::function_requires<gil::Point2DConcept<gil::point_t>>();

    test_members<gil::point<int>>();
    test_members<gil::point_t>();

    // NOTE: point2 is deprecated, available for backward compatibility
    boost::function_requires<gil::PointNDConcept<gil::point2<int>>>();
    boost::function_requires<gil::Point2DConcept<gil::point2<int>>>();
    test_members<gil::point2<int>>();

    return 0;
}


// Copyright Louis Dionne 2013-2017
// 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 <boost/hana/assert.hpp>
#include <boost/hana/core/make.hpp>
#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/unpack.hpp>

#include <laws/base.hpp>

#include <type_traits>
namespace hana = boost::hana;

int main() {
    hana::test::_injection<0> f{};

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<0>), f),
        f()
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<1>), f),
        f(hana::int_c<0>)
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<2>), f),
        f(hana::int_c<0>, hana::int_c<1>)
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<3>), f),
        f(hana::int_c<0>, hana::int_c<1>, hana::int_c<2>)
    ));

    // Previously, we would only unpack with `std::size_t`s. Make
    // sure this does not happen.
    hana::unpack(hana::make_range(hana::int_c<0>, hana::int_c<1>), [](auto x) {
        using T = hana::tag_of_t<decltype(x)>;
        static_assert(std::is_same<typename T::value_type, int>{}, "");
    });
}


// Boost.Polygon library polygon_point_test.cpp file

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

// See http://www.boost.org for updates, documentation, and revision history.

#define BOOST_TEST_MODULE POLYGON_POINT_TEST
#include <boost/mpl/list.hpp>
#include <boost/test/test_case_template.hpp>

#include "boost/polygon/point_concept.hpp"
#include "boost/polygon/point_data.hpp"
#include "boost/polygon/point_traits.hpp"
using namespace boost::polygon;

typedef boost::mpl::list<int> test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(point_data_test, T, test_types) {
  typedef point_data<T> point_type;

  point_type point1(1, 2);
  point_type point2;
  point2 = point1;
  BOOST_CHECK_EQUAL(point1.x(), 1);
  BOOST_CHECK_EQUAL(point1.y(), 2);
  BOOST_CHECK_EQUAL(point2.x(), 1);
  BOOST_CHECK_EQUAL(point2.y(), 2);
  BOOST_CHECK(point1 == point2);
  BOOST_CHECK(!(point1 != point2));
  BOOST_CHECK(!(point1 < point2));
  BOOST_CHECK(!(point1 > point2));
  BOOST_CHECK(point1 <= point2);
  BOOST_CHECK(point1 >= point2);

  point2.x(2);
  point2.y(1);
  BOOST_CHECK_EQUAL(point2.x(), 2);
  BOOST_CHECK_EQUAL(point2.y(), 1);
  BOOST_CHECK(!(point1 == point2));
  BOOST_CHECK(point1 != point2);
  BOOST_CHECK(point1 < point2);
  BOOST_CHECK(!(point1 > point2));
  BOOST_CHECK(point1 <= point2);
  BOOST_CHECK(!(point1 >= point2));

  point2.set(HORIZONTAL, 1);
  point2.set(VERTICAL, 2);
  BOOST_CHECK(point1 == point2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(point_traits_test, T, test_types) {
  typedef point_data<T> point_type;

  point_type point = point_mutable_traits<point_type>::construct(1, 2);
  BOOST_CHECK_EQUAL(point_traits<point_type>::get(point, HORIZONTAL), 1);
  BOOST_CHECK_EQUAL(point_traits<point_type>::get(point, VERTICAL), 2);

  point_mutable_traits<point_type>::set(point, HORIZONTAL, 3);
  point_mutable_traits<point_type>::set(point, VERTICAL, 4);
  BOOST_CHECK_EQUAL(point_traits<point_type>::get(point, HORIZONTAL), 3);
  BOOST_CHECK_EQUAL(point_traits<point_type>::get(point, VERTICAL), 4);
}

template <typename T>
struct Point {
  T x;
  T y;
};

namespace boost {
namespace polygon {
  template <typename T>
  struct geometry_concept< Point<T> > {
    typedef point_concept type;
  };

  template <typename T>
  struct point_traits< Point<T> > {
    typedef T coordinate_type;

    static coordinate_type get(const Point<T>& point, orientation_2d orient) {
      return (orient == HORIZONTAL) ? point.x : point.y;
    }
  };

  template <typename T>
  struct point_mutable_traits< Point<T> > {
    typedef T coordinate_type;

    static void set(Point<T>& point, orientation_2d orient, T value) {
      (orient == HORIZONTAL) ? point.x = value : point.y = value;
    }

    static Point<T> construct(coordinate_type x, coordinate_type y) {
      Point<T> point;
      point.x = x;
      point.y = y;
      return point;
    }
  };
}  // polygon
}  // boost

BOOST_AUTO_TEST_CASE_TEMPLATE(point_concept_test1, T, test_types) {
  typedef Point<T> point_type;

  point_type point1 = construct<point_type>(1, 2);
  BOOST_CHECK_EQUAL(point1.x, 1);
  BOOST_CHECK_EQUAL(point1.y, 2);

  set(point1, HORIZONTAL, 3);
  set(point1, VERTICAL, 4);
  BOOST_CHECK_EQUAL(get(point1, HORIZONTAL), 3);
  BOOST_CHECK_EQUAL(get(point1, VERTICAL), 4);

  point_type point2;
  assign(point2, point1);
  BOOST_CHECK(equivalence(point1, point2));

  x(point2, 1);
  y(point2, 2);
  BOOST_CHECK_EQUAL(x(point2), 1);
  BOOST_CHECK_EQUAL(y(point2), 2);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(point_concept_test2, T, test_types) {
  typedef Point<T> point_type;

  point_type point1 = construct<point_type>(1, 2);
  point_type point2 = construct<point_type>(5, 5);
  BOOST_CHECK_EQUAL(euclidean_distance(point1, point2, HORIZONTAL), 4);
  BOOST_CHECK_EQUAL(euclidean_distance(point1, point2, VERTICAL), 3);
  BOOST_CHECK_EQUAL(manhattan_distance(point1, point2), 7);
  BOOST_CHECK_EQUAL(euclidean_distance(point1, point2), 5.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(point_concept_test3, T, test_types) {
  typedef Point<T> point_type;

  point_type point = construct<point_type>(1, 2);
  point_type shift = construct<point_type>(4, 3);
  convolve(point, shift);
  BOOST_CHECK_EQUAL(x(point), 5);
  BOOST_CHECK_EQUAL(y(point), 5);

  deconvolve(point, shift);
  BOOST_CHECK_EQUAL(x(point), 1);
  BOOST_CHECK_EQUAL(y(point), 2);

  scale_up(point, 5);
  BOOST_CHECK_EQUAL(x(point), 5);
  BOOST_CHECK_EQUAL(y(point), 10);

  scale_down(point, 5);
  BOOST_CHECK_EQUAL(x(point), 1);
  BOOST_CHECK_EQUAL(y(point), 2);

  move(point, HORIZONTAL, 2);
  move(point, VERTICAL, 3);
  BOOST_CHECK_EQUAL(x(point), 3);
  BOOST_CHECK_EQUAL(y(point), 5);
}

template<typename T>
struct Transformer {
  void scale(T& x, T& y) const {
    x *= 2;
    y *= 2;
  }

  void transform(T& x, T& y) const {
    T tmp = x;
    x = y;
    y = tmp;
  }
};

BOOST_AUTO_TEST_CASE_TEMPLATE(point_concept_test4, T, test_types) {
  typedef Point<T> point_type;

  point_type point = construct<point_type>(1, 2);
  scale(point, Transformer<T>());
  BOOST_CHECK_EQUAL(x(point), 2);
  BOOST_CHECK_EQUAL(y(point), 4);

  transform(point, Transformer<T>());
  BOOST_CHECK_EQUAL(x(point), 4);
  BOOST_CHECK_EQUAL(y(point), 2);
}


#include "adm/elements/time.hpp"
#include <boost/format.hpp>
#include <iomanip>
#include <regex>
#include <sstream>

namespace adm {

  std::chrono::nanoseconds parseTimecode(const std::string& timecode) {
    const static std::regex commonFormat("(\\d{2}):(\\d{2}):(\\d{2}).(\\d{1,9})");
    std::smatch timecodeMatch;
    if (std::regex_match(timecode, timecodeMatch, commonFormat)) {
      // add trailing zeros to string
      auto nanoseconds =
          boost::str(boost::format("%1%") %
                     boost::io::group(std::setw(9), std::left,
                                      std::setfill('0'), timecodeMatch[4]));
      return std::chrono::hours(stoi(timecodeMatch[1])) +
             std::chrono::minutes(stoi(timecodeMatch[2])) +
             std::chrono::seconds(stoi(timecodeMatch[3])) +
             std::chrono::nanoseconds(stoi(nanoseconds));
    } else {
      std::stringstream errorString;
      errorString << "invalid timecode: " << timecode;
      throw std::runtime_error(errorString.str());
    }
  }

  std::string formatTimecode(const std::chrono::nanoseconds& time) {
    std::stringstream ss;
    ss << std::setw(2) << std::setfill('0')
       << std::chrono::duration_cast<std::chrono::hours>(time).count();
    ss << ":";
    ss << std::setw(2) << std::setfill('0')
       << std::chrono::duration_cast<std::chrono::minutes>(time).count() % 60;
    ss << ":";
    ss << std::setw(2) << std::setfill('0')
       << std::chrono::duration_cast<std::chrono::seconds>(time).count() % 60;
    ss << ".";
    ss << std::setw(9) << std::setfill('0') << time.count() % 1000000000;
    return ss.str();
  }

}  // namespace adm


//
// The MIT License(MIT)
//
// Copyright(c) 2014 Demonsaw LLC
//
// 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.

#ifdef _MSC_VER
#pragma warning(disable: 4996)
#endif

#include <cstring>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <cryptopp/filters.h>
#include <cryptopp/osrng.h>
#include <cryptopp/secblock.h>

#include "hash.h"
#include "block_cipher.h"
#include "hmac.h"
#include "pbkdf.h"
#include "security.h"

namespace eja
{
	// Cipher
	block_cipher::ptr security::get_cipher(const cipher_type type)
	{
		switch (type)
		{
			case cipher_type::aes:		return aes::create();
			case cipher_type::mars:		return mars::create();
			case cipher_type::rc6:		return rc6::create();
			case cipher_type::serpent:	return serpent::create();
			case cipher_type::twofish:	return twofish::create();
		}

		return nullptr;
	}

	block_cipher::ptr security::get_cipher(const char* name)
	{
		if (boost::iequals(name, "aes md5"))
			return aes::create();
		else if (boost::iequals(name, "mars"))
			return mars::create();
		else if (boost::iequals(name, "rc6"))
			return rc6::create();
		else if (boost::iequals(name, "serpent"))
			return serpent::create();
		else if (boost::iequals(name, "twofish"))
			return twofish::create();

		return nullptr;
	}

	const char* security::get_cipher_name(const cipher_type type)
	{
		switch (type)
		{
			case cipher_type::aes:		return "AES";
			case cipher_type::mars:		return "MARS";
			case cipher_type::rc6:		return "RC6";
			case cipher_type::serpent:	return "Serpent";
			case cipher_type::twofish:	return "Twofish";
			default:					return "";
		}
	}

	// Hash
	hash::ptr security::get_hash(const hash_type type)
	{
		switch (type)
		{
			case hash_type::md5:		return md5::create();
			case hash_type::sha1:		return sha1::create();
			case hash_type::sha224:		return sha224::create();
			case hash_type::sha256:		return sha256::create();
			case hash_type::sha384:		return sha384::create();
			case hash_type::sha512:		return sha512::create();
			case hash_type::sha3_224:	return sha3_224::create();
			case hash_type::sha3_256:	return sha3_256::create();
			case hash_type::sha3_384:	return sha3_384::create();
			case hash_type::sha3_512:	return sha3_512::create();
		}

		return nullptr;
	}

	hash::ptr security::get_hash(const char* name)
	{
		if (boost::iequals(name, "md5"))
			return md5::create();
		else if (boost::iequals(name, "sha1"))
			return sha1::create();
		else if (boost::iequals(name, "sha224"))
			return sha224::create();
		else if (boost::iequals(name, "sha256"))
			return sha256::create();
		else if (boost::iequals(name, "sha384"))
			return sha384::create();
		else if (boost::iequals(name, "sha512"))
			return sha512::create();
		else if (boost::iequals(name, "sha3 224"))
			return sha3_224::create();
		else if (boost::iequals(name, "sha3 256"))
			return sha3_256::create();
		else if (boost::iequals(name, "sha3 384"))
			return sha3_384::create();
		else if (boost::iequals(name, "sha3 512"))
			return sha3_512::create();

		return nullptr;
	}

	const char* security::get_hash_name(const hash_type type)
	{
		switch (type)
		{
			case hash_type::md5:		return "MD5";
			case hash_type::sha1:		return "SHA1";
			case hash_type::sha224:		return "SHA224";
			case hash_type::sha256:		return "SHA256";
			case hash_type::sha384:		return "SHA384";
			case hash_type::sha512:		return "SHA512";
			case hash_type::sha3_224:	return "SHA3 224";
			case hash_type::sha3_256:	return "SHA3 256";
			case hash_type::sha3_384:	return "SHA3 384";
			case hash_type::sha3_512:	return "SHA3 512";
			default:					return "";
		}
	}

	// HMAC
	hmac::ptr security::get_hmac(const hmac_type type)
	{
		switch (type)
		{
			case hmac_type::md5:		return hmac_md5::create();
			case hmac_type::sha1:		return hmac_sha1::create();
			case hmac_type::sha224:		return hmac_sha224::create();
			case hmac_type::sha256:		return hmac_sha256::create();
			case hmac_type::sha384:		return hmac_sha384::create();
			case hmac_type::sha512:		return hmac_sha512::create();
		}

		return nullptr;
	}

	hmac::ptr security::get_hmac(const char* name)
	{
		if (boost::iequals(name, "hmac md5"))
			return hmac_md5::create();
		else if (boost::iequals(name, "hmac sha1"))
			return hmac_sha1::create();
		else if (boost::iequals(name, "hmac sha224"))
			return hmac_sha224::create();
		else if (boost::iequals(name, "hmac sha256"))
			return hmac_sha256::create();
		else if (boost::iequals(name, "hmac sha384"))
			return hmac_sha384::create();
		else if (boost::iequals(name, "hmac sha512"))
			return hmac_sha512::create();

		return nullptr;
	}

	const char* security::get_hmac_name(const hmac_type type)
	{
		switch (type)
		{
			case hmac_type::md5:		return "HMAC MD5";
			case hmac_type::sha1:		return "HMAC SHA1";
			case hmac_type::sha224:		return "HMAC SHA224";
			case hmac_type::sha256:		return "HMAC SHA256";
			case hmac_type::sha384:		return "HMAC SHA384";
			case hmac_type::sha512:		return "HMAC SHA512";
			default:					return "";
		}
	}

	// PBKDF
	pbkdf::ptr security::get_pbkdf(const pbkdf_type type)
	{
		switch (type)
		{
			case pbkdf_type::md5:		return pbkdf2_hmac_md5::create();
			case pbkdf_type::sha1:		return pbkdf2_hmac_sha1::create();
			case pbkdf_type::sha224:	return pbkdf2_hmac_sha224::create();
			case pbkdf_type::sha256:	return pbkdf2_hmac_sha256::create();
			case pbkdf_type::sha384:	return pbkdf2_hmac_sha384::create();
			case pbkdf_type::sha512:	return pbkdf2_hmac_sha512::create();
		}

		return nullptr;
	}

	pbkdf::ptr security::get_pbkdf(const char* name)
	{
		if (boost::iequals(name, "pbkdf2_hmac_md5"))
			return pbkdf2_hmac_md5::create();
		else if (boost::iequals(name, "pbkdf2_hmac_sha1"))
			return pbkdf2_hmac_sha1::create();
		else if (boost::iequals(name, "pbkdf2_hmac_sha224"))
			return pbkdf2_hmac_sha224::create();
		else if (boost::iequals(name, "pbkdf2_hmac_sha256"))
			return pbkdf2_hmac_sha256::create();
		else if (boost::iequals(name, "pbkdf2_hmac_sha384"))
			return pbkdf2_hmac_sha384::create();
		else if (boost::iequals(name, "pbkdf2_hmac_sha512"))
			return pbkdf2_hmac_sha512::create();

		return nullptr;
	}

	const char* security::get_pbkdf_name(const pbkdf_type type)
	{
		switch (type)
		{
			case pbkdf_type::md5:		return "PBKDF HMAC MD5";
			case pbkdf_type::sha1:		return "PBKDF HMAC SHA1";
			case pbkdf_type::sha224:	return "PBKDF HMAC SHA224";
			case pbkdf_type::sha256:	return "PBKDF HMAC SHA256";
			case pbkdf_type::sha384:	return "PBKDF HMAC SHA384";
			case pbkdf_type::sha512:	return "PBKDF HMAC SHA512";
			default:					return "";
		}
	}

	// Utility
	std::string security::str(const byte* input, const size_t input_size)
	{
		std::string output;
		CryptoPP::StringSink sink(output);
		sink.Put(input, input_size);
		return output;
	}

	std::string security::str(const CryptoPP::SecByteBlock& input)
	{
		std::string output;
		CryptoPP::StringSink sink(output);
		sink.Put(input, input.size());
		return output;
	}

	std::string security::str(const CryptoPP::Integer& input)
	{
		const auto input_size = input.MinEncodedSize();
		CryptoPP::SecByteBlock block(input_size);
		input.Encode(block.BytePtr(), input_size);

		return str(block);

		// NOTE: CryptoPP::Integer has a weird ostream format (uses prefixes to specify numeric base)
		//
		/*std::ostringstream oss;
		oss << std::hex << input;
		return oss.str();*/
	}
}


#define BOOST_TEST_MODULE "test_excluded_volume_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>
#include <mjolnir/forcefield/3SPN2/ThreeSPN2ExcludedVolumePotential.hpp>

BOOST_AUTO_TEST_CASE(EXV_double)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_excluded_volume_potential.log");
    using real_type = double;
    using potential_type = mjolnir::ThreeSPN2ExcludedVolumePotential<real_type>;
    using parameter_type = potential_type::parameter_type;
    using traits_type    = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using boundary_type  = typename traits_type::boundary_type;
    using system_type    = mjolnir::System<traits_type>;

    {
        // to make the value realistic, we need to modify the unit system.
        using phys_type = mjolnir::physics::constants<real_type>;
        using unit_type = mjolnir::unit::constants<real_type>;

        phys_type::set_energy_unit("kcal/mol");
        phys_type::set_length_unit("angstrom");

        phys_type::set_kB(unit_type::boltzmann_constant() *
                          1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant());
        phys_type::set_NA(unit_type::avogadro_constant());
        phys_type::set_eps0((unit_type::vacuum_permittivity() /
            unit_type::elementary_charge()) / unit_type::elementary_charge() *
            (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
            (1.0 / unit_type::m_to_angstrom()));

        phys_type::set_m_to_length(1e-10);
        phys_type::set_length_to_m(1e+10);
        phys_type::set_L_to_volume(1e+27);
        phys_type::set_volume_to_L(1e-27);
    }

    constexpr std::size_t N = 10000;
    constexpr real_type   h = 1e-6;
    constexpr real_type tol = 1e-6;

    const real_type sigma   = 3.0;

    potential_type potential;

    system_type sys(1, boundary_type{});
    potential.initialize(sys);

    const real_type x_min = 0.8 * sigma;
    const real_type x_max = potential.cutoff_ratio() * sigma;

    mjolnir::test::check_potential(potential, parameter_type{sigma},
                                   x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(EXV_float)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_excluded_volume_potential.log");
    using real_type = float;
    using potential_type = mjolnir::ThreeSPN2ExcludedVolumePotential<real_type>;
    using parameter_type = potential_type::parameter_type;
    using traits_type    = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using boundary_type  = typename traits_type::boundary_type;
    using system_type    = mjolnir::System<traits_type>;

    {
        // to make the value realistic, we need to modify the unit system.
        using phys_type = mjolnir::physics::constants<real_type>;
        using unit_type = mjolnir::unit::constants<real_type>;

        phys_type::set_energy_unit("kcal/mol");
        phys_type::set_length_unit("angstrom");

        phys_type::set_kB(unit_type::boltzmann_constant() *
                          1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant());
        phys_type::set_NA(unit_type::avogadro_constant());
        phys_type::set_eps0((unit_type::vacuum_permittivity() /
            unit_type::elementary_charge()) / unit_type::elementary_charge() *
            (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
            (1.0 / unit_type::m_to_angstrom()));

        phys_type::set_m_to_length(1e-10);
        phys_type::set_length_to_m(1e+10);
        phys_type::set_L_to_volume(1e+27);
        phys_type::set_volume_to_L(1e-27);
    }

    constexpr static std::size_t N = 200;
    constexpr static real_type   h = 0.002f;
    constexpr static real_type tol = 0.005f;

    const real_type sigma   = 3.0f;

    potential_type potential;

    system_type sys(1, boundary_type{});
    potential.initialize(sys);

    const real_type x_min = 0.8f * sigma;
    const real_type x_max = potential.cutoff_ratio() * sigma;

    mjolnir::test::check_potential(potential, parameter_type{sigma},
                                   x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(EXV_cutoff_length)
{
    mjolnir::LoggerManager::set_default_logger("test_3spn2_excluded_volume_potential.log");
    using real_type = double;
    using traits_type = mjolnir::SimulatorTraits<real_type, mjolnir::UnlimitedBoundary>;
    using boundary_type  = typename traits_type::boundary_type;
    using system_type    = mjolnir::System<traits_type>;

    using potential_type = mjolnir::ThreeSPN2ExcludedVolumePotential<real_type>;
    using parameter_list_type  = mjolnir::ThreeSPN2ExcludedVolumeParameterList<traits_type>;
    using parameter_type       = parameter_list_type::parameter_type;
    using ignore_molecule_type = parameter_list_type::ignore_molecule_type;
    using ignore_group_type    = parameter_list_type::ignore_group_type;

    {
        // to make the value realistic, we need to modify the unit system.
        using phys_type = mjolnir::physics::constants<real_type>;
        using unit_type = mjolnir::unit::constants<real_type>;

        phys_type::set_energy_unit("kcal/mol");
        phys_type::set_length_unit("angstrom");

        phys_type::set_kB(unit_type::boltzmann_constant() *
                          1e-3 * unit_type::J_to_cal() * unit_type::avogadro_constant());
        phys_type::set_NA(unit_type::avogadro_constant());
        phys_type::set_eps0((unit_type::vacuum_permittivity() /
            unit_type::elementary_charge()) / unit_type::elementary_charge() *
            (1e+3 / unit_type::J_to_cal() / unit_type::avogadro_constant()) *
            (1.0 / unit_type::m_to_angstrom()));

        phys_type::set_m_to_length(1e-10);
        phys_type::set_length_to_m(1e+10);
        phys_type::set_L_to_volume(1e+27);
        phys_type::set_volume_to_L(1e-27);
    }

    potential_type potential;
    system_type sys(1, boundary_type{});
    potential.initialize(sys);

    // generate a parameter
    std::vector<std::pair<std::size_t, parameter_type>> parameters(10);
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        parameters.at(i).first = i;
    }
    parameters.at(0).second = mjolnir::parameter_3SPN2::bead_kind::BaseA;
    parameters.at(1).second = mjolnir::parameter_3SPN2::bead_kind::BaseT;
    parameters.at(2).second = mjolnir::parameter_3SPN2::bead_kind::BaseC;
    parameters.at(3).second = mjolnir::parameter_3SPN2::bead_kind::BaseG;
    parameters.at(4).second = mjolnir::parameter_3SPN2::bead_kind::BaseA;
    parameters.at(5).second = mjolnir::parameter_3SPN2::bead_kind::BaseT;
    parameters.at(6).second = mjolnir::parameter_3SPN2::bead_kind::BaseC;
    parameters.at(7).second = mjolnir::parameter_3SPN2::bead_kind::BaseG;
    parameters.at(8).second = mjolnir::parameter_3SPN2::bead_kind::Phosphate;
    parameters.at(9).second = mjolnir::parameter_3SPN2::bead_kind::Sugar;

    parameter_list_type parameter_list(
        mjolnir::ThreeSPN2ExcludedVolumePotentialParameter<real_type>{},
        parameters, {}, ignore_molecule_type{"Nothing"}, ignore_group_type({}));

    // secure maximum cutoff calculated from per-pair parameter
    // This requires O(N^2), so it will not calculated in usual cases
    real_type max_abs_cutoff = 0.0;
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            max_abs_cutoff = std::max(max_abs_cutoff, potential.absolute_cutoff(parameter_list.prepare_params(i, j)));
        }
    }

    const real_type safety = 1.0 + std::numeric_limits<real_type>::epsilon();
    for(std::size_t i=0; i<parameters.size(); ++i)
    {
        for(std::size_t j=0; j<parameters.size(); ++j)
        {
            BOOST_TEST(potential.potential(max_abs_cutoff * safety, parameter_list.prepare_params(i, j)) == 0.0);
        }
    }
}


//  Copyright John Maddock 2006.
//  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)

#define BOOST_MATH_ASSERT_UNDEFINED_POLICY false

#include <boost/math/distributions.hpp>
#include <boost/math/concepts/distributions.hpp>

template <class RealType>
void instantiate(RealType)
{
   using namespace boost;
   using namespace boost::math;
   using namespace boost::math::concepts;

   typedef policies::policy<policies::digits2<std::numeric_limits<RealType>::digits - 2> > custom_policy;

   function_requires<DistributionConcept<bernoulli_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<beta_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<binomial_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<cauchy_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<chi_squared_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<exponential_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<extreme_value_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<fisher_f_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<gamma_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<geometric_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<hypergeometric_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<hypergeometric_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<inverse_chi_squared_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<inverse_gamma_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<inverse_gaussian_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<laplace_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<logistic_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<lognormal_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<negative_binomial_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<non_central_beta_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<non_central_chi_squared_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<non_central_f_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<non_central_t_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<normal_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<pareto_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<poisson_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<rayleigh_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<skew_normal_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<students_t_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<triangular_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<uniform_distribution<RealType, custom_policy> > >();
   function_requires<DistributionConcept<weibull_distribution<RealType, custom_policy> > >();
}

int main()
{
   instantiate(float(0));
   instantiate(double(0));
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   instantiate((long double)(0));
#endif
}


/*
Copyright (c) 2020 Naomasa Matsubayashi

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 <cmath>
#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <random>
#include <boost/program_options.hpp>
#include <sndfile.h>
#include "ifm/fft.h"
#include "ifm/load_monoral.h"
#include "ifm/spectrum_image.h"
#include "ifm/store_monoral.h"

int main( int argc, char* argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(),  "入力ファイル")
    ("output,o", boost::program_options::value<std::string>(),  "出力ファイル")
    ("note,n", boost::program_options::value<int>()->default_value(60),  "音階")
    ("resolution,r", boost::program_options::value<int>()->default_value(13),  "分解能");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") || !params.count("output") ) {
    std::cout << options << std::endl;
    return 0;
  }
  const std::string input_filename = params["input"].as<std::string>();
  const std::string output_filename = params["output"].as<std::string>();
  const auto [audio,sample_rate] = ifm::load_monoral( input_filename, true );
  const auto note = params["note"].as<int>();
  ifm::spectrum_image conv( note, sample_rate, 1 << params["resolution"].as<int>() );
  auto [image,delay] = conv( audio );
  std::cout << "delay: " << delay << std::endl;
  unsigned int width = conv.get_width();
  unsigned int active_width;
  std::vector< float > output_wave;
  float resolution = 1 << params["resolution"].as<int>();
  active_width = width / 24;
  double time = 0;
  unsigned int spb = sample_rate / 100;
  output_wave.resize( spb * ( image.size() / width - 1 ), 0.f );
  const float transformed_freq = float( 24 ) / resolution * sample_rate;
  const float original_freq = std::exp2( ( ( float( note ) +  3.f ) / 12.f ) ) * 6.875f;
  const float ratio = original_freq / transformed_freq;
  std::cout << "ratio : " << original_freq << "/" << transformed_freq << std::endl;
  std::mt19937 rand_gen( 0 );
  std::uniform_real_distribution rand_dist( -1.0, 1.0 );
  for( unsigned int y = 0; y != image.size() / width - 1; ++y, time += float( spb ) / sample_rate ) {
    const auto begin = std::next( output_wave.begin(), y * spb );
    for( unsigned int x = 0; x != active_width; ++x ) {
      const auto n = ( x + 1 ) * 24;
      const auto begin_level = image[ n + y * width ];
      const auto end_level = image[ n + ( y + 1 ) * width ];
      auto iter = begin;
      auto delta = 0.f;
      for( unsigned int s = 0; s != spb; ++s, delta += 1.f / sample_rate, ++iter ) {
        float pos = s / float( spb );
        float level = begin_level * ( 1.f - pos ) + end_level * pos;
        const auto freq = float( sample_rate ) * n / resolution;
        *iter += level * std::sin( freq * ratio * 2.0 * M_PI * ( time + delta ) );
      }
    }
    {
      float begin_level = 0;
      float end_level = 0;
      for( unsigned int x = 0; x != width; ++x ) {
        if( x % 24 ) {
          begin_level += image[ x + y * width ];
          end_level += image[ x + ( y + 1 ) * width ];
        }
      }
      /*auto iter = begin;
      auto delta = 0.f;
      for( unsigned int s = 0; s != spb; ++s, delta += 1.f / sample_rate, ++iter ) {
        float pos = s / float( spb );
        float level = begin_level * ( 1.f - pos ) + end_level * pos;
        float freq = original_freq;
        *iter += level * level * std::sin( 2.0 * M_PI * ( freq * ( time + delta ) + std::sin( 2.0 * M_PI * freq * ( time + delta ) ) ) );
      }*/
    }
  }
  ifm::store_monoral( output_filename, output_wave, sample_rate );
}


/*
    Copyright (C) 2008 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <cmath>
#include <boost/functional.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/tools/roots.hpp>

#include <idmlib/lshkit/common.h>
#include <idmlib/lshkit/mplsh.h>

#define ABS_ERROR   1e-5l
#define REL_ERROR   1e-5l
#define MAX_SHAPE   1000.0l
#define LIMIT       40000

namespace lshkit {

static GaussianDouble normal;

/* Maximum Likelihood Estimation of gamma distribution */
class GammaDoubleMleHelper
{
    double rate_; // G / M
public:
    GammaDoubleMleHelper(double rate): rate_(rate) {}

    double operator () (double k)
    {
        return std::log(k) - boost::math::digamma(k) + std::log(rate_);
    }
};

bool GammaDoubleMleTol (double first, double second)
{
    return (second - first) < ABS_ERROR;
}

GammaDouble GammaDoubleMLE (double M, double G)
{
    GammaDoubleMleHelper hlp(G/M);
    std::pair<double,double> pair = boost::math::tools::bisect(hlp, ABS_ERROR, MAX_SHAPE, GammaDoubleMleTol);
    double k = (pair.first + pair.second) / 2.0;
    return GammaDouble(k, M/k);
}

/* estimate the cost and miss of LSH */
static inline double col_helper (double x)
{
    double result;
    result = 2.0*boost::math::cdf(normal, x) - 1.0;
    result += std::sqrt(2.0/M_PI) * (std::exp(-x*x/2.0)-1.0)/x;
    return result;
}

static inline double p_col_helper (double x, double k)
{
    return boost::math::cdf(normal,(1.0 + k) * x)
        - boost::math::cdf(normal, k*x);
}

double MultiProbeLshModel::recall (double x) const
{
/*
    double x2 = W_ / x;
    double p = col_helper(x2);

    unsigned MT =  __probeSequenceTemplates[M_].size();
    if (MT > T_) MT = T_;

    double result = 0;
    for (unsigned i = 0; i < MT; i++)
    {
        double r = 1.0;
        for (unsigned j = 0; j < M_; j++)
        {
            Probe &probe = __probeSequenceTemplates[M_][i];
            if (probe.mask & leftshift(j))
            {
                double delta = (j + 1.0) / (M_ + 1.0) * 0.5; // expected value
                if (probe.shift & leftshift(j))
                {
                    r *= p_col_helper(x2, 1.0 - delta);
                }
                else
                {
                    r *= p_col_helper(x2, delta);
                }
            }
            else r *= p;
        }
        result += r;
    }
    return 1.0 - std::exp(std::log(1.0 - result) * L_);
*/
    return 0;
}

struct __MpLshMdlHlpr
{
    const MultiProbeLshModel *model;
    const GammaDouble *gamma;
};
/*
static double recall_helper (double xsqr, void *_param)
{
    __MpLshMdlHlpr *param = reinterpret_cast<__MpLshMdlHlpr
        *>(_param);
    return boost::math::pdf(*param->gamma, xsqr) * param->model->recall(std::sqrt(xsqr));
}
*/
static double recall (__MpLshMdlHlpr *param)
{
/*
    static gsl_integration_workspace *workspace = NULL;
    double f, error;
    gsl_function I;
    if (workspace == NULL)
    {
        workspace = gsl_integration_workspace_alloc(LIMIT);
        BOOST_VERIFY(workspace != NULL);
    }
    I.params = param;
    I.function = recall_helper;
    if (gsl_integration_qagiu(&I, 0.0, ABS_ERROR, REL_ERROR, LIMIT, workspace, &f, &error) != 0) f = 1.0;
    return f;
*/
    return 0;
}

double MultiProbeLshDataModel::avgRecall () const
{
    double f = 0.0;
    __MpLshMdlHlpr param;
    param.model = this;
    for (unsigned k = 0; k < topkDists_.size(); k++)
    {
        param.gamma = &topkDists_[k];
        f += lshkit::recall(&param);
    }
    f /= topkDists_.size();
    return f;
}

double MultiProbeLshDataModel::cost () const
{
    __MpLshMdlHlpr param;
    param.model = this;
    param.gamma = &globalDist_;
    return lshkit::recall(&param);
}

}


/* boost random/detail/large_arithmetic.hpp header file
 *
 * Copyright Steven Watanabe 2011
 * 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://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_DETAIL_LARGE_ARITHMETIC_HPP
#define BOOST_RANDOM_DETAIL_LARGE_ARITHMETIC_HPP

#include <boost/cstdint.hpp>
#include <boost/integer.hpp>
#include <boost/limits.hpp>
#include <boost/random/detail/integer_log2.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {
namespace detail {

struct div_t {
    boost::uintmax_t quotient;
    boost::uintmax_t remainder;
};

inline div_t muldivmod(boost::uintmax_t a, boost::uintmax_t b, boost::uintmax_t m)
{
    const int bits =
        ::std::numeric_limits< ::boost::uintmax_t>::digits / 2;
    const ::boost::uintmax_t mask = (::boost::uintmax_t(1) << bits) - 1;
    typedef ::boost::uint_t<bits>::fast digit_t;

    int shift = std::numeric_limits< ::boost::uintmax_t>::digits - 1
        - detail::integer_log2(m);

    a <<= shift;
    m <<= shift;

    digit_t product[4] = { 0, 0, 0, 0 };
    digit_t a_[2] = { digit_t(a & mask), digit_t((a >> bits) & mask) };
    digit_t b_[2] = { digit_t(b & mask), digit_t((b >> bits) & mask) };
    digit_t m_[2] = { digit_t(m & mask), digit_t((m >> bits) & mask) };

    // multiply a * b
    for(int i = 0; i < 2; ++i) {
        digit_t carry = 0;
        for(int j = 0; j < 2; ++j) {
            ::boost::uint64_t temp = ::boost::uintmax_t(a_[i]) * b_[j] +
                carry + product[i + j];
            product[i + j] = digit_t(temp & mask);
            carry = digit_t(temp >> bits);
        }
        if(carry != 0) {
            product[i + 2] += carry;
        }
    }

    digit_t quotient[2];

    if(m == 0) {
        div_t result = {
            ((::boost::uintmax_t(product[3]) << bits) | product[2]),
            ((::boost::uintmax_t(product[1]) << bits) | product[0]) >> shift,
        };
        return result;
    }

    // divide product / m
    for(int i = 3; i >= 2; --i) {
        ::boost::uintmax_t temp =
            ::boost::uintmax_t(product[i]) << bits | product[i - 1];

        digit_t q = digit_t((product[i] == m_[1]) ? mask : temp / m_[1]);

        ::boost::uintmax_t rem =
            ((temp - ::boost::uintmax_t(q) * m_[1]) << bits) + product[i - 2];

        ::boost::uintmax_t diff = m_[0] * ::boost::uintmax_t(q);

        int error = 0;
        if(diff > rem) {
            if(diff - rem > m) {
                error = 2;
            } else {
                error = 1;
            }
        }
        q -= error;
        rem = rem + error * m - diff;

        quotient[i - 2] = q;
        product[i] = 0;
        product[i-1] = static_cast<digit_t>((rem >> bits) & mask);
        product[i-2] = static_cast<digit_t>(rem & mask);
    }

    div_t result = {
        ((::boost::uintmax_t(quotient[1]) << bits) | quotient[0]),
        ((::boost::uintmax_t(product[1]) << bits) | product[0]) >> shift,
    };
    return result;
}

inline boost::uintmax_t muldiv(boost::uintmax_t a, boost::uintmax_t b, boost::uintmax_t m)
{ return detail::muldivmod(a, b, m).quotient; }

inline boost::uintmax_t mulmod(boost::uintmax_t a, boost::uintmax_t b, boost::uintmax_t m)
{ return detail::muldivmod(a, b, m).remainder; }

} // namespace detail
} // namespace random
} // namespace boost

#include <boost/random/detail/enable_warnings.hpp>

#endif // BOOST_RANDOM_DETAIL_LARGE_ARITHMETIC_HPP


// Flatten.cpp : Defines the entry point for the console application.
//

#ifdef WIN32
#define NOMINMAX
#include <windows.h>
#endif

#if defined (__APPLE__) || defined (OSX)
#include <OpenGL/gl.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glut.h>
#endif

#include "GA/c3ga.h"
#include "GA/c3ga_util.h"
#include "GA/gl_util.h"

#include "primitivedraw.h"
#include "gahelper.h"
#include "Laplacian.h"

#include <memory>

#include <vector>
#include <queue>
#include <map>
#include "numerics.h"
#include "HalfEdge/Mesh.h"

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

// #include <ppl.h>

#include "Benchs.h"

const char *WINDOW_TITLE = "Interactive 3D Shape Deformation using Conformal Geometric Algebra";

// GLUT state information
int g_viewportWidth = 800;
int g_viewportHeight = 600;

void display();
void reshape(GLint width, GLint height);
void MouseButton(int button, int state, int x, int y);
void MouseMotion(int x, int y);
void KeyboardUpFunc(unsigned char key, int x, int y);
void SpecialFunc(int key, int x, int y);
void SpecialUpFunc(int key, int x, int y);
void Idle();
void DestroyWindow();

//using namespace boost;
using namespace c3ga;
using namespace std;
using namespace numerics;

class Camera
{
public:
	float		pos[3];
	float		fw[3];
	float		up[3];
	float		translateVel;
	float		rotateVel;

	Camera()
	{
		float		_pos[] = { 0, 0, -2};
		float		_fw[] = { 0, 0, 1 };
		float		_up[] = { 0, 1, 0 };

		translateVel = 0.005;
		rotateVel = 0.005;
		memcpy(pos, _pos, sizeof(float)*3);
		memcpy(fw, _fw, sizeof(float)*3);
		memcpy(up, _up, sizeof(float)*3);
	}

	void glLookAt()
	{
		gluLookAt( pos[0], pos[1], pos[2], fw[0],  fw[1],  fw[2], up[0],  up[1],  up[2] );
	}
};

class VertexBuffer
{
public:
	std::vector<Eigen::Vector3d> deformedPositions; //deformed mesh positions
	std::map<int, Eigen::Vector3d> constrainedPositions; //positional constraints
	std::vector<Eigen::Vector3d> laplacianCoordinates; //laplacian Coordinates
	std::vector<Eigen::Vector3d> normals; //for rendering (lighting)
	std::vector<Eigen::Vector3d> normalsOrig; //original normals
	std::vector<Eigen::Quaterniond> rotors;
	std::vector<Eigen::Matrix4d> JtJ;
	std::vector<Eigen::Vector4d> b;
	int size;

	VertexBuffer() : size(0)
	{
	}

	void resize(int size)
	{
		this->size = size;
		deformedPositions.resize(size);
		laplacianCoordinates.resize(size);
		normals.resize(size);
		normalsOrig.resize(size);
		rotors.resize(size);
		JtJ.resize(size);
		b.resize(size);
	}
	int get_size() { return size; }

};

class IndexBuffer {
public:
	std::vector<int> faces;
	int size;

	IndexBuffer() : size(0)
	{
	}

	void resize(int size)
	{
		this->size = size;
		faces.resize(size);
	}
	int get_size() { return size; }

};

class Handle
{
public:
	rotor R;
	translator T;
	translator Tcenter;
	dualSphere dS;
	std::set<int> constraints;

	Handle() {
		T = _rotor(1.0);
		R = _translator(1.0);
		Tcenter = _translator(1.0);
	}
	Handle(dualSphere dS)
	{
		normalizedPoint x = DualSphereCenter(dS);
		Tcenter = exp(-0.5 * _vectorE3GA(x) * ni);
		T = _translator(1.0);
		R = _rotor(1.0);
		this->dS = dS;
	}

	TRversor GetTRVersor()
	{
		return _TRversor(T * _TRversor(Tcenter * R * inverse(Tcenter)));
	}
};

Camera g_camera;
Mesh mesh, meshLow;
map<int, int> mapping, rotorClusters;
vectorE3GA g_prevMousePos;
bool g_rotateModel = false;
bool g_rotateModelOutOfPlane = false;
rotor g_modelRotor = _rotor(1.0);
bool g_rotateKeyRotors = false;
bool g_translateKeyRotors = false;
bool g_computeBasis = false;
float g_dragDistance = -1.0f;
int g_dragObject;
std::shared_ptr<SparseMatrix> A;
std::shared_ptr<SparseMatrix> AHi;
Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> solverLow, solverHi;
int systemType = LaplaceBeltrami; //MeanValue; //LaplaceBeltrami
bool g_showSpheres = true;
bool g_showWires = false;
bool g_iterateManyTimes = false;
Eigen::MatrixXd b3Low;
Eigen::MatrixXd xyzLow;
Eigen::MatrixXd b3Hi;
Eigen::MatrixXd xyzHi;
double g_meshArea = 0.0;

bool g_automaticAnimation = false;
bool g_convergence = false;
Benchmarks benchs;

VertexBuffer vertexDescriptors;
VertexBuffer vertexDescriptorsLow;
IndexBuffer trianglesHi;
std::vector<Handle> handles;

Eigen::Affine3d MotorToMatrix(const TRversor& R) {
	TRversor Ri = inverse(R);

	// compute images of basis vectors:
	c3ga::flatPoint imageOfE1NI = _flatPoint(R * c3ga::e1ni * Ri);
	c3ga::flatPoint imageOfE2NI = _flatPoint(R * c3ga::e2ni * Ri);
	c3ga::flatPoint imageOfE3NI = _flatPoint(R * c3ga::e3ni * Ri);
	c3ga::flatPoint imageOfNONI = _flatPoint(R * c3ga::noni * Ri);

	// create matrix representation:
	Eigen::Affine3d M;
	M(0, 0) = imageOfE1NI.m_c[0];
	M(1, 0) = imageOfE1NI.m_c[1];
	M(2, 0) = imageOfE1NI.m_c[2];
	M(3, 0) = imageOfE1NI.m_c[3];
	M(0, 1) = imageOfE2NI.m_c[0];
	M(1, 1) = imageOfE2NI.m_c[1];
	M(2, 1) = imageOfE2NI.m_c[2];
	M(3, 1) = imageOfE2NI.m_c[3];
	M(0, 2) = imageOfE3NI.m_c[0];
	M(1, 2) = imageOfE3NI.m_c[1];
	M(2, 2) = imageOfE3NI.m_c[2];
	M(3, 2) = imageOfE3NI.m_c[3];
	M(0, 3) = imageOfNONI.m_c[0];
	M(1, 3) = imageOfNONI.m_c[1];
	M(2, 3) = imageOfNONI.m_c[2];
	M(3, 3) = imageOfNONI.m_c[3];
	return M;
}

void ComputeLaplacianCoordinates(std::shared_ptr<SparseMatrix> A, Mesh* mesh, std::vector<Eigen::Vector3d>& laplacianCoordinates)
{
	std::fill(laplacianCoordinates.begin(), laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	auto numRows = A->numRows();

	for (int i = 0; i < numRows; ++i)
	{
		SparseMatrix::RowIterator aIter = A->iterator(i);
		for (; !aIter.end(); ++aIter)
		{
			auto j = aIter.columnIndex();
			laplacianCoordinates[i] += mesh->vertexAt(j).p * aIter.value();
		}
	}
}

bool is_constrained(std::set<int>& constraints, int vertex)
{
	return constraints.find(vertex) != constraints.end();
}

void PreFactor(std::shared_ptr<SparseMatrix> A, std::set<int>& constraints, Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>& solver)
{

	Eigen::SparseMatrix<double> Lc = Eigen::SparseMatrix<double>(A->numRows(), A->numColumns());

	auto numRows = A->numRows();
	for (int i = 0; i < numRows; ++i)
	{
		if (!is_constrained(constraints, i))
		{
			SparseMatrix::RowIterator aIter = A->iterator(i);
			for (; !aIter.end(); ++aIter)
			{
				auto j = aIter.columnIndex();
				Lc.insert(i, j) = (*A)(i, j);
			}
		}
		else
		{
			Lc.insert(i, i) = 1.0;
		}
	}

	Lc.makeCompressed();
	solver.compute(Lc);
	if (solver.info() != Eigen::Success) {
		// TODO: error handling
	}

}

double meshArea(Mesh *mesh)
{
	double area = 0.0;
	for(Face& face :  mesh->getFaces())
	{
		Eigen::Vector3d	u = face.edge->next->vertex->p - face.edge->vertex->p;
		Eigen::Vector3d	v = face.edge->next->next->vertex->p - face.edge->vertex->p;
		area += 0.5 * u.cross(v).norm();
	}
	return area;
}

void meshMapping(Mesh *meshLow, Mesh *mesh, map<int, int>& mapping)
{
	for (Vertex& vertexLow : meshLow->getVertices())
	{
		double minDist = 1e38;
		int mappedPoint = -1;
		Eigen::Vector3d& pLow = vertexLow.p;
		for (Vertex& vertexHi : mesh->getVertices())
		{
			Eigen::Vector3d& p = vertexHi.p;
			double dist = (pLow - p).norm();
			if (dist < minDist) {
				minDist = dist;
				mappedPoint = vertexHi.ID;
			}
		}
		mapping[vertexLow.ID] = mappedPoint;
	}

	vector<bool> visited = vector<bool>(mesh->numVertices(), false);
	queue<int> assigned;

	for (auto&& pair : mapping){
		visited[pair.second] = true;
		rotorClusters[pair.second] = pair.first;
		assigned.push(pair.second);
	}

	while (!assigned.empty())
	{
		int i = assigned.front();
		assigned.pop();
		visited[i] = true;

		for (Vertex::EdgeAroundIterator edgeAroundIter = mesh->vertexAt(i).iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			if (visited[j] == false)
			{
				rotorClusters[j] = rotorClusters[i];
				assigned.push(j);
			}
		}
	}
}

int main(int argc, char* argv[])
{
	mesh.readOBJ("cactus2.obj"); //armadillo-5k-smooth.obj female.obj david.obj rabbit.obj tyra.obj horse.obj cylinder.obj bar.obj planewithpeaks.obj dragon.obj catHead.obj  cactus.obj  bunny.obj  764_hand-olivier-10kf.obj armadillo.obj
	mesh.CenterAndNormalize();
	mesh.computeNormals();

	meshLow.readOBJ("cactus1.obj"); //armadillo-5k-smooth.obj female.obj david.obj rabbit.obj tyra.obj horse.obj cylinder.obj bar.obj planewithpeaks.obj dragon.obj catHead.obj  cactus.obj  bunny.obj  764_hand-olivier-10kf.obj armadillo.obj
	meshLow.CenterAndNormalize();
	meshLow.computeNormals();

	meshMapping(&meshLow, &mesh, mapping);

	// GLUT Window Initialization:
	glutInit (&argc, argv);
	glutInitWindowSize(g_viewportWidth, g_viewportHeight);
	glutInitDisplayMode( GLUT_RGB | GLUT_ALPHA | GLUT_DOUBLE | GLUT_DEPTH);
	glutCreateWindow(WINDOW_TITLE);

	// Register callbacks:
	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutMouseFunc(MouseButton);
	glutMotionFunc(MouseMotion);
	glutKeyboardUpFunc(KeyboardUpFunc);
	glutSpecialFunc(SpecialFunc);
	glutSpecialUpFunc(SpecialUpFunc);
	glutIdleFunc(Idle);
	atexit(DestroyWindow);

	InitializeDrawing();

	//cactus.obj
	handles.push_back(Handle(_dualSphere(c3gaPoint(.0, .04, 0.7) - 0.5*SQR(0.15)*ni)));
	handles.push_back(Handle(_dualSphere(c3gaPoint(.0, .04, -0.8) - 0.5*SQR(0.15)*ni)));

	////cylinder.obj
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, 0.9, .0) - 0.5*SQR(0.25)*ni), false, P1)));
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, -0.9, .0) - 0.5*SQR(0.25)*ni), true, P2)));

	//Armadillo Pie y Mano
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(-.5, 0.45, -.3) - 0.5*SQR(0.15)*ni), false, P1)));
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.2, -0.6, .1) - 0.5*SQR(0.15)*ni), true, P2)));
	//Armadillo Pubis y Cabeza
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, 0.4, -.2) - 0.5*SQR(0.15)*ni), false, P1)));
	//handles.push_back(std::shared_ptr<Handle>(new Handle(_dualSphere(c3gaPoint(.0, -0.05, .1) - 0.5*SQR(0.15)*ni), true, P2)));

	//handles[0]->extrema = 0;
	//handles[1]->extrema = 1;
	//extremas[0]->handle = 0;
	//extremas[1]->handle = 1;

	vertexDescriptorsLow.resize(meshLow.numVertices());
	vertexDescriptors.resize(mesh.numVertices());
	trianglesHi.resize(mesh.numFaces() * 3);

	for( Vertex& vertexLow : meshLow.getVertices())
	{
		vertexDescriptorsLow.rotors[vertexLow.ID] = Eigen::Quaterniond::Identity();
		normalizedPoint position = c3gaPoint(vertexLow.p.x(), vertexLow.p.y(), vertexLow.p.z());

		for (Handle& handle : handles)
		{
			TRversor TR = handle.GetTRVersor();

			if (_double(position << (TR * handle.dS * inverse(TR))) > 0) //inside the sphere
			{
				handle.constraints.insert(vertexLow.ID);
				vertexDescriptorsLow.constrainedPositions[vertexLow.ID] = vertexLow.p;
			}
		}
	}

	A = CreateLaplacianMatrix( &meshLow, systemType );

	ComputeLaplacianCoordinates(A, &meshLow, vertexDescriptorsLow.laplacianCoordinates);

	b3Low = Eigen::MatrixXd(A->numRows(), 3);

	std::set<int> allconstraints;
	for (Handle& handle : handles) {
		allconstraints.insert(handle.constraints.begin(), handle.constraints.end());
	}
	PreFactor(A, allconstraints, solverLow);

	g_meshArea = meshArea(&meshLow);

	for (Vertex& vertexHi : mesh.getVertices()) {
		vertexDescriptors.normalsOrig[vertexHi.ID] = vertexHi.n;
		vertexDescriptors.rotors[vertexHi.ID] = Eigen::Quaterniond::Identity();
	}

	for (Face& face : mesh.getFaces()) {
		int i = face.ID;
		int	v1 = face.edge->vertex->ID;
		int	v2 = face.edge->next->vertex->ID;
		int	v3 = face.edge->next->next->vertex->ID;
		trianglesHi.faces[i * 3 + 0] = v1;
		trianglesHi.faces[i * 3 + 1] = v2;
		trianglesHi.faces[i * 3 + 2] = v3;
	}

	AHi = CreateLaplacianMatrix(&mesh, systemType);
	ComputeLaplacianCoordinates(AHi, &mesh, vertexDescriptors.laplacianCoordinates);
	std::set<int> constraintsHi;
	for (map<int, int>::iterator iter = mapping.begin(); iter != mapping.end(); iter++){
		constraintsHi.insert(iter->second);
	}
	b3Hi = Eigen::MatrixXd(AHi->numRows(), 3);
	PreFactor(AHi, constraintsHi, solverHi);

	glutMainLoop();

	return 0;
}

void transferRotations(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors, VertexBuffer& vertexDescriptorsLow)
{
	for (auto&& pair : rotorClusters) {
		vertexDescriptors.rotors[pair.first] = vertexDescriptorsLow.rotors[pair.second];
	}

	std::fill(vertexDescriptors.laplacianCoordinates.begin(), vertexDescriptors.laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			double wij = (*A)(i, j);
			Eigen::Quaterniond &Ri = vertexDescriptors.rotors[i];
			Eigen::Quaterniond &Rj = vertexDescriptors.rotors[j];
			Eigen::Vector3d V = mesh->vertexAt(j).p - mesh->vertexAt(i).p;
			vertexDescriptors.laplacianCoordinates[i] += 0.5 * wij * (Ri._transformVector(V) + Rj._transformVector(V));;
		}
	}
	//});
}

void SolveLinearSystemTaucs(VertexBuffer& vertexDescriptors)
{
	int n = vertexDescriptors.get_size();

	for (int i = 0; i < n; ++i) {
		b3Low.row(i) = vertexDescriptors.laplacianCoordinates[i];
	}

	for (Handle& handle : handles) {
		Eigen::Affine3d M = MotorToMatrix(handle.GetTRVersor());
		for (int i : handle.constraints) {
			b3Low.row(i) = M * vertexDescriptors.constrainedPositions[i];
		}
	}

	xyzLow = solverLow.solve(b3Low);

	for( int i = 0 ; i < n ; ++i ) {
		vertexDescriptors.deformedPositions[i] = xyzLow.row(i);
	}
}

void SolveLinearSystemTaucsHi(VertexBuffer& vertexDescriptors, VertexBuffer& vertexDescriptorsLow)
{
	int n = vertexDescriptors.get_size();

	for (int i = 0; i < n; ++i) {
		b3Hi.row(i) = vertexDescriptors.laplacianCoordinates[i];
	}

	for (auto&& pair : mapping){
		b3Hi.row(pair.second) = vertexDescriptorsLow.deformedPositions[pair.first];
	}

	xyzHi = solverHi.solve(b3Hi);

	for (int i = 0; i < n; ++i) {
		vertexDescriptors.deformedPositions[i] = xyzHi.row(i);
		vertexDescriptors.normals[i] = vertexDescriptors.rotors[i]._transformVector(vertexDescriptors.normalsOrig[i]);
	}
}

void E3GA_Prep(const vector<Eigen::Vector3d>& P, const vector<Eigen::Vector3d>& Q, double wij2, const int N, Eigen::Matrix4d &JtJ, Eigen::Vector4d &b)
{
	JtJ.setZero();

	double q3p3;
	double q2p2;
	double p1q1;
	double q1p1;
	double p2q2;
	double p3q3;
	double p1q1_2;
	double p2q2_2;
	double p3q3_2;
	double q2p2_2;
	double q1p1_2;
	double q3p3_2;

	for (int i = 0; i < N; ++i)
	{
		const Eigen::Vector3d& Pi = P[i];
		const Eigen::Vector3d& Qi = Q[i];

		q1p1 = Qi.x() + Pi.x();
		q2p2 = Qi.y() + Pi.y();
		q3p3 = Qi.z() + Pi.z();
		p1q1 = Pi.x() - Qi.x();
		p2q2 = Pi.y() - Qi.y();
		p3q3 = Pi.z() - Qi.z();

		p1q1_2 = p1q1 * p1q1;
		p2q2_2 = p2q2 * p2q2;
		p3q3_2 = p3q3 * p3q3;
		q2p2_2 = q2p2 * q2p2;
		q1p1_2 = q1p1 * q1p1;
		q3p3_2 = q3p3 * q3p3;

		JtJ(0, 0) += p1q1_2 + p2q2_2 + p3q3_2;
		JtJ(0, 1) += p1q1 * q2p2 - p2q2 * q1p1;
		JtJ(0, 2) += p1q1 * q3p3 - p3q3 * q1p1;
		JtJ(0, 3) += p2q2 * q3p3 - p3q3 * q2p2;
		JtJ(1, 1) += q2p2_2 + q1p1_2 + p3q3_2;
		JtJ(1, 2) += q2p2 * q3p3 - p3q3 * p2q2;
		JtJ(1, 3) += -q1p1 * q3p3 + p3q3 * p1q1;
		JtJ(2, 2) += q3p3_2 + q1p1_2 + p2q2_2;
		JtJ(2, 3) += q1p1 * q2p2 - p2q2 * p1q1;
		JtJ(3, 3) += q3p3_2 + q2p2_2 + p1q1_2;
	}

	b(0) = -JtJ(0, 0);
	b(1) = -JtJ(0, 1);
	b(2) = -JtJ(0, 2);
	b(3) = -JtJ(0, 3);

	const double nwij2 = N * wij2 + 1e-6;

	JtJ(0, 0) += nwij2;
	JtJ(1, 1) += nwij2;
	JtJ(2, 2) += nwij2;
	JtJ(3, 3) += nwij2;

	JtJ(1, 0) = JtJ(0, 1);
	JtJ(2, 0) = JtJ(0, 2);
	JtJ(2, 1) = JtJ(1, 2);
	JtJ(3, 0) = JtJ(0, 3);
	JtJ(3, 1) = JtJ(1, 3);
	JtJ(3, 2) = JtJ(2, 3);

	JtJ = JtJ.inverse().eval();
}

Eigen::Quaterniond E3GA_Fast5(double wij, const vector<Eigen::Quaterniond>& Rq, const Eigen::Quaterniond& Rx, const int N, const Eigen::Matrix4d &JtJ, const Eigen::Vector4d &b)
{
	Eigen::Vector4d x;
	Eigen::Vector4d b2;
	b2.setZero();
	//const double wij2 = wij;// *wij;

	for (int i = 0; i < N; ++i)
	{
		const Eigen::Quaterniond& Rqi = Rq[i];
		b2(0) += Rqi.w() - 1.0; // 1.0
		b2(1) += -Rqi.z(); // e1 ^ e2
		b2(2) += Rqi.y(); // e1 ^ e3
		b2(3) += -Rqi.x(); // e2 ^ e3
	}

	b2 *= wij;

	b2(0) += b(0) + 1e-6 * (Rx.w() - 1.0); // 1.0
	b2(1) += b(1) + 1e-6 * -Rx.z(); // e1 ^ e2
	b2(2) += b(2) + 1e-6 * Rx.y(); // e1 ^ e3
	b2(3) += b(3) + 1e-6 * -Rx.x(); // e2 ^ e3

	x.noalias() = JtJ * b2;

	return Eigen::Quaterniond(1.0 + x(0), -x(3), x(2), -x(1)); //rotor(rotor_scalar_e1e2_e2e3_e3e1, 1.0 + x(0), x(1), x(3), -x(2));
}

void UpdateLaplaciansRotationPrep(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors)
{
	//Matrix3x3 m;
	//Matrix3x3 U, W, V, Ut;
	//Matrix3x3 M;
	//ICP icp;
	vector<Eigen::Vector3d> P;
	vector<Eigen::Vector3d> Q;

	P.resize(32);
	Q.resize(32);

	const double alpha = 0.14;
	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;

		Eigen::Vector3d &pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); vertexDegree < 32 && !edgeAroundIter.end(); edgeAroundIter++, vertexDegree++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;

			Eigen::Vector3d &tpj = vertexDescriptors.deformedPositions[j];
			Eigen::Vector3d &pj = mesh->vertexAt(j).p;
			Eigen::Vector3d eij = (pj - pi) * (*A)(i, j);
			Eigen::Vector3d teij = tpj - tpi;

			P[vertexDegree] = eij;
			Q[vertexDegree] = teij;
		}
		double invVertexDegree = alpha * g_meshArea / (double)vertexDegree;
		//rotor M = E3GA4(P, Q, vertexDescriptors[i]->M, vertexDegree);
		//rotor M = E3GA5(P, Q, vertexDegree);
		E3GA_Prep(P, Q, invVertexDegree, vertexDegree, vertexDescriptors.JtJ[i], vertexDescriptors.b[i]);
	}
	//});
}

void UpdateLaplaciansRotationExec(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors, bool updateLaplacian)
{
	const double alpha = 0.14;
	vector<Eigen::Quaterniond> Rq;

	Rq.resize(32);
	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;

		Eigen::Vector3d &pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); vertexDegree < 32 && !edgeAroundIter.end(); edgeAroundIter++, vertexDegree++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			Rq[vertexDegree] = vertexDescriptors.rotors[j];
		}
		double invVertexDegree = alpha * g_meshArea / (double)vertexDegree;
		Eigen::Quaterniond M = E3GA_Fast5(invVertexDegree, Rq, vertexDescriptors.rotors[i], vertexDegree, vertexDescriptors.JtJ[i], vertexDescriptors.b[i]);
		vertexDescriptors.rotors[i] = M.normalized();
	}
	//});

	if (!updateLaplacian) {
		return;
	}

	std::fill(vertexDescriptors.laplacianCoordinates.begin(), vertexDescriptors.laplacianCoordinates.end(), Eigen::Vector3d(0, 0, 0));

	//concurrency::parallel_for_each(mesh->getVertices().begin(), mesh->getVertices().end(), [&](Vertex& vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;
			double wij = (*A)(i, j);
			Eigen::Quaterniond&Ri = vertexDescriptors.rotors[i];
			Eigen::Quaterniond&Rj = vertexDescriptors.rotors[j];
			Eigen::Vector3d V = mesh->vertexAt(j).p - mesh->vertexAt(i).p;
			vertexDescriptors.laplacianCoordinates[i] += 0.5 * wij * (Ri._transformVector(V) + Rj._transformVector(V));;
		}
	}
	//});
}

double ComputeEnergy(Mesh *mesh, std::shared_ptr<SparseMatrix> A, VertexBuffer& vertexDescriptors)
{
	double arapEnergy = 0.0;
	const double alpha = 0.08;
	//concurrency::parallel_for_each(mesh->vertices.begin(), mesh->vertices.end(), [&](Vertex * vertex)
	for (Vertex& vertex : mesh->getVertices())
	{
		int i = vertex.ID;

		Eigen::Vector3d& pi = vertex.p;
		Eigen::Vector3d &tpi = vertexDescriptors.deformedPositions[i];
		auto& Ri = vertexDescriptors.rotors[i];

		int vertexDegree = 0;
		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			vertexDegree++;
		}

		double invVertexDegree = alpha * g_meshArea / (double)vertexDegree;

		for (Vertex::EdgeAroundIterator edgeAroundIter = vertex.iterator(); !edgeAroundIter.end(); edgeAroundIter++)
		{
			int j = edgeAroundIter.edge_out()->pair->vertex->ID;

			Eigen::Vector3d &tpj = vertexDescriptors.deformedPositions[j];
			Eigen::Vector3d &pj = mesh->vertexAt(j).p;

			Eigen::Vector3d eij = (pj - pi);
			auto q_ij = tpj - tpi;
			auto p_ij = eij;
			auto& Rj = vertexDescriptors.rotors[j];

			arapEnergy += (*A)(i, j) * (Ri._transformVector(p_ij) - q_ij).squaredNorm();
			arapEnergy += invVertexDegree * (Rj.coeffs() - Ri.coeffs()).squaredNorm();
		}
	}//);

	return arapEnergy;
}

void display()
{
	/*
	 *	matrices
	 */
	glViewport( 0, 0, g_viewportWidth, g_viewportHeight );
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	pickLoadMatrix();
	GLpick::g_frustumFar = 1000.0;
	GLpick::g_frustumNear = .1;
	gluPerspective( 60.0, (double)g_viewportWidth/(double)g_viewportHeight, GLpick::g_frustumNear, GLpick::g_frustumFar );
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glShadeModel(GL_SMOOTH);	//gouraud shading
	glClearDepth(1.0f);
	glClearColor( .75f, .75f, .75f, .0f );
	glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );

	/*
	 *	estados
	 */
	glEnable(GL_CULL_FACE);		//face culling
	glCullFace( GL_BACK );
	glFrontFace( GL_CCW );
	glEnable(GL_DEPTH_TEST);	//z-buffer
	glDepthFunc(GL_LEQUAL);

	/*
	 *	iluminacion
	 */
	float		ambient[] = { .3f, .3f, .3f, 1.f };
	float		diffuse[] = { .3f, .3f, .3f, 1.f };
	float		position[] = { .0f, 0.f, -150.f, 1.f };
	float		specular[] = { 1.f, 1.f, 1.f };

	glLightfv( GL_LIGHT0, GL_AMBIENT, ambient );
	glLightfv( GL_LIGHT0, GL_DIFFUSE, diffuse );
	glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 0);
	glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.0125);
	glEnable(  GL_LIGHT0   );
	glEnable(  GL_LIGHTING );
	glMaterialfv( GL_FRONT_AND_BACK, GL_SPECULAR, specular );
	glMaterialf( GL_FRONT_AND_BACK, GL_SHININESS, 50.f );

	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

	glLoadIdentity();

	g_camera.glLookAt();

	glLightfv( GL_LIGHT0, /*GL_POSITION*/GL_SPOT_DIRECTION, position );

	glPushMatrix();

	rotorGLMult(g_modelRotor);

	benchs.Begin();

	if(!g_computeBasis)
	{
		static bool oneTime = true;
		if (oneTime || (g_rotateKeyRotors || g_translateKeyRotors || g_automaticAnimation))
		{
			if(oneTime == true)
			{
				SolveLinearSystemTaucs(vertexDescriptorsLow);
			}

			oneTime = false;

			for(int i = 0 ; i < 3 ; ++i)
			{
				UpdateLaplaciansRotationPrep(&meshLow, A, vertexDescriptorsLow);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, true);
				SolveLinearSystemTaucs(vertexDescriptorsLow);
			}
			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolveLinearSystemTaucsHi(vertexDescriptors, vertexDescriptorsLow);
		}
	}
	if(g_iterateManyTimes)
	{
		if (g_convergence)
		{
			clock_t begin;
			clock_t end;
			double avgTime = 0.0;

			auto arapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
			int count = 0;
			for (; count < 2500; ++count)
			{
				begin = clock();

				UpdateLaplaciansRotationPrep(&meshLow, A, vertexDescriptorsLow);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, true);
				SolveLinearSystemTaucs(vertexDescriptorsLow);

				end = clock();

				double secDeform = (double)(end - begin) / CLOCKS_PER_SEC;

				avgTime += secDeform;

				auto newArapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
				if (fabs(arapEnergy - newArapEnergy) < 1e-4)
				{
					std::cout << "convergence after " << count + 1 << " iterations" << std::endl;
					break;
				}
				arapEnergy = newArapEnergy;
			}
			arapEnergy = ComputeEnergy(&meshLow, A, vertexDescriptorsLow);
			std::cout << arapEnergy << std::endl;
			std::cout << "Average iteration time (sec): " << avgTime / (double)(count + 1) << endl;
			std::cout << "Total accum time (sec): " << avgTime << endl;

			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolveLinearSystemTaucsHi(vertexDescriptors, vertexDescriptorsLow);
		}
		else
		{
			for (int i = 0; i < 100; ++i)
			{
				UpdateLaplaciansRotationPrep(&meshLow, A, vertexDescriptorsLow);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, false);
				UpdateLaplaciansRotationExec(&meshLow, A, vertexDescriptorsLow, true);
				SolveLinearSystemTaucs(vertexDescriptorsLow);
			}
			transferRotations(&mesh, AHi, vertexDescriptors, vertexDescriptorsLow);
			SolveLinearSystemTaucsHi(vertexDescriptors, vertexDescriptorsLow);
		}
		g_iterateManyTimes = false;
	}

	benchs.End();

	if (GLpick::g_pickActive) glLoadName((GLuint)-1);

	double alpha = 1.0;

	//glEnable (GL_BLEND);
	//glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	//alpha = 0.5;

	//Mesh-Faces Rendering
	glPolygonMode( GL_FRONT_AND_BACK, GL_FILL /*GL_LINE GL_FILL GL_POINT*/);
	glEnable (GL_POLYGON_OFFSET_FILL);
	glPolygonOffset (1., 1.);
	glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
	glEnable( GL_COLOR_MATERIAL );
	if (GLpick::g_pickActive) glLoadName((GLuint)10);

	glColor4d(1, 1, 1, alpha);
	glEnableClientState(GL_NORMAL_ARRAY);
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_DOUBLE, 0, &vertexDescriptors.deformedPositions[0]);
	glNormalPointer(GL_DOUBLE, 0, &vertexDescriptors.normals[0]);
	// draw the model
	glDrawElements(GL_TRIANGLES, trianglesHi.get_size(), GL_UNSIGNED_INT, &trianglesHi.faces[0]);
	// deactivate vertex arrays after drawing
	glDisableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_NORMAL_ARRAY);

	if (g_showWires)
	{
		if (!GLpick::g_pickActive)
		{
			//Mesh-Edges Rendering (superimposed to faces)
			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE /*GL_LINE GL_FILL GL_POINT*/);
			glColor4d(.5, .5, .5, alpha);
			glDisable(GL_LIGHTING);
			glEnableClientState(GL_VERTEX_ARRAY);
			glVertexPointer(3, GL_DOUBLE, 0, &vertexDescriptors.deformedPositions[0]);
			// draw the model
			glDrawElements(GL_TRIANGLES, trianglesHi.get_size(), GL_UNSIGNED_INT, &trianglesHi.faces[0]);
			// deactivate vertex arrays after drawing
			glDisableClientState(GL_VERTEX_ARRAY);
			glEnable(GL_LIGHTING);
		}
	}

	//for (map<int, int>::iterator iter = mapping.begin(); iter != mapping.end(); iter++){
	//	DrawPoint(c3gaPoint(vertexDescriptors[iter->second]->deformedPosition));
	//}

	glDisable( GL_COLOR_MATERIAL );
	glDisable(GL_POLYGON_OFFSET_FILL);

	//glDisable (GL_BLEND);

	if(g_showSpheres)
	{
		//Handles rendering
		glPolygonMode( GL_FRONT_AND_BACK, GL_FILL /*GL_LINE GL_FILL GL_POINT*/);

		float	turcoise[] = { .0f, .5f, .5f, 0.3f };
		float	red[] = { .5f, .0f, .0f, 0.3f };

		for( int k = 0 ; k < handles.size() ; ++k)
		{
			if (GLpick::g_pickActive) glLoadName((GLuint)k);
			TRversor R = handles[k].GetTRVersor();
			DrawTransparentDualSphere( _dualSphere( R * handles[k].dS * inverse(R) ), turcoise );
		}
	}

	glPopMatrix();

	glutSwapBuffers();
}

void reshape(GLint width, GLint height)
{
	g_viewportWidth = width;
	g_viewportHeight = height;

	// redraw viewport
	glutPostRedisplay();
}

vectorE3GA mousePosToVector(int x, int y) {
	x -= g_viewportWidth / 2;
	y -= g_viewportHeight / 2;
	return _vectorE3GA((float)-x * e1 - (float)y * e2);
}

void MouseButton(int button, int state, int x, int y)
{
	g_rotateModel = false;
	g_rotateKeyRotors = false;
	g_translateKeyRotors = false;

	if (button == GLUT_LEFT_BUTTON)
	{
		g_prevMousePos = mousePosToVector(x, y);

		GLpick::g_pickWinSize = 1;
		g_dragObject = pick(x, g_viewportHeight - y, display, &g_dragDistance);

		if(g_dragObject == -1 || g_dragObject == 10 )
		{
			vectorE3GA mousePos = mousePosToVector(x, y);
			g_rotateModel = true;

			if ((_Float(norm_e(mousePos)) / _Float(norm_e(g_viewportWidth * e1 + g_viewportHeight * e2))) < 0.2)
				g_rotateModelOutOfPlane = true;
			else g_rotateModelOutOfPlane = false;
		}
		else if(g_dragObject >= 0 && g_dragObject < handles.size())
		{
			g_rotateKeyRotors = true;
		}
	}

	if (button == GLUT_RIGHT_BUTTON)
	{
		g_prevMousePos = mousePosToVector(x, y);

		GLpick::g_pickWinSize = 1;
		g_dragObject = pick(x, g_viewportHeight - y, display, &g_dragDistance);

		if(g_dragObject >= 0 && g_dragObject < handles.size())
			g_translateKeyRotors = true;
	}
}

void MouseMotion(int x, int y)
{
	if (g_rotateModel || g_rotateKeyRotors || g_translateKeyRotors )
	{
		// get mouse position, motion
		vectorE3GA mousePos = mousePosToVector(x, y);
		vectorE3GA motion = mousePos - g_prevMousePos;

		if (g_rotateModel)
		{
			// update rotor
			if (g_rotateModelOutOfPlane)
				g_modelRotor = exp(g_camera.rotateVel * (motion ^ e3) ) * g_modelRotor;
			else
				g_modelRotor = exp(0.00001f * (motion ^ mousePos) ) * g_modelRotor;
		}
		if(g_rotateKeyRotors)
		{
			//rotor R1 =  _rotor( inverse(g_modelRotor) * exp(-g_camera.rotateVel * (motion ^ e3) ) * g_modelRotor);
			rotor R1 =  _rotor( exp(-g_camera.rotateVel * (motion ^ e3) ) );
			if(g_dragObject < handles.size())
			{
				rotor R = handles[g_dragObject].R;
				handles[g_dragObject].R = normalize(_TRversor( R1 * R  ) );
			}
		}

		if(g_translateKeyRotors)
		{
			normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp( _freeVector(-g_camera.translateVel*motion*ni) ) * g_modelRotor);
			if(g_dragObject < handles.size())
			{
				translator T = handles[g_dragObject].T;
				handles[g_dragObject].T = normalize(_TRversor( T1 * T ));
			}
		}

		// remember mouse pos for next motion:
		g_prevMousePos = mousePos;

		// redraw viewport
		glutPostRedisplay();
	}
}

void SpecialFunc(int key, int x, int y)
{
	switch(key) {
		case GLUT_KEY_F1 :
			{
				int mod = glutGetModifiers();
				if(mod == GLUT_ACTIVE_CTRL || mod == GLUT_ACTIVE_SHIFT )
				{
				}
			}
			break;
		case GLUT_KEY_UP:
			{
				if(g_rotateKeyRotors)
				{
					handles[g_dragObject].dS = ChangeDualSphereRadiusSize(handles[g_dragObject].dS, 0.025);

					// redraw viewport
					glutPostRedisplay();
				}
			}
			break;
		case GLUT_KEY_DOWN:
			{
				if(g_rotateKeyRotors)
				{
					handles[g_dragObject].dS = ChangeDualSphereRadiusSize(handles[g_dragObject].dS, -0.025);

					// redraw viewport
					glutPostRedisplay();
				}
			}
			break;
	}
}

void SpecialUpFunc(int key, int x, int y)
{
}

void KeyboardUpFunc(unsigned char key, int x, int y)
{
	if (key == 'c' || key == 'C')
	{
		g_convergence = !g_convergence;
		vectorE3GA motion = _vectorE3GA(-70.0, -60.0, 0);
		normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp(_freeVector(-g_camera.translateVel*motion*ni)) * g_modelRotor);
		translator T = handles[0].T;
		handles[0].T = normalize(_TRversor(T1 * T));

		glutPostRedisplay();
	}

	if (key == 'a' || key == 'A')
	{
		g_automaticAnimation = !g_automaticAnimation;
		if (g_automaticAnimation)
		{
			benchs.Start();
		}
		else
		{
			benchs.Stop();
		}

		glutPostRedisplay();
	}

	if(key == 'w' || key == 'W')
	{
		g_showWires = !g_showWires;
		glutPostRedisplay();
	}

	if( key == 'h' || key == 'H' )
	{
		g_showSpheres = !g_showSpheres;
		glutPostRedisplay();
	}

	if( key == 'x' || key == 'X' )
	{
		g_iterateManyTimes = true;
		glutPostRedisplay();
	}
}

void Idle()
{
	// redraw viewport
	if (g_automaticAnimation)
	{
		const int minCount = -50;
		const int maxCount = 50;
		static int mycount = 0;
		static int direction = 0;
		vectorE3GA motion = _vectorE3GA(1.0, 0, 0);
		if (mycount == maxCount)
		{
			direction = 1;
		}
		if (mycount == minCount)
		{
			direction = 0;
		}

		if (mycount < maxCount && direction == 0)
		{
			motion = _vectorE3GA(1.0, 0, 0);
			mycount++;
		}
		if (mycount > minCount && direction == 1)
		{
			motion = _vectorE3GA(-1.0, 0, 0);
			mycount--;
		}

		normalizedTranslator T1 = _normalizedTranslator(inverse(g_modelRotor) * exp(_freeVector(-g_camera.translateVel*motion*ni)) * g_modelRotor);
		translator T = handles[0].T;
		handles[0].T = normalize(_TRversor(T1 * T));

		if (!benchs.IsStarted())
		{
			benchs.Start();
		}

		glutPostRedisplay();
	}
}

void DestroyWindow()
{
	ReleaseDrawing();
}


///////////////////////////////////////////////////////////////////
//  Copyright Eduardo Quintana 2021
//  Copyright Janek Kozicki 2021
//  Copyright Christopher Kormanyos 2021
//  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)

// Example using custom allocators
#include <boost/math/fft/bsl_backend.hpp>
#include <array>
#include <numeric>

#if __cplusplus >= 201700L
#include <memory>
#include <memory_resource>
#endif

using namespace boost::math::fft;

int main()
{
#if __cplusplus >= 201700L
  const std::size_t N = 100;
  std::array<char,30000> buf;
  std::pmr::monotonic_buffer_resource
    pool{buf.data(),buf.size(),std::pmr::null_memory_resource()};

  using Real = double;
  using Complex = std::complex<Real>;
  using allocator_type = std::pmr::polymorphic_allocator<Complex>;

  std::vector<Complex,allocator_type> A(N,Complex(),&pool);
  std::vector<Complex,allocator_type> B(N,Complex(),&pool);
  // ...
  bsl_dft<Complex,allocator_type> plan(N,&pool);
  plan.forward(A.begin(),A.end(),B.begin());
#endif
  return 0;
}


#include <Eigen/Core>
#include <aslam/common/pose-types.h>
#include <ceres-error-terms/parameterization/pose-param-jpl.h>
#include <ceres/ceres.h>
#include <eigen-checks/gtest.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <maplab-common/quaternion-math.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>

#include "ceres-error-terms/block-pose-prior-error-term.h"
#include "ceres-error-terms/common.h"
#include "ceres-error-terms/six-dof-block-pose-error-term-with-extrinsics-autodiff.h"

class SixDofBlockPoseErrorTermsWithExtrinsics : public ::testing::Test {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 protected:
  virtual void SetUp() {
    // Some arbitrary numbers for position and orientation.
    prior_position_G_Ik_ << 1.0, 2.0, 3.0;
    prior_position_G_Ikp1_ << 4.0, 5.0, 6.0;
    prior_position_B_I_ << 7.0, 8.0, 9.0;
    prior_orientation_G_Ik_.coeffs() << sqrt(2) / 2, 0, 0, sqrt(2) / 2;
    prior_orientation_G_Ikp1_.coeffs() << 0.15496688, -0.60676538, 0.30722298,
        0.71654385;
    prior_orientation_G_Ikp1_.coeffs().normalize();
    prior_orientation_B_I_.coeffs() << 0.47112391, 0.45194212, 0.07820137,
        0.75344219;
    prior_orientation_B_I_.coeffs().normalize();

    pose_G_Ik_ << prior_orientation_G_Ik_.coeffs(), prior_position_G_Ik_;
    pose_G_Ikp1_ << prior_orientation_G_Ikp1_.coeffs(), prior_position_G_Ikp1_;

    position_B_I_ << prior_position_B_I_;
    orientation_B_I_ << prior_orientation_B_I_.inverse().coeffs();
    pose_B_I_ << orientation_B_I_, position_B_I_;

    T_Bk_Bkp1_measurement_covariance_matrix_.setIdentity();
    T_B_I_prior_covariance_matrix_.setIdentity();
  }

  void addPriorOn_T_B_I(const aslam::Transformation& T_B_I);
  void add_T_Bk_Bkp1_Measurement(
      const aslam::Transformation& T_Ik_Ikp1_measurement);
  void solve();

  void fix_T_G_Ik();
  void fix_T_G_Ikp1();
  void fix_T_B_I();

  aslam::Transformation get_T_G_Ik() const;
  aslam::Transformation get_T_G_Ikp1() const;
  aslam::Transformation get_T_B_I() const;

  ceres::Problem problem_;
  ceres::Solver::Summary summary_;

  Eigen::Vector3d prior_position_G_Ik_;
  Eigen::Vector3d prior_position_G_Ikp1_;
  Eigen::Vector3d prior_position_B_I_;
  Eigen::Quaterniond prior_orientation_G_Ik_;
  Eigen::Quaterniond prior_orientation_G_Ikp1_;
  Eigen::Quaterniond prior_orientation_B_I_;

  Eigen::Matrix<double, 7, 1> pose_G_Ik_;
  Eigen::Matrix<double, 7, 1> pose_G_Ikp1_;
  Eigen::Matrix<double, 7, 1> pose_B_I_;
  Eigen::Vector3d position_B_I_;
  Eigen::Vector4d orientation_B_I_;

  Eigen::Matrix<double, 6, 6> T_Bk_Bkp1_measurement_covariance_matrix_;
  Eigen::Matrix<double, 6, 6> T_B_I_prior_covariance_matrix_;
};

void SixDofBlockPoseErrorTermsWithExtrinsics::add_T_Bk_Bkp1_Measurement(
    const aslam::Transformation& T_Bk_Bkp1) {
  ceres::LocalParameterization* pose_parameterization =
      new ceres_error_terms::JplPoseParameterization;
  ceres::LocalParameterization* quaternion_parameterization =
      new ceres_error_terms::JplQuaternionParameterization;

  ceres::CostFunction* relative_pose_cost = new ceres::AutoDiffCostFunction<
      ceres_error_terms::SixDoFBlockPoseErrorTermWithExtrinsics,
      ceres_error_terms::SixDoFBlockPoseErrorTermWithExtrinsics::
          kResidualBlockSize,
      ceres_error_terms::poseblocks::kPoseSize,
      ceres_error_terms::poseblocks::kPoseSize,
      ceres_error_terms::poseblocks::kOrientationBlockSize,
      ceres_error_terms::poseblocks::kPositionBlockSize>(
      new ceres_error_terms::SixDoFBlockPoseErrorTermWithExtrinsics(
          T_Bk_Bkp1, T_Bk_Bkp1_measurement_covariance_matrix_));
  problem_.AddResidualBlock(
      relative_pose_cost, nullptr, pose_G_Ik_.data(), pose_G_Ikp1_.data(),
      orientation_B_I_.data(), position_B_I_.data());

  problem_.SetParameterization(pose_G_Ik_.data(), pose_parameterization);
  problem_.SetParameterization(pose_G_Ikp1_.data(), pose_parameterization);
  problem_.SetParameterization(
      orientation_B_I_.data(), quaternion_parameterization);
}

void SixDofBlockPoseErrorTermsWithExtrinsics::addPriorOn_T_B_I(
    const aslam::Transformation& T_B_I) {
  ceres::CostFunction* prior_cost_function(
      new ceres_error_terms::BlockPosePriorErrorTerm(
          T_B_I.getEigenQuaternion().coeffs(), T_B_I.getPosition(),
          T_B_I_prior_covariance_matrix_));

  problem_.AddResidualBlock(
      prior_cost_function, nullptr, orientation_B_I_.data(),
      position_B_I_.data());
}

void SixDofBlockPoseErrorTermsWithExtrinsics::fix_T_G_Ik() {
  problem_.SetParameterBlockConstant(pose_G_Ik_.data());
}

void SixDofBlockPoseErrorTermsWithExtrinsics::fix_T_G_Ikp1() {
  problem_.SetParameterBlockConstant(pose_G_Ikp1_.data());
}

void SixDofBlockPoseErrorTermsWithExtrinsics::fix_T_B_I() {
  problem_.SetParameterBlockConstant(orientation_B_I_.data());
  problem_.SetParameterBlockConstant(position_B_I_.data());
}

aslam::Transformation SixDofBlockPoseErrorTermsWithExtrinsics::get_T_G_Ik()
    const {
  const Eigen::Map<const Eigen::Quaterniond> q_G_Ik(pose_G_Ik_.data());
  return aslam::Transformation(aslam::Quaternion(q_G_Ik), pose_G_Ik_.tail(3));
}

aslam::Transformation SixDofBlockPoseErrorTermsWithExtrinsics::get_T_G_Ikp1()
    const {
  const Eigen::Map<const Eigen::Quaterniond> q_G_Ikp1(pose_G_Ikp1_.data());
  return aslam::Transformation(
      aslam::Quaternion(q_G_Ikp1), pose_G_Ikp1_.tail(3));
}

aslam::Transformation SixDofBlockPoseErrorTermsWithExtrinsics::get_T_B_I()
    const {
  const Eigen::Map<const Eigen::Quaterniond> q_I_B_JPL(pose_B_I_.data());
  return aslam::Transformation(
      aslam::Quaternion(q_I_B_JPL).inverse(), pose_B_I_.tail(3));
}

void SixDofBlockPoseErrorTermsWithExtrinsics::solve() {
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.parameter_tolerance = 1e-14;
  options.gradient_tolerance = 1e-14;
  options.function_tolerance = 1e-14;
  options.max_num_iterations = 1e5;
  ceres::Solve(options, &problem_, &summary_);
}

TEST_F(
    SixDofBlockPoseErrorTermsWithExtrinsics, TestPosePriorErrorTermZeroCost) {
  const aslam::Transformation T_Bk_Bkp1 = get_T_B_I() * get_T_G_Ik().inverse() *
                                          get_T_G_Ikp1() *
                                          get_T_B_I().inverse();

  add_T_Bk_Bkp1_Measurement(T_Bk_Bkp1);
  fix_T_B_I();
  fix_T_G_Ik();
  solve();

  EXPECT_NEAR(summary_.initial_cost, 0.0, 1e-12);
  EXPECT_NEAR(summary_.final_cost, 0.0, 1e-12);
}

TEST_F(
    SixDofBlockPoseErrorTermsWithExtrinsics,
    TestPosePriorErrorTermIdentityFixBIFixGIk) {
  add_T_Bk_Bkp1_Measurement(aslam::Transformation());
  fix_T_B_I();
  fix_T_G_Ik();
  solve();

  EXPECT_NEAR(summary_.final_cost, 0.0, 1e-12);
  EXPECT_TRUE(
      EIGEN_MATRIX_NEAR(
          get_T_G_Ikp1().getTransformationMatrix(),
          get_T_G_Ik().getTransformationMatrix(), 1e-8));
}

// TODO(andrepfr): Test disabled since quaternion convention of
// six-dof-block-transformation is different from the one expected in this test.
// Re-enable once bundle adjustment and error terms are changed to have a
// consistent convention.
TEST_F(
    SixDofBlockPoseErrorTermsWithExtrinsics,
    DISABLED_TestPosePriorErrorTermArbitraryMeasurementWithPriorOn_T_B_I) {
  Eigen::Vector3d angle_axis_Bk_Bkp1_vec;
  angle_axis_Bk_Bkp1_vec << 0.8, -1.2, 0.456;  // Some fixed arbitrary numbers.
  Eigen::Vector3d angle_axis_Bk_Bkp1_axis = angle_axis_Bk_Bkp1_vec;
  angle_axis_Bk_Bkp1_axis.normalize();
  const aslam::AngleAxis q_Bk_Bkp1_angle_axis(
      angle_axis_Bk_Bkp1_vec.norm(), angle_axis_Bk_Bkp1_axis);
  Eigen::Vector3d p_Bk_Bkp1(
      125.4, 43423.3, -33441.423);  // Some fixed arbitrary numbers.
  const aslam::Transformation T_Bk_BKp1_measurement(
      aslam::Quaternion(q_Bk_Bkp1_angle_axis), p_Bk_Bkp1);
  add_T_Bk_Bkp1_Measurement(T_Bk_BKp1_measurement);

  Eigen::Vector3d angle_axis_B_I_vec;
  angle_axis_B_I_vec << -0.4, 1.2, -0.98;  // Some fixed arbitrary numbers.
  Eigen::Vector3d angle_axis_B_I_axis = angle_axis_B_I_vec;
  angle_axis_B_I_axis.normalize();
  const aslam::AngleAxis q_B_I_angle_axis(
      angle_axis_B_I_vec.norm(), angle_axis_B_I_axis);
  Eigen::Vector3d p_B_I(
      -4575.045, 8751.15, 456.5);  // Some fixed arbitrary numbers.
  const aslam::Transformation T_B_I_measurement(
      aslam::Quaternion(q_B_I_angle_axis), p_B_I);

  addPriorOn_T_B_I(T_B_I_measurement);

  fix_T_G_Ik();
  solve();

  EXPECT_NEAR(summary_.final_cost, 0.0, 1e-6);
  EXPECT_TRUE(
      EIGEN_MATRIX_NEAR(
          get_T_B_I().getTransformationMatrix(),
          T_B_I_measurement.getTransformationMatrix(), 1e-8));

  const aslam::Transformation T_Ik_Ikp1_ground_truth =
      T_B_I_measurement.inverse() * T_Bk_BKp1_measurement * T_B_I_measurement;
  EXPECT_TRUE(
      EIGEN_MATRIX_NEAR(
          (get_T_G_Ik().inverse() * get_T_G_Ikp1()).getTransformationMatrix(),
          T_Ik_Ikp1_ground_truth.getTransformationMatrix(), 1e-5));
}
MAPLAB_UNITTEST_ENTRYPOINT


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "latbuilder/Types.h"
#include "latbuilder/Parser/SizeParam.h"
#include "latbuilder/Parser/Weights.h"
#include "latbuilder/Parser/FigureOfMerit.h"
#include "latbuilder/Parser/MeritFilterList.h"
#include "latbuilder/TextStream.h"
#include "latbuilder/LFSR258.h"

#include "latbuilder/GenSeq/VectorCreator.h"
#include "latbuilder/GenSeq/CoprimeIntegers.h"
#include "latbuilder/LatSeq/Combiner.h"

#include "latbuilder/MeritSeq/CBC.h"
#include "latbuilder/MeritSeq/CoordUniformCBC.h"
#include "latbuilder/MeritSeq/CoordUniformInnerProd.h"
#include "latbuilder/MeritSeq/LatSeqOverCBC.h"

#include <boost/lexical_cast.hpp>

using namespace LatBuilder;
using TextStream::operator<<;

template <typename T1, typename T2, typename T3, typename T4>
void printTableRow(
      const T1& x1,
      const T2& x2,
      const T3& x3,
      const T4& x4
      )
{
   using namespace std;
   cout << x1 << '\t';
   cout << scientific << setprecision(8) << x2 << '\t';
   cout << scientific << setprecision(8) << x3 << '\t';
   cout << scientific << setprecision(8) << x4 << endl;;
}

struct Execute {
   template <class FIG2>
   void operator()(
         FIG2 fig2,
         const std::string& fig1,
         std::unique_ptr<LatticeTester::Weights> weights1,
         LatBuilder::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> size,
         Dimension dimension,
         MeritFilterList<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> filters,
         size_t nrand
         ) const
   {
      Parser::FigureOfMerit<LatticeType::ORDINARY>::parse(
	    "2",
            fig1,
            1,
            std::move(weights1),
            *this,
            std::move(fig2),
            std::move(size),
            dimension,
            std::move(filters),
            nrand
            );
   }

   template <class FIG1, class FIG2>
   void operator()(
         FIG1 fig1,
         FIG2 fig2,
         LatBuilder::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> size,
         Dimension dimension,
         MeritFilterList<LatticeType::ORDINARY, EmbeddingType::UNILEVEL> filters,
         size_t nrand
         ) const
   {
      Storage<LatticeType::ORDINARY, EmbeddingType::UNILEVEL, fig1.suggestedCompression()> storage1(size);
      Storage<LatticeType::ORDINARY, EmbeddingType::UNILEVEL, fig2.suggestedCompression()> storage2(size);

      typedef GenSeq::CoprimeIntegers<fig1.suggestedCompression(), Traversal::Random<LFSR258>> Coprime;
      auto genSeqs = GenSeq::VectorCreator<Coprime>::create(size, dimension, nrand);
      genSeqs[0] = GenSeq::Creator<Coprime>::create(SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>(2));

      auto latSeq = LatSeq::combine<Zip>(size, std::move(genSeqs));

      auto lsoc1 = MeritSeq::latSeqOverCBC(MeritSeq::cbc(storage1, fig1));
      auto lsoc2 = MeritSeq::latSeqOverCBC(MeritSeq::cbc(storage2, fig2));

      auto fseq1 = filters.apply(lsoc1.meritSeq(latSeq));
      auto fseq2 = filters.apply(lsoc2.meritSeq(latSeq));

      printTableRow("gen", fig1.name(), fig2.name(),
            "rel" + fig1.name() + "m" + fig2.name());

      auto it1 = fseq1.begin();
      auto it2 = fseq2.begin();
      while (it1 != fseq1.end()) {
         printTableRow(
               it1.base().base()->gen(),
               *it1,
               *it2,
               (*it1 - *it2) / *it1
               );
         ++it1;
         ++it2;
      }

   }
};

int main(int argc, const char *argv[])
{
   if (argc < 7 + 1) {
      std::cerr << "usage: correlation <nrand> <size> <dimension> <figure1> <weights1> <figure2> <weights2> [<filter> [...]]" << std::endl;
      return 1;
   }

   int iarg = 1;
   auto nrand = boost::lexical_cast<size_t>(argv[iarg++]);
   auto size = Parser::SizeParam<LatticeType::ORDINARY, EmbeddingType::UNILEVEL>::parse(argv[iarg++]);
   auto dimension = boost::lexical_cast<Dimension>(argv[iarg++]);
   std::string figSpec1 = argv[iarg++];
   std::string weightsSpec1 = argv[iarg++];
   std::string figSpec2 = argv[iarg++];
   std::string weightsSpec2 = argv[iarg++];
   std::vector<std::string> filtersSpec(&argv[iarg], &argv[argc]);

   auto weights1 = Parser::Weights::parse(weightsSpec1);
   auto weights2 = Parser::Weights::parse(weightsSpec2);

   auto filters = Parser::MeritFilterList<LatticeType::ORDINARY>::parse("", filtersSpec, size, *weights1, 2);

   Parser::FigureOfMerit<LatticeType::ORDINARY>::parse(
         "2",
         figSpec2,
         1,
         std::move(weights2),
         Execute(),
         figSpec1,
         std::move(weights1),
         std::move(size),
         dimension,
         std::move(filters),
         nrand
         );

   return 0;
}