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

#ifndef CRYPTO3_RIPEMD_FUNCTIONS_HPP
#define CRYPTO3_RIPEMD_FUNCTIONS_HPP

#include <boost/crypto3/hash/detail/ripemd/ripemd_policy.hpp>

namespace boost {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<std::size_t DigestBits>
                struct ripemd_functions : public ripemd_policy<DigestBits> {
                    typedef ripemd_policy<DigestBits> policy_type;

                    typedef typename policy_type::word_type word_type;

                    struct f1 {
                        inline word_type operator()(word_type x, word_type y, word_type z) const {
                            return x ^ y ^ z;
                        }
                    };

                    struct f2 {
                        inline word_type operator()(word_type x, word_type y, word_type z) const {
                            return (x & y) | (~x & z);
                        }
                    };

                    struct f3 {
                        inline word_type operator()(word_type x, word_type y, word_type z) const {
                            return (x | ~y) ^ z;
                        }
                    };

                    struct f4 {
                        inline word_type operator()(word_type x, word_type y, word_type z) const {
                            return (x & z) | (y & ~z);
                        }
                    };

                    struct f5 {
                        inline word_type operator()(word_type x, word_type y, word_type z) const {
                            return x ^ (y | ~z);
                        }
                    };

                    template<typename F>
                    inline static void transform(word_type &a, word_type &b, word_type &c, word_type &d, word_type x,
                                                 word_type k, word_type s) {
                        word_type T = policy_type::rotl(a + F()(b, c, d) + x + k, s);
                        a = d;
                        d = c;
                        c = b;
                        b = T;
                    }

                    template<typename Functor>
                    inline static void transform(word_type &a, word_type &b, word_type &c, word_type &d, word_type &e,
                                                 word_type x, word_type k, word_type s) {
                        word_type T = policy_type::rotl(a + Functor()(b, c, d) + x + k, s) + e;
                        a = e;
                        e = d;
                        d = policy_type::template rotl<10>(c);
                        c = b;
                        b = T;
                    }
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_RIPEMD_FUNCTIONS_HPP


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

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

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

#include <mitsuba/bidir/manifold.h>
#include <mitsuba/bidir/path.h>
#include <mitsuba/core/statistics.h>
#define EIGEN_DONT_PARALLELIZE
#define EIGEN_NO_DEBUG
#include <Eigen/LU>
#include <Eigen/Geometry>

MTS_NAMESPACE_BEGIN

/* Some statistics counters */
static StatsCounter statsStepFailed(
		"Specular manifold", "Retries (step failed)");
static StatsCounter statsStepTooFar(
		"Specular manifold", "Retries (step increased distance)");
static StatsCounter statsStepSuccess(
		"Specular manifold", "Successful steps");
static StatsCounter statsAvgIterations(
		"Specular manifold", "Avg. iterations per walk", EAverage);
static StatsCounter statsAvgIterationsSuccess(
		"Specular manifold", "Avg. iterations per successful walk", EAverage);
static StatsCounter statsAvgManifoldSize(
		"Specular manifold", "Avg. manifold size", EAverage);
static StatsCounter statsSuccessfulWalks(
		"Specular manifold", "Successful walks", EPercentage);
static StatsCounter statsMediumSuccess(
		"Specular manifold", "Successful walks w/ media", EPercentage);
static StatsCounter statsNonManifold(
		"Specular manifold", "Non-manifold", EPercentage);
static StatsCounter statsUpdateFailed(
		"Specular manifold", "Update failed");
static StatsCounter statsMaxManifold(
		"Specular manifold", "Max. manifold size", EMaximumValue);

SpecularManifold::SpecularManifold(const Scene *scene, int maxIterations)
  : m_scene(scene) {
	m_maxIterations = maxIterations > 0 ? maxIterations :
		MTS_MANIFOLD_MAX_ITERATIONS;
}

bool SpecularManifold::init(const Path &path, int start, int end) {
	int step = start < end ? 1 : -1;
	if (path.vertex(start)->isSupernode())
		start += step;
	if (path.vertex(end)->isSupernode())
		end -= step;

	const PathVertex
		*vs = path.vertex(start),
		*ve = path.vertex(end);

	/* Create the initial vertex that is pinned in position by default */
	SimpleVertex v(EPinnedPosition, vs->getPosition());

	/* When the endpoint is on an orthographic camera or directional light
	   source, switch to a directionally pinned vertex instead */
	if (vs->getType() & (PathVertex::ESensorSample | PathVertex::EEmitterSample)) {
		const PositionSamplingRecord &pRec
			= vs->getPositionSamplingRecord();
		uint32_t type = static_cast<const AbstractEmitter *>(pRec.object)->getType()
			& (AbstractEmitter::EDeltaDirection | AbstractEmitter::EDeltaPosition);
		if (type == AbstractEmitter::EDeltaDirection) {
			v.type = EPinnedDirection;
			v.gn = v.n = pRec.n;
			coordinateSystem(pRec.n, v.dpdu, v.dpdv);
		}
	}

	m_time = vs->getTime();
	m_vertices.clear();
	m_vertices.push_back(v);

	for (int i=start + step; i != end; i += step) {
		const PathVertex
			*pred = path.vertex(i-step),
			*vertex = path.vertex(i),
			*succ = path.vertex(i+step);

		if (vertex->isSurfaceInteraction()) {
			const Intersection &its = vertex->getIntersection();
			const BSDF *bsdf = its.getBSDF();

			v.p = its.p;
			v.gn = its.geoFrame.n;
			v.n = its.shFrame.n;
			v.dpdu = its.dpdu;
			v.dpdv = its.dpdv;
			v.object = bsdf;
			v.degenerate = !vertex->isConnectable();
			const Shape *shape = its.instance != NULL ? its.instance : its.shape;
			shape->getNormalDerivative(its, v.dndu, v.dndv);

			/* Turn into an orthonormal parameterization at 'p' */
			Float invLen = 1 / v.dpdu.length();
			v.dpdu *= invLen;
			v.dndu *= invLen;
			Float dp = dot(v.dpdu, v.dpdv);
			Vector dpdv = v.dpdv - dp * v.dpdu;
			Vector dndv = v.dndv - dp * v.dndu;
			invLen = 1 / dpdv.length();
			v.dpdv = dpdv * invLen;
			v.dndv = dndv * invLen;

			Vector wPred = pred->getPosition() - v.p;
			Vector wSucc = succ->getPosition() - v.p;

			if (dot(v.gn, wPred) * dot(v.gn, wSucc) < 0) {
				v.type = ERefraction;
				v.eta = bsdf->getEta();
			} else {
				v.type = EReflection;
				v.eta = 1.0f;
			}
		} else if (vertex->isMediumInteraction()) {
			const MediumSamplingRecord &mRec = vertex->getMediumSamplingRecord();

			Vector wi = pred->getPosition() - mRec.p;
			Float invLength = 1.0f / wi.length();
			wi *= invLength;

			v.p = mRec.p;
			v.gn = v.n = Normal(0.0f);

			Vector s, t;
			coordinateSystem(wi, s, t);

		 	v.dpdu = s;
			v.dpdv = t;
		 	v.dndu = s * invLength;
			v.dndv = t * invLength;

			v.object = mRec.getPhaseFunction();
			v.eta = 1.0f;
			v.degenerate = false;
			v.type = EMedium;
		} else {
			Log(EError, "Unknown vertex type! : %s", vertex->toString().c_str());
		}

		m_vertices.push_back(v);
	}

	v = SimpleVertex(EMovable, ve->getPosition());
	m_vertices.push_back(v);

	#if MTS_MANIFOLD_DEBUG == 1
		cout << "==========================================" << endl;
		cout << "Initialized specular manifold: " << toString() << endl;
	#endif

	return true;
}

bool SpecularManifold::computeTangents() {
	const int n = static_cast<int>(m_vertices.size() - 1);

	m_vertices[0].Tp.setZero();
	m_vertices[m_vertices.size()-1].Tp.setIdentity();

	if (m_vertices.size() == 2) /* Nothing to do */
		return true;

	/* Matrix assembly stage */
	for (int i=0; i<n; ++i) {
		SimpleVertex *v = &m_vertices[i];

		Vector wo = v[1].p - v[0].p;
		Float ilo = wo.length();

		if (ilo == 0)
			return false;
		ilo = 1/ilo; wo *= ilo;

		if (v[0].type == EPinnedPosition) {
			v[0].a.setZero();
			v[0].b.setIdentity();
			v[0].c.setZero();
			continue;
		} else if (v[0].type == EPinnedDirection) {

			Vector dC_dnext_u = (v[1].dpdu - wo * dot(wo, v[1].dpdu)) * ilo;
			Vector dC_dnext_v = (v[1].dpdv - wo * dot(wo, v[1].dpdv)) * ilo;
			Vector dC_dcur_u = (wo * dot(wo, v[0].dpdu) - v[0].dpdu) * ilo;
			Vector dC_dcur_v = (wo * dot(wo, v[0].dpdv) - v[0].dpdv) * ilo;

			v[0].a.setZero();
			v[0].b = Matrix2x2(
				Vector2(dot(dC_dcur_u, v[0].dpdu), dot(dC_dcur_u, v[0].dpdv)),
				Vector2(dot(dC_dcur_v, v[0].dpdu), dot(dC_dcur_v, v[0].dpdv))
			);
			v[0].c = Matrix2x2(
				Vector2(dot(dC_dnext_u, v[0].dpdu), dot(dC_dnext_u, v[0].dpdv)),
				Vector2(dot(dC_dnext_v, v[0].dpdu), dot(dC_dnext_v, v[0].dpdv))
			);
			continue;
		}

		Vector wi = v[-1].p - v[0].p;
		Float ili = wi.length();

		if (ili == 0)
			return false;

		ili = 1/ili; wi *= ili;

		if (v[0].type == EReflection || v[0].type == ERefraction) {
			Float eta = v[0].eta;
			bool normalizeH = !(v[0].type == ERefraction && eta == 1);

			/* Compute the half vector and a few useful projections */
			Vector H;
			Float ilh;
			if (normalizeH) {
				/* Generally compute derivatives with respect to the normalized
				   half-vector. When given an index-matched refraction event,
				   don't perform this normalization, since the desired vertex
				   configuration is actually where H = 0. */

				if (dot(wi, v[0].gn) < 0)
					eta = 1 / eta;

				H = wi + eta * wo;
				ilh = 1 / H.length();
				H *= ilh;
			} else {
				H = wi + wo;
				ilh = 1.0f;
			}

			/* Orient the half-vector so that it points in the same
			   hemisphere as the geometric surface normal */

			Float dot_H_n    = dot(v[0].n, H),
			      dot_H_dndu = dot(v[0].dndu, H),
			      dot_H_dndv = dot(v[0].dndv, H),
			      dot_u_n    = dot(v[0].dpdu, v[0].n),
			      dot_v_n    = dot(v[0].dpdv, v[0].n);

			/* Local shading tangent frame */
			Vector s = v[0].dpdu - dot_u_n * v[0].n;
			Vector t = v[0].dpdv - dot_v_n * v[0].n;

			ilo *= eta * ilh; ili *= ilh;

			/* Derivatives of C with respect to x_{i-1} */
			Vector
			    dH_du = (v[-1].dpdu - wi * dot(wi, v[-1].dpdu)) * ili,
			    dH_dv = (v[-1].dpdv - wi * dot(wi, v[-1].dpdv)) * ili;

			if (normalizeH) {
				dH_du -= H * dot(dH_du, H);
				dH_dv -= H * dot(dH_dv, H);
			}

			v[0].a = Matrix2x2(
			    dot(dH_du, s), dot(dH_dv, s),
			    dot(dH_du, t), dot(dH_dv, t));

			/* Derivatives of C with respect to x_i */
			dH_du = -v[0].dpdu * (ili + ilo) + wi * (dot(wi, v[0].dpdu) * ili)
			                                 + wo * (dot(wo, v[0].dpdu) * ilo);
			dH_dv = -v[0].dpdv * (ili + ilo) + wi * (dot(wi, v[0].dpdv) * ili)
			                                 + wo * (dot(wo, v[0].dpdv) * ilo);

			if (normalizeH) {
				dH_du -= H * dot(dH_du, H);
				dH_dv -= H * dot(dH_dv, H);
			}

			v[0].b = Matrix2x2(
			    dot(dH_du, s) - dot(v[0].dpdu, v[0].dndu) * dot_H_n - dot_u_n * dot_H_dndu,
			    dot(dH_dv, s) - dot(v[0].dpdu, v[0].dndv) * dot_H_n - dot_u_n * dot_H_dndv,
			    dot(dH_du, t) - dot(v[0].dpdv, v[0].dndu) * dot_H_n - dot_v_n * dot_H_dndu,
			    dot(dH_dv, t) - dot(v[0].dpdv, v[0].dndv) * dot_H_n - dot_v_n * dot_H_dndv);

			/* Derivatives of C with respect to x_{i+1} */
			dH_du = (v[1].dpdu - wo * dot(wo, v[1].dpdu)) * ilo;
			dH_dv = (v[1].dpdv - wo * dot(wo, v[1].dpdv)) * ilo;

			if (normalizeH) {
				dH_du -= H * dot(dH_du, H);
				dH_dv -= H * dot(dH_dv, H);
			}

			v[0].c = Matrix2x2(
			    dot(dH_du, s), dot(dH_dv, s),
			    dot(dH_du, t), dot(dH_dv, t));

			/* Store the microfacet normal wrt. the local (orthonormal) shading frame */
			s = normalize(s);
			t = cross(v[0].n, s);
			v[0].m = Vector(dot(s, H), dot(t, H), dot(v[0].n, H));
			if (dot(H, v[0].gn) < 0)
				v[0].m = -v[0].m;
		} else if (v[0].type == EMedium) {
			Vector dwi_dpred_u = (v[-1].dpdu - wi * dot(wi, v[-1].dpdu)) * ili;
			Vector dwi_dpred_v = (v[-1].dpdv - wi * dot(wi, v[-1].dpdv)) * ili;
			Vector dwi_dcur_u  = (-v[0].dpdu + wi * dot(wi, v[ 0].dpdu)) * ili;
			Vector dwi_dcur_v  = (-v[0].dpdv + wi * dot(wi, v[ 0].dpdv)) * ili;

			Vector t, dt_dpred_u, dt_dpred_v, dt_dcur_u, dt_dcur_v;

			/* Compute the local frame and derivatives thereof */
			if (std::abs(wi.x) > std::abs(wi.y)) {
				Float tl = 1.0f / std::sqrt(wi.x * wi.x + wi.z * wi.z);
				t = Vector(wi.z * tl, 0.0f, -wi.x * tl);

				dt_dpred_u = Vector(dwi_dpred_u.z*tl, 0.0f, -dwi_dpred_u.x*tl);
				dt_dpred_v = Vector(dwi_dpred_v.z*tl, 0.0f, -dwi_dpred_v.x*tl);
				dt_dcur_u  = Vector(dwi_dcur_u.z*tl,  0.0f, -dwi_dcur_u.x*tl);
				dt_dcur_v  = Vector(dwi_dcur_v.z*tl,  0.0f, -dwi_dcur_v.x*tl);
			} else {
				Float tl = 1.0f / std::sqrt(wi.y * wi.y + wi.z * wi.z);
				t = Vector(0.0f, wi.z * tl, -wi.y * tl);

				dt_dpred_u = Vector(0.0f, dwi_dpred_u.z*tl, -dwi_dpred_u.y*tl);
				dt_dpred_v = Vector(0.0f, dwi_dpred_v.z*tl, -dwi_dpred_v.y*tl);
				dt_dcur_u  = Vector(0.0f, dwi_dcur_u.z*tl,  -dwi_dcur_u.y*tl);
				dt_dcur_v  = Vector(0.0f, dwi_dcur_v.z*tl,  -dwi_dcur_v.y*tl);
			}

			dt_dpred_u -= t * dot(t, dt_dpred_u);
			dt_dpred_v -= t * dot(t, dt_dpred_v);
			dt_dcur_u  -= t * dot(t, dt_dcur_u);
			dt_dcur_v  -= t * dot(t, dt_dcur_v);

			Vector s = cross(t, wi);
			Vector ds_dpred_u = cross(dt_dpred_u, wi) + cross(t, dwi_dpred_u);
			Vector ds_dpred_v = cross(dt_dpred_v, wi) + cross(t, dwi_dpred_v);
			Vector ds_dcur_u  = cross(dt_dcur_u, wi)  + cross(t, dwi_dcur_u);
			Vector ds_dcur_v  = cross(dt_dcur_v, wi)  + cross(t, dwi_dcur_v);

			/* Some tangential projections */
			Vector2
				t_cur_dpdu (dot(v[ 0].dpdu, s), dot(v[ 0].dpdu, t)),
				t_cur_dpdv (dot(v[ 0].dpdv, s), dot(v[ 0].dpdv, t)),
				t_next_dpdu(dot(v[ 1].dpdu, s), dot(v[ 1].dpdu, t)),
				t_next_dpdv(dot(v[ 1].dpdv, s), dot(v[ 1].dpdv, t)),
				t_wo = Vector2(dot(wo, s), dot(wo, t));

			v[0].a = Matrix2x2(
				Vector2(dot(ds_dpred_u, wo), dot(dt_dpred_u, wo)),
				Vector2(dot(ds_dpred_v, wo), dot(dt_dpred_v, wo))
			);

			v[0].b = Matrix2x2(
				(t_wo * dot(wo, v[0].dpdu) - t_cur_dpdu) * ilo +
				Vector2(dot(ds_dcur_u, wo), dot(dt_dcur_u, wo)),
				(t_wo * dot(wo, v[0].dpdv) - t_cur_dpdv) * ilo +
				Vector2(dot(ds_dcur_v, wo), dot(dt_dcur_v, wo)));

			v[0].c = Matrix2x2(
				(t_next_dpdu - t_wo * dot(wo, v[1].dpdu)) * ilo,
				(t_next_dpdv - t_wo * dot(wo, v[1].dpdv)) * ilo);

			v[0].m = Vector(dot(s, wo), dot(t, wo), dot(wi, wo));
		} else {
			Log(EError, "Unknown vertex type!");
		}
	}

	/* Find the tangent space with respect to translation of the last
	   vertex. For this, we must solve a tridiagonal system. The following is
	   simplified version of the block tridiagonal LU factorization algorithm
	   for this specific problem */
	Matrix2x2 Li;
	if (!m_vertices[0].b.invert(Li))
		return false;

	for (int i=0; i < n - 1; ++i) {
		m_vertices[i].u = Li * m_vertices[i].c;
		Matrix2x2 temp = m_vertices[i+1].b - m_vertices[i+1].a * m_vertices[i].u;
		if (!temp.invert(Li))
			return false;
	}

	m_vertices[n-1].Tp = -Li * m_vertices[n-1].c;

	for (int i=n-2; i>=0; --i)
		m_vertices[i].Tp = -m_vertices[i].u * m_vertices[i+1].Tp;
	return true;
}

bool SpecularManifold::project(const Vector &d) {
	const SimpleVertex &last = m_vertices[m_vertices.size()-1];
	Float du = dot(d, last.dpdu), dv = dot(d, last.dpdv);

	Ray ray(Point(0.0f), Vector(1.0f), 0); // make gcc happy
	Intersection its;

	m_proposal.clear();
	for (size_t i=0; i<m_vertices.size(); ++i) {
		m_proposal.push_back(m_vertices[i]);
		SimpleVertex &vertex = m_proposal[i];

		if (i == 0) {
			Point p0 = m_vertices[0].p + m_vertices[0].map(du, dv);
			Point p1 = m_vertices[1].p + m_vertices[1].map(du, dv);

			ray = Ray(p0, normalize(p1 - p0), m_time);
			vertex.p = ray.o;
			continue;
		} else if (vertex.type == EMovable) {
			Float dp = dot(ray.d, vertex.n);
			if (std::abs(dp) < Epsilon)
				return false;

			Float t = dot(vertex.p - ray.o, vertex.n) / dp;
			vertex.p = ray(t);
			break;
		} else if (vertex.type == EReflection) {
			if (!m_scene->rayIntersect(ray, its))
				return false;

			Vector n = its.shFrame.n,
				   s = its.dpdu, t;
			s = normalize(s - n * dot(n, s));
			t = cross(n, s);

			Vector m = s * vertex.m[0] + t * vertex.m[1] + n * vertex.m[2];

			ray.setOrigin(its.p);
			ray.setDirection(reflect(-ray.d, m));
		} else if (vertex.type == ERefraction) {
			if (!m_scene->rayIntersect(ray, its))
				return false;

			Vector n = its.shFrame.n,
				   s = its.dpdu, t;
			s = normalize(s - n * dot(n, s));
			t = cross(n, s);

			Vector m = s * vertex.m[0] + t * vertex.m[1] + n * vertex.m[2];
			Vector refracted = refract(-ray.d, m, its.shape->getBSDF()->getEta());

			if (refracted.isZero())
				return false;

			ray.setOrigin(its.p);
			ray.setDirection(refracted);
		} else if (vertex.type == EMedium) {
			Float length = (m_vertices[i].p - m_vertices[i-1].p).length(),
				  invLength = 1.0f / length;

			/* Check for occlusion */
			if (m_scene->rayIntersect(Ray(ray, Epsilon, length)))
				return false;

			vertex.p = ray(length);
			vertex.n = Vector(0.0f);

			Vector wi = -ray.d, s, t;
			coordinateSystem(wi, s, t);

		 	vertex.dpdu = s;
			vertex.dpdv = t;
		 	vertex.dndu = s * invLength;
			vertex.dndv = t * invLength;

			ray.setOrigin(vertex.p);
			ray.setDirection(s * vertex.m[0] + t * vertex.m[1] + wi * vertex.m[2]);
		} else {
			Log(EError, "Unsupported vertex type!");
		}

		if (vertex.type != EMedium) {
			if (vertex.object != its.shape->getBSDF())
				return false;

			vertex.p = its.p;
			vertex.n = its.shFrame.n;
			vertex.gn = its.geoFrame.n;
			vertex.dpdu = its.dpdu;
			vertex.dpdv = its.dpdv;

			const Shape *shape = its.instance != NULL ? its.instance : its.shape;
			shape->getNormalDerivative(its,
				vertex.dndu, vertex.dndv);

			/* Turn into an orthonormal parameterization at 'p' */
			Float invLen = 1 / vertex.dpdu.length();
			vertex.dpdu *= invLen; vertex.dndu *= invLen;
			Float dp = dot(vertex.dpdu, vertex.dpdv);
			Vector dpdv = vertex.dpdv - dp * vertex.dpdu;
			Vector dndv = vertex.dndv - dp * vertex.dndu;
			invLen = 1 / dpdv.length();
			vertex.dpdv = dpdv * invLen;
			vertex.dndv = dndv * invLen;
		}
	}
	return true;
}

bool SpecularManifold::move(const Point &target, const Normal &n) {
	SimpleVertex &last = m_vertices[m_vertices.size()-1];

	#if MTS_MANIFOLD_DEBUG == 1
		cout << "moveTo(" << last.p.toString() << " => " << target.toString() << ", n=" << n.toString() << ")" << endl;
	#endif

	if (m_vertices.size() == 2 && m_vertices[0].type == EPinnedPosition) {
		/* Nothing to do */
		return true;
	}

	bool medium = false;
	for (size_t i=0; i<m_vertices.size(); ++i) {
		if (m_vertices[i].type == EMedium)
			medium = true;
	}

	if (medium)
		statsMediumSuccess.incrementBase();

	statsAvgManifoldSize.incrementBase();
	statsAvgManifoldSize += m_vertices.size();
	statsMaxManifold.recordMaximum(m_vertices.size());

	statsSuccessfulWalks.incrementBase();

	Float invScale = 1.0f / std::max(std::max(std::abs(target.x),
			std::abs(target.y)), std::abs(target.z));
	Float stepSize = 1;

	BDAssert(last.type == EMovable);
	coordinateSystem(n, last.dpdu, last.dpdv);
	last.n = n;

	m_proposal.reserve(m_vertices.size());
	m_iterations = 0;
	statsAvgIterations.incrementBase();
	while (m_iterations < m_maxIterations) {
		Vector rel = target - m_vertices[m_vertices.size()-1].p;
		Float dist = rel.length(), newDist;
		if (dist * invScale < MTS_MANIFOLD_EPSILON) {
			/* Check for an annoying corner-case where the last
			   two vertices converge to the same point (this can
			   happen e.g. on rough planar reflectors) */
			dist = (m_vertices[m_vertices.size()-1].p
			      - m_vertices[m_vertices.size()-2].p).length();
			if (dist * invScale < Epsilon) {
				return false;
			}

			/* The manifold walk converged. */
			++statsSuccessfulWalks;
			statsAvgIterationsSuccess.incrementBase();
			statsAvgIterationsSuccess += m_iterations;
			if (medium)
				++statsMediumSuccess;
			#if MTS_MANIFOLD_DEBUG == 1
				cout << "move(): converged after " << m_iterations << " iterations" << endl;
				cout << "Final configuration:" << toString() << endl;
			#endif
			return true;
		}
		m_iterations++;
		++statsAvgIterations;

		/* Compute the tangent vectors for the current path */
		statsNonManifold.incrementBase();
		if (!computeTangents()) {
			++statsNonManifold;
			#if MTS_MANIFOLD_DEBUG == 1
				cout << "move(): unable to compute tangents!" << endl;
			#endif
			return false;
		}

		/* Take a step using the computed tangents and project
		   back on the manifold */
		#if MTS_MANIFOLD_DEBUG == 1
			const SimpleVertex &last = m_vertices[m_vertices.size()-1];
			Float du = dot(rel, last.dpdu), dv = dot(rel, last.dpdv);
			cout << "project(du=" << du << ", dv=" << dv << ", stepSize=" << stepSize << ")" << endl;
		#endif

		if (!project(rel * stepSize)) {
			#if MTS_MANIFOLD_DEBUG == 1
				cout << "project failed!" << endl;
			#endif
			++statsStepFailed;
			goto failure;
		}

		/* Reject if the step increased the distance */
		newDist = (target - m_proposal[m_proposal.size()-1].p).length();
		#if MTS_MANIFOLD_DEBUG == 1
			cout << "Distance: " << dist << " -> " << newDist << endl;
		#endif
		if (newDist > dist) {
			++statsStepTooFar;
			#if MTS_MANIFOLD_DEBUG == 1
				cout << "-> Rejecting!" << endl;
			#endif
			goto failure;
		}
		#if MTS_MANIFOLD_DEBUG == 1
			cout << "-> Accepting!" << endl;
		#endif
		++statsStepSuccess;

		m_proposal.swap(m_vertices);

		/* Increase the step size */
		stepSize = std::min((Float) 1.0f, stepSize * 2.0f);
		continue;
	failure:
		/* Reduce the step size */
		stepSize /= 2.0f;
	}
	#if MTS_MANIFOLD_DEBUG == 1
		cout << "Exceeded the max. iteration count!" << endl;
	#endif

	return false;
}

bool SpecularManifold::update(Path &path, int start, int end) {
	int step;
	ETransportMode mode;

	if (start < end) {
		step = 1; mode = EImportance;
	} else {
		step = -1; mode = ERadiance;
	}

	int last = (int) m_vertices.size() - 2;
	if (m_vertices[0].type == EPinnedDirection)
		last = std::max(last, 1);

	for (int j=0, i=start; j < last; ++j, i += step) {
		const SimpleVertex
			&v = m_vertices[j],
			&vn = m_vertices[j+1];

		PathVertex
			*pred   = path.vertexOrNull(i-step),
			*vertex = path.vertex(i),
			*succ   = path.vertex(i+step);

		int predEdgeIdx = (mode == EImportance) ? i-step : i-step-1;
		PathEdge *predEdge = path.edgeOrNull(predEdgeIdx),
		         *succEdge = path.edge(predEdgeIdx + step);

		Vector d = vn.p - v.p;
		Float length = d.length();
		d /= length;
		PathVertex::EVertexType desiredType = vn.type == EMedium ?
			PathVertex::EMediumInteraction : PathVertex::ESurfaceInteraction;

		if (v.type == EPinnedDirection) {
			/* Create a fake vertex and use it to call sampleDirect(). This is
			   kind of terrible -- a nicer API is needed to cleanly support this */
			PathVertex temp;
			temp.type = PathVertex::EMediumInteraction;
			temp.degenerate = false;
			temp.measure = EArea;
			MediumSamplingRecord &mRec = temp.getMediumSamplingRecord();
			mRec.time = m_time;
			mRec.p = vn.p;

			if (temp.sampleDirect(m_scene, NULL, vertex, succEdge, succ, mode).isZero()) {
				#if MTS_MANIFOLD_DEBUG == 1
					cout << "update(): failed in sampleDirect()!" << endl;
				#endif
				++statsUpdateFailed;
				return false;
			}

			if (m_vertices.size() >= 3) {
				PathVertex *succ2 = path.vertex(i+2*step);
				PathEdge *succ2Edge = path.edge(predEdgeIdx + 2*step);
				if (!succ->sampleNext(m_scene, NULL, vertex, succEdge, succ2Edge, succ2, mode)) {
					#if MTS_MANIFOLD_DEBUG == 1
						cout << "update(): failed in sampleNext() / pinned direction!" << endl;
					#endif
					++statsUpdateFailed;
					return false;
				}
			}
			i += step;
		} else if (!v.degenerate) {
			if (!vertex->perturbDirection(m_scene,
					pred, predEdge, succEdge, succ, d,
					length, desiredType, mode)) {
				#if MTS_MANIFOLD_DEBUG == 1
					cout << "update(): failed in perturbDirection()" << endl;
				#endif
				++statsUpdateFailed;
				return false;
			}

			Float relerr = (vn.p - succ->getPosition()).length() /
				std::max(std::max(std::abs(vn.p.x),
					std::abs(vn.p.y)), std::abs(vn.p.z));

			if (relerr > 1e-3f) {
				// be extra-cautious
				#if MTS_MANIFOLD_DEBUG == 1
					cout << "update(): failed, relative error of perturbDirection() too high:" << relerr << endl;
				#endif
				++statsUpdateFailed;
				return false;
			}
		} else {
			unsigned int compType;
			if (v.type == ERefraction)
				compType = v.eta != 1 ? BSDF::EDeltaTransmission : (BSDF::ENull | BSDF::EDeltaTransmission);
			else
				compType = BSDF::EDeltaReflection;

			if (!vertex->propagatePerturbation(m_scene,
					pred, predEdge, succEdge, succ, compType,
					length, desiredType, mode)) {
				#if MTS_MANIFOLD_DEBUG == 1
					cout << "update(): failed in propagatePerturbation()" << endl;
				#endif
				++statsUpdateFailed;
				return false;
			}

			Float relerr = (vn.p - succ->getPosition()).length() /
				std::max(std::max(std::abs(vn.p.x),
					std::abs(vn.p.y)), std::abs(vn.p.z));
			if (relerr > 1e-3f) {
				// be extra-cautious
				#if MTS_MANIFOLD_DEBUG == 1
					cout << "update(): failed, relative error of propagatePerturbation() too high:" << relerr << endl;
				#endif
				++statsUpdateFailed;
				return false;
			}
		}
	}

	return true;
}

Float SpecularManifold::det(const Path &path, int a, int b, int c) {
	int k = path.length();

	if (a == 0 || a == k)
		std::swap(a, c);

	int step = b > a ? 1 : -1, nGlossy = 0, nSpecular = 0;

	for (int i=a + step; i != c; i += step) {
		if (path.vertex(i)->isConnectable())
			++nGlossy;
		else
			++nSpecular;
	}

	if (nGlossy <= 1) /* No glossy materials -- we don't need this derivative */
		return 1.0f;

	bool success = init(path, a, c);
	BDAssert(success);

	int b_idx = std::abs(b-a);
	SimpleVertex &vb = m_vertices[b_idx];
	const PathVertex *pb = path.vertex(b);

	if (pb->isMediumInteraction()) {
		vb.n = Vector(path.edge(a < b ? (b-1) : b)->d);
	} else {
		vb.n = pb->getShadingNormal();
	}
	coordinateSystem(vb.n, vb.dpdu, vb.dpdv);

	if (!computeTangents()) {
		Log(EWarn, "Could not compute tangents!");
		return 0.0f;
	}

	m_vertices[b_idx].a.setZero();
	m_vertices[b_idx].b.setIdentity();
	m_vertices[b_idx].c.setZero();

	if (nSpecular == 0) {
		/* The chain only consists of glossy vertices -- simply compute the
		   determinant of the block tridiagonal matrix A.

		   See D.K. Salkuyeh, Comments on "A note on a three-term recurrence for a
		   tridiagonal matrix", Appl. Math. Comput. 176 (2006) 442-444. */

		Matrix2x2 Di(0.0f), D = m_vertices[1].b;

		Float det = D.det();
		for (size_t i=2; i<m_vertices.size()-1; ++i) {
			if (!D.invert(Di)) {
				Log(EWarn, "Could not invert matrix!");
				return 0.0f;
			}

			D = m_vertices[i].b - m_vertices[i].a * Di * m_vertices[i-1].c;
			det *= D.det();
		}

		return std::abs(1 / det);
	} else {
		/* The chain contains both glossy and specular materials. Compute the
		   determinant of A^-1, where rows corresponding to specular vertices
		   have been crossed out. The performance of the following is probably
		   terrible (lots of dynamic memory allocation), but it works and
		   this case happens rarely enough .. */

		Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic> A(2*(nGlossy + nSpecular), 2*(nGlossy + nSpecular));
		A.setZero();

		for (int j=0, i=0; j<nGlossy+nSpecular; ++j) {
			if (j-1 >= 0) {
				A(2*i,   2*(j-1))   = m_vertices[j+1].a(0,0);
				A(2*i,   2*(j-1)+1) = m_vertices[j+1].a(0,1);
				A(2*i+1, 2*(j-1))   = m_vertices[j+1].a(1,0);
				A(2*i+1, 2*(j-1)+1) = m_vertices[j+1].a(1,1);
			}
			A(2*i,   2*j)   = m_vertices[j+1].b(0,0);
			A(2*i,   2*j+1) = m_vertices[j+1].b(0,1);
			A(2*i+1, 2*j)   = m_vertices[j+1].b(1,0);
			A(2*i+1, 2*j+1) = m_vertices[j+1].b(1,1);

			if (j+1 < nGlossy + nSpecular) {
				A(2*i,   2*(j+1))   = m_vertices[j+1].c(0,0);
				A(2*i,   2*(j+1)+1) = m_vertices[j+1].c(0,1);
				A(2*i+1, 2*(j+1))   = m_vertices[j+1].c(1,0);
				A(2*i+1, 2*(j+1)+1) = m_vertices[j+1].c(1,1);
			}
			++i;
		}

		/* Compute the inverse and "cross out" irrelevant columns and rows */
		Eigen::Matrix<Float, Eigen::Dynamic, Eigen::Dynamic> Ai = A.inverse();

		for (int i=0; i<nGlossy+nSpecular; ++i) {
			if (!m_vertices[i+1].degenerate)
				continue;

			Ai.row(2*i).setZero();
			Ai.col(2*i).setZero();
			Ai.row(2*i+1).setZero();
			Ai.col(2*i+1).setZero();

			Ai.block<2,2>(2*i, 2*i).setIdentity();
		}

		return std::abs(Ai.determinant());
	}
}

Float SpecularManifold::multiG(const Path &path, int a, int b) {
	if (a == 0)
		++a;
	else if (a == path.length())
		--a;
	if (b == 0)
		++b;
	else if (b == path.length())
		--b;

	int step = b > a ? 1 : -1;
	while (!path.vertex(b)->isConnectable())
		b -= step;
	while (!path.vertex(a)->isConnectable())
		a += step;

	Float result = 1;

	BDAssert(path.vertex(a)->isConnectable() && path.vertex(b)->isConnectable());
	for (int i = a + step, start = a; i != b + step; i += step) {
		if (path.vertex(i)->isConnectable()) {
			result *= G(path, start, i);
			start = i;
		}
	}

	return result;
}

Float SpecularManifold::G(const Path &path, int a, int b) {
	if (std::abs(a-b) == 1) {
		if (a > b)
			std::swap(a, b);
		return path.edge(a)->evalCached(path.vertex(a),
			path.vertex(b), PathEdge::EGeometricTerm)[0];
	}

	Assert(path.vertex(a)->isConnectable());
	Assert(path.vertex(b)->isConnectable());
	int step = b > a ? 1 : -1;

	bool success = init(path, a, b);
	BDAssert(success);

	SimpleVertex &last = m_vertices[m_vertices.size()-1];
	const PathVertex *vb = path.vertex(b);
	if (!vb->isOnSurface()) {
		last.n = Vector(path.edge(a < b ? (b-1) : b)->d);
	} else {
		last.n = vb->getShadingNormal();
	}
	coordinateSystem(last.n, last.dpdu, last.dpdv);

	statsNonManifold.incrementBase();
	if (!computeTangents()) {
		++statsNonManifold;
		Log(EWarn, "SpecularManifold::evalG(): non-manifold configuration!");
		return 0;
	}

	Float result;
	if (m_vertices[0].type == EPinnedDirection) {
		result = cross(m_vertices[0].map(1, 0), m_vertices[0].map(0, 1)).length();
	} else if (m_vertices[0].type == EPinnedPosition) {
		Vector d = m_vertices[1].p - m_vertices[0].p;
		Float lengthSqr = d.lengthSquared(), invLength = 1/std::sqrt(lengthSqr);

		result = cross(m_vertices[1].map(1, 0), m_vertices[1].map(0, 1)).length() / lengthSqr;

		if (path.vertex(a)->isOnSurface())
			result *= absDot(d, path.vertex(a)->getShadingNormal()) * invLength;

		if (path.vertex(a+step)->isOnSurface())
			result *= absDot(d, path.vertex(a+step)->getShadingNormal()) * invLength;
	} else {
		Log(EError, "Invalid vertex type!");
		return 0;
	}

	return result;
}

std::string SpecularManifold::SimpleVertex::toString() const {
	std::ostringstream oss;

	oss << "SimpleVertex[" << endl
		<< "  type = ";

	switch (type) {
		case EPinnedPosition: oss << "pinnedPosition"; break;
		case EPinnedDirection: oss << "pinnedDirection"; break;
		case EReflection: oss << "reflection"; break;
		case ERefraction: oss << "refraction"; break;
		case EMedium: oss << "medium"; break;
		case EMovable: oss << "movable"; break;
		default: SLog(EError, "Unknown vertex type!");
	}

	oss << "," << endl
		<< "  p = " << p.toString() << "," << endl
		<< "  n = " << n.toString() << "," << endl
		<< "  m = " << m.toString() << "," << endl
		<< "  dpdu = " << dpdu.toString() << "," << endl
		<< "  dpdv = " << dpdv.toString() << "," << endl
		<< "  dndu = " << dndu.toString() << "," << endl
		<< "  dndv = " << dndv.toString() << "," << endl
		<< "  eta = " << eta << "," << endl
		<< "  object = " << (object ? indent(object->toString()).c_str() : "null") << endl
		<< "]";

	return oss.str();
}

std::string SpecularManifold::toString() const {
	std::ostringstream oss;

	oss << "SpecularManifold[" << endl;
	for (size_t i=0; i<m_vertices.size(); ++i) {
		oss << "  " << i << " => " << indent(m_vertices[i].toString());
		if (i+1 < m_vertices.size())
			oss << ",";
		oss << endl;
	}
	oss << "]";

	return oss.str();
}

MTS_IMPLEMENT_CLASS(SpecularManifold, false, Object)
MTS_NAMESPACE_END


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at http://www.somewhere.org/somepath/license.html.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#include <duds/hardware/devices/instruments/TSL2591.hpp>
#include <duds/hardware/interface/linux/DevI2c.hpp>
#include <duds/hardware/interface/linux/SysPwm.hpp>
#include <iostream>
#include <thread>
#include <iomanip>
#include <assert.h>
#include <boost/exception/diagnostic_information.hpp>

constexpr int valw = 8;
bool quit = false;

void runtest(
	duds::hardware::devices::instruments::TSL2591 &meter,
	duds::hardware::interface::linux::SysPwm &pwm
)
try {
	duds::data::Unit chku = duds::data::units::Watt / (
			duds::data::units::Meter * duds::data::units::Meter
		);
	std::cout.precision(6);
	double expmavg = 0;
	double emaBr = 0;
	int cnt = 0;
	int brtmr = 0;
	do {
		// the TSL2591 takes at least 100ms for integration
		std::this_thread::sleep_for(std::chrono::milliseconds(100));
		try {
			meter.sample();
		} catch (...) {
			try {
				std::this_thread::sleep_for(std::chrono::milliseconds(10));
				meter.init(1, 0);
			} catch (...) {
				continue;
			}
		}
		duds::data::Quantity visB = meter.brightness();
		duds::data::Quantity irB = meter.brightnessIr();
		assert(visB.unit == chku);
		assert(irB.unit == chku);
		emaBr = (double)meter.brightnessCount() * 0.34 + emaBr * 0.66;
		// really bright?
		if (emaBr > 26624.0) {
			// after a time limint . . .
			if (++brtmr > 24) {
				// . . . go dark
				pwm.disable();
				expmavg = 0;
			} else {
				// be as bright as possible
				pwm.dutyFull();
				expmavg = 1.0;
			}
		} else {
			brtmr = 0;
			// not quite bright enough?
			if (emaBr > 1280.0) {
				// be as bright as possible
				pwm.dutyFull();
				expmavg = 1.0;
			} else {
				// dimmer; backlight doesn't need to be so bright
				double r = (double)(emaBr - 128.0) * (1.0/1024.0);
				if (r > 1.0) {
					r = 1.0;
				}
				// impose a minimum brightness
				else if (r < 0.08) {
					r = 0.08;
				}
				expmavg = 0.66 * expmavg + 0.34 * r;
				pwm.dutyCycle(expmavg);
			}
			pwm.enable();
		}
		/*
		if ((++cnt % 16) == 0) {
			std::cout << "Brightness: " << std::setw(5) << meter.brightnessCount()
			<< "  Brightness avg: " << std::setprecision(5) << std::setw(12) <<
			emaBr << "  Duty: " << std::setprecision(5) << std::setw(8) <<
			expmavg << std::endl;
		}
		*/
	} while (!quit);
} catch (...) {
	std::cerr << "Program failed in runtest(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
}

int main(int argc, char *argv[])
try {
	std::unique_ptr<duds::hardware::interface::I2c> i2c(
		new duds::hardware::interface::linux::DevI2c(
			"/dev/i2c-1",
			0x29
		)
	);
	duds::hardware::devices::instruments::TSL2591 meter(i2c);
	int gain = 0;
	if (argc > 1) {
		gain = argv[1][0] - '0';
	}
	int integration = 0;
	if (argc > 2) {
		integration = argv[2][0] - '0';
	}
	meter.init(gain, integration);
	duds::hardware::interface::linux::SysPwm pwm(0, 0);
	pwm.frequency(400);
	pwm.dutyZero();
	pwm.disable();
	std::this_thread::sleep_for(std::chrono::milliseconds(2));
	//runtest(meter, pwm);
	std::thread doit(runtest, std::ref(meter), std::ref(pwm));
	std::cin.get();
	quit = true;
	doit.join();
} catch (...) {
	std::cerr << "Program failed in main(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
	return 1;
}


#include <boost/msm/front/euml/euml.hpp>
#include <boost/msm/front/euml/state_grammar.hpp>
#include <boost/msm/back/state_machine.hpp>
#include <iostream>

namespace msm = boost::msm;
using namespace boost::msm::front::euml;

BOOST_MSM_EUML_STATE((), Off)
BOOST_MSM_EUML_STATE((), On)

BOOST_MSM_EUML_EVENT(press)

BOOST_MSM_EUML_TRANSITION_TABLE((
  Off + press == On,
  On + press == Off
), light_transition_table)

BOOST_MSM_EUML_DECLARE_STATE_MACHINE(
(light_transition_table, init_ << Off),
light_state_machine)

int main()
{
  msm::back::state_machine<light_state_machine> light;
  std::cout << *light.current_state() << '\n';
  light.process_event(press);
  std::cout << *light.current_state() << '\n';
  light.process_event(press);
  std::cout << *light.current_state() << '\n';
}


#include <immintrin.h>
#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <algorithm>
#include <numeric>
#include <list>
#include <map>
#include <boost/timer/timer.hpp>

#include "../vecidx/vector_index.h"
#include "../vecidx/search_index.h"
#include "../vecidx/tree_index.h"
#include "../vecidx/smart_step.h"

template< class Cont_T >
struct container_only
{
	using container_type = Cont_T;
    using value_type     = typename container_type::value_type;
    using const_iterator = typename container_type::const_iterator;

    container_only( const container_type& ref ) : ref_( ref ){}

    void build_index(){}

    const_iterator find( const value_type& key )
    {
        auto first = std::lower_bound( ref_.begin(), ref_.end(), key );
        return (first!=ref_.end() && !(key<*first)) ? first : ref_.end();
    }
private:
    const container_type& ref_;
};

template< template < typename... > class Index_T, typename Index_Size_T >
size_t bench( const std::string& name, size_t size, size_t loop )
{
    boost::timer::cpu_timer timer;
    std::vector<uint32_t> org( size );

    //srand( 1 );
    //std::generate( org.begin(), org.end(), &rand );
    std::iota( org.begin(), org.end(), 0 );
    Index_T< Index_Size_T, uint32_t > index( org );

    timer.start();
    index.build_index();
    timer.stop();
    std::cout << name << "_index build...: " << timer.format();

    timer.start();
    for( size_t j = 0; j < loop; ++j )
    {
        for( auto i : org )
        {
            auto ret = index.find( i );

            if( ret == org.end() )
            {
                std::cout << "end- " << std::hex << i << std::endl;
                break;
            }
            else
            {
                if( *ret != i )
                {
                    std::cout << *ret << "," << i << "-";
                }
            }
        }
    }
    timer.stop();
    std::cout << name << "_index find all: " << timer.format();
    //std::cout << "Raw wall: " << timer.elapsed().wall << std::endl;

    return timer.elapsed().wall;
}

template< class Cont_T, template < typename... > class Index_T >
size_t bench_any( const std::string& name, size_t size, size_t loop )
{
	using container_type = Cont_T;
    using index_type = Index_T< container_type >;

    boost::timer::cpu_timer timer;
    container_type org;

    size_t cnt = 0;
    std::generate_n( std::back_inserter(org), size, [&cnt](){ return cnt++; } );
    index_type index( org );

    timer.start();
    index.build_index();
    timer.stop();
    std::cout << name << "_index build...: " << timer.format();

    timer.start();
    for( size_t j = 0; j < loop; ++j )
    {
        for( auto i : org )
        {
            auto ret = index.find( i );

            if( ret == org.end() )
            {
                std::cout << "end- " << std::hex << i << std::endl;
                break;
            }
            else
            {
                if( *ret != i )
                {
                    std::cout << *ret << "," << i << "-";
                }
            }
        }
    }
    timer.stop();
    std::cout << name << "_index find all: " << timer.format();
    //std::cout << "Raw wall: " << timer.elapsed().wall << std::endl;

    return timer.elapsed().wall;
}

int main(int /*argc*/, char* /*argv*/[])
{
    //bench<vecidx::search_index, uint64_t>( "vecidx::search_index, uint64", 15, 1 );

//    {
//        std::cout << "\nsize: 0xff\n\n";
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only,  uint8", 0xff, 100000 );
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint16", 0xff, 100000 );
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint32", 0xff, 100000 );
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint64", 0xff, 100000 );
//
//        std::cout << "\n";
//        bench<vecidx::vector_index,  uint8_t>( "vecidx::vector_index,  uint8", 0xff, 100000 );
//        bench<vecidx::vector_index, uint16_t>( "vecidx::vector_index, uint16", 0xff, 100000 );
//        bench<vecidx::vector_index, uint32_t>( "vecidx::vector_index, uint32", 0xff, 100000 );
//        bench<vecidx::vector_index, uint64_t>( "vecidx::vector_index, uint64", 0xff, 100000 );
//
//        std::cout << "\n";
//        bench<vecidx::search_index, uint8_t>( "vecidx::search_index,  uint8", 0xff, 100000 );
//        bench<vecidx::search_index, uint16_t>( "vecidx::search_index, uint16", 0xff, 100000 );
//        bench<vecidx::search_index, uint32_t>( "vecidx::search_index, uint32", 0xff, 100000 );
//        bench<vecidx::search_index, uint64_t>( "vecidx::search_index, uint64", 0xff, 100000 );
//
////        std::cout << "\n";
////        bench<vecidx::tree_index,  uint8_t>( "vecidx::tree_index,  uint8", 0xff, 100000 );
////        bench<vecidx::tree_index, uint16_t>( "vecidx::tree_index, uint16", 0xff, 100000 );
////        bench<vecidx::tree_index, uint32_t>( "vecidx::tree_index, uint32", 0xff, 100000 );
////        bench<vecidx::tree_index, uint64_t>( "vecidx::tree_index, uint64", 0xff, 100000 );
//
//        std::cout << "\n";
//        bench<vecidx::smart_step, uint32_t>( "vecidx::smart_step, uint32", 0xff, 100000 );
//        bench<vecidx::smart_step2, uint32_t>( "vecidx::smart_step2, uint32", 0xff, 100000 );
//    }
//
//    {
//        std::cout << "\nsize: 0xffff\n\n";
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint16", 0xffff, 100 );
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint32", 0xffff, 100 );
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint64", 0xffff, 100 );
//
//        std::cout << "\n";
//        bench<vecidx::vector_index, uint16_t>( "vecidx::vector_index, uint16", 0xffff, 100 );
//        bench<vecidx::vector_index, uint32_t>( "vecidx::vector_index, uint32", 0xffff, 100 );
//        bench<vecidx::vector_index, uint64_t>( "vecidx::vector_index, uint64", 0xffff, 100 );
//
//        std::cout << "\n";
//        bench<vecidx::search_index, uint16_t>( "vecidx::search_index, uint16", 0xffff, 100 );
//        bench<vecidx::search_index, uint32_t>( "vecidx::search_index, uint32", 0xffff, 100 );
//        bench<vecidx::search_index, uint64_t>( "vecidx::search_index, uint64", 0xffff, 100 );
//
////        std::cout << "\n";
////        bench<vecidx::tree_index, uint16_t>( "vecidx::tree_index, uint16", 0xffff, 100 );
////        bench<vecidx::tree_index, uint32_t>( "vecidx::tree_index, uint32", 0xffff, 100 );
////        bench<vecidx::tree_index, uint64_t>( "vecidx::tree_index, uint64", 0xffff, 100 );
//
//        std::cout << "\n";
//        bench<vecidx::smart_step, uint32_t>( "vecidx::smart_step, uint32", 0xffff, 100 );
//        bench<vecidx::smart_step2, uint32_t>( "vecidx::smart_step2, uint32", 0xffff, 100 );
//    }
//
//    {
//        std::cout << "\nsize: 0x03ff'ffff\n\n";
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint32", 0x03ffffff, 1 );
//        bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint64", 0x03ffffff, 1 );
//
//        std::cout << "\n";
//        bench<vecidx::vector_index, uint32_t>( "vecidx::vector_index, uint32", 0x03ffffff, 1 );
//        bench<vecidx::vector_index, uint64_t>( "vecidx::vector_index, uint64", 0x03ffffff, 1 );
//
//        std::cout << "\n";
//        bench<vecidx::search_index, uint32_t>( "vecidx::search_index, uint32", 0x03ffffff, 1 );
//        bench<vecidx::search_index, uint64_t>( "vecidx::search_index, uint64", 0x03ffffff, 1 );
//
////        std::cout << "\n";
////        bench<vecidx::tree_index, uint32_t>( "vecidx::tree_index, uint32", 0x03ffffff, 1 );
////        bench<vecidx::tree_index, uint64_t>( "vecidx::tree_index, uint64", 0x03ffffff, 1 );
//
//        std::cout << "\n";
//        bench<vecidx::smart_step, uint32_t>( "vecidx::smart_step, uint32", 0x03ffffff, 1 );
//        bench<vecidx::smart_step2, uint32_t>( "vecidx::smart_step2, uint32", 0x03ffffff, 1 );
//    }

    std::cout << "\nsize: 0x00ff'ffff\n\n";
    while( 1 )
    {
        size_t base = bench_any< std::vector< uint32_t >, container_only >( "vector_only, uint32", 0x00ffffff, 10 );
//        bench<vecidx::vector_index, uint32_t>( "vecidx::vector_index, uint32", 0x00ffffff, 10 );
//        bench<vecidx::search_index, uint32_t>( "vecidx::search_index, uint32", 0x00ffffff, 10 );
//        bench<vecidx::tree_index, uint32_t>( "vecidx::tree_index, uint32", 0x00ffffff, 10 );
        size_t smart2 = bench<vecidx::smart_step2, uint32_t>( "vecidx::smart_step2,  uint32", 0x00ffffff, 10 );
        size_t smart1 = bench<vecidx::smart_step, uint32_t>( "vecidx::smart_step,  uint32", 0x00ffffff, 10 );
        size_t smart3 = bench_any< std::vector< uint32_t >,
                                   vecidx::any_smart_step >( "vecidx::any_smart_step, uint32", 0x00ffffff, 10 );

        size_t lbase  = bench_any< std::list< uint32_t >, container_only >( "list_only, uint32", 0x0001ffff, 1 );
        size_t lsmart = bench_any< std::list< uint32_t >,
                                   vecidx::any_smart_step >( "vecidx::any_smart_step, uint32", 0x0001ffff, 1 );

        std::cout << std::endl << "Smart Step Diff: " << std::fixed << std::setprecision(2)
                  << 100.0f * (((float) smart1)/((float) base) - 1.0f) << "%"
                  << std::endl << "Smart2 Step Diff: " << std::fixed << std::setprecision(2)
                  << 100.0f * (((float) smart2)/((float) base) - 1.0f) << "%"
                  << std::endl << "Smart3 Step Diff: " << std::fixed << std::setprecision(2)
                  << 100.0f * (((float) smart3)/((float) base) - 1.0f) << "%"
                  << std::endl << "Smart2/Smart1 Diff: " << std::fixed << std::setprecision(2)
                  << 100.0f * (((float) smart2)/((float) smart1) - 1.0f) << "%"
                  << std::endl << "Smart3/Smart1 Diff: " << std::fixed << std::setprecision(2)
                  << 100.0f * (((float) smart3)/((float) smart1) - 1.0f) << "%"
                  << std::endl << "List Step Diff: " << std::fixed << std::setprecision(2)
                  << 100.0f * (((float) lsmart)/((float) lbase) - 1.0f) << "%"
                  << std::endl << std::endl;
    }
    return 0;
}


#include <boost/geometry/strategies/spherical/envelope_segment.hpp>


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

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision: 74248 $
//
//  Description : FPC tolerance manipulator implementation
// ***************************************************************************

#ifndef BOOST_TEST_TOOLS_DETAIL_TOLERANCE_MANIP_HPP_012705GER
#define BOOST_TEST_TOOLS_DETAIL_TOLERANCE_MANIP_HPP_012705GER

// Boost Test
#include <boost/test/tools/detail/fwd.hpp>
#include <boost/test/tools/detail/indirections.hpp>

#include <boost/test/tools/fpc_tolerance.hpp>

// Boost
#include <boost/type_traits/is_floating_point.hpp>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace test_tools {
namespace tt_detail {

// ************************************************************************** //
// **************           fpc tolerance manipulator          ************** //
// ************************************************************************** //

template<typename FPT>
struct tolerance_manip {
    explicit    tolerance_manip( FPT tol ) : m_value( tol ) {}

    FPT m_value;
};

//____________________________________________________________________________//

struct tolerance_manip_delay {};

template<typename FPT>
inline tolerance_manip<FPT>
operator%( FPT v, tolerance_manip_delay const& )
{
    BOOST_STATIC_ASSERT( is_floating_point<FPT>::value );

    return tolerance_manip<FPT>( v * static_cast<FPT>(0.01) );
}

//____________________________________________________________________________//

template<typename E, typename FPT>
inline assertion_result
operator<<(assertion_evaluate_t<E> const& ae, tolerance_manip<FPT> const& tol)
{
    local_fpc_tolerance<FPT> lt( tol.m_value );

    return ae.m_e.evaluate();
}

//____________________________________________________________________________//

template<typename FPT>
inline int
operator<<( unit_test::lazy_ostream const&, tolerance_manip<FPT> const& )   { return 0; }

//____________________________________________________________________________//

template<typename FPT>
inline check_type
operator<<( assertion_type const& at, tolerance_manip<FPT> const& )         { return CHECK_BUILT_ASSERTION; }

//____________________________________________________________________________//

} // namespace tt_detail

template<typename FPT>
inline tt_detail::tolerance_manip<FPT>
tolerance( FPT v )
{
    BOOST_STATIC_ASSERT( is_floating_point<FPT>::value );

    return tt_detail::tolerance_manip<FPT>( v );
}

//____________________________________________________________________________//

template<typename FPT>
inline tt_detail::tolerance_manip<FPT>
tolerance( fpc::percent_tolerance_t<FPT> v )
{
    BOOST_STATIC_ASSERT( is_floating_point<FPT>::value );

    return tt_detail::tolerance_manip<FPT>( v.m_value * static_cast<FPT>(0.01) );
}

//____________________________________________________________________________//

inline tt_detail::tolerance_manip_delay
tolerance()
{
    return tt_detail::tolerance_manip_delay();
}

//____________________________________________________________________________//

} // namespace test_tools
} // namespace boost

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

#endif // BOOST_TEST_TOOLS_DETAIL_TOLERANCE_MANIP_HPP_012705GER


/*
 * =====================================================================================
 *
 *       Filename:  main.cpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  09/26/2015 18:25:55
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Il Jae Lee (iljae), iljae@umich.edu
 *   Organization:  University of Michigan
 *
 * =====================================================================================
 */
#include "train.hpp"
#include "video.hpp"
#include "nnet.hpp"
#include "hog.hpp"

#include <iostream>
#include <fstream>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <cereal/types/vector.hpp>
#include <cereal/archives/binary.hpp>

namespace maav {
  class FeatureExtractMethod {
    public:
      FeatureExtractMethod(
          const ExtractInterface & extract_interface,
          std::vector<Features> & features_collection
          ) :
        extract_interface_(extract_interface),
        features_collection_(features_collection) {}

      void operator() (const cv::Mat & image) const {
        std::cout << "Extracted features " << features_collection_.size() << "..." << std::endl;
        Features features((features_collection_.size()==0?
              0:features_collection_.front().size()));
        extract_interface_.compute(image, features);
        features_collection_.push_back(features);
      }

    private:
      const ExtractInterface & extract_interface_;
      std::vector<Features> & features_collection_;
  };

  class NegativeMiningMethod {
    public:
      NegativeMiningMethod(
          const ExtractInterface & extract_interface,
          const LearnInterface & learn_interface,
          std::vector<Features> & negative_features_collection
          ) :
        extract_interface_(extract_interface),
        learn_interface_(learn_interface),
        negative_features_collection_(negative_features_collection) {}

      void operator() (const cv::Mat & image) {
        Features features((negative_features_collection_.size()==0?
              0:negative_features_collection_.front().size()));
        extract_interface_.compute(image, features);
        if(learn_interface_.test(features)) negative_features_collection_.push_back(features);
      }

    private:
      const ExtractInterface & extract_interface_;
      const LearnInterface & learn_interface_;
      std::vector<Features> & negative_features_collection_;
  };

  class TestWindowMethod {
    public:
      TestWindowMethod (
          const ExtractInterface & extract_interface,
          const LearnInterface & learn_interface,
          std::vector<cv::Rect> & locations
          ) :
        extract_interface_(extract_interface),
        learn_interface_(learn_interface),
        locations_(locations) {}

      void operator() (const cv::Mat & image, const cv::Rect & location) {
        Features features;
        extract_interface_.compute(image, features);
        if(learn_interface_.test(features)) locations_.push_back(location);
      }

    private:
      const ExtractInterface & extract_interface_;
      const LearnInterface & learn_interface_;
      std::vector<cv::Rect> & locations_;
  };
}

void TestResult(const maav::ExtractInterface & extract_interface,
                const maav::LearnInterface & learn_interface,
                const cv::Size & window_size,
                int video_source) {
  cv::Mat img, draw;
  cv::VideoCapture video;
  std::vector<cv::Rect> locations;
  maav::TestWindowMethod test(extract_interface, learn_interface, locations);

  // Open the camera.
  video.open(video_source);
  if( !video.isOpened() )
  {
    std::cerr << "Unable to open the device 0" << std::endl;
    exit( -1 );
  }

  char key=0;
  bool end_of_process = false;
  while( !end_of_process )
  {
    video >> img;
    if( img.empty() )
        break;

    draw = img.clone();

    locations.clear();

    boost::function<void (const cv::Mat &, const cv::Rect &)> f=boost::ref(test);
    maav::ApplySlidingWindow(draw, window_size, 0, 0, f);

    if( !locations.empty() )
    {
      std::vector<cv::Rect>::const_iterator loc = locations.begin();
      std::vector<cv::Rect>::const_iterator end = locations.end();
      for( ; loc != end ; ++loc )
      {
        cv::rectangle(draw, *loc, cv::Scalar( 255, 0, 255 ), 2);
      }
    }

    imshow( "Video", draw );
    key = (char)cv::waitKey( 10 );
    if( 27 == key )
        end_of_process = true;
  }
}

int main(int argc, char** argv) {
  bool extract, train;
  int width, height,
      max_horizontal_steps,
      max_vertical_steps,
      video_source;
  std::string output_file;
  std::string model_file;
  std::string positive_source_directory;
  std::string negative_source_directory;

  try {
    namespace po=boost::program_options;
    po::options_description desc("Options");
    desc.add_options()
    ("help,h", "Print help messages")
    ("extract,e", po::value<bool>(&extract)->default_value(true), "Specify whether to extract features")
    ("train,t", po::value<bool>(&train)->default_value(false), "Specify whether to train")
    ("camera,c", po::value<int>(&video_source)->default_value(0), "Specify camera to retrieve test feed")
    ("width,w", po::value<int>(&width)->required(), "Specify train window width")
    ("height,h", po::value<int>(&height)->required(), "Specify train window height")
    ("max_horizontal_steps,mh", po::value<int>(&max_horizontal_steps)->default_value(0), "Specify max horizontal steps the window can take")
    ("max_vertical_steps,mh", po::value<int>(&max_vertical_steps)->default_value(0), "Specify max vertical steps the window can take")
    ("positive,p", po::value<std::string>(&positive_source_directory)->default_value(boost::filesystem::current_path().string<std::string>()+"/positive"), "Specify positive video files directory")
    ("negative,n", po::value<std::string>(&negative_source_directory)->default_value(boost::filesystem::current_path().string<std::string>()+"/negative"), "Specify negative video files direcotry")
    ("output,o", po::value<std::string>(&output_file)->default_value(boost::filesystem::current_path().string<std::string>()+"/features.dump"), "Specify an features file for save/load")
    ("model,m", po::value<std::string>(&model_file)->default_value(boost::filesystem::current_path().string<std::string>()+"/result.model"), "Specify an model file for save/load");

    po::variables_map vm;
    po::store(po::command_line_parser(argc,argv).options(desc).run(), vm);

    if (vm.count("help")) {
      std::cout << "Usage: " << argv[0] << " [options]" << std::endl;
      std::cout << desc;
      return 0;
    }

    po::notify(vm);
  } catch(std::exception& e) {
    std::cerr << "Error: " << e.what() << std::endl;
    return 1;
  } catch(...) {
    std::cerr << "Exception of unknown type!" << std::endl;
    return 1;
  }

  const cv::Size window_size=cv::Size(width, height);

  std::vector<maav::Features> features_collection;
  std::vector<unsigned int> divider;

  maav::HOGExtractor extractor(window_size);

  if(extract) {
    maav::FeatureExtractMethod extract_method(extractor, features_collection);
    boost::function<void (const cv::Mat &)> f=boost::ref(extract_method);

    boost::filesystem::path positive_dir(positive_source_directory);
    if(boost::filesystem::is_directory(positive_dir)) {
      boost::filesystem::directory_iterator dir_iter(positive_dir), eod;

      unsigned int counter=0;
      BOOST_FOREACH(boost::filesystem::path const &file_path, std::make_pair(dir_iter, eod)) {
        maav::LoadEachFrameFromFile(file_path.string<std::string>(), f);
        for(unsigned int i=0;i<(features_collection.size()-divider.size());i++) {
          divider.push_back(counter);
        }
        counter++;
      }
    }

    boost::filesystem::path negative_dir(positive_source_directory);
    if(boost::filesystem::is_directory(negative_dir)) {
      boost::filesystem::directory_iterator dir_iter(negative_dir), eod;

      BOOST_FOREACH(boost::filesystem::path const &file_path, std::make_pair(dir_iter, eod)) {
        maav::LoadEachFrameFromFile(file_path.string<std::string>(), f);
      }
    }

    std::ofstream file_dump(output_file, std::ofstream::binary);
    cereal::BinaryOutputArchive oarchive(file_dump);
    oarchive(features_collection, divider);
  } else {
    std::ifstream file_dump(output_file, std::ifstream::binary);
    cereal::BinaryInputArchive iarchive(file_dump);
    iarchive(features_collection, divider);
  }

  maav::NeuralNet learner(3, (features_collection.back().size()+divider.back()+2)/2, 5000);

  if(train) {
    learner.train(features_collection, divider);
    learner.save(model_file);
  } else {
    learner.load(model_file);
  }

  TestResult(extractor, learner, window_size, video_source);
}


/*****************************************************************************

    (c) 2013 Hobu, Inc. hobu.inc@gmail.com

    Author: Uday Verma uday dot karan at gmail dot com

    This is free software; you can redistribute and/or modify it under the
    terms of the GNU Lesser General Licence as published by the Free Software
    Foundation. See the COPYING file for more information.

    This software is distributed WITHOUT ANY WARRANTY and without even the
    implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

*****************************************************************************/

#pragma once

#include <boost/noncopyable.hpp>
#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>

#include <algorithm>
#include <stdexcept>
#include <points2grid/export.hpp>

class P2G_DLL las_file : public boost::noncopyable {
public:
	las_file() : is_open_(false) {
    }

	~las_file() {
		close();
	}

	void open(const std::string& filename, int offset = 0, int count = -1) {
		using namespace boost::interprocess;

        start_offset_ = offset;
        count_ = count;

		pmapping_.reset(new file_mapping(filename.c_str(), read_only));
		pregion_.reset(new mapped_region(*pmapping_, read_only));

		void *addr = pregion_->get_address();

		std::string magic((char *)addr, (char *)addr + 4);

		if (!boost::iequals(magic, "LASF")) {
			throw std::runtime_error("Not a las file");
		}

		versionMajor = readAs<char>(24);
		versionMinor = readAs<char>(25);

		if ((int)(versionMajor * 10 + versionMinor) >= 13 && (int)(versionMajor * 10 + versionMinor) != 14) {
			throw std::runtime_error("Only version 1.0-1.2 and version 1.4 files are supported");
		}

		points_offset_ = readAs<unsigned int>(32*3);
		points_format_id_ = readAs<unsigned char>(32*3 + 8);
		points_struct_size_ = readAs<unsigned short>(32*3 + 8 + 1);
		points_count_ = readAs<unsigned int>(32*3 + 11);
                if(versionMinor == 4){
                    points_count_ = readAs<unsigned long>(247);

                }

        // std::cerr << "points count: " << points_count_ << std::endl;

		size_t start = 32*3 + 35;
		readN(start, scale_, 3); start += sizeof(double) * 3;
		readN(start, offset_, 3); start += sizeof(double) * 3;

		maxs_[0] = readAs<double>(start); mins_[0] = readAs<double>(start + sizeof(double)); start += 2*sizeof(double);
		maxs_[1] = readAs<double>(start); mins_[1] = readAs<double>(start + sizeof(double)); start += 2*sizeof(double);
		maxs_[2] = readAs<double>(start); mins_[2] = readAs<double>(start + sizeof(double)); start += 2*sizeof(double);

        // std::cerr << "region size: " << pregion_->get_size() << std::endl;
        // std::cerr << "points offset: " << points_offset_ << std::endl;

		uint64_t diff = pregion_->get_size() - points_offset_;

		if (diff % stride() != 0)
			throw std::runtime_error("Point record data size is inconsistent");

		if (diff / stride() != points_count_)
			throw std::runtime_error("Point record count is inconsistent with computed point records size");

        updateMinsMaxes();

        is_open_ = true;
	}

	size_t size() {
		return pregion_->get_size();
	}

	void *points_offset() {
		return (char *)pregion_->get_address() + points_offset_ + start_offset_;
	}

	void close() {
		pregion_.reset();
		pmapping_.reset();
        is_open_ = false;
	}

	size_t stride() {
		if (points_struct_size_ != 0)
			return points_struct_size_;

        switch (points_format_id_)
        {
            case 0:
                return 20;
            case 1:
                return 28;
            case 2:
                return 26;
            case 3:
                return 28 + 6;
            case 6:
                return 30;
            case 7:
                return 30 + 6;
            case 8:
                return 30 + 6 + 2;
            default:
                break;
        }
        throw std::runtime_error ("Unknown point format");
    }

	size_t points_count() const {
        if (count_ == -1)
            return points_count_;

        return std::min(points_count_, (unsigned long)count_);
	}

	double* minimums() { return mins_; }
	double* maximums() { return maxs_; }

	double *scale() { return scale_; }
	double *offset() { return offset_; }

    bool is_open() { return is_open_; }

    inline double getX(size_t point)
    {
        char *position = (char*)points_offset() + stride()* point;

        int *xi = (int *)position;

        double x = *xi * scale_[0] + offset_[0];

        return x;
    }

    inline double getY(size_t point)
    {
        char *position = (char *)points_offset() + stride() * point + sizeof(int);

        int *yi = (int *)position;

        double y = *yi * scale_[1] + offset_[1];

        return y;
    }

    inline double getZ(size_t point)
    {
        char *position = (char *)points_offset() + stride() * point + sizeof(int) + sizeof(int);

        int *zi = (int *)position;

        double z = *zi * scale_[2] + offset_[2];

        return z;
    }

    inline unsigned short getIntensity (size_t point)
    {
        char *position = (char *) points_offset () + stride () * point + 12;

        unsigned short *i = (unsigned short *) position;

        return *i;
    }

    inline unsigned char getReturnNumber (size_t point)
    {
        char *position = (char *) points_offset () + stride () * point + 14;

        unsigned char *r = (unsigned char *) position;

        if(versionMinor == 4)
        {
            return (*r) & 0Xf; // return number is in bits 0:3
        }
        else
        {
            return (*r) & 0X7; // return number is in bits 0:2
        }
    }

    inline unsigned char getReturnCount (size_t point)
    {
        char *position = (char *) points_offset () + stride () * point + 14;

        unsigned char *r = (unsigned char *) position;
        if (versionMinor == 4)
        {
            return ((*r) & 0Xf0) >> 4; //return count is in bits 4:7
        }
        else
        {
            return ((*r) & 0X38) >> 3; //return count is in bits 3:5
        }
    }

    inline unsigned char getClassification (size_t point)
    {
        if (versionMinor == 4)
        {
            char *position = (char *) points_offset () + stride () * point + 16;

            unsigned char *c = (unsigned char *) position;

            return (*c);  // classification is in bits 0:7
        }
        else
        {
            char *position = (char *) points_offset () + stride () * point + 15;

            unsigned char *c = (unsigned char *) position;

            return (*c) & 0X1F;  // classification is in bits 0:4
        }
    }

private:
    void updateMinsMaxes() {
        if (start_offset_ == 0 && count_ == -1)
            return; // no update required if no subrange is requested

        int largest = std::numeric_limits<int>::max();
        int smallest = std::numeric_limits<int>::min();

        int n[3] = { largest, largest, largest };
        int x[3] = { smallest, smallest, smallest };

        char *ip = (char *)points_offset();
        for (size_t i = 0 ; i < points_count() ; i ++) {
            int *p = (int *)ip;
            for (int j = 0 ; j < 3 ; j ++) {
                n[j] = std::min(n[j], p[j]);
                x[j] = std::max(x[j], p[j]);
            }

            ip += stride();
        }

        for (int i = 0 ; i < 3 ; i++) {
            mins_[i] = n[i] * scale_[i] + offset_[i];
            maxs_[i] = x[i] * scale_[i] + offset_[i];
        }
    }

	template<typename T>
	T readAs(size_t offset) {
		return *((T*)((char*)pregion_->get_address() + offset));
	}

	template<typename T>
	void readN(size_t offset, T* dest, size_t n) {
		char *buf = (char *)pregion_->get_address() + offset;
		for(size_t i = 0 ; i < n ; i ++) {
			dest[i] = *((T*)(buf + sizeof(T) * i));
		}
	}

private:
	boost::shared_ptr<boost::interprocess::file_mapping> pmapping_;
	boost::shared_ptr<boost::interprocess::mapped_region> pregion_;

	char versionMajor;
	char versionMinor;

        unsigned long start_offset_;
	long count_;
	unsigned int points_offset_;
	unsigned char points_format_id_;
	unsigned long points_count_;
	unsigned short points_struct_size_;

	double scale_[3], offset_[3], mins_[3], maxs_[3];

    bool is_open_;
};


#include "ros/ros.h"
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <signal.h>
#include <boost/foreach.hpp>
#include <math.h>
#include <stdio.h>
#include <cmath>
#include "std_msgs/Bool.h"
#include "common_mav.h"
#include <mav_msgs/default_topics.h>
#include <iostream>
#include <fstream>
#include <std_srvs/SetBool.h>
#include <airsim_ros_pkgs/MultiDOF.h>
#include <airsim_ros_pkgs/MultiDOFarray.h>
#include <airsim_ros_pkgs/follow_trajectory_status_srv.h>
#include <airsim_ros_pkgs/BoolPlusHeader.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>
#include <nav_msgs/Odometry.h>
#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"

#define FREQ 5
#define MARGIN 0.01     // Used as a margin of error to prevent the call to follow_trajectory from going over the node's period
#define DAMPING_FACTOR_X 0.05
#define DAMPING_FACTOR_Y 0.05
#define DAMPING_FACTOR_Z 0.15
#define likely(x)      __builtin_expect(!!(x), 1)
#define unlikely(x)    __builtin_expect(!!(x), 0)

#define MAX_YAW_RATE    90

using namespace std;

// add by feiyang jin
	//bool fly_back = false;
    //std_msgs::Bool fly_back_msg;
    bool global_stop_fly = false;
    bool stopped = true;

bool slam_lost = false;
bool created_slam_loss_traj = false;
bool copy_slam_loss_traj = false;
bool copy_panic_traj = false;
bool copy_normal_traj = false;

trajectory_t normal_traj;
trajectory_t rev_normal_traj;
trajectory_t slam_loss_traj;
trajectory_t panic_traj;
float g_v_max = 2.5;

float g_max_yaw_rate = 90;
float g_max_yaw_rate_during_flight = 90;

bool g_trajectory_done;

bool should_panic = false;
geometry_msgs::Vector3 panic_velocity;

ros::Publisher rviz_vel_pub;
int rviz_id = 0;

int traj_id = 0;

msr::airlib::MultirotorRpcLibClient* airsim_client;

void callback_trajectory(const airsim_ros_pkgs::MultiDOFarray::ConstPtr& msg)
{
	ROS_INFO("call back trajectory");
    normal_traj.clear();
    rev_normal_traj.clear();
    for (auto point : msg->points){
        multiDOFpoint traj_point;
        traj_point.x = point.x;
        traj_point.y = point.y;
        traj_point.z = point.z;
        traj_point.vx = point.vx;
        traj_point.vy = point.vy;
        traj_point.vz = point.vz;
        traj_point.yaw = point.yaw;
        traj_point.duration = point.duration;
        normal_traj.push_back(traj_point);
    }

    traj_id = msg->traj_id;

    copy_normal_traj = true;
}

bool trajectory_done(const trajectory_msgs::MultiDOFJointTrajectory& trajectory) {
    g_trajectory_done = (trajectory.points.size() == 0);
    return g_trajectory_done;
}

void print_rviz_vel(double x, double y, double z, double vx, double vy, double vz) {
    nav_msgs::Odometry rviz_point = nav_msgs::Odometry();
    rviz_point.header.seq = 0;
    rviz_point.header.stamp = ros::Time::now();
    rviz_point.header.frame_id = "world_enu";
    rviz_point.child_frame_id = rviz_id++;

    rviz_point.pose.pose.position.x = x;
    rviz_point.pose.pose.position.y = y;
    rviz_point.pose.pose.position.z = z;
    rviz_point.twist.twist.linear.x = vx;
    rviz_point.twist.twist.linear.y = vy;
    rviz_point.twist.twist.linear.z = vz;

    rviz_vel_pub.publish(rviz_point);
}

bool follow_trajectory_status_cb(airsim_ros_pkgs::follow_trajectory_status_srv::Request &req,
                                 airsim_ros_pkgs::follow_trajectory_status_srv::Response &res)
{
    //res.success.data = true;
    //ROS_INFO("trajectory done: %i", g_trajectory_done);
    res.success.data = g_trajectory_done;

    const multiDOFpoint& current_point =
        normal_traj.empty() ? rev_normal_traj.front() : normal_traj.front();

    geometry_msgs::Twist last_velocity;
    last_velocity.linear.x = current_point.vx;
    last_velocity.linear.y = current_point.vy;
    last_velocity.linear.z = current_point.vz;
    res.twist = last_velocity;

    geometry_msgs::Twist last_acceleration;
    last_acceleration.linear.x = current_point.ax;
    last_acceleration.linear.y = current_point.ay;
    last_acceleration.linear.z = current_point.az;
    res.acceleration = last_acceleration;
    return true;
}

airsim_ros_pkgs::MultiDOFarray next_steps_msg(const trajectory_t& traj, const int true_id) {
    airsim_ros_pkgs::MultiDOFarray array_of_point_msg;

    for (const auto& point : traj){
        airsim_ros_pkgs::MultiDOF point_msg;
        point_msg.x = point.x;
        point_msg.y = point.y;
        point_msg.z = point.z;
        point_msg.vx = point.vx;
        point_msg.vy = point.vy;
        point_msg.vz = point.vz;
        point_msg.ax = point.ax;
        point_msg.ay = point.ay;
        point_msg.az = point.az;
        point_msg.yaw = point.yaw;
        point_msg.duration = point.duration;
        array_of_point_msg.points.push_back(point_msg);
    }

    array_of_point_msg.traj_id = true_id;
    array_of_point_msg.header.stamp = ros::Time::now();
    return array_of_point_msg;
}

void stop_fly_callback(const std_msgs::Bool::ConstPtr& msg){
    bool stop_fly_local = msg->data;
    global_stop_fly = stop_fly_local;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "follow_trajectory", ros::init_options::NoSigintHandler);
    ros::NodeHandle n;
    ros::NodeHandle n_private("~");
    std::string host_ip;
    n.param<std::string>("airsim_hostname", host_ip, "localhost");

    airsim_client = new msr::airlib::MultirotorRpcLibClient(host_ip);
    airsim_client->takeoffAsync(5)->waitOnLastTask();

    //variable
	    std::string localization_method;
	    std::string mav_name;
	    ros::Time cur_t, last_t;
	    float cur_z, last_z = -9999;
	    bool created_future_col_traj = false;
	    trajectory_t future_col_traj;
	    ros::Rate loop_rate(FREQ);     // NOTE: Set to frequency of trajectory points
        g_trajectory_done = false;

    //publisher and subscriber
	    ros::ServiceServer trajectory_done_service = n.advertiseService("follow_trajectory_status", follow_trajectory_status_cb);

	    ros::Publisher next_steps_pub = n.advertise<airsim_ros_pkgs::MultiDOFarray>("/next_steps", 1);
	    ros::Subscriber trajectory_follower_sub = n.subscribe<airsim_ros_pkgs::MultiDOFarray>("multidoftraj", 1, callback_trajectory);

        ros::Subscriber stop_fly_sub = n.subscribe<std_msgs::Bool>("/stop_fly", 1, stop_fly_callback);

        // DEBUGGING
        rviz_vel_pub = n.advertise<nav_msgs::Odometry>("velocities", 1);

    bool following_traj = false;  //decides when the first planning has occured
                               //this allows us to activate all the
                               //functionaliy in follow_trajecotry accordingly
    std::chrono::system_clock::time_point sleep_time;

    yaw_strategy_t yaw_strategy = ignore_yaw;

    trajectory_t * traj = nullptr;
    while (ros::ok()) {
    	ros::spinOnce();

        // setup trajectory
        if (unlikely(copy_panic_traj)) {
            traj = &panic_traj;
            copy_panic_traj = false;
        } else if (unlikely(copy_slam_loss_traj)) {
            traj = &slam_loss_traj;
            copy_slam_loss_traj = false;
        }
        else if (copy_normal_traj) {
            traj = &normal_traj;
            copy_normal_traj = false;
        }

        // If there's a trajectory to follow and we haven't been commanded to halt,
        // start the clock and signal that we should follow the trajectory. If
        // commanded to halt, signal to stop following trajectory
        if (normal_traj.size() > 0 && !global_stop_fly) {
            if (!following_traj) {
                sleep_time = std::chrono::system_clock::now();
            }

            following_traj = true;
        } else if (global_stop_fly) {
            following_traj = false;
        }

        // make sure if new traj come, we do not conflict
        const int id_for_this_traj = traj_id;

        // Follow a single point in the trajectory. This will probably mean sleeping until the last one is reached
        if(following_traj) {
            if (likely(!traj->empty())){
                static double max_speed_so_far = 0;
                static int ctr = 0;

                multiDOFpoint p = traj->front();

                // Calculate the velocities we should be flying at
                double v_x = p.vx;
                double v_y = p.vy;
                double v_z = p.vz;
                float yaw = p.yaw*180/M_PI;

                Vector3r pos = airsim_client->getMultirotorState().getPosition();
                double posx = pos.x();
                double posy = pos.y();
                double posz = pos.z();
                v_x += DAMPING_FACTOR_X*(p.x-pos.x())/p.duration;
                v_y += DAMPING_FACTOR_Y*(p.y-pos.y())/p.duration;
                v_z += DAMPING_FACTOR_Z*(p.z-pos.z())/p.duration;

                // Make sure we're not going over the maximum speed
                double speed = std::sqrt((v_x*v_x + v_y*v_y + v_z*v_z));
                double scale = 1;
//                if (speed > g_v_max) {
//                    scale = g_v_max / speed;
//
//                    v_x *= scale;
//                    v_y *= scale;
//                    v_z *= scale;
//                    speed = std::sqrt((v_x*v_x + v_y*v_y + v_z*v_z));
//                }

                // Calculate the time for which this point's flight commands should run
                auto scaled_flight_time = std::chrono::duration<double>(p.duration / scale);

                // Wait until the the last point finishes processing, then tackle this point
                std::this_thread::sleep_until(sleep_time);

                auto q = airsim_client->getMultirotorState().getOrientation();
                float pitch, roll, current_yaw;
                msr::airlib::VectorMath::toEulerianAngle(q, pitch, current_yaw, roll);
                current_yaw = yaw*180 / M_PI;

                float yaw_diff = (int(yaw - current_yaw) + 360) % 360;
                yaw_diff = yaw_diff <= 180 ? yaw_diff : yaw_diff - 360;

                float yaw_rate = yaw_diff / scaled_flight_time.count();

                if (yaw_rate > MAX_YAW_RATE)
                    yaw_rate = MAX_YAW_RATE;
                else if (yaw_rate < -MAX_YAW_RATE)
                    yaw_rate = -MAX_YAW_RATE;

                auto drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom;
                auto yawmode = msr::airlib::YawMode(true, yaw_rate);

                airsim_client->moveByVelocityAsync(v_x, v_y, v_z, scaled_flight_time.count(), drivetrain, yawmode);

                print_rviz_vel(pos.y(), pos.x(), -1*pos.z(), v_x, v_y, v_z);
                ROS_INFO("Flying at (%f, %f, %f) for %f seconds", v_x, v_y, v_z, p.duration);

                // Get deadline to process next point
                sleep_time += std::chrono::duration_cast<std::chrono::system_clock::duration>(scaled_flight_time);

                // Update trajectory
                traj->pop_front();
            }
            else {
                ROS_ERROR("!!! forward trajectory empty! Doing nothing");
            }
        }
    }
	return 0;
}


#include <ros/ros.h>
#include <gazebo_msgs/ApplyBodyWrench.h>
#include <geometry_msgs/PoseStamped.h>
#include <boost/random.hpp>
#include <iostream>
#include <fstream>
#include <boost/filesystem.hpp>

#define USE_FIXED_SEED 1

namespace
{

using namespace std;
using namespace ros;

static const double DURATION_DEFAULT = 0.001;
static const unsigned int FIXED_SEED = 0;

class RandomTorqueApplier
{
private:

    //! Node handle
    ros::NodeHandle nh;

    //! Private nh
    ros::NodeHandle pnh;

    //! Gazebo client
    ros::ServiceClient gazeboClient;

    //! Notifier
    ros::Publisher notifier;

    //! X torque
    double x;

    //! Y torque
    double y;

    //! Z torque
    double z;

    //! Torque Duration
    double duration;

    //! Set random values for x-y
    bool random;

    //! rng
    boost::mt19937 rng;
public:
    RandomTorqueApplier() :
        pnh("~")
    {
        gazeboClient = nh.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazebo/apply_body_wrench", true /* persistent */);
        notifier = nh.advertise<std_msgs::Header>("/human/fall", 1, true);
        pnh.param("x", x, 0.0);
        pnh.param("y", y, 0.0);
        pnh.param("z", z, 0.0);
        pnh.param("duration", duration, DURATION_DEFAULT);
        pnh.param("random", random, true);

        if (random)
        {
            unsigned int seed = 0;
#if(!USE_FIXED_SEED)
            seed = static_cast<unsigned int>(std::time(NULL));
#else
            const char* scenarioNumberStr = std::getenv("i");
            if(scenarioNumberStr != NULL)
            {
                unsigned int scenarioNumber = boost::lexical_cast<unsigned int>(scenarioNumberStr);
                seed = scenarioNumber + 1000;
            }
            else
            {
                ROS_INFO("No scenario number set. Using %u", FIXED_SEED);
                seed = FIXED_SEED;
            }
#endif
            rng.seed(seed);

            // Generate total force
            boost::uniform_real<double> totalForceRange(150, 250);
            boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > genTotal(rng, totalForceRange);
            double total = genTotal();

            // Now distribute between x and y
            boost::uniform_real<double> ratioRange(0, 1);
            boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > genRatio(rng, ratioRange);
            double ratio = genRatio();

            x = total * ratio;
            y = total * (1 - ratio);

            // Distribute between positive and negative x force (which pushes along the y axis)
            boost::bernoulli_distribution<> negativeGenerator(0.5);
            x *= (negativeGenerator(rng) ? -1 : 1);
        }
    }

    void writeHeader(ofstream& outputCSV)
    {
        outputCSV << "Scenario Number, Wrench (x), Wrench (y), Wrench(z)" << endl;
    }

    void printWrench(const geometry_msgs::Wrench& wrench)
    {
        // Get the name of the folder to store the result in
        const char* resultsFolder = std::getenv("RESULTS_FOLDER");
        if(resultsFolder == NULL)
        {
            ROS_DEBUG_STREAM("Results folder not set. Using current directory.");
            resultsFolder = "";
        }

        const char* scenarioNumberStr = std::getenv("i");
        if(scenarioNumberStr == NULL)
        {
            ROS_DEBUG_STREAM("Scenario number not set. Using 0.");
            scenarioNumberStr = "0";
        }

        const string resultsFileName = boost::filesystem::current_path().string() + "/" + string(resultsFolder) + "/" + "wrenches.csv";
        ROS_DEBUG_STREAM("Using results file: " << resultsFileName);
        bool exists = boost::filesystem::exists(resultsFileName);
        ofstream outputCSV;
        outputCSV.open(resultsFileName.c_str(), ios::out | ios::app);
        assert(outputCSV.is_open());

        if(!exists)
        {
            writeHeader(outputCSV);
        }

        outputCSV << scenarioNumberStr << ", " << wrench.torque.x << ", " << wrench.torque.y << ", " << wrench.torque.z << endl;
        outputCSV.close();
        ROS_DEBUG_STREAM("Printing output file complete");
    }

    void apply()
    {
    	ROS_INFO("Applying forces [%f, %f, %f]", x, y, z);
        gazebo_msgs::ApplyBodyWrench wrench;
        wrench.request.body_name = "human::link";
        wrench.request.wrench.torque.x = x;
        wrench.request.wrench.torque.y = y;
        wrench.request.wrench.torque.z = z;
        wrench.request.duration = ros::Duration(duration);

        printWrench(wrench.request.wrench);

        if (!gazeboClient.call(wrench))
        {
           ROS_ERROR("Failed to apply wrench");
           return;
        }

        ROS_DEBUG("Notifying");
        std_msgs::Header header;
        header.stamp = ros::Time::now();
        notifier.publish(header);
        ROS_DEBUG("Torque application complete");
    }
};
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "random_torque_applier");
    RandomTorqueApplier rta;

    // Allow the socket to connect
    ros::Duration(1.0).sleep();

    // Send the torque and message
    rta.apply();

    // Ensure the message is delivered
    ros::Duration(3.0).sleep();
}


// Copyright (c) 2019 The Unit-e developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <test/util/util.h>

#include <consensus/ltor.h>
#include <consensus/merkle.h>
#include <key/mnemonic/mnemonic.h>

#include <boost/test/unit_test.hpp>

KeyFixture MakeKeyFixture(const std::string &seed_words) {
  // a block is signed by the proposer, thus we need some key setup here
  const key::mnemonic::Seed seed(seed_words);
  const CExtKey &ext_key = seed.GetExtKey();
  // public key for signing block
  const CPubKey pub_key = ext_key.key.GetPubKey();
  return {
      blockchain::Behavior::NewFromParameters(blockchain::Parameters::TestNet()),
      ext_key,
      pub_key,
      std::vector<unsigned char>(pub_key.begin(), pub_key.end())};
}

CTransactionRef MakeCoinbaseTransaction(const KeyFixture &key_fixture, const blockchain::Height height) {

  CMutableTransaction tx;
  tx.SetType(TxType::COINBASE);

  // meta input: block height, snapshot hash, terminator
  CScript script_sig = CScript() << CScriptNum::serialize(4711)
                                 << ToByteVector(uint256S("689dae90b6913ff34a64750dd537177afa58b3d012803a10793d74f1ebb88da9"));
  tx.vin.emplace_back(COutPoint(), script_sig);
  // stake
  tx.vin.emplace_back(uint256::zero, 1);
  tx.vin[1].scriptWitness.stack.emplace_back();  // signature, not checked
  tx.vin[1].scriptWitness.stack.emplace_back(key_fixture.pub_key_data);
  // can be spent by anyone, simply yields "true"
  CScript script_pub_key = CScript() << OP_TRUE;
  tx.vout.emplace_back(50, script_pub_key);
  return MakeTransactionRef(CTransaction(tx));
}

CBlock MinimalBlock(const KeyFixture &key_fixture) {
  return MinimalBlock([](CBlock& block) {}, key_fixture);
}

CBlock MinimalBlock(const std::function<void(CBlock &)> block_augmentor,
                    const KeyFixture &key_fixture) {
  CBlock block;
  block.nTime = key_fixture.blockchain_behavior->CalculateProposingTimestamp(std::time(nullptr));
  block.vtx.emplace_back(MakeCoinbaseTransaction(key_fixture));
  {
    CMutableTransaction tx;
    tx.SetType(TxType::REGULAR);
    tx.vin.emplace_back(uint256::zero, 3);
    tx.vout.emplace_back(8, CScript());
    block.vtx.push_back(MakeTransactionRef(CTransaction(tx)));
  }
  block_augmentor(block);
  ltor::SortTransactions(block.vtx);
  block.hashMerkleRoot = BlockMerkleRoot(block);
  block.hash_witness_merkle_root = BlockWitnessMerkleRoot(block);
  block.hash_finalizer_commits_merkle_root = BlockFinalizerCommitsMerkleRoot(block);
  const uint256 blockHash = block.GetHash();
  key_fixture.ext_key.key.Sign(blockHash, block.signature);
  return block;
}


#include "ros/ros.h"
#include <boost/bind.hpp>
#include <cmath>
#include <nav_msgs/Odometry.h>
#include <o_msgs/Monitor.h>
#include <o_msgs/RoboClawStatus.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

tf2_ros::Buffer tf_buffer;

ros::Publisher monitorPublisher;            // Publisher of monitor messages.
ros::Subscriber odometry_subscriber;        // Subscriber to nav_msgs/odometry message.
ros::Subscriber roboclawStatus_subscriber;
ros::Subscriber t265_odometry_subscriber;   // Subscriber to nav_msgs/odometry message.

u_long last_odometry_msg_counter = 0;
geometry_msgs::Pose rebased_odometry_pose;
nav_msgs::Odometry last_odometry_msg;

u_long t265_last_odometry_msg_counter = 0;
nav_msgs::Odometry t265_last_odometry_msg;

u_long last_roboClawStatus_msg_counter = 0;
o_msgs::RoboClawStatus last_roboClawStatus_msg;

float motor_current_max = 0.0;
float odom_max_valid_distance_from_t265_odom = 0.0;
double quad_pulses_per_meter = 0.0;

void roboclawStatusCallback(const o_msgs::RoboClawStatus::ConstPtr& msg) {
    last_roboClawStatus_msg = *msg;
    last_roboClawStatus_msg_counter++;
    // ROS_INFO("[monitor_node::roboclawStatusCallback] "
    //          "m1MotorCurrent: %7.4f"
    //          ", m2MotorCurrent: %7.4f"
    //          ", motor_current_max: %7.4f"
    //          , last_roboClawStatus_msg.m1MotorCurrent
    //          , last_roboClawStatus_msg.m2MotorCurrent
    //          , motor_current_max
    //          );
    if ((last_roboClawStatus_msg.m1MotorCurrent > motor_current_max) ||
        (last_roboClawStatus_msg.m2MotorCurrent > motor_current_max)) {
        o_msgs::Monitor alert_message;
        alert_message.message = o_msgs::Monitor::MOTOR_CURRENT_HIGH;
        monitorPublisher.publish(alert_message);
    }
}

void handledomsAreOutOfSync() {
    if ((last_odometry_msg_counter > 0) && (t265_last_odometry_msg_counter > 0)) {
        // We have been receiving the two odometry messages.
        // Compute distance between the two odometry points.
        tf::Transform odom_tf;
        geometry_msgs::Pose o_pose = last_odometry_msg.pose.pose;

        // Make correction to capture difference from last detected wheel sliding event.
        o_pose.position.x += rebased_odometry_pose.position.x;
        o_pose.position.y += rebased_odometry_pose.position.y;

        // Compute vector distance between wheel odometry and visual odometry.
        tf::poseMsgToTF(o_pose, odom_tf);
        tf::Transform t265_odom_tf;
        tf::poseMsgToTF(t265_last_odometry_msg.pose.pose, t265_odom_tf);
        tf::Transform tf = odom_tf.inverseTimes(t265_odom_tf);
        float odoms_distance =
            sqrt((tf.getOrigin()[0] * tf.getOrigin()[0]) +
                 (tf.getOrigin()[1] * tf.getOrigin()[1]));
        if (odoms_distance > odom_max_valid_distance_from_t265_odom) {
            // ROS_INFO("[monitor_node::handledomsAreOutOfSync] orig x: %7.4f"
            //         ", y: %7.4f"
            //         ", correction x: %7.4f"
            //         ", y: %7.4f"
            //         ", result x: %7.4f"
            //         ", y: %7.4f"
            //     , last_odometry_msg.pose.pose.position.x
            //     , last_odometry_msg.pose.pose.position.y
            //     , rebased_odometry_pose.position.x
            //     , rebased_odometry_pose.position.y
            //     , o_pose.position.x
            //     , o_pose.position.y);

            o_msgs::Monitor alert_message;
            alert_message.message = o_msgs::Monitor::WHEELS_SLIPPING;
            monitorPublisher.publish(alert_message);
            rebased_odometry_pose.position.x = t265_last_odometry_msg.pose.pose.position.x - last_odometry_msg.pose.pose.position.x;
            rebased_odometry_pose.position.y = t265_last_odometry_msg.pose.pose.position.y - last_odometry_msg.pose.pose.position.y;
            // ROS_INFO("[handledomsAreOutOfSync] o_x: %f, o_y: %f, o_z: %f, t_x: %f, t_y: %f, t_z: %f",
            //     last_odometry_msg.pose.pose.position.x,
            //     last_odometry_msg.pose.pose.position.y,
            //     last_odometry_msg.pose.pose.position.z,
            //     t265_last_odometry_msg.pose.pose.position.x,
            //     t265_last_odometry_msg.pose.pose.position.y,
            //     t265_last_odometry_msg.pose.pose.position.z);
            // ROS_INFO("[handledomsAreOutOfSync] length %f, 2D-length: %f",
            //     tf.getOrigin().length(),
            //     odoms_distance);
            // ROS_ERROR("[monitor_node::handledomsAreOutOfSync]"
            //             " odom difference is %7.4f"
            //             ", which is greater than the allowed threshold of %7.4f"
            //             ", wheel odometry is reset to x: %7.4f, y: %7.4f",
            //             odoms_distance,
            //             odom_max_valid_distance_from_t265_odom,
            //             t265_last_odometry_msg.pose.pose.position.x,
            //             t265_last_odometry_msg.pose.pose.position.y);
            // ROS_ERROR("rebased x: %7.4f, y: %7.4f", rebased_odometry_pose.position.x, rebased_odometry_pose.position.y);

            // if (last_roboClawStatus_msg_counter > 0) {
            //         ROS_INFO("[monitor_node]::handledomsAreOutOfSync] m1MotorCurrent: %f, m1MotorCurrent: %f"
            //                 , last_roboClawStatus_msg.m1MotorCurrent
            //                 , last_roboClawStatus_msg.m2MotorCurrent);
            // }
        }
    }
}

void odometryCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    last_odometry_msg = *msg;
    last_odometry_msg_counter++;
    handledomsAreOutOfSync();
}

void t265OdometryCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    t265_last_odometry_msg = *msg;
    try{
        // Translate the t265 odom values to base_link values to match wheel odometry.
        geometry_msgs::PoseStamped t265_pose;

        t265_pose.header = t265_last_odometry_msg.header;
        t265_pose.pose = t265_last_odometry_msg.pose.pose;

        //ROS_INFO(" in: x: %f, y: %f, z: %f", msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);

        geometry_msgs::TransformStamped t265_pose_frame_to_base_link;
        t265_pose_frame_to_base_link = tf_buffer.lookupTransform("base_link", "t265_pose_frame", ros::Time(0), ros::Duration(1.0) );
        tf2::doTransform(t265_pose, t265_pose, t265_pose_frame_to_base_link);
        // ROS_INFO("tf2-out: x: %f, y: %f, z: %f", t265_pose.pose.position.x, t265_pose.pose.position.y, t265_pose.pose.position.z);

        t265_last_odometry_msg.pose.pose = t265_pose.pose;
    }
    catch (tf::TransformException ex) {
        ROS_ERROR("%s" ,ex.what());
    }

    t265_last_odometry_msg_counter++;
    handledomsAreOutOfSync();
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "monitor_node");
	ros::NodeHandle nh;

    assert(ros::param::get("monitor/motor_current_max", motor_current_max));
    ROS_INFO("[monitor_node] monitor/motor_current_max: %7.4f", motor_current_max);
    assert(ros::param::get("monitor/odom_max_valid_distance_from_t265_odom", odom_max_valid_distance_from_t265_odom));
	ROS_INFO("[monitor_node] monitor/odom_max_valid_distance_from_t265_odom: %7.4f", odom_max_valid_distance_from_t265_odom);
	assert(ros::param::get("motor_controller/quad_pulses_per_meter", quad_pulses_per_meter));
    ROS_INFO("[monitor_node] motor_controller/quad_pulses_per_meter: %7.4f", quad_pulses_per_meter);

    tf2_ros::TransformListener tf2_listener(tf_buffer);

    odometry_subscriber = nh.subscribe<nav_msgs::Odometry>("diff_drive_controller/odom", 10, odometryCallback);
	monitorPublisher = nh.advertise<o_msgs::Monitor>("monitor", 1);
    roboclawStatus_subscriber = nh.subscribe<o_msgs::RoboClawStatus>("RoboClawStatus", 10, roboclawStatusCallback);
    t265_odometry_subscriber = nh.subscribe<nav_msgs::Odometry>("/t265/odom/sample", 10, t265OdometryCallback);

    rebased_odometry_pose.position.x= 0.0;
    rebased_odometry_pose.position.y = 0.0;

	ROS_INFO("[monitor_node] starting loop");
    ros::Rate r(10);
	while (ros::ok()) {
        ros::spinOnce();
        r.sleep();
	}

	ROS_INFO("[monitor_node] shutdown");
}


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SUBADD_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SUBADD_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/function/slice.hpp>
#include <boost/simd/function/combine.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( subadd_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {

      template<typename... N>
      BOOST_FORCEINLINE A0 do_( const A0& a0, const A0& a1
                              , scalar_storage const&,  nsm::list<N...> const&
                              ) const BOOST_NOEXCEPT
      {
        return A0 ( (nsm::bool_<N::value%2>::value ? extract<N::value>(a0) + extract<N::value>(a1)
                                                   : extract<N::value>(a0) - extract<N::value>(a1)
                    )...
                  );
      }

      template<typename ... N>
      BOOST_FORCEINLINE A0 do_( const A0& a0, const A0& a1
                              , native_storage const&, nsm::list<N...> const&
                              ) const BOOST_NOEXCEPT
      {
        A0 twd( nsm::ptrdiff_t<(N::value%2 ?  1 : -1)>::value... );
        return fma(a1,twd,a0);
      }

      template<typename L>
      BOOST_FORCEINLINE A0 do_( const A0& a0, const A0& a1
                              , aggregate_storage const&, L const&
                              ) const BOOST_NOEXCEPT
      {
        auto h0 = slice_high(a0);
        auto h1 = slice_high(a1);
        auto l0 = slice_low(a0);
        auto l1 = slice_low(a1);
        return combine( subadd(l0,l1), subadd(h0,h1) );
      }

      BOOST_FORCEINLINE A0 operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        return do_(a0, a1, typename A0::storage_kind{}, typename A0::traits::element_range{});
      }
   };
} } }

#endif