#ifndef _DIMENSIONAL_ANALYSIS_HPP_
#define _DIMENSIONAL_ANALYSIS_HPP_

#include "MathLibDefinitions.h"
#include <boost\units\systems\si\acceleration.hpp>
#include <boost\units\systems\si\force.hpp>
#include <boost\units\systems\si\length.hpp>
#include <boost\units\systems\si\energy.hpp>
#include <boost\units\systems\si\current.hpp>
#include <boost\units\systems\si\io.hpp>

using namespace boost::units;
using namespace boost::units::si;

namespace   mathlib{

	namespace physics {

		void test(){

			quantity<force, double> Force = quantity<force, double>(2.0 * newton);

		}

		/*

		     *** SIForce   wrapper  class around boost::unit quantity<force,T> ***
		*/

		template<typename T>  class SIForce {

		public:

			/*
			@brief   Zero-force unit.
			*/

		constexpr	SIForce() : m_magnitude{ static_cast<T>(0) } { this->m_oForce = quantity<force, T>(this->m_magnitude * newton); }

			/*
			@brief    Create Force object with user passed magnitude value.
			*/

		    SIForce(_In_ T const mag) : m_magnitude{ mag } { this->m_oForce = quantity<force, T>(this->m_magnitude * newton); }

			/*
			@brief  Copy-Ctor
			*/
			SIForce(_In_ const SIForce &rhs) : m_magnitude{ rhs.m_magnitude }, m_oForce{ rhs.m_oForce }{}

			/*
			@brief   Move-Ctor
			*/
			SIForce(_In_ SIForce &&rhs) : m_magnitude{ std::forward<T>(rhs.m_magnitude) }, m_oForce{ std::forward<T>(rhs.m_oForce) } {}

			/*
			@brief  Dtor == default
			*/
			~SIForce() = default;

			/*

			      ****   Member functions  ****
			*/

			/*  Returns  this->m_magnitude  */
			__forceinline     auto       Magnitude()->const T {

				return (this->m_magnitude);
			}

			/*  Returns  this->m_oForce  */
			__forceinline    auto Force()->const quantity<force,T>{

				return (this->m_oForce);
			}

			/*

			      ***  Member operators  ***
			*/

			/*
			@brief    *this = const &rhs
			*/
			SIForce &     operator=(_In_ const SIForce &rhs) {

				if (this == &rhs) return (*this);

				this->m_magnitude = rhs.m_magnitude;
				this->m_oForce.operator=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief    *this = &&rhs
			*/
			SIForce &     operator=(_In_ SIForce &&rhs) {

				if (this == &rhs) return (*this);

				this->m_magnitude = std::forward<T>(rhs.m_magnitude);
				this->m_oForce.operator=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief    *this +=   const  &rhs
		   */
			SIForce &      operator+=(_In_ const SIForce &rhs) {

				this->m_magnitude += rhs.m_magnitude;
				this->m_oForce.operator+=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief     *this +=    &&rhs
			*/
			SIForce  &      operator+=(_In_ SIForce &&rhs) {

				this->m_magnitude += rhs.m_magnitude;
				this->m_oForce.operator+=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief     *this -=    const  &rhs
			*/
			SIForce  &       operator-=(_In_ const SIForce &rhs) {

				this->m_magnitude -= rhs.m_magnitude;
				this->m_oForce.operator-=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief      *this -=   &&rhs
			*/
			SIForce  &       operator-=(_In_ SIForce &&rhs) {

				this->m_magnitude -= rhs.m_magnitude;
				this->m_oForce.operator-=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief       *this *=  const &rhs
			*/
			SIForce  &       operator*=(_In_  const SIForce &rhs) {

				this->m_magnitude *= rhs.m_magnitude;
				this->m_oForce.operator*=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief       *this *=  &&rhs
			*/
			SIForce  &       operator*=(_In_  SIForce &&rhs) {

				this->m_magnitude *= rhs.m_magnitude;
				this->m_oForce.operator*=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief        *this /=  const &rhs
			*/
			SIForce  &       operator/=(_In_ const SIForce &rhs) {

				this->m_magnitude /= rhs.m_magnitude;
				this->m_oForce.operator/=(rhs.m_oForce);
				return (*this);
			}

			/*
			@brief        *this /=   &&rhs
			*/
			SIForce  &       operator/=(_In_ SIForce &&rhs) {

				this->m_magnitude /= rhs.m_magnitude;
				this->m_oForce.operator/=(std::forward<T>(rhs.m_oForce));
				return (*this);
			}

			/*
			@brief         *this == rhs
			*/
			auto             operator==(_In_ const SIForce &rhs)->const bool {

				return (this->m_oForce.value() == rhs.m_oForce.value() &&
					this->m_magnitude == rhs.m_magnitude);
			}

			/*
			@brief           *this != rhs
			*/
			auto              operator!=(_In_ const SIForce &rhs)->const bool {

				return !(this->operator==(rhs));
			}

			/*
			@brief      operator<<
			*/
			friend	std::ostream &      operator<<(_In_ std::ostream &os, _In_   SIForce &rhs) {

				os << "Force = " << rhs.Force().value() << " N" << std::endl;
				return os;
			}

		private:

			/*
			@brief   Amount of force in unit of newton.
			*/
			T   m_magnitude;

			/*
			@brief   unit of Force.
			*/
			quantity<force, T> m_oForce;
		};

		/*
		          ***  Wrapper class for the boost::units quantity of Length.

		*/

		template<typename T>   class SILength {

		public:

			/*
			    Zero-length Default Ctor
			*/
			SILength() : m_magnitude{ static_cast<T>(0) } { this->m_oLength = quantity<length, T>(this->m_magnitude * meter) }

			/*
			    One-arg Ctor.
			*/
			SILength(const T &mag) : m_magnitude{ mag } { this->m_oLength = quantity<length, T>(this->m_magnitude * meter); }

			/*
			    Two-arg Ctor.
			*/
			SILength(const T &rhs, const quantity<length, T> &lhs) : m_magnitude{ rhs }, m_oLength{ lhs } {};
			/*
			    Copy-Ctor
			*/
			SILength(const SILength &rhs) : m_magnitude{ rhs.m_magnitude }, m_oLength{ rhs.m_oLength } {}

			/*
			    Move-Ctor
			*/
			SILength(SILength &&rhs) : m_magnitude{ std::move(rhs.m_magnitude) }, m_oLength{ std::move(rhs.m_oLength) } {}

			/*
			    Dtor = default
			*/
			~SILength() noexcept(true) = default;

			/*

			         ***   Member accessors  ***
			*/

			/*
			     Returns magnitude argument.
			*/
			__forceinline  auto   Magnitude()->const T{

				return (this->m_magnitude);
			}

			/*
			@brief   Returns  m_oLength class variable.
			*/
			__forceinline  auto   Length()->const quantity<length, T> {

				return (this->m_oLength);
			}

			/*

			     ***   Class member and friend operators.  ***
			*/

			/*
			@brief     copy-assignment.
			*/
			auto  operator=(_In_ const SILength &rhs)->SILength<T> & {

				if (this == &rhs) return (*this);

				this->m_magnitude = rhs.m_magnitude;
				this->m_oLength = rhs.m_oLength;
				return (*this);
			}

			/*
			@brief      move-assignment.
			*/
			auto  operator=(_In_ SILength &&rhs)->SILength<T> & {

				if (this == &rhs) return (*this);

				this->m_magnitude = std::forward<T>(rhs.m_magnitude);
				this->m_oLength = std::forward<T>(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief     *this += rhs
			*/
			auto   operator+=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude += rhs.m_magnitude;
				this->m_oLength.operator+=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief    *this += rhs (move)
			*/
			auto    operator+=(_In_  SILength<T> &&rhs)->SILength<T> & {

				this->m_magnitude += std::move(rhs.m_magnitude);
				this->m_oLength.operator+=(std::move(rhs.m_oLength));
				return (*this);
			}

			/*
			@brief     *this -= rhs
			*/
			auto   operator-=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude -= rhs.m_magnitude;
				this->m_oLength.operator-=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief      *this -= rhs (move)
			*/
			auto    operator-=(_In_ SILength &&rhs)->SILength<T> & {

				this->m_magnitude -= std::move(rhs.m_magnitude);
				this->m_oLength.operator-=(std::move(rhs.m_oLength));
				return (*this);
			}

			/*
			@brief     *this *= rhs
			*/
			auto    operator*=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude *= rhs.m_magnitude;
				this->m_oLength.operator*=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief      *this /= rhs
			*/
			auto    operator/=(_In_ SILength &&rhs)->SILength<T> & {

				this->Magnitude = std::move(rhs.m_magnitude);
				this->m_oLength.operator*=(std::move(rhs.m_oLength));
				return (*this);
			}

			/*
			@brief    *this /= rhs
			*/
			auto    operator/=(_In_ const SILength &rhs)->SILength<T> & {

				this->m_magnitude /= rhs.m_magnitude;
				this->m_oLength.operator/=(rhs.m_oLength);
				return (*this);
			}

			/*
			@brief        *this == rhs
		    */
			auto    operator==(_In_ const SILength &rhs)->const bool {

				return (this->m_oLength.value() == rhs.m_oLength.value() &&
					this->m_magnitude == rhs.m_magnitude);
			}

			/*
			@brief        *this != rhs
			*/
			auto    operator!=(_In_ const SILength &rhs)->const bool {

				return (!(this->operator==(rhs)));
			}

			/*
			@brief    operator<<
			*/
			friend  std::ostream &   operator<<(_In_ std::ostream &os, _In_   SILength &rhs) {

				os << "Length = " << rhs.Length().value() << " m" << std::endl;
				return os;
			}

		private:

			/*
			    T m_magnitude class variable
			*/
			T    m_magnitude;

			/*
			    T m_oLength class variable

			*/

			quantity<length, T> m_oLength;

		};

		/*

		        ***  Generic class Work   ***

		*/

		template<class Force, class Distance, typename T>
		class SIWork {

		public:

			/*
			@brief Surpress creation of Default Ctor
			*/
			SIWork() = delete;

			/*
			 @brief    One-arg Ctor
			*/

			SIWork(_In_  Force &f, _In_  Distance &d) : m_oWork{ f.Force() * d.Length() } {}

			/*
			@brief     Copy-Ctor.
			*/
			SIWork(_In_ const SIWork &rhs) : m_oWork{ rhs.m_oWork } {}

			/*
			@brief     Move-Ctor
			*/
			SIWork(_In_ SIWork &&rhs) : m_oWork{ std::move(rhs.m_oWork) } {}

			/*
			@brief   Dtor
			*/
			~SIWork() noexcept(true) = default;

			/*

			      Class member operators, and accessor.

			*/

			auto     Work()->const SIWork<Force, Distance, T> {

				return (this->m_oWork);
			}

			/*
			         *this = rhs (copy)
			*/
			auto     operator=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				if (this == &rhs) return (*this);
				this->m_oWork.operator=(rhs.m_oWork);
				return (*this);
			}

			/*
			         *this = rhs (move)
			*/
			auto      operator=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				if (this == &rhs) return (*this);
				this->m_oWork.operator=(rhs.m_oWork);
				return (*this);
			}

			/*
			             *this += rhs
			 */
			auto      operator+=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator+=(rhs.m_oWork);
				return (*this);
			}

			/*
			            *this += rhs (move)
			*/
			auto       operator+=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator+=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			              *this -= rhs
			*/
			auto      operator-=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator-=(rhs.m_oWork);
				return (*this);
			}

			/*
			               *this -= rhs
			*/
			auto       operator-=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator-=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			              *this *= rhs
			*/
			auto       operator*=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator*=(rhs.m_oWork);
				return (*this);
			}

			/*
			               *this *= rhs (move)
			 */
			auto       operator*=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator*=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			               *this /= rhs
			*/
			auto        operator/=(_In_ const SIWork &rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator/=(rhs.m_oWork);
				return (*this);
			}

			/*
			               *this /= rhs (move)
			*/
			auto        operator/=(_In_ SIWork &&rhs)->SIWork<Force, Distance, T> & {

				this->m_oWork.operator/=(std::move(rhs.m_oWork));
				return (*this);
			}

			/*
			               *this == rhs
		    */
			auto         operator==(_In_ const SIWork &rhs)->const bool {
				return (this->m_oWork.value() == rhs.m_oWork.value());
			}

			/*
			               *this == rhs (move)
			*/
			auto          operator==(_In_ SIWork &&rhs)-> const bool {
				return (std::move(this->m_oWork.value() == std::move(this->m_oWork.value())));
			}
			/*
			               *this != rhs
		    */
			auto         operator!=(_In_ const SIWork &rhs)->const bool {
				return (!(this->operator==(rhs)));
			}

			/*
			               std::cout << *this

			*/
			auto     display()->const void {

				std::printf("Work = %4.16f J\n", this->m_oWork.value());
			}

		private:

			/*
			@brief    Energy(Work) class variable m_oWork
			*/
			quantity<energy, T> m_oWork;

		};

		/*

		             Wrapper class for boost::units::current

		*/

		/*  primary template */

		template<typename T>  class SICurrent {

		public:

			/*
			@brief    Zero-current default Ctor
			*/
			SICurrent() : m_Magnitude{ static_cast<T>(0.0) }, m_oCurrent{ m_Magnitude * amperes } {}

			/*
			@brief    One-arg Ctor , constructs object of type Current.
			*/
			SICurrent(_In_ const T mag) : m_Magnitude{ mag }, m_oCurrent{ m_Magnitude * amperes } {}

			/*
			@brief    Copy-Ctor.
			*/
			SICurrent(_In_ const SICurrent &rhs) : m_Magnitude{ rhs.m_Magnitude }, m_oCurrent{ rhs.m_oCurrent } {}

			/*
			@brief    Move-Ctor
			*/
			SICurrent(_In_ SICurrent &&rhs) : m_Magnitude{ std::move(rhs.m_Magnitude) }, m_oCurrent{ std::move(rhs.m_oCurrent) } {}

			/*
			@brief    Dtor  = default.
			*/
			~SICurrent() noexcept(true) = default;

			/*

			Class member accessors and operators.

			*/

			/*
			@brief     this->m_Magnitude
			*/
			__forceinline  auto         Magnitude()->const T{

				return (this->m_Magnitude);
			}

			/*
			@brief     this->m_oCurrent
			*/
		   __forceinline auto            Current()->const quantity<current, T> {

			  return (this->m_oCurrent);
			}

		    /*
		    @brief      *this = rhs (copy)
		    */
		   auto        operator=(_In_ const SICurrent &rhs)->SICurrent<T> {

			   if (this == &rhs) return (*this);

			   this->m_Magnitude = rhs.m_Magnitude;
			   this->m_oCurrent.operator=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief        *this = rhs (move)
		   */
		   auto         operator=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   if (this == &rhs) return (*this);

			   this->m_Magnitude = std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief         *this += rhs
		   */
		   auto          operator+=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude += rhs.m_Magnitude;
			   this->m_oCurrent.operator+=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief         *this += rhs (move)
		   */
		   auto           operator+=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude += std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator+=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief         *this -= rhs
		   */
		   auto            operator-=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude -= rhs.m_Magnitude;
			   this->m_oCurrent.operator-=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief          *this -= rhs (move)
		   */
		   auto             operator-=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude -= std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator-=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief         *this *= rhs
		   */
		   auto             operator*=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude *= rhs.m_Magnitude;
			   this->m_oCurrent.operator*=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief          *this *= rhs (move)
		   */
		   auto             operator*=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude *= std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator*=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief           *this /= rhs
		   */
		   auto              operator/=(_In_ const SICurrent &rhs)->SICurrent<T> & {

			   this->m_Magnitude /= rhs.m_Magnitude;
			   this->m_oCurrent.operator/=(rhs.m_oCurrent);
			   return (*this);
		   }

		   /*
		   @brief            *this /= rhs (move)
		   */
		   auto              operator/=(_In_ SICurrent &&rhs)->SICurrent<T> & {

			   this->m_Magnitude /= std::move(rhs.m_Magnitude);
			   this->m_oCurrent.operator/=(std::move(rhs.m_oCurrent));
			   return (*this);
		   }

		   /*
		   @brief              *this == rhs
		   */
		   auto              operator==(_In_ const SICurrent &rhs)->const bool {

			   return (this->m_Magnitude == rhs.m_Magnitude &&
				   this->m_oCurrent.value() == rhs.m_oCurrent.value());
		   }

		   /*
		   @brief                *this !=  rhs
		   */
		   auto              operator!=(_In_ const SICurrent &rhs)->const bool {

			   return (!(this->operator==(rhs)));
		   }

		   /*
		   @brief               operator<< , specialized only for std::complex<T> type.
		   */
		   friend  std::enable_if<std::is_class<std::complex<T>>::value,std::ostream &>::type
			   operator<<(_In_ std::ostream &os, _In_ const SICurrent<T> &rhs) {

				       os << "Current=" << rhs.Magnitude().real() << "Re" << "+" <<
					   rhs.Magnitude().imag() << std::endl;
			   }

		   /*
		   @brief         displays the value of this->m_oCurrent in Ampere unit. Displayed value of
		                   Ampere defualted to double-precision.
		   */
		   auto         display()->void {

			   std::printf("Current=%4.16f A/n", this->m_oCurrent.value());
		   }

		private:

			/*
			@brief   quantity magnitude m_oMagnitude.
			*/
			T   m_Magnitude;

			/*
			@brief   quantity of current m_oCurrent
			*/
			quantity<current, T>  m_oCurrent;

		};

		/*

		          Wrapper class for boost::unit::quantity<resistance,T>

		*/

		template<typename T> class SIResistance {

		   public:

			   /*
			         Zero-resistance Ctor.
			   */
			   SIResistance() : m_Magnitude{ static_cast<T>(0.0) }, m_oResistance{ m_Magnitude * ohm } {}

			   /*
			         Variable-resistance Ctor.
			   */
			   SIResistance(_In_ const T &mag) : m_Magnitude{ mag }, m_oResistance{ m_Magnitude * ohm } {}

			   /*
			          Copy-Ctor.
			   */
			   SIResistance(_In_ const SIResistance &rhs) : m_Magnitude{ rhs.m_Magnitude }, m_oResistance{ rhs.m_oResistance } {}

			   /*
			           Move-Ctor.
			   */
			   SIResistance(_In_ SIResistance &&rhs) : m_Magnitude{ std::move(rhs.m_Magnitude) }, m_oResistance{ std::move(rhs.m_oResistance) } {}

			   /*
			           Dtor = default.
			   */
			   ~SIResistance() noexcept(true) = default;

			   /*

			   Accessor and member operators.

			   */

			   /*
			             Returns this->m_Magnitude
			   */
			   __forceinline auto            Magnitude()->const T{

				   return (this->m_Magnitude);
			   }

			   /*
			             Returns this->m_oResistance.
			   */
			   __forceinline  auto           Resistance()->const quantity<resistance, T> {

				   return (this->m_oResistance);
			   }

			   /*
			                 *this = rhs (copy)
			   */
			   auto            operator=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   if (this == &rhs) return (*this);

				   this->m_Magnitude = rhs.m_Magnitude;
				   this->m_oResistance.operator=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                  *this = rhs (move)
			   */
			   auto            operator=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   if (this == &rhs) return (*this);

				   this->m_Magnitude = std::move(rhs.m_Magnitude);
				   this->m_oResistance = std::move(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                    *this += rhs
				*/
			   auto             operator+=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude += rhs.m_Magnitude;
				   this->m_oResistance.operator+=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                     *this += rhs (move)
			   */
			   auto              operator+=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude += std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator+=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                      *this -= rhs
				*/
			   auto              operator-=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude -= rhs.m_Magnitude;
				   this->m_oResistance.operator-=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                       *this -= rhs (move)
				*/
			   auto              operator-=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude -= std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator-=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                       *this *= rhs
			   */
			   auto               operator*=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude *= rhs.m_Magnitude;
				   this->m_oResistance.operator*=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                        *this *= rhs  (move)
			   */
			   auto               operator*=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude *= std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator*=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                         *this /= rhs
			   */
			   auto                operator/=(_In_ const SIResistance &rhs)->SIResistance<T> & {

				   this->m_Magnitude /= rhs.m_Magnitude;
				   this->m_oResistance.operator/=(rhs.m_oResistance);
				   return (*this);
			   }

			   /*
			                         *this /= rhs
				*/
			   auto                operator/=(_In_ SIResistance &&rhs)->SIResistance<T> & {

				   this->m_Magnitude /= std::move(rhs.m_Magnitude);
				   this->m_oResistance.operator/=(std::move(rhs.m_oResistance));
				   return (*this);
			   }

			   /*
			                         *this == rhs
				*/
		   private:

			   /*
			   @brief       magnitude of quantity<resistance,ohm>
			   */

			   T     m_Magnitude;

			   /*
			   @brief        quantity<resistance,ohm>
			   */
			   quantity<resistance, T> m_oResistance;

		};

		/*
		             mathlib::physics::SIForce namespace operators
		*/

		/*
		     c = a + b
		*/
		template<typename T> __forceinline auto operator+(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		     c  = a * b
		*/
		template<typename T> __forceinline auto operator*(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		     c = a - b
		*/
		template<typename T> __forceinline auto operator-(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		     c  = a / b
		*/
		template<typename T> __forceinline auto operator/(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			SIForce<T> ret_val = SIForce<T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		       a == b

		*/
		template<typename T> __forceinline auto operator==(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			return (lhs.operator==(rhs));
		}

		/*
		       a !=  b
		*/
		template<typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SIForce<T> &lhs,
			_In_ const mathlib::physics::SIForce<T> &rhs)->mathlib::physics::SIForce<T> {

			return (lhs.operator==(rhs));
		}

		/*

		                  mathlib::physics::SILength namespace operators

		*/

		/*
		      c = a + b
		*/
		template<typename T> __forceinline auto operator+(_In_  mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		      c =  a * b
		*/
		template<typename T> __forceinline auto operator*(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		       c = a - b
		*/
		template<typename T> __forceinline auto operator-(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		        c =  a / b
		*/
		template<typename T> __forceinline auto operator/(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->mathlib::physics::SILength<T> {

			SILength<T> ret_val = SILength<T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		        a == b
		*/
		template<typename T> __forceinline auto operator==(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->bool {

			return (lhs.operator==(rhs));
		}

		/*
		       a != b
		*/
		template<typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SILength<T> &lhs,
			_In_ const mathlib::physics::SILength<T> &rhs)->bool {

			return (lhs.operator!=(rhs));
		}

		/*

		           mathlib::physics::SIWork namespace operators.

		*/

		/*
		         c  = lhs + rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator+(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		         c = lhs - rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator-(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		        c = lhs * rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator*(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		         c = lhs / rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator/(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->mathlib::physics::SIWork<Force, Distance, T> {

			SIWork<Force, Distance, T> ret_val = SIWork<Force, Distance, T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		        lhs == rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator==(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->bool {

			return (lhs.operator==(rhs));
		}

		/*
		        lhs != rhs
		*/
		template<class Force, class Distance, typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SIWork<Force, Distance, T> &lhs,
			_In_ const mathlib::physics::SIWork<Force, Distance, T> &rhs)->bool {

			return (lhs.operator!=(rhs));
		}

		/*

		              mathlib::physics::SICurrent namespace operators.

		*/

		/*
		                 c = lhs + rhs
		*/
		template<typename T> __forceinline auto  operator+(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator+=(rhs)};
			return (ret_val);
		}

		/*
		                c = lhs - rhs
		*/
		template<typename T> __forceinline auto operator-(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator-=(rhs)};
			return (ret_val);
		}

		/*
		                c = lhs * rhs
		*/
		template<typename T> __forceinline auto operator*(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator*=(rhs)};
			return (ret_val);
		}

		/*
		                c =  lhs / rhs
		*/
		template<typename T> __forceinline auto operator/(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->SICurrent<T> {

			SICurrent<T> ret_val = SICurrent<T>{lhs.operator/=(rhs)};
			return (ret_val);
		}

		/*
		                 rhs == lhs
		*/
		template<typename T> __forceinline auto operator==(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->bool {

			return (lhs.operator==(rhs));
		}

		/*
		                 rhs != lhs
		*/
		template<typename T> __forceinline auto operator!=(_In_ const mathlib::physics::SICurrent<T> &lhs,
			_In_ const mathlib::physics::SICurrent<T> &rhs)->bool {

			return (lhs.operator!=(rhs));
		}
	}
}
#endif   /*_DIMENSIONAL_ANALYSIS_HPP_*/


//
//  Copyright (c) 2012, Institue of Cancer Research.
//  All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//     * Neither the name of Institue of Cancer Research.
//       nor the names of its contributors may be used to endorse or promote
//       products derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// For more information on the Plane of Best Fit please see http://pubs.acs.org/doi/abs/10.1021/ci300293f
//
//  If this code has been useful to you, please include the reference
//  in any work which has made use of it:

//  Plane of Best Fit: A Novel Method to Characterize the Three-Dimensionality of Molecules, Nicholas C. Firth, Nathan Brown, and Julian Blagg, Journal of Chemical Information and Modeling 2012 52 (10), 2516-2525

//
//
// Created by Nicholas Firth, November 2011
// Modified by Greg Landrum for inclusion in the RDKit distribution November 2012
//

#include "PBFRDKit.h"
#include <Numerics/Matrix.h>
#include <Numerics/SquareMatrix.h>
#include <Numerics/SymmMatrix.h>
#include <boost/foreach.hpp>

#include <Eigen/Dense>
using namespace RDKit;

void getSmallestEigenVector(double fSumXX,double fSumXY,double fSumXZ,
                            double fSumYY,double fSumYZ,double fSumZZ,
                            double &x,double &y, double &z);

double distanceFromAPlane(const RDGeom::Point3D &pt,const std::vector<double> &plane, double denom){
  double numer=0.0;
  numer = std::fabs(pt.x*plane[0]+pt.y*plane[1]+pt.z*plane[2]+plane[3]);

  return numer/denom;
}

bool getBestFitPlane(const std::vector<RDGeom::Point3D> &points,
                     std::vector<double> &plane,
                     const std::vector<double> *weights) {
  PRECONDITION((!weights || weights->size()>=points.size()),"bad weights vector");
  RDGeom::Point3D origin(0,0,0);
  double wSum=0.0;

  for(unsigned int i=0;i<points.size();++i){
    if(weights){
      double w=(*weights)[i];
      wSum+=w;
      origin+=points[i]*w;
    } else {
      wSum+=1;
      origin+=points[i];
    }
  }
  origin /= wSum;

  double sumXX=0,sumXY=0,sumXZ=0,sumYY=0,sumYZ=0,sumZZ=0;
  for(unsigned int i=0;i<points.size();++i){
    RDGeom::Point3D delta=points[i]-origin;
    if(weights){
      double w=(*weights)[i];
      delta *= w;
    }
    sumXX += delta.x*delta.x;
    sumXY += delta.x*delta.y;
    sumXZ += delta.x*delta.z;
    sumYY += delta.y*delta.y;
    sumYZ += delta.y*delta.z;
    sumZZ += delta.z*delta.z;
  }
  sumXX/=wSum;
  sumXY/=wSum;
  sumXZ/=wSum;
  sumYY/=wSum;
  sumYZ/=wSum;
  sumZZ/=wSum;

  Eigen::Matrix3d mat;
  mat << sumXX, sumXY, sumXZ,
    sumXY, sumYY, sumYZ,
    sumXZ, sumYZ, sumZZ;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(mat);
  if(eigensolver.info()!=Eigen::Success){
    BOOST_LOG(rdErrorLog)<<"eigenvalue calculation did not converge"<<std::endl;
    return 0.0;
  }
  RDGeom::Point3D normal;
  normal.x=eigensolver.eigenvectors()(0,0);
  normal.y=eigensolver.eigenvectors()(1,0);
  normal.z=eigensolver.eigenvectors()(2,0);

  plane[0] = normal.x;
  plane[1] = normal.y;
  plane[2] = normal.z;
  plane[3] = -1*normal.dotProduct(origin);

}

double PBFRD(ROMol& mol,int confId){
  PRECONDITION(mol.getNumConformers()>=1,"molecule has no conformers")
  int numAtoms = mol.getNumAtoms();
  if(numAtoms<4) return 0;

  const Conformer &conf = mol.getConformer(confId);
  if(!conf.is3D()) return 0 ;

  std::vector<RDGeom::Point3D> points;
  points.reserve(numAtoms);
  for(unsigned int i=0; i<numAtoms; ++i){
    points.push_back(conf.getAtomPos(i));
  }

  std::vector<double> plane(4);
  getBestFitPlane(points,plane,0);

  double denom=0.0;
  for(unsigned int i=0; i<3; ++i){
    denom += plane[i]*plane[i];
  }
  denom = pow(denom,0.5);

  double res=0.0;
  for(unsigned int i=0; i<numAtoms; ++i){
    res+= distanceFromAPlane(points[i], plane, denom);
  }
  res /= numAtoms;

  return res;
}


// Copyright (c) 2016 Till Kolditz
//
// 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.

/*
 * File:   Euclidean.hpp
 * Author: Till Kolditz <till.kolditz@gmail.com>
 *
 * Created on 6. Dezember 2016, 00:55
 */

#ifndef EUCLIDEAN_HPP
#define EUCLIDEAN_HPP

#include <iostream>
#include <vector>
#include <cinttypes>
#include <exception>
#include <climits>

#include <boost/multiprecision/cpp_int.hpp>

using boost::multiprecision::uint128_t;

namespace Private {
    template<typename T, typename S>
    struct extractor {
    };

    template<typename T>
    struct extractor<T, uint128_t> {
        static T doIt(
                const uint128_t source) {
            const constexpr unsigned nBitsLimb = sizeof(boost::multiprecision::limb_type) * CHAR_BIT; // size of limb in bits
            boost::multiprecision::limb_type target = 0;
            const unsigned nLimbs = source.backend().size(); // number of limbs
            auto pLimbs = source.backend().limbs();
            for (unsigned i = 0; i < nLimbs && ((i * nBitsLimb) < (sizeof(T) * CHAR_BIT)); ++i) {
                target |= (pLimbs[i]) << (i * nBitsLimb);
            }
            return static_cast<T>(target);
        }
    };
}

/*
 * This algorithm actually computes the modulo inverse of the given first argument
 * b0 in the residual class ring modulo <codewidth> (the second argument). One
 * requirement of this algorithm is, that template type <T> is large enough to
 * store <codewidth>+1 bits!
 */
template<typename T>
T ext_euclidean(
        T b0,
        size_t codewidth) {
    if ((sizeof(T) * CHAR_BIT) <= codewidth) {
        throw std::runtime_error("The template datatype is too small!");
    }
    T a0(1);
    a0 <<= codewidth;
    // T a[20], b[20], q[20], r[20], s[20], t[20];
    std::vector<T> a(32), b(32), q(32), r(32), s(32), t(32);
    // std::vector<uint128_t> a(32), b(32), q(32), r(32), s(32), t(32);
    uint8_t aI = 1, bI = 1, qI = 0, rI = 0, sI = 1, tI = 1;
    a[0] = a0;
    b[0] = b0;
    s[0] = 0;
    t[0] = 0;
    ssize_t i = 0;
    do {
        q[qI++] = a[i] / b[i];
        r[rI++] = a[i] % b[i];
        a[aI++] = b[i];
        b[bI++] = r[i];
        s[sI++] = 0;
        t[tI++] = 0;
    } while (b[++i] > 0);
    s[i] = 1;
    t[i] = 0;

    for (ssize_t j = i; j > 0; --j) {
        s[j - 1] = t[j];
        t[j - 1] = s[j] - q[j - 1] * t[j];
    }
    T result = ((b0 * t[0]) % a0);
    result += result < 0 ? a0 : 0;
    if (result == 1) {
        if constexpr (std::is_fundamental_v<T>) {
            return t[0];
        } else if constexpr (std::is_base_of_v<T, uint128_t>) {
            return Private::extractor<T, uint128_t>::doIt(t[0]);
        } else {
            throw std::runtime_error("ext_euclidean not supported for this type!");
        }
    }
    return 0;
}

#endif /* EUCLIDEAN_HPP */


/**
 * Created by Beck on 3/7/2018
 * Indirect Extended Kalman Filter for the translation of the camera
 * fusing visual information with the accelerometer, given the pose;
 * With imu in 400Hz and camera translation in 30Hz
 * V1: estimation for rotation only
 * V2: give the translation of the shield
 */
#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <queue>
//#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;

ros::Publisher pose_pub, debug_pub, odom_pub, debug_propagate_pub;
string imu_topic, pose_topic, visual_topic;
string debug_topic, publisher_topic;
double acc_weight;
double visual_q_weight, visual_t_weight;
int sleep_time;

/**
 * Define states:
 *      x = [translation_x, y, z; velocity_x, y, z]
 * Define inputs:
 *      u = [acc], raw accelerometer
 * Define noises:
 *      n = [n_acc]
 */
VectorXd x(6);         // state
MatrixXd P = MatrixXd::Identity(6, 6); // covariance
MatrixXd R = MatrixXd::Identity(3, 3); // prediction noise covariance
MatrixXd Q = MatrixXd::Identity(3, 3); // observation noise covariance

// buffers to save gyro and visual reading
queue<sensor_msgs::Imu::ConstPtr> imu_buf;
queue<geometry_msgs::TwistStamped::ConstPtr> visual_buf;
queue<Matrix<double, 6, 1>> x_history;
queue<Matrix<double, 6, 6>> P_history;
Vector3d G = {0, 0, 9.8}; // Consider to add initialization later

double t_prev;          // previous propagated time

// Initialization
const int IMU_INIT_COUNT = 10;
const int MAX_GYRO_QUEUE_SIZE = 400;
const double IMU_UPDATE_TIME = 0.0025; // 1 / 400Hz
int imu_count  = 0;
bool imu_initialized = false;
bool visual_initialized = false;
bool visual_valid = false;
MatrixXd imu_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
Vector3d imu_T_camera = MatrixXd::Zero(3, 1);

//// DEBUG only
double t_prev_update;   // previous update time
Vector3d world_T_shield_prev = MatrixXd::Zero(3, 1); // previous visual translation

void pub_shield_odom(const std_msgs::Header& header)
{
    nav_msgs::Odometry odom;
    odom.header = header;
    odom.child_frame_id = "world";
    odom.pose.pose.position.x = x(0);
    odom.pose.pose.position.y = x(1);
    odom.pose.pose.position.z = x(2);
    odom.twist.twist.linear.x = x(3);
    odom.twist.twist.linear.y = x(4);
    odom.twist.twist.linear.z = x(5);

    odom.pose.covariance[0]  = P(0, 0);
    odom.pose.covariance[7]  = P(1, 1);
    odom.pose.covariance[14] = P(2, 2);
    odom.pose.covariance[21] = P(3, 3);
    odom.pose.covariance[28] = P(4, 4);
    odom.pose.covariance[35] = P(5, 5);
    odom.pose.covariance[3]  = P(0, 3);
    odom.pose.covariance[10] = P(1, 4);
    odom.pose.covariance[17] = P(2, 5);
    odom.pose.covariance[18] = P(3, 0);
    odom.pose.covariance[25] = P(4, 1);
    odom.pose.covariance[32] = P(5, 2);
    odom_pub.publish(odom);
}

void pub_debug_update(const std_msgs::Header& header,
                      const Ref<const Vector3d> p,
                      const Quaterniond& q,
                      double t_update)
{
    nav_msgs::Odometry odom;
    odom.header = header;
    odom.child_frame_id = "world";
    odom.pose.pose.position.x = p[0];
    odom.pose.pose.position.y = p[1];
    odom.pose.pose.position.z = p[2];
    odom.pose.pose.orientation.w = q.w();
    odom.pose.pose.orientation.x = q.x();
    odom.pose.pose.orientation.y = q.y();
    odom.pose.pose.orientation.z = q.z();
    if (visual_initialized) {
        double dt_update = t_update - t_prev_update;
        odom.twist.twist.linear.x = (p[0] - world_T_shield_prev[0]) / dt_update;
        odom.twist.twist.linear.y = (p[1] - world_T_shield_prev[1]) / dt_update;
        odom.twist.twist.linear.z = (p[2] - world_T_shield_prev[2]) / dt_update;

        world_T_shield_prev = p;
        t_prev_update = t_update;
    }
    else {
        odom.twist.twist.linear.x = 0;
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.linear.z = 0;
        world_T_shield_prev = p;
        t_prev_update = t_update;
    }

    debug_pub.publish(odom);
}

void pub_debug_propagate(const std_msgs::Header& header,
                         const Ref<const Vector3d> v,
                         const Ref<const Vector3d> a_int)
{
    geometry_msgs::TwistStamped acc_compare;
    acc_compare.header = header;
    acc_compare.twist.linear.x = v[0];
    acc_compare.twist.linear.y = v[1];
    acc_compare.twist.linear.z = v[2];
    acc_compare.twist.angular.x= a_int[0];
    acc_compare.twist.angular.y= a_int[1];
    acc_compare.twist.angular.z= a_int[2];
    debug_propagate_pub.publish(acc_compare);
}

// DEBUG only
Vector3d a_int = MatrixXd::Zero(3, 1);
Vector3d a_int_int = MatrixXd::Zero(3, 1);
void propagate(const sensor_msgs::Imu &imu)
{
    double cur_t = imu.header.stamp.toSec();
    Vector3d a, acc_wo_g;
    a(0) = imu.linear_acceleration.x;
    a(1) = imu.linear_acceleration.y;
    a(2) = imu.linear_acceleration.z;
    Quaterniond world_R_imu(imu.orientation.w, imu.orientation.x, imu.orientation.y, imu.orientation.z);

    double dt = cur_t - t_prev;
    dt = (dt < IMU_UPDATE_TIME * 5) ? dt : IMU_UPDATE_TIME * 5;
    ROS_INFO("dt in propagate is %f, at the cur_t %f", dt, cur_t);
    acc_wo_g = world_R_imu.toRotationMatrix() * a - G;
    x.segment<3>(0) += x.segment<3>(3) * dt + 0.5 * acc_wo_g * dt * dt;
    x.segment<3>(3) += acc_wo_g * dt;

    a_int_int += a_int * dt + 0.5 * acc_wo_g * dt * dt;
    a_int += acc_wo_g * dt;
    pub_debug_propagate(imu.header, acc_wo_g, a_int_int);

    MatrixXd A = MatrixXd::Zero(6, 6);
    A.block<3, 3>(0, 3) = MatrixXd::Identity(3, 3);

    MatrixXd U = -MatrixXd::Zero(6, 3);
    U.block<3, 3>(3, 0) = -world_R_imu.toRotationMatrix();

    MatrixXd F, V;
    F = MatrixXd::Identity(6, 6) + dt * A;
    V = dt * U;
//    cout << "F " << endl << F << endl;
//    cout << "R " << endl << R << endl;
//    cout << "V " << endl << V << endl;
    P = F * P * F.transpose() + V * R * V.transpose();

    t_prev = cur_t;
//    cout << "P " << endl << P << endl;
//    cout << "x " << endl << x.transpose() << endl;
}

static void throwState(double current_time)
{
    while(imu_buf.size() > 1 &&
          imu_buf.front()->header.stamp.toSec() < current_time)
    {
        // trace backward the time to the imu timestamp
        t_prev = imu_buf.front()->header.stamp.toSec();
        ROS_INFO("throw state with time: %f", t_prev);
        imu_buf.pop();
        x_history.pop();
        P_history.pop();
    }
}

static void repropagate()
{
    while (!x_history.empty()) x_history.pop();
    while (!P_history.empty()) P_history.pop();

    queue <sensor_msgs::Imu::ConstPtr> temp_imu_buf;
    while (!imu_buf.empty() && imu_buf.size() > 1)
    {
        propagate(*imu_buf.front());
        temp_imu_buf.push(imu_buf.front());
        x_history.push(x);
        P_history.push(P);
        imu_buf.pop();
    }
    if (!temp_imu_buf.empty()) {
        pub_shield_odom(temp_imu_buf.back()->header);
    }
    swap(imu_buf, temp_imu_buf);
}

static void update(const geometry_msgs::TwistStamped &pnp)
{
    double cur_t = pnp.header.stamp.toSec();
    Vector3d camera_T_shield;

    camera_T_shield[0] = pnp.twist.linear.x;
    camera_T_shield[1] = pnp.twist.linear.y;
    camera_T_shield[2] = pnp.twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    imu_T_shield *= 0.001; // Convert millimeter to meter

    throwState(cur_t);

    ROS_INFO("Update, at time %f", cur_t);
    Quaterniond world_R_imu;

    if (imu_buf.empty()) {
        world_R_imu.setIdentity();
    }
    else {
        world_R_imu = Quaterniond(imu_buf.front()->orientation.w,
                                  imu_buf.front()->orientation.x,
                                  imu_buf.front()->orientation.y,
                                  imu_buf.front()->orientation.z);
    }
    Vector3d world_T_shield = world_R_imu.toRotationMatrix() * imu_T_shield;
    pub_debug_update(pnp.header, world_T_shield, world_R_imu, cur_t);

    MatrixXd C = MatrixXd::Zero(3, 6);
    C.block<3, 3>(0, 0) = MatrixXd::Identity(3, 3);

    Matrix3d W = MatrixXd::Identity(3, 3);

    MatrixXd K(6, 3);
    K = P * C.transpose() * (C * P * C.transpose() + W * Q * W.transpose()).inverse();
//    cout << "C " << endl << C << endl;
//    cout << "K " << endl << K << endl;
//    cout << "Q " << endl << Q << endl;
    x = x + K * (world_T_shield - C * x);
    P = P - K * C * P;
//    cout << "P " << endl << P << endl;
//    cout << "x " << endl << x.transpose() << endl;

    if (imu_buf.size() > 1) {
        // repropagate();
    }
}

/**
 * initialize the imu messages
 * @param imu
 */
static void initialize_imu(const sensor_msgs::Imu::ConstPtr &imu)
{
    t_prev = imu->header.stamp.toSec();
    x_history.push(x);
    P_history.push(P);
    imu_buf.push(imu);
    imu_count++;
    if (imu_count == IMU_INIT_COUNT) {
        imu_initialized = true;
    }
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    double cur_t = pnp->header.stamp.toSec();
    Vector3d camera_T_shield;
    ROS_INFO("visual init at %f", cur_t);

    camera_T_shield[0] = pnp->twist.linear.x;
    camera_T_shield[1] = pnp->twist.linear.y;
    camera_T_shield[2] = pnp->twist.linear.z;
    Vector3d imu_T_shield = imu_R_camera * camera_T_shield + imu_T_camera;

    imu_T_shield *= 0.001; // Convert millimeter to meter

    throwState(cur_t);

    Quaterniond world_R_imu;

    if (imu_buf.empty()) {
        world_R_imu.setIdentity();
    }
    else {
        world_R_imu = Quaterniond(imu_buf.front()->orientation.w,
                                  imu_buf.front()->orientation.x,
                                  imu_buf.front()->orientation.y,
                                  imu_buf.front()->orientation.z);
    }
    Vector3d world_T_shield = world_R_imu.toRotationMatrix() * imu_T_shield;
    pub_debug_update(pnp->header, world_T_shield, world_R_imu, cur_t);

    x.setZero();
    x.segment<3>(0) = world_T_shield;
    x.segment<3>(3) << 0, 0, 0;

    cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;

    visual_initialized = true;

    // repropagate();
}

/**
 * handle, save, and process visual messages
 * @param pnp
 */
void visual_callback(const geometry_msgs::TwistStamped::ConstPtr &pnp)
{
    visual_valid = !(pnp->twist.linear.x == 0 &&
        pnp->twist.linear.y == 0 &&
        pnp->twist.linear.z == 0 );

    if (visual_valid) {
        if (visual_initialized) {
            update(*pnp);
        }
        else {
            initialize_visual(pnp);
        }
    }
}

/**
 * handle 400Hz raw imu data
 * @param imu
 */
void imu_callback(const sensor_msgs::Imu::ConstPtr &imu)
{
    if (!imu_initialized) {
        initialize_imu(imu);
    }
    else {
        propagate(*imu);
        x_history.push(x);
        P_history.push(P);
        imu_buf.push(imu);
		if (imu_buf.size() > MAX_GYRO_QUEUE_SIZE) {
			x_history.pop();
			P_history.pop();
			imu_buf.pop();
		}
        pub_shield_odom(imu->header);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "visual_gyro_fused");
    ros::NodeHandle n("~");

//    n.param("imu_raw", imu_topic, string("/dji_sdk/imu")); // 400Hz
    n.param("imu_pose", pose_topic, string("/attitude_estimator/imu")); // 400Hz
    n.param("visual_topic", visual_topic, string("/pnp_twist"));
    n.param("publisher_topic", publisher_topic, string("/visual_ekf/shield_T_world"));
    n.param("debug_topic", debug_topic, string("/visual_ekf/debug_odom"));
    n.param("accelerometer_noise_weight", acc_weight, 1000.0);
    n.param("visual_pose_weight", visual_q_weight, 10.0);
    n.param("node_sleep_time", sleep_time, 0);

    ros::Duration(sleep_time).sleep();

    // TODO: initalize the R and Q matrix
    R =      acc_weight * MatrixXd::Identity(3, 3); // accelerometer noise
    Q = visual_q_weight * MatrixXd::Identity(3, 3); // observation noise

    x.setZero();

    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;

    imu_T_camera <<  200, 50, 0; // in millimeter

    ros::Subscriber s2 = n.subscribe(visual_topic, 10, visual_callback);
    ros::Subscriber s3 = n.subscribe(pose_topic, 100, imu_callback);
//    pose_pub = n.advertise<geometry_msgs::PoseStamped>(publisher_topic, 100);
    odom_pub = n.advertise<nav_msgs::Odometry>(publisher_topic, 100);
    debug_pub= n.advertise<nav_msgs::Odometry>(debug_topic, 100);
    debug_propagate_pub = n.advertise<geometry_msgs::TwistStamped>(string("/visual_ekf/propagate"), 100);

    ros::Rate r(100);
    ros::spin();
}

/**
 *  0   T_x     translation of the shield in world frame
 *  1   T_y
 *  2   T_z
 *  3   v_x     velocity of the shield in world frame
 *  4   v_y
 *  5   v_z
 */


#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#endif
#include <cmath>

#ifndef NOMINMAX
#define NOMINMAX
#endif

#include <nlohmann/json.hpp>
#include <Eigen/Dense>
#include <cxxopts.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/calib3d.hpp>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <string>
#include <array>
#include <algorithm>
#include <exception>
#include <functional>

using json = nlohmann::json;

using mat2x4 = Eigen::Matrix<double, 2, 4>;
using mat3x4 = Eigen::Matrix<double, 3, 4>;
using mat3 = Eigen::Matrix3d;
using mat4 = Eigen::Matrix4d;
using vec2 = Eigen::Vector2d;
using vec3 = Eigen::Vector3d;
using vec4 = Eigen::Vector4d;

// Fixed-size Eigen types' allocation must be aligned
template<typename T>
using e_vec = std::vector<T, Eigen::aligned_allocator<T>>;

void parse_camera_intrinsics(json const &camera_intrinsics,
                             mat3x4& intrinsic_matrix)
{
    auto const focal_length_x = camera_intrinsics["focalLengthX"].get<float>();
    auto const focal_length_y = camera_intrinsics["focalLengthY"].get<float>();
    auto const principal_point_x = camera_intrinsics["principalPointX"].get<float>();
    auto const principal_point_y = camera_intrinsics["principalPointY"].get<float>();

    intrinsic_matrix = mat3x4::Zero();
    intrinsic_matrix(0, 0) = focal_length_x;
    intrinsic_matrix(1, 1) = focal_length_y;
    intrinsic_matrix(0, 2) = principal_point_x;
    intrinsic_matrix(1, 2) = principal_point_y;
    intrinsic_matrix(2, 2) = 1.0f;
}

mat4 find_tag_space_pose(mat3x4 const& P, mat2x4 const& Y, mat4 const& Z)
{
    auto cv_Z = std::vector<cv::Point3d>{
        cv::Point3d{ Z(0, 0), Z(1, 0), Z(2, 0) },
        cv::Point3d{ Z(0, 1), Z(1, 1), Z(2, 1) },
        cv::Point3d{ Z(0, 2), Z(1, 2), Z(2, 2) },
        cv::Point3d{ Z(0, 3), Z(1, 3), Z(2, 3) },
    };
    auto cv_Y = std::vector<cv::Point2d>{
        cv::Point2d{ Y(0, 0), Y(1, 0) }, // bottom-left
        cv::Point2d{ Y(0, 1), Y(1, 1) }, // bottom-right
        cv::Point2d{ Y(0, 2), Y(1, 2) }, // top-right
        cv::Point2d{ Y(0, 3), Y(1, 3) }, // top-left
    };
    cv::Matx33d K = {
        P(0, 0), P(0, 1), P(0, 2),
        P(1, 0), P(1, 1), P(1, 2),
        P(2, 0), P(2, 1), P(2, 2),
    };
    cv::Vec3d r;
    cv::Vec3d T;
    cv::solvePnP(cv_Z, cv_Y, K, cv::Vec4d{ 0, 0, 0, 0 }, r, T);
    cv::Matx33d R;
    cv::Rodrigues(r, R);
    mat4 C;
    C << R(0, 0), R(0, 1), R(0, 2), T(0),
        R(1, 0), R(1, 1), R(1, 2), T(1),
        R(2, 0), R(2, 1), R(2, 2), T(2),
        0, 0, 0, 1;
    return C;
}

// Input in jsonl format (file or stdin) (file not supported currently):
//
//      {
//          "framePath": "/path/to/frames/123.png",
//          "cameraIntrinsics": {focal lengths, principal point...},
//          ... (any other elements, which are not used)
//      }
//
// Output is same data but with added 'tag_space_pose' 3x4 matrix (as a list of rows)
int main(int argc, char* argv[])
{
    auto input_file_option = std::string{};
    auto output_file_option = std::string{};
    auto tag_side_length = 0.0;

    cxxopts::Options options(argv[0], "");
    options.add_options()
        ("i,input", "Path to the VIO input file", cxxopts::value(input_file_option))
        ("o,output", "Path to the output file", cxxopts::value(output_file_option))
        ("s,tag_side_length", "Length of the tag's sides in meters in the input images", cxxopts::value(tag_side_length))
        ;

    auto parsed_args = options.parse(argc, argv);
    if (parsed_args.count("help"))
    {
        std::cout << options.help() << std::endl;
        return 0;
    }

    auto arg_missing = [&](const char* arg_name)
    {
        if (parsed_args.count(arg_name) == 0)
        {
            std::fprintf(stderr, "Missing argument: %s.\n", arg_name);
            std::cerr << options.help() << std::endl;
            std::cerr << "See README.md for more instructions." << std::endl;
            return true;
        }
        return false;
    };

    if (arg_missing("input")) return 1;
    if (arg_missing("output")) return 1;
    if (arg_missing("tag_side_length")) return 1;

    std::ifstream input(input_file_option);
    std::ofstream output(output_file_option);

    auto s = tag_side_length;
    mat4 Z;
    Z.col(0) = vec4{ -s/2, -s/2, 0, 1, }; // bottom-left
    Z.col(1) = vec4{ s/2, -s/2, 0, 1, }; // bottom-right
    Z.col(2) = vec4{ s/2, s/2, 0, 1, }; // top-right
    Z.col(3) = vec4{ -s/2, s/2, 0, 1, }; // top-left

    // This is constant, tag is placed on top of base station in expected way
    mat4 tag_orientation_in_tracking_space = mat4::Identity();
    tag_orientation_in_tracking_space.col(0) = vec4{ -1, 0, 0, 0 };
    tag_orientation_in_tracking_space.col(1) = vec4{ 0, 0, 1, 0 };
    tag_orientation_in_tracking_space.col(2) = vec4{ 0, 1, 0, 0 };

    // Read in VIO data, and solve VIO device position in tracking space from Apriltag in frame
    std::string line;
    while (std::getline(input, line))
    {
        auto j = json::parse(line);
        auto markers = j["markers"];

        // Note: no output if tag is not found or is ambiguous
        if (markers.size() == 1)
        {
            mat3x4 P;
            parse_camera_intrinsics(j["cameraIntrinsics"], P);

            mat2x4 cv_Y;
            auto const& d = markers[0];
            cv_Y << d[0][0], d[1][0], d[2][0], d[3][0],
                d[0][1], d[1][1], d[2][1], d[3][1];

            auto C = find_tag_space_pose(P, cv_Y, Z);

            j["tag_space_pose"] = {
                { C(0, 0), C(0, 1), C(0, 2), C(0, 3), },
                { C(1, 0), C(1, 1), C(1, 2), C(1, 3), },
                { C(2, 0), C(2, 1), C(2, 2), C(2, 3), },
            };

            output << j.dump() << "\n";

        }
    }
}


#include <stdio.h>
#include <math.h>

#include <QGraphicsView>
#include <QPen>

#include <Eigen/Dense>

#include <libbwcnc/part.h>
#include <libbwcnc/mceschliz.h>
#include <libbwcnc/bwcnc.h>
#include <libbwcnc/stdtforms.h>

#include "mainwindow.h"

//using namespace BWCNC;

void mainwindow::refresh_hexgrid()
{
    BWCNC::PartContext k;
    BWCNC::LizardGrid lg;

    lg.fill_partctx_with_grid( k );

    shift2positive( k );
    //shift2center_z( k );

    QImage img( scene->sceneRect().width(), scene->sceneRect().height(), QImage::Format_RGB32 );
    BWCNC::PixmapRenderer renderer( &img );
    renderer.render_positive_z = p_bool;
    renderer.render_negative_z = n_bool;

    renderer.set_moveto_color( nullptr );
    renderer.set_lineto_color( "#ff0000" );
    renderer.set_backgd_color( "#fe8736" );

#if 1
    BWCNC::SVG svgrenderer;
    svgrenderer.render_all( k );
#endif

    renderer.render_all( k );

    QPixmap pxmp;
    if( pxmp.convertFromImage( img ) )
        pixmap_item->setPixmap(pxmp);
}


#include <iostream>
#include <fstream>
#include <ctime>
#include <cmath>
#include <vector>
#include <set>
#include <map>
#include <sstream>
#include <algorithm>

// #define CMG_VERBOSE
#define MEMORYBOOKKEEPING

#include "Model.h"

#include "Map.h"
#include "ChompMap.h"
#include "MorseGraph.h"
#include "Compute_Morse_Graph.h"
#include "RectGeo.h"

#include "SingleOutput.h"
#include "simple_interval.h"

#include "Configuration.h"

#include "libgp/libgp.hpp"

#include <boost/serialization/export.hpp>
#include "SuccinctGrid.h"
BOOST_CLASS_EXPORT_IMPLEMENT(SuccinctGrid);
#include "PointerGrid.h"
BOOST_CLASS_EXPORT_IMPLEMENT(PointerGrid);

std::pair<MorseGraph, MapGraph> ComputeConleyMorseGraph ( Model const& model ) {
  std::shared_ptr<const Map> map = model . map ();
  MorseGraph morsegraph ( model . phaseSpace () );
  std::shared_ptr < Grid > phase_space = morsegraph . phaseSpace ();

  int phase_subdiv_init = model . phase_subdiv_init ();
  int phase_subdiv_min = model . phase_subdiv_min ();
  int phase_subdiv_max = model . phase_subdiv_max ();
  int phase_subdiv_limit = model . phase_subdiv_limit ();

  // Compute Morse graph
  Compute_Morse_Graph ( & morsegraph, phase_space, map, phase_subdiv_init,
                        phase_subdiv_min, phase_subdiv_max, phase_subdiv_limit );

  std::shared_ptr < TreeGrid > phase_space_chomp =
    std::dynamic_pointer_cast<TreeGrid> ( morsegraph . phaseSpace () );

  if ( not phase_space_chomp ) {
    throw std::runtime_error ( "Cannot interface with chomp for this grid type!" );
  }

  typedef std::vector < Grid::GridElement > Subset;
  for ( size_t v = 0; v < morsegraph . NumVertices (); ++ v) {
    Subset subset = phase_space_chomp -> subset ( * morsegraph . grid ( v ) );
    std::shared_ptr<chomp::ConleyIndex_t> conley ( new chomp::ConleyIndex_t );
    morsegraph . conleyIndex ( v ) = conley;
    ChompMap chomp_map ( map );
    chomp::ConleyIndex ( conley . get (), *phase_space_chomp, subset, chomp_map );
  }

  // Compute multi-valued map digraph
  MapGraph map_graph ( phase_space, map );

  return std::make_pair ( morsegraph, map_graph );
}

std::pair<MorseGraph, MapGraph> ComputeMorseGraph ( Model const& model ) {
  std::shared_ptr<const Map> map = model . map ();
  MorseGraph morsegraph ( model . phaseSpace () );
  std::shared_ptr < Grid > phase_space = morsegraph . phaseSpace ();

  int phase_subdiv_init = model . phase_subdiv_init ();
  int phase_subdiv_min = model . phase_subdiv_min ();
  int phase_subdiv_max = model . phase_subdiv_max ();
  int phase_subdiv_limit = model . phase_subdiv_limit ();

  // Compute Morse graph
  Compute_Morse_Graph ( & morsegraph, phase_space, map, phase_subdiv_init,
                        phase_subdiv_min, phase_subdiv_max, phase_subdiv_limit );

  // Compute multi-valued map digraph
  MapGraph map_graph ( phase_space, map );

  return std::make_pair ( morsegraph, map_graph );
}

void computeMorseGraph ( MorseGraph & morsegraph,
                         std::shared_ptr<const Map> map,
                         const int SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                         const int SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                         const int SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                         const int SINGLECMG_COMPLEXITY_LIMIT,
                         const char * outputfile ) {
#ifdef CMG_VERBOSE
  std::cout << "SingleCMG: computeMorseGraph.\n";
#endif
  std::shared_ptr < Grid > phase_space = morsegraph . phaseSpace ();
  clock_t start_time = clock ();
  Compute_Morse_Graph ( & morsegraph,
                        phase_space,
                        map,
                        SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                        SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                        SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                        SINGLECMG_COMPLEXITY_LIMIT );
  clock_t stop_time = clock ();
  if ( outputfile != NULL ) {
    morsegraph . save ( outputfile );
  }
  std::ofstream stats_file ( "SingleCMG_statistics.txt" );
  stats_file << "Morse Graph calculation resource usage statistics.\n";
  stats_file << "The final grid has " << phase_space -> size () << " grid elements.\n";
  stats_file << "The computation took " << ((double)(stop_time-start_time)/(double)CLOCKS_PER_SEC)
             << " seconds.\n";
  stats_file << "All memory figures are in bytes:\n";
  stats_file << "grid_memory_use = " << phase_space -> memory () << "\n";
  stats_file << "max_graph_memory = " << max_graph_memory << "\n";
  stats_file << "max_scc_memory_internal = " << max_scc_memory_internal << "\n";
  stats_file << "max_scc_memory_external = " << max_scc_memory_external << "\n";
  stats_file . close ();
}

MorseGraph MorseGraphIntvalMap ( int phase_subdiv_min, int phase_subdiv_max,
                                 std::vector<double> const& phase_lower_bounds,
                                 std::vector<double> const& phase_upper_bounds,
                                 std::vector<double> const& params,
                                 std::string output_file_name ) {
  std::vector<double> param_lower_bounds = params;
  std::vector<double> param_upper_bounds = params;
  int param_dim = params . size();
  int phase_dim = phase_lower_bounds . size();
  std::vector<bool> phase_periodic ( phase_dim, false );
  int phase_subdiv_init = 0;
  int phase_subdiv_limit = 10000;

  Model model;
  model . initialize ( param_dim, phase_dim,
                       phase_subdiv_min, phase_subdiv_max,
                       phase_subdiv_init, phase_subdiv_limit,
                       param_lower_bounds, param_upper_bounds,
                       phase_lower_bounds, phase_upper_bounds,
                       phase_periodic );
  std::shared_ptr<const Map> map = model . map ();

  MorseGraph morsegraph ( model . phaseSpace () );

  // INITIALIZE THE PHASE SPACE SUBDIVISION PARAMETERS
  int SINGLECMG_INIT_PHASE_SUBDIVISIONS = phase_subdiv_init;
  int SINGLECMG_MIN_PHASE_SUBDIVISIONS = phase_subdiv_min;
  int SINGLECMG_MAX_PHASE_SUBDIVISIONS = phase_subdiv_max;
  int SINGLECMG_COMPLEXITY_LIMIT= phase_subdiv_limit;

  // COMPUTE MORSE GRAPH
  computeMorseGraph ( morsegraph, map,
                      SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                      SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                      SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                      SINGLECMG_COMPLEXITY_LIMIT,
                      output_file_name . c_str () );

  std::cout << "Total Time for Finding Morse Sets ";
  std::cout << "and reachability relation: ";
  std::cout << ": ";

  // Always output the Morse Graph
  // std::cout << "Creating graphviz .dot file...\n";
  // CreateDotFile ( "morsegraph.gv", conleymorsegraph );

  return morsegraph;
}

MorseGraph MorseGraphMap ( int phase_subdiv_min, int phase_subdiv_max,
                           std::vector<double> const& phase_lower_bounds,
                           std::vector<double> const& phase_upper_bounds,
                           std::string output_file_name,
                           std::function<std::vector<double>(std::vector<double>)> const& F ) {
  std::vector<double> params {0.0};
  std::vector<double> param_lower_bounds = params;
  std::vector<double> param_upper_bounds = params;
  int param_dim = params . size();
  int phase_dim = phase_lower_bounds . size();
  std::vector<bool> phase_periodic ( phase_dim, false );
  int phase_subdiv_init = 0;
  int phase_subdiv_limit = 10000;

  Model model;
  model . initialize ( param_dim, phase_dim,
                       phase_subdiv_min, phase_subdiv_max,
                       phase_subdiv_init, phase_subdiv_limit,
                       param_lower_bounds, param_upper_bounds,
                       phase_lower_bounds, phase_upper_bounds,
                       phase_periodic, F );
  std::shared_ptr<const Map> map = model . map ();

  MorseGraph morsegraph ( model . phaseSpace () );

  // INITIALIZE THE PHASE SPACE SUBDIVISION PARAMETERS
  int SINGLECMG_INIT_PHASE_SUBDIVISIONS = phase_subdiv_init;
  int SINGLECMG_MIN_PHASE_SUBDIVISIONS = phase_subdiv_min;
  int SINGLECMG_MAX_PHASE_SUBDIVISIONS = phase_subdiv_max;
  int SINGLECMG_COMPLEXITY_LIMIT= phase_subdiv_limit;

  // COMPUTE MORSE GRAPH
  computeMorseGraph ( morsegraph, map,
                      SINGLECMG_INIT_PHASE_SUBDIVISIONS,
                      SINGLECMG_MIN_PHASE_SUBDIVISIONS,
                      SINGLECMG_MAX_PHASE_SUBDIVISIONS,
                      SINGLECMG_COMPLEXITY_LIMIT,
                      output_file_name . c_str () );

  std::cout << "Total Time for Finding Morse Sets ";
  std::cout << "and reachability relation: ";
  std::cout << ": ";

  // Always output the Morse Graph
  // std::cout << "Creating graphviz .dot file...\n";
  // CreateDotFile ( "morsegraph.gv", conleymorsegraph );

  return morsegraph;
}

/// Python Bindings

#include <pybind11/pybind11.h>
#include <pybind11/functional.h>
#include <pybind11/stl.h>

namespace py = pybind11;

PYBIND11_MODULE(_cmgdb, m) {
  ModelBinding(m);
  GridBinding(m);
  MapGraphBinding(m);
  MorseGraphBinding(m);

  m.doc() = "Conley Morse Graph Database Module";

  m.def("ComputeConleyMorseGraph", &ComputeConleyMorseGraph);
  m.def("ComputeMorseGraph", &ComputeMorseGraph);
  m.def("MorseGraphIntvalMap", &MorseGraphIntvalMap);
  m.def("MorseGraphMap", &MorseGraphMap);
}


#ifndef MULTIPRECISION_MPENCVECTOR_HPP
#define MULTIPRECISION_MPENCVECTOR_HPP
#include "algebra/EncVector.hpp"
#include "algebra/Vector.hpp"
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
class MPSecKey;
class MPPubKey;
class MPEncArray;
class MPEncVector {
public:
    MPEncVector(const MPPubKey &pk);

    void pack(const MDL::Vector<long> &vec,
              const MPEncArray &ea);

    void unpack(MDL::Vector<NTL::ZZ> &vec,
                const MPSecKey &sk,
                const MPEncArray &ea,
                bool negate = true);

    void multiplyBy(const MPEncVector &oth);

    /// scalar product
    MPEncVector& dot(const MPEncVector &oth,
                     const MPEncArray &ea);

    MPEncVector& operator*=(const MPEncVector &oth);

    MPEncVector& operator+=(const MPEncVector &oth);

    MPEncVector& operator-=(const MPEncVector &oth);

    void negate();

    MPEncVector& addConstant(const MDL::Vector<long> &con,
                             const MPEncArray &ea);

    MPEncVector& mulConstant(const MDL::Vector<long> &con,
                             const MPEncArray &ea);

    size_t partsNum() const { return ctxts.size(); }

    MDL::EncVector& get(int index) { return ctxts[index]; }

    void reLinearize();

    long getLength() const { return length; }

    void setLength(long l) { length = l; }
private:
    long length = -1;
    std::vector<MDL::EncVector> ctxts;
};
#endif // multiprecision/MPEncVector.hpp


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

#ifndef CRYPTO3_HASH_POSEIDON_CONSTANTS_HPP
#define CRYPTO3_HASH_POSEIDON_CONSTANTS_HPP

#include <nil/crypto3/hash/detail/poseidon/poseidon_policy.hpp>
#include <nil/crypto3/hash/detail/poseidon/poseidon_mds_matrix.hpp>
#include <nil/crypto3/hash/detail/poseidon/poseidon_lfsr.hpp>

#include <boost/assert.hpp>

namespace nil {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<typename FieldType, std::size_t Arity, std::size_t PartRounds>
                struct poseidon_constants_operator {
                    typedef FieldType field_type;
                    typedef poseidon_policy<field_type, Arity, PartRounds> policy_type;
                    typedef poseidon_mds_matrix<field_type, Arity, PartRounds> matrix_policy_type;
                    typedef poseidon_lfsr<field_type, Arity, PartRounds> constants_generator_policy_type;

                    typedef typename field_type::value_type element_type;
                    typedef typename matrix_policy_type::state_vector_type state_vector_type;

                    constexpr static const std::size_t state_words = policy_type::state_words;
                    typedef typename policy_type::state_type state_type;

                    constexpr static const std::size_t full_rounds = policy_type::full_rounds;
                    constexpr static const std::size_t half_full_rounds = policy_type::half_full_rounds;
                    constexpr static const std::size_t part_rounds = policy_type::part_rounds;

                    constexpr static const std::size_t round_constants_size = (full_rounds + part_rounds) * state_words;
                    constexpr static const std::size_t equivalent_round_constants_size =
                        (full_rounds + 1) * state_words + part_rounds - 1;
                    typedef algebra::vector<element_type, equivalent_round_constants_size>
                        equivalent_round_constants_type;

                    /*
                     * =========================================================
                     * Optimized
                     * =========================================================
                     */

                    inline void arc_sbox_mds_full_round_optimized_first(state_vector_type &A,
                                                                        std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number < half_full_rounds,
                                         "wrong using: arc_sbox_mds_full_round_optimized_first");
                        std::size_t constant_number_base = round_number * state_words;
                        for (std::size_t i = 0; i < state_words; i++) {
                            A[i] += get_equivalent_round_constant(constant_number_base + i);
                            A[i] = A[i] * A[i] * A[i] * A[i] * A[i];
                        }
                        policy_matrix.product_with_mds_matrix(A);
                    }

                    inline void arc_sbox_mds_full_round_optimized_last(state_vector_type &A,
                                                                       std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number >= half_full_rounds + part_rounds,
                                         "wrong using: arc_sbox_mds_full_round_optimized_last");
                        std::size_t constant_number_base =
                            (half_full_rounds + 1) * state_words + (part_rounds - 1) +
                            (round_number - half_full_rounds - part_rounds) * state_words;
                        for (std::size_t i = 0; i < state_words; i++) {
                            A[i] += get_equivalent_round_constant(constant_number_base + i);
                            A[i] = A[i] * A[i] * A[i] * A[i] * A[i];
                        }
                        policy_matrix.product_with_mds_matrix(A);
                    }

                    inline void arc_mds_part_round_optimized_init(state_vector_type &A,
                                                                  std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number == half_full_rounds,
                                         "wrong using: arc_mds_part_round_optimized_init");
                        std::size_t constant_number_base = half_full_rounds * state_words;
                        for (std::size_t i = 0; i < state_words; i++) {
                            A[i] += get_equivalent_round_constant(constant_number_base + i);
                        }
                        policy_matrix.product_with_equivalent_mds_matrix_init(A, round_number);
                    }

                    inline void sbox_arc_mds_part_round_optimized(state_vector_type &A,
                                                                  std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number >= half_full_rounds &&
                                             round_number < half_full_rounds + part_rounds - 1,
                                         "wrong using: sbox_arc_mds_part_round_optimized");
                        std::size_t constant_number_base =
                            (half_full_rounds + 1) * state_words + (round_number - half_full_rounds - 1) + 1;
                        A[0] = A[0] * A[0] * A[0] * A[0] * A[0];
                        A[0] += get_equivalent_round_constant(constant_number_base);
                        policy_matrix.product_with_equivalent_mds_matrix(A, round_number);
                    }

                    inline void sbox_mds_part_round_optimized_last(state_vector_type &A,
                                                                   std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number == half_full_rounds + part_rounds - 1,
                                         "wrong using: sbox_mds_part_round_optimized_last");
                        A[0] = A[0] * A[0] * A[0] * A[0] * A[0];
                        policy_matrix.product_with_equivalent_mds_matrix(A, round_number);
                    }

                    /*
                     * =========================================================
                     * Default
                     * =========================================================
                     */

                    inline void arc_sbox_mds_full_round(state_vector_type &A, std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number < half_full_rounds ||
                                             round_number >= half_full_rounds + part_rounds,
                                         "wrong using: arc_sbox_mds_full_round");
                        for (std::size_t i = 0; i < state_words; i++) {
                            A[i] += get_round_constant(round_number * state_words + i);
                            A[i] = A[i] * A[i] * A[i] * A[i] * A[i];
                        }
                        policy_matrix.product_with_mds_matrix(A);
                    }

                    inline void arc_sbox_mds_part_round(state_vector_type &A, std::size_t round_number) const {
                        BOOST_ASSERT_MSG(round_number >= half_full_rounds &&
                                             round_number < half_full_rounds + part_rounds,
                                         "wrong using: arc_sbox_mds_part_round");
                        for (std::size_t i = 0; i < state_words; i++) {
                            A[i] += get_round_constant(round_number * state_words + i);
                        }
                        A[0] = A[0] * A[0] * A[0] * A[0] * A[0];
                        policy_matrix.product_with_mds_matrix(A);
                    }

                    // private:
                    constexpr inline const element_type &get_round_constant(std::size_t constant_number) const {
                        return round_constants_generator.round_constants[constant_number];
                    }

                    constexpr inline state_vector_type
                        get_round_constants_slice(std::size_t constants_number_base) const {
                        return algebra::slice<state_words>(round_constants_generator.round_constants,
                                                           constants_number_base);
                    }

#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                    constexpr
#endif
                    inline void generate_equivalent_round_constants() {
                        state_vector_type inv_cip1;
                        state_vector_type agregated_round_constants;
                        std::size_t equivalent_constant_number_base =
                            (half_full_rounds + 1) * state_words - half_full_rounds;

                        for (std::size_t i = 0; i < half_full_rounds * state_words; i++) {
                            equivalent_round_constants[i] = get_round_constant(i);
                            equivalent_round_constants[equivalent_round_constants_size - i - 1] =
                                get_round_constant(round_constants_size - i - 1);
                        }

                        for (std::size_t i = half_full_rounds * state_words;
                             i < half_full_rounds * state_words + state_words;
                             i++) {
                            equivalent_round_constants[i] = get_round_constant(i);
                        }

                        for (std::size_t r = half_full_rounds + part_rounds - 2; r >= half_full_rounds; r--) {
                            agregated_round_constants = get_round_constants_slice((r + 1) * state_words) + inv_cip1;
                            policy_matrix.product_with_inverse_mds_matrix_noalias(agregated_round_constants, inv_cip1);
                            equivalent_round_constants[equivalent_constant_number_base + r] = inv_cip1[0];
                            inv_cip1[0] = 0;
                        }

                        policy_matrix.product_with_inverse_mds_matrix_noalias(agregated_round_constants, inv_cip1);
                        inv_cip1[0] = 0;
                        for (std::size_t i = 0; i < state_words; i++) {
                            equivalent_round_constants[half_full_rounds * state_words + i] += inv_cip1[i];
                        }
                    }

                    inline const element_type &get_equivalent_round_constant(std::size_t constant_number) const {
                        return equivalent_round_constants[constant_number];
                    }

#ifdef CRYPTO3_HASH_POSEIDON_COMPILE_TIME
                    constexpr
#endif
                    poseidon_constants_operator() :
                        policy_matrix(), round_constants_generator(), equivalent_round_constants() {
                        generate_equivalent_round_constants();
                    }

                    matrix_policy_type policy_matrix;
                    constants_generator_policy_type round_constants_generator;
                    equivalent_round_constants_type equivalent_round_constants;
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_HASH_POSEIDON_CONSTANTS_HPP


#include <functional>
#include <vector>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <iterator>
#include <boost/bind.hpp>

int sub( int a , int b ) { return a-b ; }
int sub2( int a , int b ) { int t = a-b ; std::cout << t << std::endl ; return t ;}
int add3( int a , int b , int c ) { return a + b + c; }

void stl_bind_overview()
{
  //
  // the funciton std::less<int>( int x1 , int x2 ) takes 2 arguments and returns a bool which si the result of X1 < X2;
  //
  /*
    Example 0 demonstration of stl 2 argument functtion
  */
  std::cout << "STL bind1st and bind2nd overview " << std::endl;
  std::cout << "Is 3 < 5 ? " << (  std::less<int>()(3,5) ? "true" : "false" ) << std::endl;; // is three less than 5
  std::cout << std::endl;
  //
  // we can use the bind function in conjuction with a predicate like std::less to "attach" a varaible to either the
  // first or second argment of bind;
  //
  //
  // int X ;
  // so the expression std::bind1st( std::less<int>() , 3 )(X);
  //
  // makes 3 the first argument of std::less<int>(int,int) and X the second;
  //
  // while the expression std::bind2nd( std::less<int>() , 3 )(X);
  //
  // makes 3 the second argument and X the first

  /*
    Example 1 Demonstration of using std::bind1st and std::bind2nd
  */
  int X = 5;

  std::cout << "using bind1st , bind 3 as the first  arg to std::less<>, so is 3 < 5 ? " << ( std::bind1st( std::less<int>() , 3 )(X) ? "true" : "false" ) << std::endl;

  std::cout << "using bind2nd , bind 3 as the second arg to std::less<>, so is 5 < 3 ? " << ( std::bind2nd( std::less<int>() , 3 )(X) ? "true" : "false" ) << std::endl;

  std::cout << std::endl;

  /*
    Example 2 Demonstration of using std::bind2nd with stl::algorithms
  */

  // using bind in conjution with other stl functions to manipualte a vectorwith a vector
  // lets create a vector with 10 elements going in sequence from 1 to 10

  std::vector<int> V(10); // note space is pre-allocated !

  std::cout << "create a vector with elements               : V = " ;
  std::iota( V.begin() , V.end() , 1 ) ; // creates a sequence from 1 to 10
  std::copy( V.begin() , V.end() , std::ostream_iterator<int>(std::cout," " ) ) ; std::cout << std::endl;

  std::vector<int> V_lower, V_upper;

  // dont copy any element from V into V_lower if V[i] >= 5
  // bind2nd is used to bind 5 to the second argument of std::greater_equal<>
  std::cout << "remove elements greatner than or equal to 5 : V_lower = ";
  std::remove_copy_if( V.begin() , V.end() , std::back_inserter( V_lower ) ,
		       std::bind2nd( std::greater_equal<int>() , 5 ) );
  std::copy( V_lower.begin() , V_lower.end() , std::ostream_iterator<int>(std::cout," " ) ) ; std::cout << std::endl;

  // dont copy any element from V into V_upper if V[i] < 5
  // bind2nd is used to bind 5 to the second argument of std::less<>
  std::cout << "remove elements less than 5                 : V_upper = ";
  std::remove_copy_if( V.begin() , V.end() , std::back_inserter( V_upper ) ,
		       std::bind2nd( std::less<int>() , 5 ) );
  std::copy( V_upper.begin() , V_upper.end() , std::ostream_iterator<int>(std::cout," " ) ) ; std::cout << std::endl;
  std::cout << std::endl;

  /*
    Example 3 - using stl::ptr_fun , stl::bind1st and stl::bind2nd with custom functions
  */

  int Z = 3;
  std::cout << "bind 5 to the first  arg of sub , 5 - Z = 5 - 3 = " << (std::bind1st( std::ptr_fun(sub) , 5 )(Z)) << std::endl;
  std::cout << "bind 5 to the second arg of sub , Z - 5 = 3 - 5 = " << (std::bind2nd( std::ptr_fun(sub) , 5 )(Z)) << std::endl;

  std::cout << std::endl;
}

void boost_bind_overview()
{
  // boost::bind uses places holders _1, _2 , _3 for args 1 , 2 , 3 etc
  // #include <boost/bind.hpp>
  //
  //
  // so while stl requires use of std::ptr_fun and either std::bind1st or std::bind2nd boost
  // boost uses place holers , to from our previous example :

  std::cout << "boost::bind overview " << std::endl;

  int Z = 3;

  // this statment binds "Z" to the first argument of sub, note the "_1" placeholder in position to
  // so the effect is to call ls sub( Z , 5 ) which is sub( 3 , 5 ) = -2
  int R1 = boost::bind( sub , _1, 5 )(Z);
  std::cout << "the result of boost::bind( sub , _1 , 5 )(Z), where Z = 3 , is : " << R1 << std::endl;

  // this statment binds "Z" to the second argument of sub, note the "_1" placeholder in position to
  // so the effect is to call ls sub( 5 , Z ) which is sub( 5, 3 ) = 2
  int R2 = boost::bind( sub , 5 , _1 )(Z);
  std::cout << "the result of boost::bind( sub , 5 , _1 )(Z), where Z = 3 , is : " << R2 << std::endl;

  int X1=11,X2=18;
  int R3 = boost::bind( sub , _1 , _2 )(X1,X2);
  std::cout << "the result of boost::bind( sub , _1 , _2 )(X1,X2), where X1=11,X2=18 is : " << R3 << std::endl;

  // note its possible to call boost bind with no place holders
  int R4 = boost::bind( sub , 11, 15 )();
  std::cout << "calling boost::bind( sub, 11, 15)() , note no place holders is equivalent to calling sub(11,15) : " << R4 << std::endl;

  // example binding 3 arguments
  int A=1,B=2,C=3;
  int R5 = boost::bind( add3, _1 , _2 , _3 )(A,B,C);
  std::cout << "calling boost::bind( add3, _1,_2,_3 ),where A=1,B=2,C=3 : " << R5 << std::endl;

  // this will bind all three argument to place holder 1, which is "A"
  int R6 = boost::bind( add3, _1 , _1 , _1 )(A,B,C);
  std::cout << "calling boost::bind( add3, _1,_1,_1 ),where A=1,B=2,C=3 : " << R6 << std::endl;

  // boost bind can be used to make a new function pointer
  // In ths example we are going to bind the first arg of sub to the variable i and get the resultant function pointer
  // note the return type is really ugly ,
  //
  // boost::_bi::bind_t<int, int (*)(int, int), boost::_bi::list2<boost::_bi::value<int>, boost::arg<1> > >
  //
  // int j = 3
  // boost::_bi::bind_t<int, int (*)(int, int), boost::_bi::list2<boost::_bi::value<int>, boost::arg<1> > > q = boost::bind( sub, j , _1);
  //
  // so the better way is to use the key word "auto"

  int j = 3;
  // this creaes a function pointer where the first arg of sub is bindecd to variable j =3 in this example
  auto q = boost::bind( sub, j , _1);
  std::cout << "result of auto q = boost::bind( sub, j , _1), where j = 3 and calling , q(4) = 3 - 4 = " << q(4) << std::endl;

  std::vector<int> W(10);
  std::iota( W.begin(), W.end() , 1 );
  auto q1 = boost::bind( sub2, 2 , _1 );
  for_each( W.begin() , W.end() , q1);

  for_each( W.begin() , W.end() , boost::bind( sub2, 1000 , _1 ));

  std::cout << "done" << std::endl;
}

int main( int argc , char ** argv )
{
  //stl_bind_overview();

  boost_bind_overview();
}


//  (C) Copyright Matt Borland 2022.
//  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)

#include <array>
#include <random>
#include <limits>
#include <boost/math/tools/color_maps.hpp>
#include <benchmark/benchmark.h>

template <typename Real>
void viridis_bm(benchmark::State& state)
{
    std::random_device rd;
    std::mt19937_64 gen(rd());
    std::uniform_real_distribution<Real> dist(0, 0.125);
    Real x = dist(gen);
    for (auto _ : state)
    {
        benchmark::DoNotOptimize(boost::math::tools::viridis(x));
        x += std::numeric_limits<Real>::epsilon();
    }
}

BENCHMARK_TEMPLATE(viridis_bm, float);
BENCHMARK_TEMPLATE(viridis_bm, double);

BENCHMARK_MAIN();


#include <tiny.h>
#include <geometry.h> // needed for geometry::Sphere
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(motion_utils);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef tiny::MathTypes<double>                 M;
  typedef M::vector3_type                         V;
  typedef M::real_type                            T;
  typedef M::quaternion_type                      Q;
  typedef M::coordsys_type                        X;

  X from;
  X to;

  from   = X::identity();
  to.T() = V::make(1.0,2.0,3.0);
  to.Q() = Q::Ru( 3.0, V::make(1.0,0.0,0.0) );
  V v;
  V w;
  convex::compute_velocities<M>( from, to, 1.0, v, w);

  BOOST_CHECK_CLOSE( v(0), to.T()(0), 0.01 );
  BOOST_CHECK_CLOSE( v(1), to.T()(1), 0.01 );
  BOOST_CHECK_CLOSE( v(2), to.T()(2), 0.01 );
  BOOST_CHECK_CLOSE( w(0), 3.0, 0.01 );
  BOOST_CHECK_CLOSE( w(1), 0.0, 0.01 );
  BOOST_CHECK_CLOSE( w(2), 0.0, 0.01 );

  T tau = 0.5;
  X cur = convex::integrate_motion<M>(from, tau, v, w);

  BOOST_CHECK_CLOSE( 0.5, cur.T()(0), 0.01 );
  BOOST_CHECK_CLOSE( 1.0, cur.T()(1), 0.01 );
  BOOST_CHECK_CLOSE( 1.5, cur.T()(2), 0.01 );

  T theta;
  V n;
  tiny::get_axis_angle( cur.Q(), n, theta );

  BOOST_CHECK_CLOSE( theta, 1.5, 0.01 );
  BOOST_CHECK_CLOSE( n(0), 1.0, 0.01 );
  BOOST_CHECK_CLOSE( n(1), 0.0, 0.01 );
  BOOST_CHECK_CLOSE( n(2), 0.0, 0.01 );

  geometry::Sphere<V> const A;
  geometry::Sphere<V> const B;
  X X_a;
  X X_b;

  X_a.T() = V::make(-2.0,0.0,0.0);
  X_a.Q() = Q::identity();

  X_b.T() = V::make( 2.0,0.0,0.0);
  X_b.Q() = Q::identity();

  V p_a;
  V p_b;

  convex::compute_closest_points<M>( X_a, &A, X_b, &B, p_a, p_b);

  BOOST_CHECK_CLOSE( p_a(0), -1.0, 0.1);
  BOOST_CHECK_CLOSE( p_a(1),  0.0, 0.1);
  BOOST_CHECK_CLOSE( p_a(2),  0.0, 0.1);

  BOOST_CHECK_CLOSE( p_b(0),  1.0, 0.1);
  BOOST_CHECK_CLOSE( p_b(1),  0.0, 0.1);
  BOOST_CHECK_CLOSE( p_b(2),  0.0, 0.1);
}

BOOST_AUTO_TEST_SUITE_END();


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001.
 *      The MathWorks, Inc. Symbolic Math Toolbox, 2012.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <string>

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

#include <Eigen/Core>

#include "tudat/io/basicInputOutput.h"
#include "tudat/io/matrixTextFileReader.h"

#include "tudat/math/integrators/numericalIntegrator.h"
#include "tudat/math/integrators/reinitializableNumericalIntegrator.h"
#include "tudat/math/integrators/rungeKutta4Integrator.h"
#include "tudat/support/numericalIntegratorTests.h"
#include "tudat/math/integrators/numericalIntegratorTestFunctions.h"

namespace tudat
{
namespace unit_tests
{

using numerical_integrators::RungeKutta4IntegratorXd;
using numerical_integrators::NumericalIntegratorXdPointer;
using numerical_integrators::ReinitializableNumericalIntegratorXdPointer;

using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative;

BOOST_AUTO_TEST_SUITE( test_runge_kutta_4_integrator )

//! Test Runge-Kutta 4 integrator using benchmark data from (The MathWorks, 2012).
BOOST_AUTO_TEST_CASE( testRungeKutta4IntegratorUsingMatlabData )
{
    using namespace numerical_integrator_tests;

    // Read in benchmark data (generated using Symbolic Math Toolbox in Matlab
    // (The MathWorks, 2012)). This data is generated using the RK4 numerical integrator.
    const std::string pathToForwardIntegrationOutputFile = paths::getTudatTestDataPath( )
            + "/matlabOutputRungeKutta4Forwards.txt";
    const std::string pathToBackwardIntegrationOutputFile = paths::getTudatTestDataPath( )
            + "/matlabOutputRungeKutta4Backwards.txt";
    const std::string pathToDiscreteEventIntegrationOutputFile = paths::getTudatTestDataPath( )
            + "/matlabOutputRungeKutta4DiscreteEvent.txt";

    // Store benchmark data in matrix.
    const Eigen::MatrixXd matlabForwardIntegrationData =
            input_output::readMatrixFromFile( pathToForwardIntegrationOutputFile, "," );
    const Eigen::MatrixXd matlabBackwardIntegrationData =
            input_output::readMatrixFromFile( pathToBackwardIntegrationOutputFile, "," );
    const Eigen::MatrixXd matlabDiscreteEventIntegrationData =
            input_output::readMatrixFromFile( pathToDiscreteEventIntegrationOutputFile, "," );

    // Case 1: Execute integrateTo() to integrate one step forward in time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKutta4IntegratorXd >(
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ) );

        executeOneIntegrateToStep( matlabForwardIntegrationData, 1.0e-15, integrator );
    }

    // Case 2: Execute performIntegrationStep() to perform multiple integration steps until final
    //         time.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKutta4IntegratorXd >(
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ) );

        performIntegrationStepToSpecifiedTime( matlabForwardIntegrationData,
                                               1.0e-15,  1.0e-14, integrator );
    }

    // Case 3: Execute performIntegrationStep() to perform multiple integration steps until initial
    //         time (backwards).
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKutta4IntegratorXd >(
                    &computeNonAutonomousModelStateDerivative,
                    matlabBackwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabBackwardIntegrationData( FIRST_ROW,
                                                        STATE_COLUMN_INDEX ) ).finished( ) );

        performIntegrationStepToSpecifiedTime( matlabBackwardIntegrationData,
                                               1.0e-15, 1.0e-14, integrator );
    }

    // Case 4: Execute integrateTo() to integrate to specified time in one step.
    {
        // Declare integrator with all necessary settings.
        NumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKutta4IntegratorXd >(
                    &computeNonAutonomousModelStateDerivative,
                    matlabForwardIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabForwardIntegrationData( FIRST_ROW,
                                                       STATE_COLUMN_INDEX ) ).finished( ) );

        executeIntegrateToToSpecifiedTime( matlabForwardIntegrationData, 1.0e-14, integrator,
                                           matlabForwardIntegrationData(
                                               matlabForwardIntegrationData.rows( ) - 1,
                                               TIME_COLUMN_INDEX ) );
    }

    // Case 5: Execute performIntegrationstep() to integrate to specified time in multiple steps,
    //         including discrete events.
    {
        // Declare integrator with all necessary settings.
        ReinitializableNumericalIntegratorXdPointer integrator
                = std::make_shared< RungeKutta4IntegratorXd >(
                    &computeNonAutonomousModelStateDerivative,
                    matlabDiscreteEventIntegrationData( FIRST_ROW, TIME_COLUMN_INDEX ),
                    ( Eigen::VectorXd( 1 )
                      << matlabDiscreteEventIntegrationData( FIRST_ROW,
                                                             STATE_COLUMN_INDEX ) ).finished( ) );

        performIntegrationStepToSpecifiedTimeWithEvents( matlabDiscreteEventIntegrationData,
                                                         1.0e-15, 1.0e-13, integrator );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#if !defined(_GATE_EST_BA_HPP_)
#define _GATE_EST_BA_HPP_

#include <Eigen/Dense>
#include <ceres/ceres.h>

namespace gate_est {

  class CostFunctionReProjection {
    public:
    inline CostFunctionReProjection(
      const Eigen::Vector3d& pt,
      const Eigen::MatrixXd& t,
      const Eigen::Matrix3d& cm,
      const Eigen::Vector2d& uv)
      : fx_(cm(0, 0)), fy_(cm(1, 1)), cx_(cm(0, 2)), cy_(cm(1, 2)),
        u_(uv[0]), v_(uv[1]),
        t_(t),
        pt_x_(pt[0]), pt_y_(pt[1]), pt_z_(pt[2]) { }

    template <typename T>
    inline bool operator() (const T* const point, T* residual) const {
      auto x = point[0] + T(pt_x_);
      auto y = point[1] + T(pt_y_);
      auto z = point[2] + T(pt_z_);

      auto px = T(t_(0, 0)) * x + T(t_(0, 1)) * y + T(t_(0, 2)) * z  + T(t_(0, 3));
      auto py = T(t_(1, 0)) * x + T(t_(1, 1)) * y + T(t_(1, 2)) * z  + T(t_(1, 3));
      auto pz = T(t_(2, 0)) * x + T(t_(2, 1)) * y + T(t_(2, 2)) * z  + T(t_(2, 3));

      auto u = T(fx_) * px / pz + T(cx_);
      auto v = T(fy_) * py / pz + T(cy_);

      residual[0] = T(u_) - u;
      residual[1] = T(v_) - v;

      return true;
    }

    private:
      const double fx_;
      const double fy_;
      const double cx_;
      const double cy_;
      const double u_;
      const double v_;
      const double pt_x_;
      const double pt_y_;
      const double pt_z_;
      const Eigen::MatrixXd t_;
  };

} // gate_est

#endif // _GATE_EST_BA_HPP_