#pragma once

#include <BRepPrimAPI_MakeCone.hxx>
#include <BRepPrimAPI_MakeTorus.hxx>
#include "Shape.hpp"
#include <gce_MakeScale.hxx>
#include <boost/function.hpp>

class EditMode{
public:
	struct GameStatesRef{
		std::vector<SimpleBoxComponent> &boxes;
		std::vector<SimpleBoxComponent> &protoBoxes;
		std::vector<std::size_t> &sbcToDelete;
	};
private:
	GameStatesRef m_gs;

	std::size_t m_hoveringOverBox,m_selectedBox/*,m_draggingBox*/;
	static std::size_t InvalidId(){ return std::numeric_limits<std::size_t>::max(); }

	Ogre::Vector2 m_mouseH;

	Ogre::SceneManager *m_pSceneManager;

	static Standard_Real NearestRayIntersection( const gp_Lin &rayOS, const TopoDS_Shape &s ){
		IntCurvesFace_ShapeIntersector intersector;
		intersector.Load(s,0);
		intersector.Perform/*Nearest*/(rayOS,0,RealLast());
		Standard_Real w = intersector.NbPnt()?intersector.WParameter(1):RealLast();
		for(int i=2;i<=intersector.NbPnt();++i){
			NumericRange::ExtendL( intersector.WParameter(i), w );
		}
		return w;
	}
	static Standard_Real NearestRayIntersection( const gp_Lin &ray, const gp_Trsf &transformToOS, const TopoDS_Shape &s ){
		return NearestRayIntersection(ray.Transformed(transformToOS),s);
	}
	template< typename _PositionVec,typename _OrientationQuat >
	static gp_Trsf GetWS2OSTransform( const _PositionVec &pos, const _OrientationQuat &ort ){
		gp_Trsf rotation;
		rotation.SetRotation( boost::fusion::invoke( boost::value_factory<gp_Quaternion>(), boost::fusion::as_nview<1,2,3,0>(ort) ) );
		return (gce_MakeTranslation( boost::fusion::invoke( boost::value_factory<gp_Vec>(), pos ) ).Value()*rotation).Inverted();
	}

	struct OCCTRayIntersector{
		const gp_Lin rayWS;

		template<typename E>
		typename boost::enable_if<GenericSim::Meta::IsEntity<E>, Standard_Real>::type operator()(const E &e) const{
			return OnEntityByConcept( e, typename GenericSim::Meta::GetEntityConcept<E>::Type() );
		}

		Standard_Real operator()(const boost::fusion::vector< const gp_Trsf &, const TopoDS_Shape& > &p) const{
			using namespace boost::fusion;
			return NearestRayIntersection(rayWS, at_c<0>(p), at_c<1>(p))/at_c<0>(p).ScaleFactor();
		}

	private:

		template<typename E>
		Standard_Real OnEntityByConcept( const E &e, EC::Component ) const{
			return NearestRayIntersection(
				rayWS,
				GetWS2OSTransform(
					e.at_e<EC::Component::RigidBodyStates>().at_e<EC::RigidBodyDynamics3D::PositionVector>(),
					e.at_e<EC::Component::RigidBodyStates>().at_e<EC::RigidBodyDynamics3D::OrientationQuat>()
				),
				GenericShapeToOCCT(e.at_e<EC::Component::Shape>())
			);
		}
	};

	template< typename _SP_R_Range, typename Intersector >
	static std::pair< Standard_Real, typename boost::range_difference<_SP_R_Range>::type >
	SelectNearest( const _SP_R_Range &r, const Intersector &f, const Standard_Real &upperLimit ){
		boost::range_iterator<const _SP_R_Range>::type pos = boost::end(r);
		Standard_Real m = upperLimit;
		for( boost::range_iterator<const _SP_R_Range>::type i = boost::begin(r); i!=boost::end(r); ++i ){
			Standard_Real d = f(*i);
			if(d<m){
				m = d;
				pos = i;
			}
		}
		return std::make_pair(m,pos-boost::begin(r));
	}

	Ogre::SceneNode * m_spatialTransformDraggablesOgreSceneNode;
	std::string m_translationArrowMeshName;
	TopoDS_Shape m_translationArrowOCCTShape;
	boost::array<gp_Trsf,6> m_translationArrowOCCTShapeWS2OSTransforms;
	std::size_t m_hoveringOverTranslationArrow;
	std::size_t m_draggingTranslationArrow;

	std::string m_rotationHandleMeshName;
	TopoDS_Shape m_rotationHandleOCCTShape;
	boost::array<gp_Trsf,6> m_rotationHandleOCCTShapeWS2OSTransforms;
	std::size_t m_hoveringOverRotationHandle;
	std::size_t m_draggingRotationHandle;

	void UpdateTransformDraggablesAuxTransforms(){
		Real3 pos = boost::qvm::make<Real3>(m_gs.boxes[m_selectedBox].at_e<EC::Component::RigidBodyStates>().at_e<EC::RigidBodyDynamics3D::PositionVector>());
		m_spatialTransformDraggablesOgreSceneNode->setPosition(boost::qvm::make<Ogre::Vector3>(pos));
		Real radius = sqrt(GetShapeBoundingSphereRadiusSquared(m_gs.boxes[m_selectedBox].at_e<EC::Component::Shape>()));
		//Ogre::Node::ChildNodeIterator itr = m_spatialTransformDraggablesOgreSceneNode->getChildIterator();
		using namespace boost::qvm;
		{
			boost::array<Real,4> qs[6] = {
				{sqrt(0.5),0,sqrt(0.5),0},
				{-sqrt(0.5),sqrt(0.5),0,0},
				{1,0,0,0},
				{-sqrt(0.5),0,sqrt(0.5),0},
				{sqrt(0.5),sqrt(0.5),0,0},
				{0,1,0,0}
			};
			float r = 1.5*radius;
			Ogre::Node *p = m_spatialTransformDraggablesOgreSceneNode->getChild("T");
			for(int i=0;i<6;++i){
				Real a = i/3?-r:r;
				int iDim = i%3;
				Ogre::Node *pN = p->getChild(i);//itr.getNext();
				pN->setPosition(VectorParallelToCoordSysBase<Ogre::Vector3>(iDim,a));
				pN->setOrientation(make<Ogre::Quaternion>(qs[i]));
				m_translationArrowOCCTShapeWS2OSTransforms[i] = GetWS2OSTransform(VectorParallelToCoordSysBase<Real3>(iDim,a)+pos,qs[i]);
			}
		}
		{
			boost::array<Real,4> qa[3] = {
				{-sqrt(0.5),0,sqrt(0.5),0},
				{sqrt(0.5),sqrt(0.5),0,0},
				{1,0,0,0},
			};
			boost::array<Real,4> qb[2] = {
				{1,0,0,0},
				{0,-sqrt(0.5),sqrt(0.5),0}
			};
			boost::array<Real,4> qc = {	cos(PI/12),0,0,sin(PI/12) };
			float r = 1.5*radius;
			Ogre::Node *p = m_spatialTransformDraggablesOgreSceneNode->getChild("R");
			p->setScale( r,r,r );
			for(int i=0;i<6;++i){
				int iDim = i%3;
				int iDir = i/3;
				//Real3 t = zero_v<Real,3>();
				//t[(iDim+2)%3] = t[(iDim+1)%3] = iDir ? -1:1;
				Ogre::Node *pN = p->getChild(i);
				//pN->setPosition(make<Ogre::Vector3>(t));
				boost::array<Real,4> q = qa[iDim]*qb[iDir]*qc;
				//normalize(q);
				pN->setOrientation(make<Ogre::Quaternion>(q));
				m_rotationHandleOCCTShapeWS2OSTransforms[i] = gce_MakeScale(gp_Pnt(),1/r).Value()*GetWS2OSTransform(pos,q);
			}
		}
	}

	template<typename F>
	void CallOnSelectedComponent( F &f ){
		if(InvalidId()!=m_selectedBox) f(m_gs.boxes[m_selectedBox]);
		else assert(0);
	}

	struct ToGetComponentPos{ Real3 ret;	template< typename C > void operator()( const C &c ){
		boost::qvm::assign(ret,c.at_e<EC::Component::RigidBodyStates>().at_e<EC::RigidBodyDynamics3D::PositionVector>());
	}};

	struct ToSetComponentPos{ const Real3 &v;	template< typename C > void operator()( C &c ) const{
		boost::qvm::assign(c.at_e<EC::Component::RigidBodyStates>().at_e<EC::RigidBodyDynamics3D::PositionVector>(),v);
	}};

	struct ToGetComponentOrientation{ Quaternion ret;	template< typename C > void operator()( const C &c ){
		boost::qvm::assign(boost::qvm::qref(ret),c.at_e<EC::Component::RigidBodyStates>().at_e<EC::RigidBodyDynamics3D::OrientationQuat>());
	}};

	struct ToSetComponentOrientation{ const Quaternion &q;	template< typename C > void operator()( C &c ) const{
		boost::qvm::assign(c.at_e<EC::Component::RigidBodyStates>().at_e<EC::RigidBodyDynamics3D::OrientationQuat>(),boost::qvm::qref(q));
	}};

	Real3 GetSelectedComponentPos(){
		ToGetComponentPos f;
		CallOnSelectedComponent(f);
		return f.ret;
	}
	void SetSelectedComponentPos(const Real3 &pos){
		ToSetComponentPos f = {pos};
		CallOnSelectedComponent(f);
	}
	Quaternion GetSelectedComponentOrientation(){
		ToGetComponentOrientation f;
		CallOnSelectedComponent(f);
		return f.ret;
	}
	void SetSelectedComponentOrientation(const Quaternion &q){
		ToSetComponentOrientation f = {q};
		CallOnSelectedComponent(f);
	}
	//void UpdateTransformDraggablesOgreSceneNodeSubNodes(){
	//}

	bool m_primaryPointerDown, m_secondaryPointerDown;

	template< typename Intersector >
	struct IntersectionSelector{
		const Intersector intersector;
		Standard_Real nearest;
		std::size_t elementId;
		std::size_t *pToSet;
		template< typename _SP_R_Range >
		void operator()(const _SP_R_Range &r, std::size_t *pRangeResult){
			std::pair< Standard_Real, std::size_t > s = SelectNearest( r,intersector,nearest );
			if(s.first < nearest){
				pToSet = pRangeResult;
				nearest = s.first;
				elementId = s.second;
			}
		}
	};

	bool HasSelectedSomething(){ return InvalidId()!=m_selectedBox; }

	static const Ogre::Vector3 ComputeOgreCameraRay(Ogre::Vector2 posH,Ogre::Camera *pCam)
	{
		const Ogre::Matrix4 mp = pCam->getProjectionMatrix();
		const Ogre::Matrix4 m = (mp*pCam->getViewMatrix()).inverse();
		Ogre::Vector4 v = m*Ogre::Vector4( posH.x, posH.y, -mp[2][2], 1 );
		return Ogre::Vector3(v.x,v.y,v.z);
	}
	void Deselect(){
		m_hoveringOverBox = m_selectedBox = InvalidId();
		m_hoveringOverTranslationArrow = m_draggingTranslationArrow = InvalidId();
		m_hoveringOverRotationHandle = m_draggingRotationHandle = InvalidId();
		if(m_spatialTransformDraggablesOgreSceneNode)	m_spatialTransformDraggablesOgreSceneNode->setVisible(false);
	}

	template< typename _UnaryFunc >
	struct LinearDraggableFunc{
		const _UnaryFunc f;
		//const Real3 rail; Just use (0,0,1)
		void operator()(const Real3 &ray0, const Real3 &ray1, const Real3 &eye, const Real3 &camAxis) const{// all in object space
			using namespace boost::qvm;
			const Real3 rail = {0,0,1};
			Real3 normal = cross(rail,cross(camAxis,rail));
			Real signedDist = dot(-eye,normal);
			Real k1 = signedDist/dot(ray1,normal)*dot(ray1,rail);
			Real k0 = signedDist/dot(ray0,normal)*dot(ray0,rail);
			f(k1-k0);
		}
	};

	template< typename _UnaryFunc >
	struct RotaryDraggableFunc{
		_UnaryFunc f;
		void operator()(const Real3 &ray0, const Real3 &ray1, const Real3 &eye, const Real3 &camAxis)const{// all in object space
			using namespace boost::qvm;
			unsigned i = 2, j = 0, k = 1;
			Real3 d = -eye;
			Real r1 = d[i]/ray1[i];
			Real r0 = d[i]/ray0[i];
			Real2 s1 = { -d[j]+ray1[j]*r1, -d[k]+ray1[k]*r1 };
			Real2 s0 = { -d[j]+ray0[j]*r0, -d[k]+ray0[k]*r0 };
			Real x = dot(s0,s1);
			Real y = -s0[1]*s1[0]+s0[0]*s1[1];
			Real a = atan2( y, x )*0.5;
			f(a);
		}
	};

	template< typename _HandleToScalar >
	struct Simple1DDragInterpretationFunc{
		_HandleToScalar h;
		void operator() (const Real &delta) const {	*h+=delta;	}
	};

	struct Parameterizer{
		boost::function<void (const Real3 &,const Real3 &,const Real3 &,const Real3 &)> f;
		boost::function<SpatialTransform ()> s;
	};

	std::vector<Parameterizer> m_parameterizers;

	template<typename _ParameterizerContainer>
	struct ToAttachParameterizer{
		//typedef boost::function<void (const Real3 &,const Real3 &,const Real3 &,const Real3 &)> result_type;
		_ParameterizerContainer &cont;
		template<typename H>
		void operator()( const H &h ){
			cont.clear();
			Add(h);
		}
	private:
		template<typename _HandleToSimpleBoxComponent>
		void Add( const _HandleToSimpleBoxComponent &pSbc){
			using Utility::Make;
			using Utility::MakeAggr;
			using namespace GenericSim::Handles;
			typedef HandleToEntityState<_HandleToSimpleBoxComponent, EC::Component::Shape> H0;
			typedef HandleToEntityState< H0, EC::Box::Size > H1;
			H1 hs( (H0(pSbc)) ); //additional paren to avoid parsing as func
			struct STF{
				H1 hs;
				int iAxis;
				SpatialTransform operator()(){
					return SpatialTransform();
				}
			};
			for(int i=0;i<3;++i){
				cont.push_back( MakeAggr<Parameterizer>(
					MakeAggr<LinearDraggableFunc>(
						MakeAggr<Simple1DDragInterpretationFunc>(
							//&sbc.at_e<EC::Component::Shape>().at_e<EC::Box::Size>()[i]
							Make<HandleToElementOfRandomAccessRange>( hs, i )
						)
					),
					MakeAggr<STF>(hs,i)
				));
			}
		}
	};

public:
	EditMode(const GameStatesRef &gs):m_gs(gs),
		m_hoveringOverBox(InvalidId()),m_selectedBox(InvalidId()),/*m_draggingBox(InvalidId()),*/
//			m_hoveringOverRotationHandle(InvalidId()),
		m_pSceneManager(NULL),m_primaryPointerDown(false), m_secondaryPointerDown(false),
		m_spatialTransformDraggablesOgreSceneNode(NULL),
		m_translationArrowOCCTShape(BRepPrimAPI_MakeCone(0.1,0.02,0.5)),
		m_translationArrowMeshName("translationArrowOgreMeshH2J2DFUFZK91417PNGA"+boost::lexical_cast<std::string>(clock())),
		m_hoveringOverTranslationArrow(InvalidId()),
		m_draggingTranslationArrow(InvalidId()),
		m_rotationHandleOCCTShape(/*BRepPrimAPI_MakeSphere(0.2)),*/BRepPrimAPI_MakeTorus(1,0.05,PI/6)),
		m_rotationHandleMeshName("rotationHandleOgreMeshN94T2U245SRT7STYS8"+boost::lexical_cast<std::string>(clock())),
		m_hoveringOverRotationHandle(InvalidId()),
		m_draggingRotationHandle(InvalidId())
	{
	}
	void OnPrimaryPointerDown(){
		if(m_primaryPointerDown) return;
		m_primaryPointerDown = true;
		std::cout<<"d\n";
//			if(InvalidId()!=m_hoveringOverRotationHandle){
//			}else if(InvalidId()!=m_hoveringOverTranslationAxisHandle){
//			}else
		if(InvalidId()!=m_hoveringOverBox){
			m_selectedBox = m_hoveringOverBox;

			if(!m_spatialTransformDraggablesOgreSceneNode){
				m_spatialTransformDraggablesOgreSceneNode = m_pSceneManager->getRootSceneNode()->createChildSceneNode();
				Ogre::SceneNode *pT = m_spatialTransformDraggablesOgreSceneNode->createChildSceneNode("T");
				for(int i=0;i<6;++i){
					if(!Ogre::MeshManager::getSingleton().resourceExists(m_translationArrowMeshName)){
						OCCTMesh2OGRE::FillBlankOgreMeshUsingOCCTShape(
							Ogre::MeshManager::getSingleton().createManual( m_translationArrowMeshName , "__OCCTGEN" ),
							m_translationArrowOCCTShape,
							1
						);
					}
					pT->createChildSceneNode()->attachObject(m_pSceneManager->createEntity(
						"translationArrowOgreEntitiyNG92HY20JQY95R"+boost::lexical_cast<std::string>(clock())+"|"+boost::lexical_cast<std::string>(unsigned(this)+i),
						m_translationArrowMeshName,
						"__OCCTGEN"
					));
				}
				Ogre::SceneNode *pR = m_spatialTransformDraggablesOgreSceneNode->createChildSceneNode("R");
				for(int i=0;i<6;++i){
					if(!Ogre::MeshManager::getSingleton().resourceExists(m_rotationHandleMeshName)){
						OCCTMesh2OGRE::FillBlankOgreMeshUsingOCCTShape(
							Ogre::MeshManager::getSingleton().createManual( m_rotationHandleMeshName , "__OCCTGEN" ),
							m_rotationHandleOCCTShape,
							0.1
						);
					}
					pR->createChildSceneNode()->attachObject(m_pSceneManager->createEntity(
						"rotationHandleOgreEntityYH220JS0OR2WNSDF"+boost::lexical_cast<std::string>(clock())+"|"+boost::lexical_cast<std::string>(unsigned(this)+i),
						m_rotationHandleMeshName,
						"__OCCTGEN"
					));
				}
			}
			m_spatialTransformDraggablesOgreSceneNode->setVisible(true);
			UpdateTransformDraggablesAuxTransforms();
			using Utility::Make;
			using Utility::MakeAggr;
			using Utility::TieIntoAggr;
			using namespace GenericSim::Handles;
			TieIntoAggr<ToAttachParameterizer>(m_parameterizers)(Make<HandleToElementOfRandomAccessRange>(&m_gs.boxes,m_selectedBox));
		}else if(InvalidId()!=m_hoveringOverTranslationArrow){
			m_draggingTranslationArrow = m_hoveringOverTranslationArrow;
		}else if(InvalidId()!=m_hoveringOverRotationHandle){
			m_draggingRotationHandle = m_hoveringOverRotationHandle;
		}else{
			Deselect();
		}
	}
	void OnPrimaryPointerUp(){
		if(!m_primaryPointerDown) return;
		m_primaryPointerDown = false;
		std::cout<<"u\n";
		m_draggingTranslationArrow = InvalidId();
		m_draggingRotationHandle = InvalidId();
		//m_draggingBox = InvalidId();
	}
	void OnSecondaryPointerDown(){
		m_secondaryPointerDown = true;
	}
	void OnSecondaryPointerUp(){
		m_secondaryPointerDown = false;
	}
	void DeleteSelected(){
		if(m_selectedBox!=InvalidId()){
			m_gs.sbcToDelete.push_back(m_selectedBox);
			Deselect();
		}
	}
	void SetOgreScene(Ogre::SceneManager *pSceneManager){
		if(m_pSceneManager){
			if(m_spatialTransformDraggablesOgreSceneNode){
				DestroyOgreSceneNodeAttachedMovableObjects(m_spatialTransformDraggablesOgreSceneNode,m_pSceneManager);
				m_pSceneManager->destroySceneNode(m_spatialTransformDraggablesOgreSceneNode);
				m_spatialTransformDraggablesOgreSceneNode = NULL;
			}
		}
		m_pSceneManager = pSceneManager;
	}

	void Update(Real elapsedSeconds,const Ogre::Vector2 &mouseH,Ogre::Camera *pCam)
	{
		if(HasSelectedSomething()){
			assert(m_pSceneManager);
			UpdateTransformDraggablesAuxTransforms();
		}
		if(InvalidId()!=m_draggingTranslationArrow||InvalidId()!=m_draggingRotationHandle){
			using namespace boost::qvm;
			Real3 eye = make<Real3>(pCam->getRealPosition());
			Real3 ray1 = make<Real3>(ComputeOgreCameraRay(mouseH,pCam));
			Real3 ray0 = make<Real3>(ComputeOgreCameraRay(m_mouseH,pCam));
			Real3 pos = GetSelectedComponentPos();
			Quaternion ort = GetSelectedComponentOrientation();
			if(InvalidId()!=m_draggingTranslationArrow){
				//Ogre::Vector2 dMouseH = mouseH - m_mouseH;
				Real3 cam = make<Real3>(pCam->getRealDirection().normalisedCopy());
				Real3 rail = VectorParallelToCoordSysBase<Real3>(m_draggingTranslationArrow%3,1);
				Real3 normal = cross(rail,cross(cam,rail));
				Real3 eye = make<Real3>(pCam->getRealPosition());
				Real signedDist = dot(pos-eye,normal);
				Real k1 = signedDist/dot(ray1,normal)*dot(ray1,rail);
				Real k0 = signedDist/dot(ray0,normal)*dot(ray0,rail);
				pos[m_draggingTranslationArrow%3]+=k1-k0;
				SetSelectedComponentPos(pos);
			}else if(InvalidId()!=m_draggingRotationHandle){
				unsigned i = m_draggingRotationHandle%3, j = (m_draggingRotationHandle+1)%3, k = (m_draggingRotationHandle+2)%3;
				Real3 d = pos-eye;
				Real r1 = d[i]/ray1[i];
				Real r0 = d[i]/ray0[i];
				Real2 s1 = { -d[j]+ray1[j]*r1, -d[k]+ray1[k]*r1 };
				Real2 s0 = { -d[j]+ray0[j]*r0, -d[k]+ray0[k]*r0 };
				Real x = dot(s0,s1);
				Real y = -s0[1]*s1[0]+s0[0]*s1[1];
				Real a = atan2( y, x )*0.5;
//					Real t[] = { ::atan2( 1,0 ), ::atan2(2,0), ::atan2(-1,0) };
				Real4 q = { cos(a),0,0,0 };
				q[1+i] = sin(a);
				SetSelectedComponentOrientation(normalized(qref(q*ort)));
			}
		}
		if(m_primaryPointerDown){
		}else{
			Ogre::Vector3 dir = ComputeOgreCameraRay(mouseH,pCam);
			Ogre::Vector3 pos = pCam->getRealPosition();
			gp_Lin rayWS( gp_Pnt(pos.x,pos.y,pos.z), gp_Dir(dir.x,dir.y,dir.z) );

			IntersectionSelector<OCCTRayIntersector> is = {
				{rayWS},
				RealLast(),
				InvalidId(),
				NULL
			};
			m_hoveringOverBox = InvalidId();
			m_hoveringOverTranslationArrow = InvalidId();
			m_hoveringOverRotationHandle = InvalidId();

			is(m_gs.boxes,&m_hoveringOverBox);
			if(HasSelectedSomething()){
				is(boost::adaptors::transform(
					m_translationArrowOCCTShapeWS2OSTransforms,
					boost::bind( boost::value_factory< boost::fusion::vector< const gp_Trsf &, const TopoDS_Shape& > >(), _1, m_translationArrowOCCTShape )
				), &m_hoveringOverTranslationArrow);
				is(boost::adaptors::transform(
					m_rotationHandleOCCTShapeWS2OSTransforms,
					boost::bind( boost::value_factory< boost::fusion::vector< const gp_Trsf &, const TopoDS_Shape& > >(), _1, m_rotationHandleOCCTShape )
				), &m_hoveringOverRotationHandle);
			}

			if(is.pToSet)	*is.pToSet = is.elementId;
			std::cout<<is.elementId<<' '<<is.nearest<<'\n';
		}
		m_mouseH = mouseH;
	}
};


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE TestGeometry

#include "main.cc"

#include <cap/energy_storage_device.h>
#include <cap/supercapacitor.h>
#include <cap/utils.h>
#include <cap/geometry.h>
#include <deal.II/grid/grid_out.h>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/info_parser.hpp>
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <unordered_map>

// - Check that a mesh can be loaded, that the areas are computed correctly, and
// check that a mesh can be written.
// - Check that we can build a 3D geometry.

template <int dim>
void write_mesh(std::string const &mesh_file,
                std::shared_ptr<dealii::distributed::Triangulation<dim> const>
                    triangulation)
{
  dealii::GridOut mesh_writer;
  std::fstream fout;
  fout.open(mesh_file.c_str(), std::fstream::out);
  mesh_writer.write_vtu(*triangulation, fout);
  fout.close();
}

BOOST_AUTO_TEST_CASE(test_reset_geometry)
{
  std::shared_ptr<boost::property_tree::ptree> params(
      new boost::property_tree::ptree);
  params->put("anode_collector_material_id", 4);
  params->put("anode_electrode_material_id", 1);
  params->put("separator_material_id", 2);
  params->put("cathode_electrode_material_id", 3);
  params->put("cathode_collector_material_id", 5);
  params->put("type", "file");
  params->put("mesh_file", "mesh_2d.ucd");
  params->put("materials", 1);
  params->put("material_0.name", "all");
  params->put("material_0.material_id", "1,2,3,4,5");
  params->put("boundary_values.anode_boundary_id", "1");
  params->put("boundary_values.cathode_boundary_id", "2");

  cap::Geometry<2> geo(params, boost::mpi::communicator());
  write_mesh("output_test_geometry_0.vtu", geo.get_triangulation());

  dealii::distributed::Triangulation<2> const &tria = *geo.get_triangulation();
  std::cout << "cells=" << tria.n_active_cells() << "  "
            << "faces=" << tria.n_active_faces() << "  "
            << "vertices=" << tria.n_used_vertices() << "\n";

  std::unordered_map<dealii::types::material_id, double> measure;
  auto cell = tria.begin_active();
  auto end_cell = tria.end();
  double m(0.);
  for (; cell != end_cell; ++cell)
    if (cell->is_locally_owned())
    {
      measure[cell->material_id()] += cell->measure();
      m += cell->measure();
    }

  for (auto x : measure)
    std::cout << std::to_string(x.first) << "  " << x.second << "\n";

  double const percent_tolerance = 1.0e-12;
  std::array<double, 5> reference_measure = {6.25e-10, 1.25e-9, 1.25e-9,
                                             1.5e-10, 1.5e-10};
  unsigned int pos(0);
  for (std::string const layer :
       {"separator", "anode_electrode", "cathode_electrode", "anode_collector",
        "cathode_collector"})
  {
    BOOST_CHECK_CLOSE(measure[params->get<dealii::types::material_id>(
                          layer + "_material_id")],
                      reference_measure[pos], percent_tolerance);
    ++pos;
  }
}

BOOST_AUTO_TEST_CASE(test_throw_geometry)
{
  std::shared_ptr<boost::property_tree::ptree> params(
      new boost::property_tree::ptree);
  params->put("type", "supercapacitor");
  params->put("anode_collector_thickness", 5.0e-4);
  params->put("anode_electrode_thickness", 50.0e-4);
  params->put("separator_thickness", 25.0e-4);
  params->put("cathode_electrode_thickness", 50.0e-4);
  params->put("cathode_collector_thickness", 50.0e-4);
  params->put("geometric_area", 25.0e-2);
  params->put("tab_height", 5.0e-4);

  // For now, both collectors must have the same dimensions.
  BOOST_CHECK_THROW(cap::Geometry<2> geo(params, boost::mpi::communicator()),
                    std::runtime_error);
}

BOOST_AUTO_TEST_CASE(test_3d_geometry)
{
  boost::property_tree::ptree device_database;
  boost::property_tree::info_parser::read_info("super_capacitor.info",
                                               device_database);
  boost::property_tree::ptree geometry_database;
  boost::property_tree::info_parser::read_info("generate_mesh.info",
                                               geometry_database);
  std::vector<unsigned int> divisions(3);
  // Collector
  divisions[0] = 3;
  divisions[1] = 3;
  divisions[2] = 3;
  geometry_database.put("collector.divisions", cap::to_string(divisions));
  // Anode
  divisions[0] = 3;
  divisions[1] = 3;
  divisions[2] = 2;
  geometry_database.put("anode.divisions", cap::to_string(divisions));
  // Separator
  divisions[0] = 3;
  divisions[1] = 3;
  divisions[2] = 2;
  geometry_database.put("separator.divisions", cap::to_string(divisions));
  // Cathode
  divisions[0] = 3;
  divisions[1] = 3;
  divisions[2] = 2;
  geometry_database.put("cathode.divisions", cap::to_string(divisions));

  device_database.put_child("geometry", geometry_database);
  device_database.put("dim", 3);

  std::shared_ptr<cap::EnergyStorageDevice> device =
      cap::EnergyStorageDevice::build(device_database,
                                      boost::mpi::communicator());
  std::shared_ptr<cap::SuperCapacitor<3>> supercapacitor =
      std::static_pointer_cast<cap::SuperCapacitor<3>>(device);
  std::shared_ptr<cap::Geometry<3>> geometry = supercapacitor->get_geometry();
  std::shared_ptr<dealii::distributed::Triangulation<3> const> triangulation =
      geometry->get_triangulation();
  const unsigned int n_cells = 6912;
  BOOST_CHECK(n_cells == triangulation->n_active_cells());
}


#pragma once
/**

   @file KinematicVessel.hpp
   @brief Kinematic planar vessel.

*/

#include <atomic>
#include <chrono>
#include <cinttypes>
#include <memory>

#include <boost/statechart/detail/memory.hpp>
#include <boost/statechart/fifo_scheduler.hpp>
#include <boost/statechart/event_base.hpp>

#include <yaml-cpp/yaml.h>

#ifdef _MSC_VER
#pragma warning(push, 0)
#endif
#include <dds/pub/Publisher.hpp>
#include <dds/sub/Subscriber.hpp>
#ifdef _MSC_VER
#pragma warning(pop)
#endif

#include <mimir/IAlgorithm.hpp>

namespace mimir
{
  namespace algorithm
  {

    /**
       @brief Kinematic planar vessel numerical simulation model.

       \rst

       This class implements a kinematic planar vessel that are controlled by inputs,
       which are velocities and rate of turn.

       .. math::
          :nowrap:

          \begin{align}
          \boldsymbol{p} = \begin{bmatrix}N\\E\end{bmatrix} &\in \mathbb{R}^2 &\text{position}\\
          \boldsymbol{\psi} \in \mathbb{S} &&\text{heading}\\
          \boldsymbol{v} = \begin{bmatrix}u\\v\end{bmatrix} &\in \mathbb{R}^2 &\text{linear velocity}\\
          \boldsymbol{r} \in \mathbb{R} &&\text{angular velocity}\\
          x(t) = \begin{bmatrix}\boldsymbol{p}\\\psi\end{bmatrix}, &\quad\mathbb{R} \to \mathbb{R}^2 \times \mathbb{S}&\text{state vector}
          \end{align}

       .. math::
          :nowrap:

          \begin{align}
          \dat x &= \begin{bmatrix}\cos(\psi)& -\sin(\psi) &0\\
                                  \sin(\psi)& \cos(\psi) & 0\\
                                  0&0&1
                                  \end{bmatrix}
                   \begin{bmatrix}u\\v\\r\end{bmatrix}\\
          \boldsymbol{u} &= \begin{bmatrix}u\\v\\r\end{bmatrix}\\
          \boldsymbol{y} &= x
          \end{align}

       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+
       | Name                 | Symbol                     | Description                         | Causality     | Variability    | Default              | Unit              |
       +======================+============================+=====================================+===============+================+======================+===================+
       | ``vessel_ctrl``      | :math:`u`                  | Desired: Surge, Sway rate of turn   | ``input``     | ``discrete``   | \                    | [m/s, m/s, rad/s] |
       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+
       | ``kinematics``       | :math:`y`                  | ``fkin::Kinematics2D``              | ``output``    | ``continuous`` | \                    | \                 |
       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+
       | ``position_course``  | :math:`[p_0,\psi_0]`       | Initial; North, East, Yaw           | ``parameter`` | ``fixed``      | :math:`[0, 50, 0]`   | m                 |
       +----------------------+----------------------------+-------------------------------------+---------------+----------------+----------------------+-------------------+

       .. warning::

          This simulation model is very basic and has been abandoned in favor of a higher
          fidelity model implemented using FMI :cite:`fmi2`.

       \endrst

    */
    class KinematicVessel : public IAlgorithm
    {
    public:
      /**
         @brief KinematicVessel constructor.

         The constructor parses the specification from the given YAML
         node. It establishes data structures and sets up DDS readers
         and writers according to the input/output scheme of the
         algorithm. The following YAML code block shows the expected
         layout of the ``KinematicVessel`` map of a input config file. The
         inputs, outputs, and initial conditions are communicated with
         DDS communication. Common is their DDS topic and DDS
         identifier. The specification is deemed self-explanatory.

         \rst

         .. code-block:: yaml

             time_step_ms: 200
             inputs:
               vessel_ctrl:                 # DDS type: fkin::IdVec3d
                 topic: fkinVesselCtrl
                 id: Vessel
                 max_age_ms: -1
             outputs:                       # DDS type: fkin::Kinematics2D
               kinematics:
                 topic: fkinKinematics2D
                 id: Vessel
             initial_conditions:
               position_course:             # DDS type: fkin::IdVec3d
                 topic: fkinPositionCourse
                 id: Vessel
                 max_wait_ms: 50
                 fallback: [0, 50, 0]
         \endrst

         @param [in] config YAML configuration from input file.
         @param [in] scheduler State machine scheduler, needed to post events to state machine.
         @param [in] machine State machine processor handle, needed to post events to state machine.
         @param publisher DDS data writer to send data.
         @param subscriber DDS data reader to receive data.
      */
      explicit KinematicVessel(
          const YAML::Node& config,
          boost::statechart::fifo_scheduler<>& scheduler,
          boost::statechart::fifo_scheduler<>::processor_handle machine,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber);
      /// Destructor.
      virtual ~KinematicVessel();
      /// See base class.
      virtual void solve(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void initialize(const std::atomic<bool>& cancel_token);
      /// See base class.
      virtual void timer(const std::atomic<bool>& cancel_token);
      /// Name identifier of algorithm.
      virtual inline const char* name() { return "KinematicVessel"; }
      /// Helper function that queues event to state machine.
      void event(boost::statechart::event_base * const event);

    private:
      /// March simulation time one time step ahead.
      inline void step_time() { m_next_step += m_time_step; }
      KinematicVessel() = delete;
      class Impl;
      std::unique_ptr<Impl> m_impl; ///< Holds the implementation of the algorithm.
      boost::statechart::fifo_scheduler<> & m_scheduler; ///< Members for state machine.
      boost::statechart::fifo_scheduler<>::processor_handle m_stateMachine; ///< Member for state machine.
      const std::chrono::milliseconds m_time_step; ///< Discrete time step.
      std::chrono::steady_clock::time_point m_next_step; ///< Simulation time point.
      const YAML::Node m_config; ///< YAML configuration for algorithm.
    };

  }
}


//===- odla_dnnl_binary.cc ---------------------------------------------===//
//
// Copyright (C) 2019-2021 Alibaba Group Holding Limited.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//   http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// =============================================================================

#include <Eigen/Core>

#include "odla_dnnl.h"

enum class alg_binary_eltwise {
  logic_or,
  logic_and,
  logic_xor,
  cmp_equal,
  cmp_less,
  cmp_less_equal,
  cmp_greater,
  cmp_greater_equal,
  max,
  min,
  mod,
  fmod,
  pow,
  sum,
};

// Input and output are both bool type.
static void binary_eltwise_logic(alg_binary_eltwise alg, void* dst,
                                 const void* data_l, const void* data_r,
                                 int n) {
  const bool* input_l = static_cast<const bool*>(data_l);
  Eigen::Map<const Eigen::Array<bool, Eigen::Dynamic, 1>> in_l(input_l, n);
  const bool* input_r = static_cast<const bool*>(data_r);
  Eigen::Map<const Eigen::Array<bool, Eigen::Dynamic, 1>> in_r(input_r, n);
  bool* dst_t = static_cast<bool*>(dst);
  Eigen::Map<Eigen::Array<bool, Eigen::Dynamic, 1>> out(dst_t, n);
  switch (alg) {
    case alg_binary_eltwise::logic_and:
      out = in_l && in_r;
      break;
    case alg_binary_eltwise::logic_or:
      out = in_l || in_r;
      break;
    case alg_binary_eltwise::logic_xor:
      out = in_l != in_r;
      break;
    default:
      assert(0);
  }
}

// Binary elementwise helper with return type bool
template <typename T>
static void binary_eltwise_bool(alg_binary_eltwise alg, void* dst,
                                const void* data_l, const void* data_r, int n) {
  const T* input_l = static_cast<const T*>(data_l);
  Eigen::Map<const Eigen::Array<T, Eigen::Dynamic, 1>> in_l(input_l, n);
  const T* input_r = static_cast<const T*>(data_r);
  Eigen::Map<const Eigen::Array<T, Eigen::Dynamic, 1>> in_r(input_r, n);
  bool* dst_t = static_cast<bool*>(dst);
  Eigen::Map<Eigen::Array<bool, Eigen::Dynamic, 1>> out(dst_t, n);
  switch (alg) {
    case alg_binary_eltwise::cmp_equal:
      out = (in_l == in_r);
      break;
    case alg_binary_eltwise::cmp_less:
      out = (in_l < in_r);
      break;
    case alg_binary_eltwise::cmp_less_equal:
      out = (in_l <= in_r);
      break;
    case alg_binary_eltwise::cmp_greater:
      out = (in_l > in_r);
      break;
    case alg_binary_eltwise::cmp_greater_equal:
      out = (in_l >= in_r);
      break;
    default:
      assert(0);
  }
}

// Binary elementwise helper with return type T
template <typename T>
static void binary_eltwise_T(alg_binary_eltwise alg, void* dst,
                             const void* data_l, const void* data_r, int n) {
  const T* input_l = static_cast<const T*>(data_l);
  Eigen::Map<const Eigen::Array<T, Eigen::Dynamic, 1>> in_l(input_l, n);
  const T* input_r = static_cast<const T*>(data_r);
  Eigen::Map<const Eigen::Array<T, Eigen::Dynamic, 1>> in_r(input_r, n);
  T* dst_t = static_cast<T*>(dst);
  Eigen::Map<Eigen::Array<T, Eigen::Dynamic, 1>> out(dst_t, n);
  switch (alg) {
    case alg_binary_eltwise::mod: {
      Eigen::Array<double, Eigen::Dynamic, 1> t_l =
          in_l.template cast<double>();
      Eigen::Array<double, Eigen::Dynamic, 1> t_r =
          in_r.template cast<double>();
      Eigen::Array<T, Eigen::Dynamic, 1> quo =
          Eigen::floor(t_l / t_r).cast<T>();
      out = in_l - in_r * quo;
      break;
    }
    case alg_binary_eltwise::fmod: {
      Eigen::Array<double, Eigen::Dynamic, 1> t_l =
          in_l.template cast<double>();
      Eigen::Array<double, Eigen::Dynamic, 1> t_r =
          in_r.template cast<double>();
      Eigen::Array<double, Eigen::Dynamic, 1> t = t_l / t_r;
      Eigen::Array<int, Eigen::Dynamic, 1> quo_floor =
          Eigen::floor(t).cast<int>();
      Eigen::Array<int, Eigen::Dynamic, 1> quo_ceil =
          Eigen::ceil(t).cast<int>();
      Eigen::Array<T, Eigen::Dynamic, 1> quo =
          (t > 0.0f).select(quo_floor, quo_ceil).cast<T>();
      out = in_l - in_r * quo;
      break;
    }
    case alg_binary_eltwise::pow: {
      out = in_l.pow(in_r);
      break;
    }
    default: {
      assert(0);
    }
  }
}

bool binary_ret_bool(alg_binary_eltwise alg) {
  return (alg == alg_binary_eltwise::logic_or) ||
         (alg == alg_binary_eltwise::logic_and) ||
         (alg == alg_binary_eltwise::logic_xor) ||
         (alg == alg_binary_eltwise::cmp_equal) ||
         (alg == alg_binary_eltwise::cmp_less) ||
         (alg == alg_binary_eltwise::cmp_less_equal) ||
         (alg == alg_binary_eltwise::cmp_greater) ||
         (alg == alg_binary_eltwise::cmp_greater_equal);
}

bool is_binary_logic(alg_binary_eltwise alg) {
  return (alg == alg_binary_eltwise::logic_or) ||
         (alg == alg_binary_eltwise::logic_and) ||
         (alg == alg_binary_eltwise::logic_xor);
}

static odla_value odla_binary_eltwise(alg_binary_eltwise alg, odla_value lhs,
                                      odla_value rhs,
                                      const odla_value_id value_id) {
  assert(lhs->elem_type == rhs->elem_type);
  // Broadcast lhs and rhs
  auto input_type = lhs->elem_type;
  // std::cout << input_type << std::endl;
  // std::cout << ODLA_INT32 << std::endl;
  auto ret_type = binary_ret_bool(alg) ? ODLA_BOOL : input_type;
  odla_value_shape ret_shape;
  auto new_mems = broadcast_operands(lhs, rhs, &ret_shape);
  auto lhs_m = new_mems.first;
  auto rhs_m = new_mems.second;
  // Prepare ret memory
  auto ret_md = getMemoryDesc(ret_shape, ret_type);
  auto ret_mem = dnnl::memory(ret_md, g_comp->eng);
  auto n = GetTotalElements(ret_shape);
  // Create lambda func as op
  auto op = [alg, input_type, ret_type, lhs_m, rhs_m, ret_mem, n] {
    void* dst = ret_mem.get_data_handle();
    const void* data_l = lhs_m.get_data_handle();
    const void* data_r = rhs_m.get_data_handle();
    if (is_binary_logic(alg)) {
      binary_eltwise_logic(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_FLOAT32) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<float>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<float>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_FLOAT64) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<double>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<double>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_UINT8) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<uint8_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<uint8_t>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_UINT16) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<uint16_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<uint16_t>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_UINT32) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<uint32_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<uint32_t>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_UINT64) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<uint64_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<uint64_t>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_INT8) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<int8_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<int8_t>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_INT16) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<int16_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<int16_t>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_INT32) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<int32_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<int32_t>(alg, dst, data_l, data_r, n);
    } else if (input_type == ODLA_INT64) {
      ret_type == ODLA_BOOL
          ? binary_eltwise_bool<int64_t>(alg, dst, data_l, data_r, n)
          : binary_eltwise_T<int64_t>(alg, dst, data_l, data_r, n);
    } else {
      assert(0);
    }
  };
  // Postprocess
  add_op(op);
  odla_value v = CreateValue(ret_mem, ret_shape, value_id);
  v->elem_type = ret_type;
  InterpretIfNeeded();
  return v;
}

odla_value odla_And(odla_value lhs, odla_value rhs,
                    const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::logic_and, lhs, rhs, value_id);
}

odla_value odla_Or(odla_value lhs, odla_value rhs,
                   const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::logic_or, lhs, rhs, value_id);
}

odla_value odla_Xor(odla_value lhs, odla_value rhs,
                    const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::logic_xor, lhs, rhs, value_id);
}

odla_value odla_Equal(odla_value lhs, odla_value rhs,
                      const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::cmp_equal, lhs, rhs, value_id);
}

odla_value odla_Less(odla_value lhs, odla_value rhs,
                     const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::cmp_less, lhs, rhs, value_id);
}

odla_value odla_LessOrEqual(odla_value lhs, odla_value rhs,
                            const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::cmp_less_equal, lhs, rhs,
                             value_id);
}

odla_value odla_Greater(odla_value lhs, odla_value rhs,
                        const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::cmp_greater, lhs, rhs,
                             value_id);
}

odla_value odla_GreaterOrEqual(odla_value lhs, odla_value rhs,
                               const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::cmp_greater_equal, lhs, rhs,
                             value_id);
}

odla_value odla_Pow(odla_value base, odla_value exponent,
                    const odla_value_id value_id) {
  return odla_binary_eltwise(alg_binary_eltwise::pow, base, exponent, value_id);
}

odla_value odla_Mod(odla_value lhs, odla_value rhs, odla_int64 fmod,
                    const odla_value_id value_id) {
  if (fmod == 1) {
    return odla_binary_eltwise(alg_binary_eltwise::fmod, lhs, rhs, value_id);
  } else {
    auto t = odla_binary_eltwise(alg_binary_eltwise::mod, lhs, rhs, value_id);
    return t;
  }
}


#ifndef __PICSAR_MULTIPHYSICS_QUADRATURE__
#define __PICSAR_MULTIPHYSICS_QUADRATURE__

//This .hpp file is a wrapper aroud the trapezoidal, gauss_kronrod & tanh_sinh methods
//provided by the Boost library

#include <cmath>
#include <functional>
#include <limits>

#include <boost/math/quadrature/trapezoidal.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>

//Should be included by all the src files of the library
#include "qed_commons.h"

//############################################### Declaration

namespace picsar{
    namespace multi_physics{

        //Provides integration in a finite interval [a,b]
        template<typename _REAL>
        PXRMP_FORCE_INLINE
        _REAL quad_a_b(const std::function<_REAL(_REAL)>& f, _REAL a, _REAL b);

        //Provides integration in a finite interval [a,b] (more suitable for
        //singularities at boundaries )
        template<typename _REAL>
        PXRMP_FORCE_INLINE
        _REAL quad_a_b_s(const std::function<_REAL(_REAL)>& f, _REAL a, _REAL b);

        //Provides integration in a semi-infinite interval [a,inf)
        template<typename _REAL>
        PXRMP_FORCE_INLINE
        _REAL quad_a_inf(const std::function<_REAL(_REAL)>& f, _REAL a);
    }
}

//############################################### Implementation

//This is really just a wrapper around the function provided by
//the Boost libray
template<typename _REAL>
PXRMP_FORCE_INLINE
_REAL picsar::multi_physics::quad_a_b
(const std::function<_REAL(_REAL)>& f, _REAL a, _REAL b)
{
    return boost::math::quadrature::trapezoidal(f, a, b);
}

//This is really just a wrapper around the function provided by
//the Boost libray
template<typename _REAL>
PXRMP_FORCE_INLINE
_REAL picsar::multi_physics::quad_a_b_s
(const std::function<_REAL(_REAL)>& f, _REAL a, _REAL b)
{
        boost::math::quadrature::tanh_sinh<_REAL> integrator;
        return integrator.integrate(f, a, b);
}

//This is really just a wrapper around the function provided by
//the Boost libray
template<typename _REAL>
PXRMP_FORCE_INLINE
_REAL picsar::multi_physics::quad_a_inf
(const std::function<_REAL(_REAL)>& f, _REAL a)
{
    return boost::math::quadrature::gauss_kronrod<_REAL, 15>
        ::integrate(f, a, std::numeric_limits<_REAL>::infinity());
}

#endif //__PICSAR_MULTIPHYSICS_QUADRATURE__


#ifndef AUDIO_PARAMS_HPP
#define AUDIO_PARAMS_HPP

#include <scluk/aliases.hpp>
#include <scluk/array.hpp>
#include <boost/fiber/buffered_channel.hpp>
#include <bit>
#include "portaudio/stream_wrapper.hpp"
#include "dft/sliding_dft.hpp"

namespace audio {
    using scluk::u64, scluk::f32, scluk::heap_array;
    #ifndef LOWER_PERFORMANCE_MODE
    constexpr u64 ft_win = 1024;
    constexpr u64 rate = ft_win*50;//~40-50k
    constexpr u64 ift_overlap = 4;
    constexpr u64 ft_dist = ft_win / ift_overlap;
    #else
    constexpr u64 rate = 10000;
    constexpr u64 ft_win = 512;
    constexpr u64 ift_overlap = 4;
    constexpr u64 ft_dist = ft_win / ift_overlap;
    #endif
    using sliding_dft = dft::sliding_dft<f32, ft_win>;
    using dft_array = sliding_dft::dft_array;
    using frame_chunk = scluk::heap_array<f32, ft_dist>;
    using ift_chunk = scluk::heap_array<f32, ft_win>;
    using gui_simplex_chan = boost::fibers::buffered_channel<dft_array>;

    static_assert(std::has_single_bit(ft_win), "ft_win must be a power of two to allow us to use cooley-tukey ifft");

    struct duplex_chan {
        boost::fibers::buffered_channel<audio::frame_chunk> cb_to_main, main_to_cb;
        duplex_chan(u64 cb_to_main_sz=2, u64 main_to_cb_sz=2) : cb_to_main(cb_to_main_sz), main_to_cb(main_to_cb_sz) {}
    };

    int cb(const void* v_i_buf, void* v_o_buf, [[maybe_unused]]u64 frames, auto, auto, void* userdata) {
        using namespace std::chrono_literals;
        assert(frames == audio::ft_dist && "You were my brother Anakin");
        auto& chans = *reinterpret_cast<audio::duplex_chan*>(userdata);
        const f32* i_buf = reinterpret_cast<const f32*>(v_i_buf);
        f32* o_buf = reinterpret_cast<f32*>(v_o_buf);
        audio::frame_chunk to_main, to_pa;

        std::copy(i_buf, i_buf + to_main.size(), to_main.begin());
        chans.cb_to_main.push_wait_for(std::move(to_main), 100ms);

        chans.main_to_cb.pop_wait_for(to_pa, 100ms);
        std::copy(to_pa.begin(), to_pa.end(), o_buf);

        return paContinue;
    }
}

#endif


//
// Copyright 2010 Scott McMurray.
// 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 BOOST_HASH_BLOCK_CYPHERS_SHACAL2_HPP
#define BOOST_HASH_BLOCK_CYPHERS_SHACAL2_HPP

#include <boost/hash/block_cyphers/detail/shacal2_policy.hpp>
#include <boost/static_assert.hpp>

#ifdef BOOST_HASH_SHOW_PROGRESS
#include <cstdio>
#endif

//
// Encrypt implemented directly from the SHA standard as found at
// http://csrc.nist.gov/publications/fips/fips180-2/fips180-2.pdf
//
// Decrypt is a straight-forward inverse
//
// In SHA terminology:
// - plaintext = H^(i-1)
// - cyphertext = H^(i)
// - key = M^(i)
// - schedule = W
//

namespace boost {
namespace hashes {
namespace block_cyphers {

template <unsigned Version>
class shacal2 {
  public:
    static unsigned const version = Version;
    typedef detail::shacal2_policy<version> policy_type;

    static unsigned const word_bits = policy_type::word_bits;
    typedef typename policy_type::word_type word_type;

    static unsigned const key_bits = policy_type::key_bits;
    static unsigned const key_words = policy_type::key_words;
    typedef typename policy_type::key_type key_type;

    static unsigned const block_bits = policy_type::block_bits;
    static unsigned const block_words = policy_type::block_words;
    typedef typename policy_type::block_type block_type;

    static unsigned const rounds = policy_type::rounds;
    typedef typename policy_type::schedule_type schedule_type;

  public:
    shacal2(key_type const &key)
     : schedule(build_schedule(key)) {}
    shacal2(schedule_type s)
     : schedule((prepare_schedule(s), s)) {}
  private:
    static schedule_type
    build_schedule(key_type const &key) {
        // Copy key into beginning of schedule
        schedule_type schedule;
        for (unsigned t = 0; t < key_words; ++t) {
            schedule[t] = key[t];
        }
        prepare_schedule(schedule);
        return schedule;
    }
    static void
    prepare_schedule(schedule_type &schedule) {
#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < key_words; ++t) {
            std::printf(word_bits == 32 ?
                        "W[%2d] = %.8x\n" :
                        "W[%2d] = %.16lx\n",
                        t, schedule[t]);
        }
#endif

        for (unsigned t = key_words; t < rounds; ++t) {
            schedule[t] = policy_type::sigma_1(schedule[t-2])
                        + schedule[t-7]
                        + policy_type::sigma_0(schedule[t-15])
                        + schedule[t-16];
        }
    }
    schedule_type const schedule;

  public:
    block_type
    encypher(block_type const &plaintext) {
        return encypher_block(plaintext);
    }
  private:
    block_type
    encypher_block(block_type const &plaintext) {
        return encypher_block(schedule, plaintext);
    }
    static block_type
    encypher_block(schedule_type const &schedule,
                   block_type const &plaintext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf(word_bits == 32 ?
                        "H[%d] = %.8x\n" :
                        "H[%d] = %.16lx\n",
                        t, plaintext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = plaintext[0], b = plaintext[1],
                  c = plaintext[2], d = plaintext[3],
                  e = plaintext[4], f = plaintext[5],
                  g = plaintext[6], h = plaintext[7];

        // Encypher block
#ifdef BOOST_HASH_NO_OPTIMIZATION

        for (unsigned t = 0; t < rounds; ++t) {
            word_type T1 = h
                         + policy_type::Sigma_1(e)
                         + policy_type::Ch(e, f, g)
                         + policy_type::constant(t)
                         + schedule[t];
            word_type T2 = policy_type::Sigma_0(a)
                         + policy_type::Maj(a, b, c);

            h = g;
            g = f;
            f = e;
            e = d + T1;
            d = c;
            c = b;
            b = a;
            a = T1 + T2;

#ifdef BOOST_HASH_SHOW_PROGRESS
            std::printf(word_bits == 32 ?
                        "t = %2d: %.8x %.8x %.8x %.8x"
                                " %.8x %.8x %.8x %.8x\n" :
                        "t = %2d: %.16lx %.16lx %.16lx %.16lx"
                                " %.16lx %.16lx %.16lx %.16lx\n",
                        t, a, b, c, d,
                           e, f, g, h);
#endif
        }

#else // BOOST_HASH_NO_OPTIMIZATION

        BOOST_STATIC_ASSERT(rounds % block_words == 0);
        for (unsigned t = 0; t < rounds; ) {
            for (int n = block_words; n--; ++t) {
                word_type T1 = h
                             + policy_type::Sigma_1(e)
                             + policy_type::Ch(e, f, g)
                             + policy_type::constant(t)
                             + schedule[t];
                word_type T2 = policy_type::Sigma_0(a)
                             + policy_type::Maj(a, b, c);

                h = g;
                g = f;
                f = e;
                e = d + T1;
                d = c;
                c = b;
                b = a;
                a = T1 + T2;

#ifdef BOOST_HASH_SHOW_PROGRESS
                std::printf(word_bits == 32 ?
                            "t = %2d: %.8x %.8x %.8x %.8x"
                                    " %.8x %.8x %.8x %.8x\n" :
                            "t = %2d: %.16lx %.16lx %.16lx %.16lx"
                                    " %.16lx %.16lx %.16lx %.16lx\n",
                            t, a, b, c, d,
                               e, f, g, h);
#endif
            }
        }

#endif

        block_type cyphertext = {{a, b, c, d, e, f, g, h}};
        return cyphertext;
    }

  public:
    block_type
    decypher(block_type const &plaintext) {
        return decypher_block(plaintext);
    }
  private:
    block_type
    decypher_block(block_type const &plaintext) {
        return decypher_block(schedule, plaintext);
    }
    static block_type
    decypher_block(schedule_type const &schedule,
                   block_type const &cyphertext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf(word_bits == 32 ?
                        "H[%d] = %.8x\n" :
                        "H[%d] = %.16lx\n",
                        t, cyphertext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = cyphertext[0], b = cyphertext[1],
                  c = cyphertext[2], d = cyphertext[3],
                  e = cyphertext[4], f = cyphertext[5],
                  g = cyphertext[6], h = cyphertext[7];

        // Decypher block
        for (unsigned t = rounds; t--; ) {
            word_type T2 = policy_type::Sigma_0(b)
                         + policy_type::Maj(b, c, d);
            word_type T1 = a - T2;

            a = b;
            b = c;
            c = d;
            d = e - T1;
            e = f;
            f = g;
            g = h;
            h = T1
              - policy_type::Sigma_1(e)
              - policy_type::Ch(e, f, g)
              - policy_type::constant(t)
              - schedule[t];

#ifdef BOOST_HASH_SHOW_PROGRESS
            std::printf(word_bits == 32 ?
                        "t = %2d: %.8x %.8x %.8x %.8x"
                                " %.8x %.8x %.8x %.8x\n" :
                        "t = %2d: %.16lx %.16lx %.16lx %.16lx"
                                " %.16lx %.16lx %.16lx %.16lx\n",
                        t, a, b, c, d,
                           e, f, g, h);
#endif
        }

        block_type plaintext = {{a, b, c, d, e, f, g, h}};
        return plaintext;
    }

};

} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_SHACAL2_HPP


#include <gtest/gtest.h>
#include "filters.hpp"
#include <filters/filter_chain.h>
#include <math.h>
#include <Eigen/Dense>

#define GTEST_COUT std::cerr << "[   INFO   ] "

TEST(BayesUpdate, active)
{
    EXPECT_EQ(0, 0);
}

/**
 * Test that error checking works for cases in
 * which the
 */
TEST(BayesUpdate, layers_dont_exist)
{
    ros::NodeHandle nh("~");

    grid_map::GridMap gridMap({"random_layer"});
    gridMap.setFrameId("random");
    gridMap.setGeometry(grid_map::Length(3, 3), 1);

    filters::FilterChain<grid_map::GridMap> filterChain("grid_map::GridMap");

    if (!filterChain.configure("mitre_fast_layered_map_filters_bayes_update", nh))
    {
        GTEST_FATAL_FAILURE_("Unable to configure filter chain."); // We should never get here
    }

    EXPECT_FALSE(filterChain.update(gridMap, gridMap));

    // Bayes filter needs current_probability and history layers
    gridMap.add("current_probability", 0);
    EXPECT_FALSE(filterChain.update(gridMap, gridMap));

    // Expects 4 history layers, only add 2 and ensure it throws error
    gridMap.add("history_0", 0);
    gridMap.add("history_1", 0);
    EXPECT_FALSE(filterChain.update(gridMap, gridMap));

    // Add other 2 history layers
    gridMap.add("history_2", 0);
    gridMap.add("history_3", 0);
    EXPECT_TRUE(filterChain.update(gridMap, gridMap));

}

// TO ADD: TEST RUNNING FILTER WHEN FRAMES_IN_SCOPE IS JUST ALL 0's

/***
 * All of the tests below this point are just checking that the math
 * comes out as expected. You can use this to refer to the expected performance
 * given the settings.
 */

/**
 * Ensure its outputting expected values when a reading is given as 1 or 0
 */
TEST(BayesUpdate, binary)
{
    ros::NodeHandle nh("~");

    // NOTE: Current probability will be filled with nans, but the filter should take care of it
    grid_map::GridMap gridMap({"current_probability"});
    gridMap.setFrameId("random");
    gridMap.setGeometry(grid_map::Length(3, 3), 1);

    filters::FilterChain<grid_map::GridMap> filterChain("grid_map::GridMap");

    if (!filterChain.configure("mitre_fast_layered_map_filters_bayes_update", nh))
    {
        GTEST_FATAL_FAILURE_("Unable to configure filter chain."); // We should never get here
    }

    // Add all of our history layers, -1 for no data
    gridMap.add("history_0", -1);
    gridMap.add("history_1", -1);
    gridMap.add("history_2", -1);
    gridMap.add("history_3", -1);

    //
    // FRAME ONE
    //

    // Initialize obstacle detected, only two cells have detection in first frame
    gridMap["history_0"] <<
    1, 1, 0,
    0, 0, 0,
    0, 0, 0;

    // Create matrix of expected output to compare to
    Eigen::MatrixXf answerMat(3, 3);
    answerMat << (2.0 / 3), (2.0 / 3), (1.0 / 3),
        (1.0 / 3), (1.0 / 3), (1.0 / 3),
        (1.0 / 3), (1.0 / 3), (1.0 / 3);

    if (!filterChain.update(gridMap, gridMap))
    {
        GTEST_FATAL_FAILURE_("Unable to update grid map.");
    }

    for (grid_map::GridMapIterator it(gridMap); !it.isPastEnd(); ++it)
    {
        const grid_map::Index index(*it);

        EXPECT_FLOAT_EQ(answerMat(index(0), index(1)), gridMap.at("current_probability", index)) << "Index: " << index(0) << " " << index(1);
    }

    //
    // FRAME TWO
    //

    // Set points for second history frame
    gridMap["history_1"] <<
    1, 0, 0,
    1, 0, 0,
    0, 0, 0;

    // Create matrix of expected output to compare to
    answerMat << 0.8, 0.5, 0.2,
        0.5, 0.2, 0.2,
        0.2, 0.2, 0.2;

    if (!filterChain.update(gridMap, gridMap))
    {
        GTEST_FATAL_FAILURE_("Unable to update grid map.");
    }

    for (grid_map::GridMapIterator it(gridMap); !it.isPastEnd(); ++it)
    {
        const grid_map::Index index(*it);

        EXPECT_FLOAT_EQ(answerMat(index(0), index(1)), gridMap.at("current_probability", index)) << "Index: " << index(0) << " " << index(1);
    }
}

// /**
//  * Ensure its outputting expected values for non-binary readings
//  */
TEST(BayesUpdate, readings)
{
    ros::NodeHandle nh("~");

    // NOTE: Current probability will be filled with nans, but the filter should take care of it
    grid_map::GridMap gridMap({"current_probability"});
    gridMap.setFrameId("random");
    gridMap.setGeometry(grid_map::Length(3, 3), 1);

    filters::FilterChain<grid_map::GridMap> filterChain("grid_map::GridMap");

    if (!filterChain.configure("mitre_fast_layered_map_filters_bayes_update", nh))
    {
        GTEST_FATAL_FAILURE_("Unable to configure filter chain."); // We should never get here
    }

    //
    // FRAME ONE
    //

    // Add all of our history layers, -1 for no data
    gridMap.add("history_0", -1);
    gridMap.add("history_1", -1);
    gridMap.add("history_2", -1);
    gridMap.add("history_3", -1);

    gridMap["history_0"] <<
    0, 1, 2,
    3, 4, 5,
    6, 7, 8;

    // Create matrix of expected output to compare to
    // ALL PROBABILITY CALCULATED USING src/utils/bayes_filter_profiler.py
    Eigen::MatrixXf answerMat(3, 3);
    answerMat << (1.0 / 3), (2.0 / 3), 0.8333333,
        0.9836, 0.9859, 0.9876,
        0.9890, 0.99, 0.99;

    if (!filterChain.update(gridMap, gridMap))
    {
        GTEST_FATAL_FAILURE_("Unable to update grid map.");
    }

    for (grid_map::GridMapIterator it(gridMap); !it.isPastEnd(); ++it)
    {
        const grid_map::Index index(*it);

        EXPECT_NEAR(answerMat(index(0), index(1)), gridMap.at("current_probability", index), 0.001) << "Index: " << index(0) << " " << index(1);
    }

    //
    // FRAME TWO
    //

    gridMap["history_1"] <<
    4, 3, 0,
    5, 2, 1,
    8, 9, 3;

    // Create matrix of expected output to compare to
    answerMat << 0.9722, 0.9917, 0.714285,
        0.9997, 0.9971, 0.9937,
        0.9998, 0.9998, 0.9998;

    if (!filterChain.update(gridMap, gridMap))
    {
        GTEST_FATAL_FAILURE_("Unable to update grid map.");
    }

    for (grid_map::GridMapIterator it(gridMap); !it.isPastEnd(); ++it)
    {
        const grid_map::Index index(*it);

        EXPECT_NEAR(answerMat(index(0), index(1)), gridMap.at("current_probability", index), 0.001) << "Index: " << index(0) << " " << index(1);
    }
}

// TEST(BayesUpdate, maxReadings)
// {
//     // CREATE TEST THAT USES MAX NUMBER OF HISTORY READINGS
// }


// File: shallow_copy_problems_type.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

int main(int, char**)
{
    dense2D<double>     A(3, 3), B(3, 3);
    dense2D<float>      C(3, 3);

    A= 4.0;

    B= A;               // Create an alias
    B*= 2.0;            // Changes also A

    C= A;               // Copies the values
    C*= 2.0;            // A is unaffected

    return 0;
}


#pragma once

//FIXME this is a duplicate from rock-core/boost_serialization. It exists because I did not want all the dependencies that come with the rock-core

#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace boost { namespace serialization
{

    /** Serializes all Eigen::Matrix types */
    template<typename _Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
    inline void serialize(
        _Archive & ar,
        Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols >& matrix,
        const unsigned int version)
    {
        int rows = matrix.rows();
        int cols = matrix.cols();
        ar & BOOST_SERIALIZATION_NVP(rows);
        ar & BOOST_SERIALIZATION_NVP(cols);
        if(rows != matrix.rows() || cols != matrix.cols())
            matrix.resize(rows, cols);
        ar & boost::serialization::make_nvp("matrix", boost::serialization::make_array(matrix.data(), rows * cols));
    }

    /** Serializes all Eigen::Quaternion types */
    template<typename _Archive, typename _Scalar, int _Options>
    inline void serialize(
        _Archive & ar,
        Eigen::Quaternion<_Scalar, _Options>& quaternion,
        const unsigned int version)
    {
        ar & boost::serialization::make_nvp("x", quaternion.x());
        ar & boost::serialization::make_nvp("y", quaternion.y());
        ar & boost::serialization::make_nvp("z", quaternion.z());
        ar & boost::serialization::make_nvp("w", quaternion.w());
    }

    /** Serializes Eigen::Transform types */
    template<typename _Archive, typename _Scalar, int _Dim, int _Mode, int _Options>
    inline void serialize(
        _Archive & ar,
        Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform,
        const unsigned int version)
    {
        ar & transform.matrix();
    }

}}


/* boost random/lagged_fibonacci.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: lagged_fibonacci.hpp,v 1.28 2005/05/21 15:57:00 dgregor Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_LAGGED_FIBONACCI_HPP
#define BOOST_RANDOM_LAGGED_FIBONACCI_HPP

#include <cmath>
#include <iostream>
#include <algorithm>     // std::max
#include <iterator>
#include <cmath>         // std::pow
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/cstdint.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/detail/pass_through_engine.hpp>

namespace boost {
namespace random {

#if BOOST_WORKAROUND(_MSC_FULL_VER, BOOST_TESTED_AT(13102292)) && BOOST_MSVC > 1300
#  define BOOST_RANDOM_EXTRACT_LF
#endif

#if defined(__APPLE_CC__) && defined(__GNUC__) && (__GNUC__ == 3) && (__GNUC_MINOR__ <= 3)
#  define BOOST_RANDOM_EXTRACT_LF
#endif

#  ifdef BOOST_RANDOM_EXTRACT_LF
namespace detail
{
  template<class IStream, class F, class RealType>
  IStream&
  extract_lagged_fibonacci_01(
      IStream& is
      , F const& f
      , unsigned int& i
      , RealType* x
      , RealType modulus)
  {
        is >> i >> std::ws;
        for(unsigned int i = 0; i < f.long_lag; ++i)
        {
            RealType value;
            is >> value >> std::ws;
            x[i] = value / modulus;
        }
        return is;
  }

  template<class IStream, class F, class UIntType>
  IStream&
  extract_lagged_fibonacci(
      IStream& is
      , F const& f
      , unsigned int& i
      , UIntType* x)
  {
      is >> i >> std::ws;
      for(unsigned int i = 0; i < f.long_lag; ++i)
          is >> x[i] >> std::ws;
      return is;
  }
}
#  endif

template<class UIntType, int w, unsigned int p, unsigned int q,
         UIntType val = 0>
class lagged_fibonacci
{
public:
  typedef UIntType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
  BOOST_STATIC_CONSTANT(int, word_size = w);
  BOOST_STATIC_CONSTANT(unsigned int, long_lag = p);
  BOOST_STATIC_CONSTANT(unsigned int, short_lag = q);

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return wordmask; }

  lagged_fibonacci() { init_wordmask(); seed(); }
  explicit lagged_fibonacci(uint32_t value) { init_wordmask(); seed(value); }
  template<class It> lagged_fibonacci(It& first, It last)
  { init_wordmask(); seed(first, last); }
  // compiler-generated copy ctor and assignment operator are fine

private:
  void init_wordmask()
  {
    wordmask = 0;
    for(int i = 0; i < w; ++i)
      wordmask |= (1u << i);
  }

public:
  void seed(uint32_t value = 331u)
  {
    minstd_rand0 gen(value);
    for(unsigned int j = 0; j < long_lag; ++j)
      x[j] = gen() & wordmask;
    i = long_lag;
  }

  template<class It>
  void seed(It& first, It last)
  {
    // word size could be smaller than the seed values
    unsigned int j;
    for(j = 0; j < long_lag && first != last; ++j, ++first)
      x[j] = *first & wordmask;
    i = long_lag;
    if(first == last && j < long_lag)
      throw std::invalid_argument("lagged_fibonacci::seed");
  }

  result_type operator()()
  {
    if(i >= long_lag)
      fill();
    return x[i++];
  }

  static bool validation(result_type x)
  {
    return x == val;
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const lagged_fibonacci& f)
  {
    os << f.i << " ";
    for(unsigned int i = 0; i < f.long_lag; ++i)
      os << f.x[i] << " ";
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT, Traits>&
  operator>>(std::basic_istream<CharT, Traits>& is, lagged_fibonacci& f)
  {
# ifdef BOOST_RANDOM_EXTRACT_LF
      return detail::extract_lagged_fibonacci(is, f, f.i, f.x);
# else
      is >> f.i >> std::ws;
      for(unsigned int i = 0; i < f.long_lag; ++i)
          is >> f.x[i] >> std::ws;
      return is;
# endif
  }
#endif

  friend bool operator==(const lagged_fibonacci& x, const lagged_fibonacci& y)
  { return x.i == y.i && std::equal(x.x, x.x+long_lag, y.x); }
  friend bool operator!=(const lagged_fibonacci& x,
                         const lagged_fibonacci& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const lagged_fibonacci& rhs) const
  { return i == rhs.i && std::equal(x, x+long_lag, rhs.x); }
  bool operator!=(const lagged_fibonacci& rhs) const
  { return !(*this == rhs); }
#endif

private:
  void fill();
  UIntType wordmask;
  unsigned int i;
  UIntType x[long_lag];
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UIntType, int w, unsigned int p, unsigned int q, UIntType val>
const bool lagged_fibonacci<UIntType, w, p, q, val>::has_fixed_range;
template<class UIntType, int w, unsigned int p, unsigned int q, UIntType val>
const unsigned int lagged_fibonacci<UIntType, w, p, q, val>::long_lag;
template<class UIntType, int w, unsigned int p, unsigned int q, UIntType val>
const unsigned int lagged_fibonacci<UIntType, w, p, q, val>::short_lag;
#endif

template<class UIntType, int w, unsigned int p, unsigned int q, UIntType val>
void lagged_fibonacci<UIntType, w, p, q, val>::fill()
{
  // two loops to avoid costly modulo operations
  {  // extra scope for MSVC brokenness w.r.t. for scope
  for(unsigned int j = 0; j < short_lag; ++j)
    x[j] = (x[j] + x[j+(long_lag-short_lag)]) & wordmask;
  }
  for(unsigned int j = short_lag; j < long_lag; ++j)
    x[j] = (x[j] + x[j-short_lag]) & wordmask;
  i = 0;
}

// lagged Fibonacci generator for the range [0..1)
// contributed by Matthias Troyer
// for p=55, q=24 originally by G. J. Mitchell and D. P. Moore 1958

template<class T, unsigned int p, unsigned int q>
struct fibonacci_validation
{
  BOOST_STATIC_CONSTANT(bool, is_specialized = false);
  static T value() { return 0; }
  static T tolerance() { return 0; }
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class T, unsigned int p, unsigned int q>
const bool fibonacci_validation<T, p, q>::is_specialized;
#endif

#define BOOST_RANDOM_FIBONACCI_VAL(T,P,Q,V,E) \
template<> \
struct fibonacci_validation<T, P, Q>  \
{                                     \
  BOOST_STATIC_CONSTANT(bool, is_specialized = true);     \
  static T value() { return V; }      \
  static T tolerance()                \
{ return (std::max)(E, static_cast<T>(5*std::numeric_limits<T>::epsilon())); } \
};
// (The extra static_cast<T> in the std::max call above is actually
// unnecessary except for HP aCC 1.30, which claims that
// numeric_limits<double>::epsilon() doesn't actually return a double.)

BOOST_RANDOM_FIBONACCI_VAL(double, 607, 273, 0.4293817707235914, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 1279, 418, 0.9421630240437659, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 2281, 1252, 0.1768114046909004, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 3217, 576, 0.1956232694868209, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 4423, 2098, 0.9499762202147172, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 9689, 5502, 0.05737836943695162, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 19937, 9842, 0.5076528587449834, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 23209, 13470, 0.5414473810619185, 1e-14)
BOOST_RANDOM_FIBONACCI_VAL(double, 44497,21034, 0.254135073399297, 1e-14)

#undef BOOST_RANDOM_FIBONACCI_VAL

template<class RealType, int w, unsigned int p, unsigned int q>
class lagged_fibonacci_01
{
public:
  typedef RealType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
  BOOST_STATIC_CONSTANT(int, word_size = w);
  BOOST_STATIC_CONSTANT(unsigned int, long_lag = p);
  BOOST_STATIC_CONSTANT(unsigned int, short_lag = q);

  lagged_fibonacci_01() { init_modulus(); seed(); }
  explicit lagged_fibonacci_01(uint32_t value) { init_modulus(); seed(value); }
  template<class Generator>
  explicit lagged_fibonacci_01(Generator & gen) { init_modulus(); seed(gen); }
  template<class It> lagged_fibonacci_01(It& first, It last)
  { init_modulus(); seed(first, last); }
  // compiler-generated copy ctor and assignment operator are fine

private:
  void init_modulus()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::pow;
#endif
    _modulus = pow(RealType(2), word_size);
  }

public:
  void seed(uint32_t value = 331u)
  {
    minstd_rand0 intgen(value);
    seed(intgen);
  }

  // For GCC, moving this function out-of-line prevents inlining, which may
  // reduce overall object code size.  However, MSVC does not grok
  // out-of-line template member functions.
  template<class Generator>
  void seed(Generator & gen)
  {
    // use pass-by-reference, but wrap argument in pass_through_engine
    typedef detail::pass_through_engine<Generator&> ref_gen;
    uniform_01<ref_gen, RealType> gen01 =
      uniform_01<ref_gen, RealType>(ref_gen(gen));
    // I could have used std::generate_n, but it takes "gen" by value
    for(unsigned int j = 0; j < long_lag; ++j)
      x[j] = gen01();
    i = long_lag;
  }

  template<class It>
  void seed(It& first, It last)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::fmod;
    using std::pow;
#endif
    unsigned long mask = ~((~0u) << (w%32));   // now lowest w bits set
    RealType two32 = pow(RealType(2), 32);
    unsigned int j;
    for(j = 0; j < long_lag && first != last; ++j, ++first) {
      x[j] = RealType(0);
      for(int i = 0; i < w/32 && first != last; ++i, ++first)
        x[j] += *first / pow(two32,i+1);
      if(first != last && mask != 0)
        x[j] += fmod((*first & mask) / _modulus, RealType(1));
    }
    i = long_lag;
    if(first == last && j < long_lag)
      throw std::invalid_argument("lagged_fibonacci_01::seed");
  }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(0); }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return result_type(1); }

  result_type operator()()
  {
    if(i >= long_lag)
      fill();
    return x[i++];
  }

  static bool validation(result_type x)
  {
    result_type v = fibonacci_validation<result_type, p, q>::value();
    result_type epsilon = fibonacci_validation<result_type, p, q>::tolerance();
    // std::abs is a source of trouble: sometimes, it's not overloaded
    // for double, plus the usual namespace std noncompliance -> avoid it
    // using std::abs;
    // return abs(x - v) < 5 * epsilon
    return x > v - epsilon && x < v + epsilon;
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const lagged_fibonacci_01&f)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::pow;
#endif
    os << f.i << " ";
    std::ios_base::fmtflags oldflags = os.flags(os.dec | os.fixed | os.left);
    for(unsigned int i = 0; i < f.long_lag; ++i)
      os << f.x[i] * f._modulus << " ";
    os.flags(oldflags);
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT, Traits>&
  operator>>(std::basic_istream<CharT, Traits>& is, lagged_fibonacci_01& f)
    {
# ifdef BOOST_RANDOM_EXTRACT_LF
        return detail::extract_lagged_fibonacci_01(is, f, f.i, f.x, f._modulus);
# else
        is >> f.i >> std::ws;
        for(unsigned int i = 0; i < f.long_lag; ++i) {
            typename lagged_fibonacci_01::result_type value;
            is >> value >> std::ws;
            f.x[i] = value / f._modulus;
        }
        return is;
# endif
    }
#endif

  friend bool operator==(const lagged_fibonacci_01& x,
                         const lagged_fibonacci_01& y)
  { return x.i == y.i && std::equal(x.x, x.x+long_lag, y.x); }
  friend bool operator!=(const lagged_fibonacci_01& x,
                         const lagged_fibonacci_01& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const lagged_fibonacci_01& rhs) const
  { return i == rhs.i && std::equal(x, x+long_lag, rhs.x); }
  bool operator!=(const lagged_fibonacci_01& rhs) const
  { return !(*this == rhs); }
#endif

private:
  void fill();
  unsigned int i;
  RealType x[long_lag];
  RealType _modulus;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class RealType, int w, unsigned int p, unsigned int q>
const bool lagged_fibonacci_01<RealType, w, p, q>::has_fixed_range;
template<class RealType, int w, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci_01<RealType, w, p, q>::long_lag;
template<class RealType, int w, unsigned int p, unsigned int q>
const unsigned int lagged_fibonacci_01<RealType, w, p, q>::short_lag;
template<class RealType, int w, unsigned int p, unsigned int q>
const int lagged_fibonacci_01<RealType,w,p,q>::word_size;

#endif

template<class RealType, int w, unsigned int p, unsigned int q>
void lagged_fibonacci_01<RealType, w, p, q>::fill()
{
  // two loops to avoid costly modulo operations
  {  // extra scope for MSVC brokenness w.r.t. for scope
  for(unsigned int j = 0; j < short_lag; ++j) {
    RealType t = x[j] + x[j+(long_lag-short_lag)];
    if(t >= RealType(1))
      t -= RealType(1);
    x[j] = t;
  }
  }
  for(unsigned int j = short_lag; j < long_lag; ++j) {
    RealType t = x[j] + x[j-short_lag];
    if(t >= RealType(1))
      t -= RealType(1);
    x[j] = t;
  }
  i = 0;
}

} // namespace random

typedef random::lagged_fibonacci_01<double, 48, 607, 273> lagged_fibonacci607;
typedef random::lagged_fibonacci_01<double, 48, 1279, 418> lagged_fibonacci1279;
typedef random::lagged_fibonacci_01<double, 48, 2281, 1252> lagged_fibonacci2281;
typedef random::lagged_fibonacci_01<double, 48, 3217, 576> lagged_fibonacci3217;
typedef random::lagged_fibonacci_01<double, 48, 4423, 2098> lagged_fibonacci4423;
typedef random::lagged_fibonacci_01<double, 48, 9689, 5502> lagged_fibonacci9689;
typedef random::lagged_fibonacci_01<double, 48, 19937, 9842> lagged_fibonacci19937;
typedef random::lagged_fibonacci_01<double, 48, 23209, 13470> lagged_fibonacci23209;
typedef random::lagged_fibonacci_01<double, 48, 44497, 21034> lagged_fibonacci44497;

// It is possible to partially specialize uniform_01<> on lagged_fibonacci_01<>
// to help the compiler generate efficient code.  For GCC, this seems useless,
// because GCC optimizes (x-0)/(1-0) to (x-0).  This is good enough for now.

} // namespace boost

#endif // BOOST_RANDOM_LAGGED_FIBONACCI_HPP


#include "stlparse.h"

#include <fstream>
#include <vector>
#include <map>
#include <iostream>

#include <Eigen/Dense>

#include "parse_common.h"

#pragma GCC visibility push(hidden)

namespace meshparse {

using namespace Eigen;

using std::cout;
using std::endl;
using std::ifstream;
using std::ios_base;
using std::istream;
using std::pair;

inline uint32_t read_int(istream &file) {
    char buf32[4];
    file.read(buf32, 4);
    return *((uint32_t*) buf32);
}

inline float read_float(istream &file) {
    char buf32[4];
    file.read(buf32, 4);
    return *((float*) buf32);
}

void try_read_face(istream &file, stlmesh &stlm) {
    stlface face;
    face.normal[0] = read_float(file);
    face.normal[1] = read_float(file);
    face.normal[2] = read_float(file);

    for (int i = 0; i < 3; i++) {
        float vx = read_float(file);
        float vy = read_float(file);
        float vz = read_float(file);
        Vector3f vec(vx, vy, vz);

        uint32_t id;
        auto idp = stlm.vert_to_id.find(vec);
        if (idp == stlm.vert_to_id.end()) {
            id = stlm.next_vertid++;
            stlm.vert_to_id.insert(pair<Vector3f, uint32_t>(vec, id));
        } else {
            id = idp->second;
        }
        face.verts[i] = id;
    }

    file.seekg(2, ios_base::cur);
    stlm.faces.push_back(face);
}

bool load_binary_stl(ifstream &file, mesh &mesh) {
    file.seekg(80);
    const uint32_t tris = read_int(file);

    stlmesh stlm;
    for (int i = 0; i < tris; i++) {
        try_read_face(file, stlm);
    }

    stl_to_mesh(stlm, mesh);
    merge_half_edges(mesh);

    cout << "Loaded mesh: " << endl
        << "  " << mesh.verteces.size() << " verteces." << endl
        << "  " << mesh.edges.size() << " edges." << endl;
    return true;
}

} // namespace meshparse

#pragma GCC visibility pop


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is 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.
 */

// Command line flags
#include <gflags/gflags.h>
#include <gflags/gflags_completions.h>

// Include RPOS
#include <ros/ros.h>

// Listen for transforms
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/static_transform_broadcaster.h>

// FSW includes
#include <msg_conversions/msg_conversions.h>

// Messages
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <ff_msgs/Trigger.h>

// Eigen C++ includes
#include <Eigen/Dense>
#include <Eigen/Geometry>

// C++ STL inclues
#include <iostream>
#include <string>

// Gflag definitions
DEFINE_string(name, "marker", "Calibration tool frame name'");
DEFINE_double(height, 0.0062, "Calibration tool z position in global frame");

// Trannsormation buffer
tf2_ros::Buffer buffer_;

// Data structure containing average points
geometry_msgs::TransformStamped c_, x_, p_;

// Get the position and mark it
bool Mark(ff_msgs::Trigger::Request & req, ff_msgs::Trigger::Response & res,
  geometry_msgs::TransformStamped & tf) {
  try {
    // DIRECTLY FROM TF2 DOCUMENTATION...
    /////////////////////////////////////////////////////////////////////////////////
    // Transforms are specified in the direction such that they will transform     //
    // the coordinate frame "frame_id" into "child_frame_id" in                    //
    // tf::StampedTransform and geometry_msgs/TransformStamped. This is the        //
    // same transform which will take data from "child_frame_id" into "frame_id".  //
    /////////////////////////////////////////////////////////////////////////////////
    // The direction of the transform returned will be from the target_frame to    //
    // the source_frame. Which if applied to data, will transform data in the      //
    // source_frame into the target_frame.                                         //
    /////////////////////////////////////////////////////////////////////////////////
    // USGAE: lookupTransform(TARGET|FRAME_ID, SOURCE|CHILD_FRAME_ID, ...)
    geometry_msgs::TransformStamped old_tf = tf;
    tf = buffer_.lookupTransform("vive", FLAGS_name, ros::Time(0));
    // Print the transform
    ROS_INFO_STREAM("Transform:");
    ROS_INFO_STREAM(tf);
    Eigen::Vector3d vec(tf.transform.translation.x - old_tf.transform.translation.x,
                        tf.transform.translation.y - old_tf.transform.translation.y,
                        tf.transform.translation.z - old_tf.transform.translation.z);
    ROS_INFO_STREAM("Distance from last point in mm: " << vec.norm()*1000.0);
  }
  catch (tf2::TransformException &ex) {
    ROS_WARN_STREAM("Could not get transform");
  }
  return true;
}

// Mark the X axis
bool MarkX(ff_msgs::Trigger::Request & req, ff_msgs::Trigger::Response & res) {
  return Mark(req, res, x_);
}

// Mark the corner
bool MarkC(ff_msgs::Trigger::Request & req, ff_msgs::Trigger::Response & res) {
  return Mark(req, res, c_);
}

// Mark the plane axis
bool MarkP(ff_msgs::Trigger::Request & req, ff_msgs::Trigger::Response & res) {
  return Mark(req, res, p_);
}

// Solve for the origin transform
bool Solve(ff_msgs::Trigger::Request &req, ff_msgs::Trigger::Response &res) {
  // Extract the points
  Eigen::Vector3d x = msg_conversions::ros_to_eigen_vector(x_.transform.translation);
  Eigen::Vector3d c = msg_conversions::ros_to_eigen_vector(c_.transform.translation);
  Eigen::Vector3d p = msg_conversions::ros_to_eigen_vector(p_.transform.translation);
  // Calculate the direction vectors
  Eigen::Vector3d v_x = (x - c).normalized();
  Eigen::Vector3d v_p = (c - p).normalized();
  Eigen::Vector3d v_z = v_x.cross(v_p).normalized();
  Eigen::Vector3d v_y = v_z.cross(v_x).normalized();
  // Obtain the rotation from vive -> world
  Eigen::Matrix3d vRc = Eigen::Matrix3d::Zero();
  vRc.block<3, 1>(0, 0) = v_x;
  vRc.block<3, 1>(0, 1) = v_y;
  vRc.block<3, 1>(0, 2) = v_z;
  Eigen::Quaterniond q(vRc);
  Eigen::Vector3d wPc(-(x - c).norm() / 2.0, (c - p).norm() / 2.0, -FLAGS_height);
  // Print out some debug code for a sanity check
  ROS_INFO_STREAM("X width: " << (x - c).norm());
  ROS_INFO_STREAM("Y width: " << (p - c).norm());
  // Eigen::Vector3d wPv = -q.inverse().matrix()*c + wPc;
  Eigen::Vector3d wPv = -q.inverse().matrix()*c + wPc;
  Eigen::Quaterniond wRv = q.inverse();
  // Send the message
  geometry_msgs::TransformStamped tfs;
  tfs.header.stamp = ros::Time::now();
  tfs.header.frame_id = "world";
  tfs.child_frame_id = "vive";
  tfs.transform.rotation = msg_conversions::eigen_to_ros_quat(wRv);
  tfs.transform.translation = msg_conversions::eigen_to_ros_vector(wPv);
  // Now send the transform
  static tf2_ros::StaticTransformBroadcaster br;
  br.sendTransform(tfs);
  // Success!
  return true;
}

// Main entry point for application
int main(int argc, char *argv[]) {
  // Gather some data from the command
  google::SetUsageMessage("Usage: rosrun vive vive_calibrate <opts>");
  google::SetVersionString("1.0.0");
  google::ParseCommandLineFlags(&argc, &argv, true);
  // Initialize a ros node
  ros::init(argc, argv, "vive_calibrate", ros::init_options::AnonymousName);
  ros::NodeHandle nh;
  // Setup the TF2 listener for transforms
  tf2_ros::TransformListener listener(buffer_);
  // Default value from lab
  x_.transform.translation.x = -0.781168;
  x_.transform.translation.y =-0.816707;
  x_.transform.translation.z = -0.910298;
  c_.transform.translation.x = -0.748132;
  c_.transform.translation.y = -0.830513;
  c_.transform.translation.z = -2.7531;
  p_.transform.translation.x = 1.12511;
  p_.transform.translation.y = -0.822793;
  p_.transform.translation.z = -2.7224;
  // Declare services that help with marking, solving and clearing
  ros::ServiceServer srv_x = nh.advertiseService("/vive/set_x", MarkX);
  ros::ServiceServer srv_p = nh.advertiseService("/vive/set_p", MarkP);
  ros::ServiceServer srv_c = nh.advertiseService("/vive/set_c", MarkC);
  ros::ServiceServer srv_s = nh.advertiseService("/vive/solve", Solve);
  ros::spin();
  // Finish commandline flags
  google::ShutDownCommandLineFlags();
  // Make for great success
  return 0;
}


// Copyright 2014, Max Planck Society.
// Distributed under the BSD 3-Clause license.
// (See accompanying file LICENSE.txt or copy at
// http://opensource.org/licenses/BSD-3-Clause)

#ifndef BOOST_UBLAS_MATLAB_HELPER_GRASSMANN_AVERAGES_PCA_HPP__
#define BOOST_UBLAS_MATLAB_HELPER_GRASSMANN_AVERAGES_PCA_HPP__

#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

/*!@file
 * This file contains some helper function for wrapping matlab arrays into C++. One of the achievements
 * is the implementation of a specific storage for uBlas wrapping existing array, which avoids the need
 * to copy data, and behaves well with uBlas.
 */

namespace grassmann_averages_pca
{
  namespace details
  {
    namespace ublas_helpers
    {

      namespace ub = boost::numeric::ublas;

      /*!@brief uBlas allocator for external storage.
       *
       * Adapted from Gunter Winkler tricks http://www.guwi17.de/ublas/index.html. This class should follow the storage concept
       * of uBlas. However, the size of the container cannot change (same memory area), some functions are omitted and should generate a compile-time error.
       *
       * When used from Matlab, the matrices are column_major. The storage is as-is there, so the matrix should be declared column major.
       * @author Raffi Enficiaud
       *
       * @todo may be replaced by array_adaptor implementation, which is not properly documented in the boost.uBlas.
       */
      template <class T>
      class external_storage_adaptor : public ub::storage_array< external_storage_adaptor<T> >
      {

        typedef external_storage_adaptor<T> this_type;

      public:
        typedef size_t size_type;
        typedef ptrdiff_t difference_type;
        typedef T value_type;

        typedef typename boost::add_reference<T>::type reference;
        typedef typename boost::add_pointer<T>::type pointer;
        typedef typename boost::add_pointer<typename boost::add_const<T>::type>::type const_pointer;

      public:

        //! Default constructor
        external_storage_adaptor() :
          size_(0),
          data_(0)
        {}

        external_storage_adaptor(size_type size, pointer data) :
          size_(size),
          data_(data)
        {}

        /*! Constructor from another instance
         *
         * The two instances will share the data
         */
        external_storage_adaptor(const this_type& rhs) :
          size_(rhs.size_), data_(rhs.data_)
        {}

        ~external_storage_adaptor() {}

        // Resizing
        void resize(size_type size)
        {
          // the size cannot change, otherwise there is a problem with the external storage.
          // it can only be set to 0
          assert(size == 0 || size_ == 0 || size == size_);
          size_ = size;
        }

        void resize(size_type size, pointer data)
        {
          assert(size == 0 || size_ == 0 || size == size_);
          size_ = size;

          assert(data == 0 || data_ == 0 || data == data_);
          data_ = data;
        }

        // Random Access Container
        size_type max_size() const
        {
          return std::numeric_limits<size_type>::max();
        }

        //! Returns true if the storage is empty, false otherwise.
        bool empty() const
        {
          return size_ == 0;
        }

        //! Returns the current size contained.
        size_type size() const
        {
          return size_;
        }

        //!@name Random access container concept
        //!@{

        //! Implements the random access container.
        reference operator [] (size_type i) const
        {
          assert(i < size_);
          return data_[i];
        }

        // Iterators simply are pointers. There is no need to
        // provide an iterator for the const version, as the normal pointer is convertible
        // to the const pointer. There is no ambiguity with the call neither, since the
        // call should not change the storage itself.
        typedef pointer iterator;
        typedef const_pointer const_iterator;

        iterator begin() const
        {
          return data_;
        }
        iterator end() const
        {
          return data_ + size_;
        }

        // Reverse iterators
        typedef std::reverse_iterator<iterator> reverse_iterator;
        typedef std::reverse_iterator<const_iterator> const_reverse_iterator;

        reverse_iterator rbegin() const
        {
          return reverse_iterator(end());
        }

        reverse_iterator rend() const
        {
          return reverse_iterator(begin());
        }

        //! @}
        // Random access container concept.

      private:
        size_type size_;
        pointer data_;
      };

    }
  }
}

namespace boost { namespace numeric { namespace ublas {
  /*! Specializing the vector temporary type policy of uBlas.
   *
   * Sticking to the standard definition of this class would create temporary
   * vectors with the same allocation object as the matrix to which it is applied.
   * However, external_storage_adaptor does not allow construction with size only. A
   * way to compile
   * @code
   * matrix_row< matrix<double, row_major, external_storage_adaptor> >(some_matrix, some_row) = zero_vector<double>(nb_columns)
   * @endcode
   * is precisely to avoid the creation of temporaries with external_storage_adaptor allocator.
   */
  template <class T, class L>
  struct vector_temporary_traits<matrix<T, L, grassmann_averages_pca::details::ublas_helpers::external_storage_adaptor<T> > >
  {
    typedef vector<T> type;
  };
}}}

#endif /* BOOST_UBLAS_MATLAB_HELPER_GRASSMANN_AVERAGES_PCA_HPP__ */


#include <boost/lexical_cast.hpp>

#include "main.h"
#include "init.h"
#include "bitcoinrpc.h"
#include "kernelrecord.h"

using namespace json_spirit;
using namespace std;

string AccountFromValue(const Value& value);

Value listminting(const Array& params, bool fHelp)
{
    if(fHelp || params.size() > 2)
        throw runtime_error(
                "listminting [count=-1] [from=0]\n"
                "Return all mintable outputs and provide details for each of them.");

    int64 count = -1;
    if(params.size() > 0)
        count = params[0].get_int();

    int64 from = 0;
    if(params.size() > 1)
        from = params[1].get_int();

    Array ret;

    const CBlockIndex *p = GetLastBlockIndex(pindexBest, true);
    double difficulty = p->GetBlockDifficulty();

    for (map<uint256, CWalletTx>::iterator it = pwalletMain->mapWallet.begin(); it != pwalletMain->mapWallet.end(); ++it)
    {

        std::vector<KernelRecord> txList = KernelRecord::decomposeOutput(pwalletMain, it->second);
        int minAge = nStakeMinAge / 60 / 60 / 24;
        BOOST_FOREACH(KernelRecord& kr, txList) {
            if(!kr.spent) {

                if(count > 0 && ret.size() >= count) {
                    break;
                }

                string strTime = boost::lexical_cast<std::string>(kr.nTime);
                string strAmount = boost::lexical_cast<std::string>(kr.nValue);
                string strAge = boost::lexical_cast<std::string>(kr.getAge());
                string strCoinAge = boost::lexical_cast<std::string>(kr.coinAge);

                Array params;
                params.push_back(kr.address);
                string account = AccountFromValue(getaccount(params, false));

                string status = "immature";
                int searchInterval = 0;
                int attemps = 0;
                if(kr.getAge() >=  minAge)
                {
                    status = "mature";
                    searchInterval = (int)nLastCoinStakeSearchInterval;
                    attemps = GetAdjustedTime() - kr.nTime - nStakeMinAge;
                }

                Object obj;
                obj.push_back(Pair("account",                   account));
                obj.push_back(Pair("address",                   kr.address));
                obj.push_back(Pair("input-txid",                kr.hash.ToString()));
                obj.push_back(Pair("time",                      strTime));
                obj.push_back(Pair("amount",                    strAmount));
                obj.push_back(Pair("status",                    status));
                obj.push_back(Pair("age-in-day",                strAge));
                obj.push_back(Pair("coin-day-weight",           strCoinAge));
                obj.push_back(Pair("proof-of-stake-difficulty", difficulty));
                obj.push_back(Pair("minting-probability-10min", kr.getProbToMintWithinNMinutes(difficulty, 10)));
                obj.push_back(Pair("minting-probability-24h",   kr.getProbToMintWithinNMinutes(difficulty, 60*24)));
                obj.push_back(Pair("minting-probability-30d",   kr.getProbToMintWithinNMinutes(difficulty, 60*24*30)));
                obj.push_back(Pair("minting-probability-90d",   kr.getProbToMintWithinNMinutes(difficulty, 60*24*90)));
                obj.push_back(Pair("search-interval-in-sec",    searchInterval));
                obj.push_back(Pair("attempts",                  attemps));
                ret.push_back(obj);
            }
        }
    }

    return ret;
}