/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 *
 *************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_basegfx.hxx"
#include <osl/diagnose.h>
#include <basegfx/polygon/b2dpolygon.hxx>
#include <basegfx/point/b2dpoint.hxx>
#include <basegfx/vector/b2dvector.hxx>
#include <basegfx/matrix/b2dhommatrix.hxx>
#include <basegfx/curve/b2dcubicbezier.hxx>
#include <rtl/instance.hxx>
#include <basegfx/polygon/b2dpolygontools.hxx>
#include <boost/scoped_ptr.hpp>
#include <vector>
#include <algorithm>

//////////////////////////////////////////////////////////////////////////////

struct CoordinateData2D : public basegfx::B2DPoint
{
public:
    CoordinateData2D() {}

	explicit CoordinateData2D(const basegfx::B2DPoint& rData)
    :	B2DPoint(rData)
	{}

    CoordinateData2D& operator=(const basegfx::B2DPoint& rData)
    {
        B2DPoint::operator=(rData);
        return *this;
	}

	void transform(const basegfx::B2DHomMatrix& rMatrix)
	{
        *this *= rMatrix;
	}
};

//////////////////////////////////////////////////////////////////////////////

class CoordinateDataArray2D
{
	typedef ::std::vector< CoordinateData2D > CoordinateData2DVector;

	CoordinateData2DVector							maVector;

public:
	explicit CoordinateDataArray2D(sal_uInt32 nCount)
	:	maVector(nCount)
	{
	}

	explicit CoordinateDataArray2D(const CoordinateDataArray2D& rOriginal)
	:	maVector(rOriginal.maVector)
	{
	}

	CoordinateDataArray2D(const CoordinateDataArray2D& rOriginal, sal_uInt32 nIndex, sal_uInt32 nCount)
	:	maVector(rOriginal.maVector.begin() + nIndex, rOriginal.maVector.begin() + (nIndex + nCount))
	{
	}

	sal_uInt32 count() const
	{
		return maVector.size();
	}

	bool operator==(const CoordinateDataArray2D& rCandidate) const
	{
		return (maVector == rCandidate.maVector);
	}

	const basegfx::B2DPoint& getCoordinate(sal_uInt32 nIndex) const
	{
        return maVector[nIndex];
	}

	void setCoordinate(sal_uInt32 nIndex, const basegfx::B2DPoint& rValue)
	{
        maVector[nIndex] = rValue;
	}

	void reserve(sal_uInt32 nCount)
	{
		maVector.reserve(nCount);
	}

	void append(const CoordinateData2D& rValue)
	{
		maVector.push_back(rValue);
	}

	void insert(sal_uInt32 nIndex, const CoordinateData2D& rValue, sal_uInt32 nCount)
	{
		if(nCount)
		{
			// add nCount copies of rValue
			CoordinateData2DVector::iterator aIndex(maVector.begin());
			aIndex += nIndex;
			maVector.insert(aIndex, nCount, rValue);
		}
	}

	void insert(sal_uInt32 nIndex, const CoordinateDataArray2D& rSource)
	{
		const sal_uInt32 nCount(rSource.maVector.size());

		if(nCount)
		{
			// insert data
			CoordinateData2DVector::iterator aIndex(maVector.begin());
			aIndex += nIndex;
			CoordinateData2DVector::const_iterator aStart(rSource.maVector.begin());
			CoordinateData2DVector::const_iterator aEnd(rSource.maVector.end());
			maVector.insert(aIndex, aStart, aEnd);
		}
	}

	void remove(sal_uInt32 nIndex, sal_uInt32 nCount)
	{
		if(nCount)
		{
			// remove point data
			CoordinateData2DVector::iterator aStart(maVector.begin());
			aStart += nIndex;
			const CoordinateData2DVector::iterator aEnd(aStart + nCount);
			maVector.erase(aStart, aEnd);
		}
	}

	void flip(bool bIsClosed)
	{
		if(maVector.size() > 1)
		{
			// to keep the same point at index 0, just flip all points except the
			// first one when closed
			const sal_uInt32 nHalfSize(bIsClosed ? (maVector.size() - 1) >> 1 : maVector.size() >> 1);
			CoordinateData2DVector::iterator aStart(bIsClosed ? maVector.begin() + 1 : maVector.begin());
			CoordinateData2DVector::iterator aEnd(maVector.end() - 1);

			for(sal_uInt32 a(0); a < nHalfSize; a++)
			{
				::std::swap(*aStart, *aEnd);
				aStart++;
				aEnd--;
			}
		}
	}

	void removeDoublePointsAtBeginEnd()
	{
		// remove from end as long as there are at least two points
		// and begin/end are equal
		while((maVector.size() > 1) && (maVector[0] == maVector[maVector.size() - 1]))
		{
			maVector.pop_back();
		}
	}

	void removeDoublePointsWholeTrack()
	{
		sal_uInt32 nIndex(0);

		// test as long as there are at least two points and as long as the index
		// is smaller or equal second last point
		while((maVector.size() > 1) && (nIndex <= maVector.size() - 2))
		{
			if(maVector[nIndex] == maVector[nIndex + 1])
			{
				// if next is same as index, delete next
				maVector.erase(maVector.begin() + (nIndex + 1));
			}
			else
			{
				// if different, step forward
				nIndex++;
			}
		}
	}

	void transform(const basegfx::B2DHomMatrix& rMatrix)
	{
		CoordinateData2DVector::iterator aStart(maVector.begin());
		CoordinateData2DVector::iterator aEnd(maVector.end());

		for(; aStart != aEnd; aStart++)
		{
			aStart->transform(rMatrix);
		}
	}

    const basegfx::B2DPoint* begin() const
    {
        if(maVector.empty())
            return 0;
        else
            return &maVector.front();
    }

    const basegfx::B2DPoint* end() const
    {
        if(maVector.empty())
            return 0;
        else
            return (&maVector.back())+1;
    }

    basegfx::B2DPoint* begin()
    {
        if(maVector.empty())
            return 0;
        else
            return &maVector.front();
    }

    basegfx::B2DPoint* end()
    {
        if(maVector.empty())
            return 0;
        else
            return (&maVector.back())+1;
    }
};

//////////////////////////////////////////////////////////////////////////////

class ControlVectorPair2D
{
	basegfx::B2DVector							maPrevVector;
	basegfx::B2DVector							maNextVector;

public:
    explicit ControlVectorPair2D () { }

	const basegfx::B2DVector& getPrevVector() const
	{
		return maPrevVector;
	}

	void setPrevVector(const basegfx::B2DVector& rValue)
	{
		if(rValue != maPrevVector)
			maPrevVector = rValue;
	}

	const basegfx::B2DVector& getNextVector() const
	{
		return maNextVector;
	}

	void setNextVector(const basegfx::B2DVector& rValue)
	{
		if(rValue != maNextVector)
			maNextVector = rValue;
	}

	bool operator==(const ControlVectorPair2D& rData) const
	{
		return (maPrevVector == rData.getPrevVector() && maNextVector == rData.getNextVector());
	}

	void flip()
	{
		::std::swap(maPrevVector, maNextVector);
	}
};

//////////////////////////////////////////////////////////////////////////////

class ControlVectorArray2D
{
	typedef ::std::vector< ControlVectorPair2D > ControlVectorPair2DVector;

	ControlVectorPair2DVector							maVector;
	sal_uInt32											mnUsedVectors;

public:
	explicit ControlVectorArray2D(sal_uInt32 nCount)
	:	maVector(nCount),
		mnUsedVectors(0)
	{}

	ControlVectorArray2D(const ControlVectorArray2D& rOriginal, sal_uInt32 nIndex, sal_uInt32 nCount)
	:	maVector(),
		mnUsedVectors(0)
	{
		ControlVectorPair2DVector::const_iterator aStart(rOriginal.maVector.begin());
		aStart += nIndex;
		ControlVectorPair2DVector::const_iterator aEnd(aStart);
		aEnd += nCount;
		maVector.reserve(nCount);

		for(; aStart != aEnd; aStart++)
		{
			if(!aStart->getPrevVector().equalZero())
				mnUsedVectors++;

			if(!aStart->getNextVector().equalZero())
				mnUsedVectors++;

			maVector.push_back(*aStart);
		}
	}

	sal_uInt32 count() const
	{
		return maVector.size();
	}

	bool operator==(const ControlVectorArray2D& rCandidate) const
	{
		return (maVector == rCandidate.maVector);
	}

	bool isUsed() const
	{
		return (0 != mnUsedVectors);
	}

	const basegfx::B2DVector& getPrevVector(sal_uInt32 nIndex) const
	{
		return maVector[nIndex].getPrevVector();
	}

	void setPrevVector(sal_uInt32 nIndex, const basegfx::B2DVector& rValue)
	{
		bool bWasUsed(mnUsedVectors && !maVector[nIndex].getPrevVector().equalZero());
		bool bIsUsed(!rValue.equalZero());

		if(bWasUsed)
		{
			if(bIsUsed)
			{
				maVector[nIndex].setPrevVector(rValue);
			}
			else
			{
				maVector[nIndex].setPrevVector(basegfx::B2DVector::getEmptyVector());
				mnUsedVectors--;
			}
		}
		else
		{
			if(bIsUsed)
			{
				maVector[nIndex].setPrevVector(rValue);
				mnUsedVectors++;
			}
		}
	}

	const basegfx::B2DVector& getNextVector(sal_uInt32 nIndex) const
	{
		return maVector[nIndex].getNextVector();
	}

	void setNextVector(sal_uInt32 nIndex, const basegfx::B2DVector& rValue)
	{
		bool bWasUsed(mnUsedVectors && !maVector[nIndex].getNextVector().equalZero());
		bool bIsUsed(!rValue.equalZero());

		if(bWasUsed)
		{
			if(bIsUsed)
			{
				maVector[nIndex].setNextVector(rValue);
			}
			else
			{
				maVector[nIndex].setNextVector(basegfx::B2DVector::getEmptyVector());
				mnUsedVectors--;
			}
		}
		else
		{
			if(bIsUsed)
			{
				maVector[nIndex].setNextVector(rValue);
				mnUsedVectors++;
			}
		}
	}

	void append(const ControlVectorPair2D& rValue)
	{
		maVector.push_back(rValue);

		if(!rValue.getPrevVector().equalZero())
			mnUsedVectors += 1;

		if(!rValue.getNextVector().equalZero())
			mnUsedVectors += 1;
	}

	void insert(sal_uInt32 nIndex, const ControlVectorPair2D& rValue, sal_uInt32 nCount)
	{
		if(nCount)
		{
			// add nCount copies of rValue
			ControlVectorPair2DVector::iterator aIndex(maVector.begin());
			aIndex += nIndex;
			maVector.insert(aIndex, nCount, rValue);

			if(!rValue.getPrevVector().equalZero())
				mnUsedVectors += nCount;

			if(!rValue.getNextVector().equalZero())
				mnUsedVectors += nCount;
		}
	}

	void insert(sal_uInt32 nIndex, const ControlVectorArray2D& rSource)
	{
		const sal_uInt32 nCount(rSource.maVector.size());

		if(nCount)
		{
			// insert data
			ControlVectorPair2DVector::iterator aIndex(maVector.begin());
			aIndex += nIndex;
			ControlVectorPair2DVector::const_iterator aStart(rSource.maVector.begin());
			ControlVectorPair2DVector::const_iterator aEnd(rSource.maVector.end());
			maVector.insert(aIndex, aStart, aEnd);

			for(; aStart != aEnd; aStart++)
			{
				if(!aStart->getPrevVector().equalZero())
					mnUsedVectors++;

				if(!aStart->getNextVector().equalZero())
					mnUsedVectors++;
			}
		}
	}

	void remove(sal_uInt32 nIndex, sal_uInt32 nCount)
	{
		if(nCount)
		{
			const ControlVectorPair2DVector::iterator aDeleteStart(maVector.begin() + nIndex);
			const ControlVectorPair2DVector::iterator aDeleteEnd(aDeleteStart + nCount);
			ControlVectorPair2DVector::const_iterator aStart(aDeleteStart);

			for(; mnUsedVectors && aStart != aDeleteEnd; aStart++)
			{
				if(!aStart->getPrevVector().equalZero())
					mnUsedVectors--;

				if(mnUsedVectors && !aStart->getNextVector().equalZero())
					mnUsedVectors--;
			}

			// remove point data
			maVector.erase(aDeleteStart, aDeleteEnd);
		}
	}

	void flip(bool bIsClosed)
	{
		if(maVector.size() > 1)
		{
			// to keep the same point at index 0, just flip all points except the
			// first one when closed
			const sal_uInt32 nHalfSize(bIsClosed ? (maVector.size() - 1) >> 1 : maVector.size() >> 1);
			ControlVectorPair2DVector::iterator aStart(bIsClosed ? maVector.begin() + 1 : maVector.begin());
			ControlVectorPair2DVector::iterator aEnd(maVector.end() - 1);

			for(sal_uInt32 a(0); a < nHalfSize; a++)
			{
				// swap Prev and Next
				aStart->flip();
				aEnd->flip();

				// swap entries
				::std::swap(*aStart, *aEnd);

				aStart++;
				aEnd--;
			}

			if(aStart == aEnd)
			{
				// swap Prev and Next at middle element (if exists)
				aStart->flip();
			}

			if(bIsClosed)
			{
				// swap Prev and Next at start element
				maVector.begin()->flip();
			}
		}
	}
};

//////////////////////////////////////////////////////////////////////////////

class ImplBufferedData
{
private:
	// Possibility to hold the last subdivision
	boost::scoped_ptr< basegfx::B2DPolygon >		mpDefaultSubdivision;

    // Possibility to hold the last B2DRange calculation
	boost::scoped_ptr< basegfx::B2DRange >		    mpB2DRange;

public:
    ImplBufferedData()
    :   mpDefaultSubdivision(),
        mpB2DRange()
    {}

    const basegfx::B2DPolygon& getDefaultAdaptiveSubdivision(const basegfx::B2DPolygon& rSource) const
	{
		if(!mpDefaultSubdivision)
		{
			const_cast< ImplBufferedData* >(this)->mpDefaultSubdivision.reset(new basegfx::B2DPolygon(basegfx::tools::adaptiveSubdivideByCount(rSource, 9)));
		}

        return *mpDefaultSubdivision;
	}

    const basegfx::B2DRange& getB2DRange(const basegfx::B2DPolygon& rSource) const
    {
		if(!mpB2DRange)
		{
			basegfx::B2DRange aNewRange;
			const sal_uInt32 nPointCount(rSource.count());

			if(nPointCount)
			{
				for(sal_uInt32 a(0); a < nPointCount; a++)
				{
					aNewRange.expand(rSource.getB2DPoint(a));
				}

				if(rSource.areControlPointsUsed())
				{
					const sal_uInt32 nEdgeCount(rSource.isClosed() ? nPointCount : nPointCount - 1);

					if(nEdgeCount)
					{
						basegfx::B2DCubicBezier aEdge;
						aEdge.setStartPoint(rSource.getB2DPoint(0));

						for(sal_uInt32 b(0); b < nEdgeCount; b++)
						{
							const sal_uInt32 nNextIndex((b + 1) % nPointCount);
							aEdge.setControlPointA(rSource.getNextControlPoint(b));
							aEdge.setControlPointB(rSource.getPrevControlPoint(nNextIndex));
							aEdge.setEndPoint(rSource.getB2DPoint(nNextIndex));

							if(aEdge.isBezier())
							{
								const basegfx::B2DRange aBezierRangeWithControlPoints(aEdge.getRange());

								if(!aNewRange.isInside(aBezierRangeWithControlPoints))
								{
									// the range with control points of the current edge is not completely
									// inside the current range without control points. Expand current range by
									// subdividing the bezier segment.
									// Ideal here is a subdivision at the extreme values, so use
									// getAllExtremumPositions to get all extremas in one run
									::std::vector< double > aExtremas;

									aExtremas.reserve(4);
									aEdge.getAllExtremumPositions(aExtremas);

									const sal_uInt32 nExtremaCount(aExtremas.size());

									for(sal_uInt32 c(0); c < nExtremaCount; c++)
									{
										aNewRange.expand(aEdge.interpolatePoint(aExtremas[c]));
									}
								}
							}

							// prepare next edge
							aEdge.setStartPoint(aEdge.getEndPoint());
						}
					}
				}
			}

			const_cast< ImplBufferedData* >(this)->mpB2DRange.reset(new basegfx::B2DRange(aNewRange));
		}

        return *mpB2DRange;
    }
};

//////////////////////////////////////////////////////////////////////////////

class ImplB2DPolygon
{
private:
	// The point vector. This vector exists always and defines the
	// count of members.
	CoordinateDataArray2D							maPoints;

	// The control point vectors. This vectors are created on demand
	// and may be zero.
	boost::scoped_ptr< ControlVectorArray2D >		mpControlVector;

    // buffered data for e.g. default subdivision and range
    boost::scoped_ptr< ImplBufferedData >           mpBufferedData;

	// flag which decides if this polygon is opened or closed
	bool                                            mbIsClosed;

public:
	const basegfx::B2DPolygon& getDefaultAdaptiveSubdivision(const basegfx::B2DPolygon& rSource) const
	{
		if(!mpControlVector || !mpControlVector->isUsed())
        {
    		return rSource;
        }

        if(!mpBufferedData)
        {
			const_cast< ImplB2DPolygon* >(this)->mpBufferedData.reset(new ImplBufferedData);
        }

        return mpBufferedData->getDefaultAdaptiveSubdivision(rSource);
	}

	const basegfx::B2DRange& getB2DRange(const basegfx::B2DPolygon& rSource) const
    {
        if(!mpBufferedData)
        {
			const_cast< ImplB2DPolygon* >(this)->mpBufferedData.reset(new ImplBufferedData);
        }

        return mpBufferedData->getB2DRange(rSource);
    }

	ImplB2DPolygon()
	:	maPoints(0),
		mpControlVector(),
		mpBufferedData(),
        mbIsClosed(false)
	{}

	ImplB2DPolygon(const ImplB2DPolygon& rToBeCopied)
	:	maPoints(rToBeCopied.maPoints),
		mpControlVector(),
		mpBufferedData(),
		mbIsClosed(rToBeCopied.mbIsClosed)
	{
		// complete initialization using copy
		if(rToBeCopied.mpControlVector && rToBeCopied.mpControlVector->isUsed())
        {
			mpControlVector.reset( new ControlVectorArray2D(*rToBeCopied.mpControlVector) );
        }
	}

	ImplB2DPolygon(const ImplB2DPolygon& rToBeCopied, sal_uInt32 nIndex, sal_uInt32 nCount)
	:	maPoints(rToBeCopied.maPoints, nIndex, nCount),
		mpControlVector(),
		mpBufferedData(),
		mbIsClosed(rToBeCopied.mbIsClosed)
	{
		// complete initialization using partly copy
		if(rToBeCopied.mpControlVector && rToBeCopied.mpControlVector->isUsed())
		{
			mpControlVector.reset( new ControlVectorArray2D(*rToBeCopied.mpControlVector, nIndex, nCount) );

			if(!mpControlVector->isUsed())
                mpControlVector.reset();
		}
	}

    ImplB2DPolygon& operator=( const ImplB2DPolygon& rToBeCopied )
    {
		maPoints = rToBeCopied.maPoints;
		mpControlVector.reset();
		mpBufferedData.reset();
		mbIsClosed = rToBeCopied.mbIsClosed;

		// complete initialization using copy
		if(rToBeCopied.mpControlVector && rToBeCopied.mpControlVector->isUsed())
			mpControlVector.reset( new ControlVectorArray2D(*rToBeCopied.mpControlVector) );

        return *this;
    }

	sal_uInt32 count() const
	{
		return maPoints.count();
	}

	bool isClosed() const
	{
		return mbIsClosed;
	}

	void setClosed(bool bNew)
	{
		if(bNew != mbIsClosed)
		{
			mpBufferedData.reset();
			mbIsClosed = bNew;
		}
	}

	bool operator==(const ImplB2DPolygon& rCandidate) const
	{
		if(mbIsClosed == rCandidate.mbIsClosed)
		{
			if(maPoints == rCandidate.maPoints)
			{
				bool bControlVectorsAreEqual(true);

				if(mpControlVector)
				{
					if(rCandidate.mpControlVector)
					{
						bControlVectorsAreEqual = ((*mpControlVector) == (*rCandidate.mpControlVector));
					}
					else
					{
						// candidate has no control vector, so it's assumed all unused.
						bControlVectorsAreEqual = !mpControlVector->isUsed();
					}
				}
				else
				{
					if(rCandidate.mpControlVector)
					{
						// we have no control vector, so it's assumed all unused.
						bControlVectorsAreEqual = !rCandidate.mpControlVector->isUsed();
					}
				}

				if(bControlVectorsAreEqual)
				{
					return true;
				}
			}
		}

		return false;
	}

	const basegfx::B2DPoint& getPoint(sal_uInt32 nIndex) const
	{
		return maPoints.getCoordinate(nIndex);
	}

	void setPoint(sal_uInt32 nIndex, const basegfx::B2DPoint& rValue)
	{
		mpBufferedData.reset();
		maPoints.setCoordinate(nIndex, rValue);
	}

	void reserve(sal_uInt32 nCount)
	{
		maPoints.reserve(nCount);
	}

	void append(const basegfx::B2DPoint& rPoint)
	{
		mpBufferedData.reset(); // TODO: is this needed?
		const CoordinateData2D aCoordinate(rPoint);
		maPoints.append(aCoordinate);

		if(mpControlVector)
		{
			const ControlVectorPair2D aVectorPair;
			mpControlVector->append(aVectorPair);
		}
	}

	void insert(sal_uInt32 nIndex, const basegfx::B2DPoint& rPoint, sal_uInt32 nCount)
	{
		if(nCount)
		{
			mpBufferedData.reset();
			CoordinateData2D aCoordinate(rPoint);
			maPoints.insert(nIndex, aCoordinate, nCount);

			if(mpControlVector)
			{
				ControlVectorPair2D aVectorPair;
				mpControlVector->insert(nIndex, aVectorPair, nCount);
			}
		}
	}

	const basegfx::B2DVector& getPrevControlVector(sal_uInt32 nIndex) const
	{
		if(mpControlVector)
		{
			return mpControlVector->getPrevVector(nIndex);
		}
		else
		{
			return basegfx::B2DVector::getEmptyVector();
		}
	}

	void setPrevControlVector(sal_uInt32 nIndex, const basegfx::B2DVector& rValue)
	{
		if(!mpControlVector)
		{
			if(!rValue.equalZero())
			{
				mpBufferedData.reset();
				mpControlVector.reset( new ControlVectorArray2D(maPoints.count()) );
				mpControlVector->setPrevVector(nIndex, rValue);
			}
		}
		else
		{
			mpBufferedData.reset();
			mpControlVector->setPrevVector(nIndex, rValue);

			if(!mpControlVector->isUsed())
                mpControlVector.reset();
		}
	}

	const basegfx::B2DVector& getNextControlVector(sal_uInt32 nIndex) const
	{
		if(mpControlVector)
		{
			return mpControlVector->getNextVector(nIndex);
		}
		else
		{
			return basegfx::B2DVector::getEmptyVector();
		}
	}

	void setNextControlVector(sal_uInt32 nIndex, const basegfx::B2DVector& rValue)
	{
		if(!mpControlVector)
		{
			if(!rValue.equalZero())
			{
				mpBufferedData.reset();
				mpControlVector.reset( new ControlVectorArray2D(maPoints.count()) );
				mpControlVector->setNextVector(nIndex, rValue);
			}
		}
		else
		{
			mpBufferedData.reset();
			mpControlVector->setNextVector(nIndex, rValue);

			if(!mpControlVector->isUsed())
                mpControlVector.reset();
		}
	}

	bool areControlPointsUsed() const
	{
		return (mpControlVector && mpControlVector->isUsed());
	}

	void resetControlVectors(sal_uInt32 nIndex)
	{
		setPrevControlVector(nIndex, basegfx::B2DVector::getEmptyVector());
		setNextControlVector(nIndex, basegfx::B2DVector::getEmptyVector());
	}

	void resetControlVectors()
	{
		mpBufferedData.reset();
		mpControlVector.reset();
	}

	void setControlVectors(sal_uInt32 nIndex, const basegfx::B2DVector& rPrev, const basegfx::B2DVector& rNext)
	{
		setPrevControlVector(nIndex, rPrev);
		setNextControlVector(nIndex, rNext);
	}

	void appendBezierSegment(const basegfx::B2DVector& rNext, const basegfx::B2DVector& rPrev, const basegfx::B2DPoint& rPoint)
	{
		mpBufferedData.reset();
		const sal_uInt32 nCount(maPoints.count());

        if(nCount)
		{
			setNextControlVector(nCount - 1, rNext);
		}

		insert(nCount, rPoint, 1);
		setPrevControlVector(nCount, rPrev);
	}

	void insert(sal_uInt32 nIndex, const ImplB2DPolygon& rSource)
	{
		const sal_uInt32 nCount(rSource.maPoints.count());

		if(nCount)
		{
			mpBufferedData.reset();

			if(rSource.mpControlVector && rSource.mpControlVector->isUsed() && !mpControlVector)
			{
				mpControlVector.reset( new ControlVectorArray2D(maPoints.count()) );
			}

			maPoints.insert(nIndex, rSource.maPoints);

			if(rSource.mpControlVector)
			{
				mpControlVector->insert(nIndex, *rSource.mpControlVector);

				if(!mpControlVector->isUsed())
                    mpControlVector.reset();
			}
			else if(mpControlVector)
			{
				ControlVectorPair2D aVectorPair;
				mpControlVector->insert(nIndex, aVectorPair, nCount);
			}
		}
	}

	void remove(sal_uInt32 nIndex, sal_uInt32 nCount)
	{
		if(nCount)
		{
			mpBufferedData.reset();
			maPoints.remove(nIndex, nCount);

			if(mpControlVector)
			{
				mpControlVector->remove(nIndex, nCount);

				if(!mpControlVector->isUsed())
                    mpControlVector.reset();
			}
		}
	}

	void flip()
	{
		if(maPoints.count() > 1)
		{
			mpBufferedData.reset();

			// flip points
			maPoints.flip(mbIsClosed);

			if(mpControlVector)
			{
				// flip control vector
				mpControlVector->flip(mbIsClosed);
			}
		}
	}

	bool hasDoublePoints() const
	{
		if(mbIsClosed)
		{
			// check for same start and end point
			const sal_uInt32 nIndex(maPoints.count() - 1);

			if(maPoints.getCoordinate(0) == maPoints.getCoordinate(nIndex))
			{
				if(mpControlVector)
				{
					if(mpControlVector->getNextVector(nIndex).equalZero() && mpControlVector->getPrevVector(0).equalZero())
					{
						return true;
					}
				}
				else
				{
					return true;
				}
			}
		}

		// test for range
		for(sal_uInt32 a(0); a < maPoints.count() - 1; a++)
		{
			if(maPoints.getCoordinate(a) == maPoints.getCoordinate(a + 1))
			{
				if(mpControlVector)
				{
					if(mpControlVector->getNextVector(a).equalZero() && mpControlVector->getPrevVector(a + 1).equalZero())
					{
						return true;
					}
				}
				else
				{
					return true;
				}
			}
		}

		return false;
	}

	void removeDoublePointsAtBeginEnd()
	{
		// Only remove DoublePoints at Begin and End when poly is closed
		if(mbIsClosed)
		{
			mpBufferedData.reset();

            if(mpControlVector)
			{
				bool bRemove;

				do
				{
					bRemove = false;

					if(maPoints.count() > 1)
					{
						const sal_uInt32 nIndex(maPoints.count() - 1);

						if(maPoints.getCoordinate(0) == maPoints.getCoordinate(nIndex))
						{
							if(mpControlVector)
							{
								if(mpControlVector->getNextVector(nIndex).equalZero() && mpControlVector->getPrevVector(0).equalZero())
								{
									bRemove = true;
								}
							}
							else
							{
								bRemove = true;
							}
						}
					}

					if(bRemove)
					{
						const sal_uInt32 nIndex(maPoints.count() - 1);

						if(mpControlVector && !mpControlVector->getPrevVector(nIndex).equalZero())
						{
							mpControlVector->setPrevVector(0, mpControlVector->getPrevVector(nIndex));
						}

						remove(nIndex, 1);
					}
				}
				while(bRemove);
			}
			else
			{
				maPoints.removeDoublePointsAtBeginEnd();
			}
		}
	}

	void removeDoublePointsWholeTrack()
	{
		mpBufferedData.reset();

        if(mpControlVector)
		{
			sal_uInt32 nIndex(0);

			// test as long as there are at least two points and as long as the index
			// is smaller or equal second last point
			while((maPoints.count() > 1) && (nIndex <= maPoints.count() - 2))
			{
				bool bRemove(maPoints.getCoordinate(nIndex) == maPoints.getCoordinate(nIndex + 1));

				if(bRemove)
				{
					if(mpControlVector)
					{
						if(!mpControlVector->getNextVector(nIndex).equalZero() || !mpControlVector->getPrevVector(nIndex + 1).equalZero())
						{
							bRemove = false;
						}
					}
				}

				if(bRemove)
				{
					if(mpControlVector && !mpControlVector->getPrevVector(nIndex).equalZero())
					{
						mpControlVector->setPrevVector(nIndex + 1, mpControlVector->getPrevVector(nIndex));
					}

					// if next is same as index and the control vectors are unused, delete index
					remove(nIndex, 1);
				}
				else
				{
					// if different, step forward
					nIndex++;
				}
			}
		}
		else
		{
			maPoints.removeDoublePointsWholeTrack();
		}
	}

	void transform(const basegfx::B2DHomMatrix& rMatrix)
	{
		mpBufferedData.reset();

        if(mpControlVector)
		{
			for(sal_uInt32 a(0); a < maPoints.count(); a++)
			{
				basegfx::B2DPoint aCandidate = maPoints.getCoordinate(a);

				if(mpControlVector->isUsed())
				{
					const basegfx::B2DVector& rPrevVector(mpControlVector->getPrevVector(a));
					const basegfx::B2DVector& rNextVector(mpControlVector->getNextVector(a));

					if(!rPrevVector.equalZero())
					{
						basegfx::B2DVector aPrevVector(rMatrix * rPrevVector);
						mpControlVector->setPrevVector(a, aPrevVector);
					}

					if(!rNextVector.equalZero())
					{
						basegfx::B2DVector aNextVector(rMatrix * rNextVector);
						mpControlVector->setNextVector(a, aNextVector);
					}
				}

				aCandidate *= rMatrix;
				maPoints.setCoordinate(a, aCandidate);
			}

			if(!mpControlVector->isUsed())
                mpControlVector.reset();
		}
		else
		{
			maPoints.transform(rMatrix);
		}
	}

    const basegfx::B2DPoint* begin() const
    {
        return maPoints.begin();
    }

    const basegfx::B2DPoint* end() const
    {
        return maPoints.end();
    }

    basegfx::B2DPoint* begin()
    {
       mpBufferedData.reset();
       return maPoints.begin();
    }

    basegfx::B2DPoint* end()
    {
        mpBufferedData.reset();
        return maPoints.end();
    }
};

//////////////////////////////////////////////////////////////////////////////

namespace basegfx
{
    namespace
	{
		struct DefaultPolygon: public rtl::Static<B2DPolygon::ImplType, DefaultPolygon> {};
	}

	B2DPolygon::B2DPolygon()
	:	mpPolygon(DefaultPolygon::get())
	{}

	B2DPolygon::B2DPolygon(const B2DPolygon& rPolygon)
	:	mpPolygon(rPolygon.mpPolygon)
	{}

	B2DPolygon::B2DPolygon(const B2DPolygon& rPolygon, sal_uInt32 nIndex, sal_uInt32 nCount)
	:	mpPolygon(ImplB2DPolygon(*rPolygon.mpPolygon, nIndex, nCount))
	{
        // TODO(P2): one extra temporary here (cow_wrapper copies
        // given ImplB2DPolygon into its internal impl_t wrapper type)
		OSL_ENSURE(nIndex + nCount <= rPolygon.mpPolygon->count(), "B2DPolygon constructor outside range (!)");
	}

	B2DPolygon::~B2DPolygon()
	{
	}

	B2DPolygon& B2DPolygon::operator=(const B2DPolygon& rPolygon)
	{
		mpPolygon = rPolygon.mpPolygon;
		return *this;
	}

    void B2DPolygon::makeUnique()
    {
        mpPolygon.make_unique();
    }

	bool B2DPolygon::operator==(const B2DPolygon& rPolygon) const
	{
		if(mpPolygon.same_object(rPolygon.mpPolygon))
			return true;

		return ((*mpPolygon) == (*rPolygon.mpPolygon));
	}

	bool B2DPolygon::operator!=(const B2DPolygon& rPolygon) const
	{
        return !(*this == rPolygon);
	}

	sal_uInt32 B2DPolygon::count() const
	{
		return mpPolygon->count();
	}

    B2DPoint B2DPolygon::getB2DPoint(sal_uInt32 nIndex) const
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		return mpPolygon->getPoint(nIndex);
	}

	void B2DPolygon::setB2DPoint(sal_uInt32 nIndex, const B2DPoint& rValue)
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		if(mpPolygon->getPoint(nIndex) != rValue)
		{
			mpPolygon->setPoint(nIndex, rValue);
		}
	}

	void B2DPolygon::reserve(sal_uInt32 nCount)
	{
		mpPolygon->reserve(nCount);
	}

	void B2DPolygon::insert(sal_uInt32 nIndex, const B2DPoint& rPoint, sal_uInt32 nCount)
	{
		OSL_ENSURE(nIndex <= mpPolygon->count(), "B2DPolygon Insert outside range (!)");

		if(nCount)
		{
			mpPolygon->insert(nIndex, rPoint, nCount);
		}
	}

	void B2DPolygon::append(const B2DPoint& rPoint, sal_uInt32 nCount)
	{
		if(nCount)
		{
			mpPolygon->insert(mpPolygon->count(), rPoint, nCount);
		}
	}

	void B2DPolygon::append(const B2DPoint& rPoint)
	{
		mpPolygon->append(rPoint);
	}

	B2DPoint B2DPolygon::getPrevControlPoint(sal_uInt32 nIndex) const
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		if(mpPolygon->areControlPointsUsed())
		{
			return mpPolygon->getPoint(nIndex) + mpPolygon->getPrevControlVector(nIndex);
		}
		else
		{
			return mpPolygon->getPoint(nIndex);
		}
	}

	B2DPoint B2DPolygon::getNextControlPoint(sal_uInt32 nIndex) const
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		if(mpPolygon->areControlPointsUsed())
		{
			return mpPolygon->getPoint(nIndex) + mpPolygon->getNextControlVector(nIndex);
		}
		else
		{
			return mpPolygon->getPoint(nIndex);
		}
	}

	void B2DPolygon::setPrevControlPoint(sal_uInt32 nIndex, const B2DPoint& rValue)
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");
		const basegfx::B2DVector aNewVector(rValue - mpPolygon->getPoint(nIndex));

		if(mpPolygon->getPrevControlVector(nIndex) != aNewVector)
		{
			mpPolygon->setPrevControlVector(nIndex, aNewVector);
		}
	}

	void B2DPolygon::setNextControlPoint(sal_uInt32 nIndex, const B2DPoint& rValue)
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");
		const basegfx::B2DVector aNewVector(rValue - mpPolygon->getPoint(nIndex));

		if(mpPolygon->getNextControlVector(nIndex) != aNewVector)
		{
			mpPolygon->setNextControlVector(nIndex, aNewVector);
		}
	}

	void B2DPolygon::setControlPoints(sal_uInt32 nIndex, const basegfx::B2DPoint& rPrev, const basegfx::B2DPoint& rNext)
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");
		const B2DPoint aPoint(mpPolygon->getPoint(nIndex));
		const basegfx::B2DVector aNewPrev(rPrev - aPoint);
		const basegfx::B2DVector aNewNext(rNext - aPoint);

		if(mpPolygon->getPrevControlVector(nIndex) != aNewPrev || mpPolygon->getNextControlVector(nIndex) != aNewNext)
		{
			mpPolygon->setControlVectors(nIndex, aNewPrev, aNewNext);
		}
	}

	void B2DPolygon::resetPrevControlPoint(sal_uInt32 nIndex)
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		if(mpPolygon->areControlPointsUsed() && !mpPolygon->getPrevControlVector(nIndex).equalZero())
		{
			mpPolygon->setPrevControlVector(nIndex, B2DVector::getEmptyVector());
		}
	}

	void B2DPolygon::resetNextControlPoint(sal_uInt32 nIndex)
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		if(mpPolygon->areControlPointsUsed() && !mpPolygon->getNextControlVector(nIndex).equalZero())
		{
			mpPolygon->setNextControlVector(nIndex, B2DVector::getEmptyVector());
		}
	}

	void B2DPolygon::resetControlPoints(sal_uInt32 nIndex)
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		if(mpPolygon->areControlPointsUsed() &&
			(!mpPolygon->getPrevControlVector(nIndex).equalZero() || !mpPolygon->getNextControlVector(nIndex).equalZero()))
		{
			mpPolygon->resetControlVectors(nIndex);
		}
	}

	void B2DPolygon::resetControlPoints()
	{
		if(mpPolygon->areControlPointsUsed())
		{
			mpPolygon->resetControlVectors();
		}
	}

	void B2DPolygon::appendBezierSegment(
		const B2DPoint& rNextControlPoint,
		const B2DPoint& rPrevControlPoint,
		const B2DPoint& rPoint)
	{
		const B2DVector aNewNextVector(mpPolygon->count() ? B2DVector(rNextControlPoint - mpPolygon->getPoint(mpPolygon->count() - 1)) : B2DVector::getEmptyVector());
		const B2DVector aNewPrevVector(rPrevControlPoint - rPoint);

		if(aNewNextVector.equalZero() && aNewPrevVector.equalZero())
		{
			mpPolygon->insert(mpPolygon->count(), rPoint, 1);
		}
		else
		{
			mpPolygon->appendBezierSegment(aNewNextVector, aNewPrevVector, rPoint);
		}
	}

	bool B2DPolygon::areControlPointsUsed() const
	{
		return mpPolygon->areControlPointsUsed();
	}

	bool B2DPolygon::isPrevControlPointUsed(sal_uInt32 nIndex) const
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		return (mpPolygon->areControlPointsUsed() && !mpPolygon->getPrevControlVector(nIndex).equalZero());
	}

	bool B2DPolygon::isNextControlPointUsed(sal_uInt32 nIndex) const
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		return (mpPolygon->areControlPointsUsed() && !mpPolygon->getNextControlVector(nIndex).equalZero());
	}

	B2VectorContinuity B2DPolygon::getContinuityInPoint(sal_uInt32 nIndex) const
	{
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

		if(mpPolygon->areControlPointsUsed())
		{
			const B2DVector& rPrev(mpPolygon->getPrevControlVector(nIndex));
			const B2DVector& rNext(mpPolygon->getNextControlVector(nIndex));

			return getContinuity(rPrev, rNext);
		}
		else
		{
			return CONTINUITY_NONE;
		}
	}

    bool B2DPolygon::isBezierSegment(sal_uInt32 nIndex) const
    {
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");

        if(mpPolygon->areControlPointsUsed())
		{
            // Check if the edge exists
            const bool bNextIndexValidWithoutClose(nIndex + 1 < mpPolygon->count());

            if(bNextIndexValidWithoutClose || mpPolygon->isClosed())
            {
                const sal_uInt32 nNextIndex(bNextIndexValidWithoutClose ? nIndex + 1 : 0);
                return (!mpPolygon->getPrevControlVector(nNextIndex).equalZero()
                    || !mpPolygon->getNextControlVector(nIndex).equalZero());
            }
            else
            {
                // no valid edge -> no bezier segment, even when local next
                // vector may be used
                return false;
            }
        }
        else
        {
            // no control points -> no bezier segment
            return false;
        }
    }

    void B2DPolygon::getBezierSegment(sal_uInt32 nIndex, B2DCubicBezier& rTarget) const
    {
		OSL_ENSURE(nIndex < mpPolygon->count(), "B2DPolygon access outside range (!)");
        const bool bNextIndexValidWithoutClose(nIndex + 1 < mpPolygon->count());

        if(bNextIndexValidWithoutClose || mpPolygon->isClosed())
        {
            const sal_uInt32 nNextIndex(bNextIndexValidWithoutClose ? nIndex + 1 : 0);
            rTarget.setStartPoint(mpPolygon->getPoint(nIndex));
            rTarget.setEndPoint(mpPolygon->getPoint(nNextIndex));

            if(mpPolygon->areControlPointsUsed())
            {
                rTarget.setControlPointA(rTarget.getStartPoint() + mpPolygon->getNextControlVector(nIndex));
                rTarget.setControlPointB(rTarget.getEndPoint() + mpPolygon->getPrevControlVector(nNextIndex));
            }
            else
            {
                // no bezier, reset control poins at rTarget
                rTarget.setControlPointA(rTarget.getStartPoint());
                rTarget.setControlPointB(rTarget.getEndPoint());
            }
        }
        else
        {
            // no valid edge at all, reset rTarget to current point
    		const B2DPoint aPoint(mpPolygon->getPoint(nIndex));
            rTarget.setStartPoint(aPoint);
            rTarget.setEndPoint(aPoint);
            rTarget.setControlPointA(aPoint);
            rTarget.setControlPointB(aPoint);
        }
    }

    B2DPolygon B2DPolygon::getDefaultAdaptiveSubdivision() const
	{
		return mpPolygon->getDefaultAdaptiveSubdivision(*this);
	}

    B2DRange B2DPolygon::getB2DRange() const
	{
		return mpPolygon->getB2DRange(*this);
	}

	void B2DPolygon::insert(sal_uInt32 nIndex, const B2DPolygon& rPoly, sal_uInt32 nIndex2, sal_uInt32 nCount)
	{
		OSL_ENSURE(nIndex <= mpPolygon->count(), "B2DPolygon Insert outside range (!)");

		if(rPoly.count())
		{
			if(!nCount)
			{
				nCount = rPoly.count();
			}

			if(0 == nIndex2 && nCount == rPoly.count())
			{
				mpPolygon->insert(nIndex, *rPoly.mpPolygon);
			}
			else
			{
				OSL_ENSURE(nIndex2 + nCount <= rPoly.mpPolygon->count(), "B2DPolygon Insert outside range (!)");
				ImplB2DPolygon aTempPoly(*rPoly.mpPolygon, nIndex2, nCount);
				mpPolygon->insert(nIndex, aTempPoly);
			}
		}
	}

	void B2DPolygon::append(const B2DPolygon& rPoly, sal_uInt32 nIndex, sal_uInt32 nCount)
	{
		if(rPoly.count())
		{
			if(!nCount)
			{
				nCount = rPoly.count();
			}

			if(0 == nIndex && nCount == rPoly.count())
			{
				mpPolygon->insert(mpPolygon->count(), *rPoly.mpPolygon);
			}
			else
			{
				OSL_ENSURE(nIndex + nCount <= rPoly.mpPolygon->count(), "B2DPolygon Append outside range (!)");
				ImplB2DPolygon aTempPoly(*rPoly.mpPolygon, nIndex, nCount);
				mpPolygon->insert(mpPolygon->count(), aTempPoly);
			}
		}
	}

	void B2DPolygon::remove(sal_uInt32 nIndex, sal_uInt32 nCount)
	{
		OSL_ENSURE(nIndex + nCount <= mpPolygon->count(), "B2DPolygon Remove outside range (!)");

		if(nCount)
		{
			mpPolygon->remove(nIndex, nCount);
		}
	}

	void B2DPolygon::clear()
	{
		mpPolygon = DefaultPolygon::get();
	}

	bool B2DPolygon::isClosed() const
	{
		return mpPolygon->isClosed();
	}

	void B2DPolygon::setClosed(bool bNew)
	{
		if(isClosed() != bNew)
		{
			mpPolygon->setClosed(bNew);
		}
	}

	void B2DPolygon::flip()
	{
		if(count() > 1)
		{
			mpPolygon->flip();
		}
	}

	bool B2DPolygon::hasDoublePoints() const
	{
		return (mpPolygon->count() > 1 && mpPolygon->hasDoublePoints());
	}

	void B2DPolygon::removeDoublePoints()
	{
		if(hasDoublePoints())
		{
			mpPolygon->removeDoublePointsAtBeginEnd();
			mpPolygon->removeDoublePointsWholeTrack();
		}
	}

	void B2DPolygon::transform(const B2DHomMatrix& rMatrix)
	{
		if(mpPolygon->count() && !rMatrix.isIdentity())
		{
			mpPolygon->transform(rMatrix);
		}
	}

    const B2DPoint* B2DPolygon::begin() const
    {
        return mpPolygon->begin();
    }

    const B2DPoint* B2DPolygon::end() const
    {
        return mpPolygon->end();
    }

    B2DPoint* B2DPolygon::begin()
    {
        return mpPolygon->begin();
    }

    B2DPoint* B2DPolygon::end()
    {
        return mpPolygon->end();
    }
} // end of namespace basegfx

//////////////////////////////////////////////////////////////////////////////
// eof


#include "Object.h"
#include "parameters.h"
#include <boost/lexical_cast.hpp>

#include <Eigen/Geometry>
#include <tf/transform_datatypes.h>

namespace hector_object_tracker {

std::map<std::string,unsigned int> Object::object_count;
std::string Object::object_namespace;

Object::Object(const std::string class_id, const std::string object_id)
{
  if (!class_id.empty()) {
    this->info.class_id = class_id;
  } else
  {
    this->info.class_id = "object";
  }

  if (!object_id.empty()) {
    this->info.object_id = object_id;
  } else {
    this->info.object_id = this->info.class_id + "_" + boost::lexical_cast<std::string>(object_count[this->info.class_id]++);
  }

  // if (this->info.object_id[0] != '/') this->info.object_id = object_namespace + "/" + this->info.object_id;
}

Object::Object(const hector_worldmodel_msgs::Object& other) {
  *this = other;
}

Object::~Object()
{
  ROS_WARN("who kill me?");
}

void Object::reset() {
  object_count.clear();
}

void Object::getMessage(hector_worldmodel_msgs::Object& object) const {
  object.header = this->header;
  this->getPoseWithCovariance(object.pose);
  object.info   = this->info;
  object.state  = this->state;
}

hector_worldmodel_msgs::Object Object::getMessage() const {
  hector_worldmodel_msgs::Object object;
  getMessage(object);
  return object;
}

void Object::getPoseWithCovariance(geometry_msgs::PoseWithCovariance& pose) const {
  getPose(pose.pose);
  getCovariance(pose.covariance);
}

void Object::setPose(const geometry_msgs::PoseWithCovariance& pose) {
  setPose(pose.pose);
  setCovariance(pose.covariance);
}

void Object::getPose(geometry_msgs::Pose& pose) const
{
  pose.position.x = this->position.x();
  pose.position.y = this->position.y();
  pose.position.z = this->position.z();
  pose.orientation.w = this->orientation.w();
  pose.orientation.x = this->orientation.x();
  pose.orientation.y = this->orientation.y();
  pose.orientation.z = this->orientation.z();
}

void Object::getPose(tf::Pose& pose) const
{
  pose.getOrigin().setValue(this->position.x(), this->position.y(), this->position.z());
  pose.getBasis().setRotation(tf::Quaternion(this->orientation.x(), this->orientation.y(), this->orientation.z(), this->orientation.w()));
}

void Object::setPose(const geometry_msgs::Pose& pose)
{
  setPosition(pose.position);
  setOrientation(pose.orientation);
}

void Object::setPose(const tf::Pose &pose)
{
  setPosition(pose.getOrigin());
  tf::Quaternion rot;
  pose.getBasis().getRotation(rot);
  setOrientation(rot);
}

const Eigen::Vector3f& Object::getPosition() const {
  return position;
}

void Object::setPosition(const Eigen::Vector3f& position) {
  this->position = position;
}

void Object::setPosition(const geometry_msgs::Point& position) {
  this->position << position.x, position.y, position.z;
}

void Object::setPosition(const tf::Point& position)
{
  this->position << position.x(), position.y(), position.z();
}

const Eigen::Quaternionf& Object::getOrientation() const
{
  return this->orientation;
}

void Object::setOrientation(const geometry_msgs::Quaternion& orientation)
{
  this->orientation.coeffs() << orientation.x, orientation.y, orientation.z, orientation.w;
}

void Object::setOrientation(const tf::Quaternion& orientation)
{
  this->orientation.coeffs() << orientation.x(), orientation.y(), orientation.z(), orientation.w();
}

const Eigen::Matrix3f& Object::getCovariance() const {
  return covariance;
}

void Object::getCovariance(geometry_msgs::PoseWithCovariance::_covariance_type& covariance) const {
  covariance[0]  = this->covariance(0,0);
  covariance[1]  = this->covariance(0,1);
  covariance[2]  = this->covariance(0,2);
  covariance[6]  = this->covariance(1,0);
  covariance[7]  = this->covariance(1,1);
  covariance[8]  = this->covariance(1,2);
  covariance[12] = this->covariance(2,0);
  covariance[13] = this->covariance(2,1);
  covariance[14] = this->covariance(2,2);
}

void Object::setCovariance(const Eigen::Matrix3f& eigen) {
  this->covariance = eigen;
}

void Object::setCovariance(const tf::Matrix3x3& tf) {
  this->covariance << tf[0][0], tf[0][1], tf[0][2],
                      tf[1][0], tf[1][1], tf[1][2],
                      tf[2][0], tf[2][1], tf[2][2];
}

void Object::setCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& covariance) {
  this->covariance << covariance[0],  covariance[1],  covariance[2],
                      covariance[6],  covariance[7],  covariance[8],
                      covariance[12], covariance[13], covariance[14];
}

void Object::setState(const StateType& state) {
  if (this->state.state == state) return;
  ROS_INFO("Setting object state for %s to %s", getObjectId().c_str(), hector_worldmodel_msgs::getObjectStateString(state));
  this->state.state = state;
}

void Object::intersect(const tf::Pose& poseB, const Eigen::Matrix3f& covarianceB, float support) {
  Eigen::Vector3f positionB(poseB.getOrigin().x(), poseB.getOrigin().y(), poseB.getOrigin().z());
  tf::Quaternion orientationB = poseB.getRotation();
    // old cov/covariance is A , new cov/covIn is B
  float omega = 0.5f;

  Eigen::Matrix3f A(covariance.inverse() * omega);
  Eigen::Matrix3f B(covarianceB.inverse() * (1.0f - omega));
  double infA = 1./covariance.trace();
  double infB = 1./covarianceB.trace();

  covariance = (A + B).inverse();
  position = covariance * (A * position + B * positionB);
  updateOrientation(orientationB, infB / (infA + infB));

  setPosition(position);
  setCovariance(covariance);
  addSupport(support);
}

void Object::update(const tf::Pose& poseB, const Eigen::Matrix3f& covarianceB, float support) {
  Eigen::Vector3f positionB(poseB.getOrigin().x(), poseB.getOrigin().y(), poseB.getOrigin().z());
  tf::Quaternion orientationB = poseB.getRotation();
    // old information is A , new information is B

  Eigen::Matrix3f A(covariance.inverse());
  Eigen::Matrix3f B(covarianceB.inverse());
  double infA = 1./covariance.trace();
  double infB = 1./covarianceB.trace();

  covariance = (A + B).inverse();
  position = covariance * (A * position + B * positionB);
  updateOrientation(orientationB, infB / (infA + infB));

  setPosition(position);
  setCovariance(covariance);
  addSupport(support);
}

void Object::updateOrientation(const tf::Quaternion& orientationB, double slerp_factor) {
  // update orientation of objects using low-pass filtering
  if (parameter(_with_orientation, getClassId())) {
      tf::Quaternion q(orientation.x(), orientation.y(), orientation.z(), orientation.w());
      q.slerp(orientationB, slerp_factor);
      setOrientation(q);
  }
  // or simply set new orientation
  else {
    setOrientation(orientationB);
  }
}

void Object::getVisualization(visualization_msgs::MarkerArray &markers) const {
  visualization_msgs::Marker marker;
  std::string postfix;

  // default color
  marker.color = parameter(_marker_color, this->info.class_id);

  switch(this->state.state) {
    case hector_worldmodel_msgs::ObjectState::CONFIRMED:
      marker.color.r = 0.0;
      marker.color.g = 0.8;
      marker.color.b = 0.0;
      postfix = " (CONFIRMED)";
      break;
    case hector_worldmodel_msgs::ObjectState::DISCARDED:
      marker.color.a = 0.5;
      postfix = " (DISCARDED)";
      break;
    case hector_worldmodel_msgs::ObjectState::INACTIVE:
      marker.color.a = 0.5;
      postfix = " (INACTIVE)";
      break;
    case hector_worldmodel_msgs::ObjectState::UNKNOWN:
      marker.color.a = 0.5;
      break;
    case hector_worldmodel_msgs::ObjectState::PENDING:
      marker.color.a = 0.5;
      postfix = " (PENDING)";
      break;
    default:
      break;
  }

  marker.header = this->header;
  marker.action = visualization_msgs::Marker::ADD;
  getPose(marker.pose);
  marker.ns = this->info.class_id;

  marker.type = visualization_msgs::Marker::ARROW;
  marker.scale.x = 1.0;
  marker.scale.y = 1.0;
  marker.scale.z = 1.0;
  // markers.markers.push_back(marker);

  marker.type = visualization_msgs::Marker::SPHERE;
  marker.scale.x = 0.1;
  marker.scale.y = 0.1;
  marker.scale.z = 0.1;
  markers.markers.push_back(marker);

  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
  marker.text = (!this->info.name.empty() ? this->info.name : this->info.object_id) + postfix;
  marker.scale.x = 0.0;
  marker.scale.y = 0.0;
  marker.scale.z = 0.1;
  marker.pose.position.z += 1.5 * marker.scale.z;
  markers.markers.push_back(marker);
}

void Object::setNamespace(const std::string &ns) {
  object_namespace = ns;
}

Object& Object::operator =(const hector_worldmodel_msgs::Object& other) {
  header = other.header;
  info = other.info;
  state = other.state;
  setPose(other.pose);
  return *this;
}

ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame) const
{
  return transform(tf, target_frame, this->header.stamp);
}

ObjectPtr Object::transform(tf::Transformer& tf, const std::string& target_frame, const ros::Time& target_time) const
{
  tf::StampedTransform transform;
  tf.lookupTransform(target_frame, this->header.frame_id, target_time, transform);

  ObjectPtr result(new Object(*this));

  tf::Pose pose;
  this->getPose(pose);

  // transform pose
  pose = transform * pose;
  result->setPose(pose);

  // rotate covariance matrix
  tf::Matrix3x3 rotation(transform.getBasis());
  tf::Matrix3x3 cov(covariance(0,0), covariance(0,1), covariance(0,2),
                    covariance(1,0), covariance(1,1), covariance(1,2),
                    covariance(2,0), covariance(2,1), covariance(2,2));
  result->setCovariance(rotation * cov * rotation.transpose());

  // set new frame_id
  result->header.frame_id = target_frame;

  return result;
}

double Object::getDistance(const Object &other)
{
  return (this->getPosition() - other.getPosition()).norm();
}

} // namespace hector_object_tracker


#include "workerthread.h"

#include <opencv2/opencv.hpp>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <QCoreApplication>

#include "imageprovider.h"
#include "faceparts.h"
#include "pupilfinder.h"
#include "mutualgazelearner.h"
#include "verticalgazelearner.h"
#include "eyelidlearner.h"
#include "relativeeyelidlearner.h"
#include "relativegazelearner.h"
#include "facedetectionworker.h"
#include "shapedetectionworker.h"
#include "regressionworker.h"
#include "eyepatcher.h"
#include "rlssmoother.h"

#ifdef ENABLE_YARP_SUPPORT
    #include "yarpsupport.h"
#endif

using namespace std;
using namespace boost::accumulators;

class TemporalStats {
private:
    const int accumulatorWindowSize = 50;
    int counter = 0;
    accumulator_set<double, stats<tag::rolling_sum>> fps_acc;
    accumulator_set<double, stats<tag::rolling_sum>> latency_acc;
    std::chrono::time_point<std::chrono::system_clock> starttime;

public:
    TemporalStats() : fps_acc(tag::rolling_window::window_size = accumulatorWindowSize),
                      latency_acc(tag::rolling_window::window_size = accumulatorWindowSize),
                      starttime(std::chrono::system_clock::now())
    {}
    void operator()(GazeHypsPtr gazehyps) {
        auto tnow = chrono::high_resolution_clock::now();
        auto mcs = chrono::duration_cast<std::chrono::microseconds> (tnow - starttime);
        auto lat = chrono::duration_cast<std::chrono::microseconds>(tnow - gazehyps->frameTime);
        starttime = tnow;
        fps_acc(mcs.count());
        latency_acc(lat.count());
        gazehyps->frameCounter = counter;
        if (counter > accumulatorWindowSize) {
            gazehyps->fps = 1e6*min(accumulatorWindowSize, counter) / rolling_sum(fps_acc);
            gazehyps->latency = rolling_sum(latency_acc) / (1e3*accumulatorWindowSize);
        }
        counter++;
    }
    void printStats(GazeHypsPtr gazehyps) {
        if (gazehyps->frameCounter % 10 == 0)  {
            cerr << "fps: " << round(gazehyps->fps) << " | lat: " << round(gazehyps->latency)
                 << " | cnt: " << gazehyps->frameCounter << endl;
        }
    }
};

WorkerThread::WorkerThread(QObject *parent) :
    QObject(parent)
{
}

std::unique_ptr<ImageProvider> WorkerThread::getImageProvider() {
    std::unique_ptr<ImageProvider> imgProvider;
    if (inputType == "port") {
#ifdef ENABLE_YARP_SUPPORT
        imgProvider.reset(new YarpImageProvider(inputParam));
#else
        throw runtime_error("yarp support not enabled");
#endif
    } else if (inputType == "camera") {
        imgProvider.reset(new CvVideoImageProvider(boost::lexical_cast<int>(inputParam), inputSize, desiredFps));
    } else if (inputType == "video") {
        imgProvider.reset(new CvVideoImageProvider(inputParam, inputSize));
    } else if (inputType == "batch") {
        imgProvider.reset(new BatchImageProvider(inputParam));
    } else if (inputType == "image") {
        vector<string> filenames;
        filenames.push_back(inputParam);
        imgProvider.reset(new BatchImageProvider(filenames));
    } else {
        throw runtime_error("invalid input type " + inputType);
    }
    return imgProvider;
}

void WorkerThread::normalizeMat(const cv::Mat& in, cv::Mat& out) {
    cv::Scalar avg, sdv;
    cv::meanStdDev(in, avg, sdv);
    sdv.val[0] = sqrt(in.cols*in.rows*sdv.val[0]*sdv.val[0]);
    in.convertTo(out, CV_64FC1, 1/sdv.val[0], -avg.val[0]/sdv.val[0]);
}

void WorkerThread::dumpPpm(ofstream& fout, const cv::Mat& frame) {
    if (fout.is_open()) {
        vector<uchar> buff;
        cv::imencode(".pgm", frame, buff);
        fout.write((char*)&buff.front(), buff.size());
    }
}

void WorkerThread::writeEstHeader(ofstream& fout) {
    fout << "Frame" << "\t"
         << "Id" << "\t"
         << "Label" << "\t"
         << "Lid" << "\t"
         << "HorizGaze" << "\t"
         << "VertGaze" << "\t"
         << "MutualGaze"
         << endl;
}

void WorkerThread::dumpEst(ofstream& fout, GazeHypsPtr gazehyps) {
    if (fout.is_open()) {
        double lid = std::nan("not set");
        double gazeest = std::nan("not set");
        double vertest = std::nan("not set");
        bool mutgaze = false;
        if (gazehyps->size()) {
            GazeHyp& ghyp = gazehyps->hyps(0);
            lid = ghyp.eyeLidClassification.get_value_or(lid);
            gazeest = ghyp.horizontalGazeEstimation.get_value_or(gazeest);
            vertest = ghyp.verticalGazeEstimation.get_value_or(vertest);
            mutgaze = ghyp.isMutualGaze.get_value_or(false);
        }
        fout << gazehyps->frameCounter << "\t"
             << gazehyps->id << "\t"
             << gazehyps->label << "\t"
             << lid << "\t"
             << gazeest << "\t"
             << vertest << "\t"
             << mutgaze
             << endl;
    }
}

void WorkerThread::stop() {
    shouldStop = true;
}

void WorkerThread::setHorizGazeTolerance(double tol)
{
    horizGazeTolerance = tol;
}

void WorkerThread::setVerticalGazeTolerance(double tol)
{
    verticalGazeTolerance = tol;
}

void WorkerThread::setSmoothing(bool enabled)
{
    smoothingEnabled = enabled;
}

void WorkerThread::interpretHyp(GazeHyp& ghyp) {
    double lidclass = ghyp.eyeLidClassification.get_value_or(0);
    if (ghyp.eyeLidClassification.is_initialized()) {
        ghyp.isLidClosed = (lidclass > 0.7);
    }
    if (ghyp.mutualGazeClassification.is_initialized()) {
        ghyp.isMutualGaze = (ghyp.mutualGazeClassification.get() > 0) && !ghyp.isLidClosed.get_value_or(false);
    }
    if (ghyp.horizontalGazeEstimation.is_initialized()) {
        ghyp.isMutualGaze = ghyp.isMutualGaze.get_value_or(true)
             && (abs(ghyp.horizontalGazeEstimation.get()) < horizGazeTolerance);
    }
    if (ghyp.verticalGazeEstimation.is_initialized()) {
        ghyp.isMutualGaze = ghyp.isMutualGaze.get_value_or(true)
             && (abs(ghyp.verticalGazeEstimation.get()) < verticalGazeTolerance);
    }
    if (ghyp.isMutualGaze.is_initialized()) {
        ghyp.isMutualGaze = ghyp.isMutualGaze.get() && !ghyp.isLidClosed.get_value_or(false);
    }
}

template<typename T>
static void tryLoadModel(T& learner, const string& filename) {
    try {
        if (!filename.empty())
            learner.loadClassifier(filename);
    } catch (dlib::serialization_error &e) {
        cerr << filename << ":" << e.what() << endl;
    }
}

void WorkerThread::process() {
    MutualGazeLearner glearner(trainingParameters);
    RelativeGazeLearner rglearner(trainingParameters);
    EyeLidLearner eoclearner(trainingParameters);
    RelativeEyeLidLearner rellearner(trainingParameters);
    VerticalGazeLearner vglearner(trainingParameters);
    tryLoadModel(glearner, classifyGaze);
    tryLoadModel(eoclearner, classifyLid);
    tryLoadModel(rglearner, estimateGaze);
    tryLoadModel(rellearner, estimateLid);
    tryLoadModel(vglearner, estimateVerticalGaze);
    emit statusmsg("Setting up detector threads...");
    std::unique_ptr<ImageProvider> imgProvider(getImageProvider());
    FaceDetectionWorker faceworker(std::move(imgProvider), threadcount);
    ShapeDetectionWorker shapeworker(faceworker.hypsqueue(), modelfile, max(1, threadcount/2));
    RegressionWorker regressionWorker(shapeworker.hypsqueue(), eoclearner, glearner, rglearner, rellearner, vglearner, max(1, threadcount));
    emit statusmsg("Detector threads started");
#ifdef ENABLE_YARP_SUPPORT
    unique_ptr<YarpSender> yarpSender;
    if (inputType == "port") {
        yarpSender.reset(new YarpSender(inputParam));
    }
#endif
    ofstream ppmout;
    if (!streamppm.empty()) {
        ppmout.open(streamppm);
    }
    ofstream estimateout;
    if (!dumpEstimates.empty()) {
        estimateout.open(dumpEstimates);
        if (estimateout.is_open()) {
            writeEstHeader(estimateout);
        } else {
            cerr << "Warning: could not open " << dumpEstimates << endl;
        }
    }
    RlsSmoother horizGazeSmoother;
    RlsSmoother vertGazeSmoother;
    RlsSmoother lidSmoother(5, 0.95, 0.09);
    emit statusmsg("Entering processing loop...");
    cerr << "Processing frames..." << endl;
    TemporalStats temporalStats;
    while(!shouldStop) {
        GazeHypsPtr gazehyps;
        try {
            gazehyps = regressionWorker.hypsqueue().peek();
            gazehyps->waitready();
        } catch(QueueInterruptedException) {
            break;
        }
        cv::Mat frame = gazehyps->frame;

        for (auto& ghyp : *gazehyps) {
            if (smoothingEnabled) {
                horizGazeSmoother.smoothValue(ghyp.horizontalGazeEstimation);
                vertGazeSmoother.smoothValue(ghyp.verticalGazeEstimation);
                lidSmoother.smoothValue(ghyp.eyeLidClassification);
            }
            interpretHyp(ghyp);
            auto& pupils = ghyp.pupils;
            auto& faceparts = ghyp.faceParts;
            faceparts.draw(frame);
            pupils.draw(frame);
            glearner.visualize(ghyp);
            eoclearner.visualize(ghyp);
            rellearner.visualize(ghyp);
            vglearner.visualize(ghyp, verticalGazeTolerance);
            rglearner.visualize(ghyp, horizGazeTolerance);
            if (!trainLid.empty()) eoclearner.accumulate(ghyp);
            if (!trainGaze.empty()) glearner.accumulate(ghyp);
            if (!trainGazeEstimator.empty()) rglearner.accumulate(ghyp);
            if (!trainLidEstimator.empty()) rellearner.accumulate(ghyp);
            if (!trainVerticalGazeEstimator.empty()) vglearner.accumulate(ghyp);
        }
        temporalStats(gazehyps);
        dumpPpm(ppmout, frame);
        dumpEst(estimateout, gazehyps);
        if (showstats) temporalStats.printStats(gazehyps);
#ifdef ENABLE_YARP_SUPPORT
        if (yarpSender) yarpSender->sendGazeHypotheses(gazehyps);
#endif
        emit imageProcessed(gazehyps);
        QCoreApplication::processEvents();
        if (limitFps > 0) {
            usleep(1e6/limitFps);
        }
        regressionWorker.hypsqueue().pop();
    }
    regressionWorker.hypsqueue().interrupt();
    regressionWorker.wait();
    cerr << "Frames processed..." << endl;
    if (glearner.sampleCount() > 0) {
        glearner.train(trainGaze);
    }
    if (eoclearner.sampleCount() > 0) {
        eoclearner.train(trainLid);
    }
    if (vglearner.sampleCount() > 0) {
        vglearner.train(trainVerticalGazeEstimator);
    }
    if (rglearner.sampleCount() > 0) {
        rglearner.train(trainGazeEstimator);
    }
    if (rellearner.sampleCount() > 0) {
        rellearner.train(trainLidEstimator);
    }
    emit finished();
    cerr << "Primary worker thread finished processing" << endl;
}


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

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "Lookup/lookup.hpp"
#include "Utils/all_utils.hpp"

#ifndef NETKET_RBM_SPIN_HPP
#define NETKET_RBM_SPIN_HPP

namespace netket {

/** Restricted Boltzmann machine class with spin 1/2 hidden units.
 *
 */
template <typename T>
class RbmSpin : public AbstractMachine<T> {
  using VectorType = typename AbstractMachine<T>::VectorType;
  using MatrixType = typename AbstractMachine<T>::MatrixType;

  // number of visible units
  int nv_;

  // number of hidden units
  int nh_;

  // number of parameters
  int npar_;

  // weights
  MatrixType W_;

  // visible units bias
  VectorType a_;

  // hidden units bias
  VectorType b_;

  VectorType thetas_;
  VectorType lnthetas_;
  VectorType thetasnew_;
  VectorType lnthetasnew_;

  bool usea_;
  bool useb_;

  int mynode_;

  const Hilbert &hilbert_;

 public:
  using StateType = typename AbstractMachine<T>::StateType;
  using LookupType = typename AbstractMachine<T>::LookupType;

  // constructor
  explicit RbmSpin(const Hilbert &hilbert, const json &pars)
      : nv_(hilbert.Size()), hilbert_(hilbert) {
    from_json(pars);
  }

  void Init() {
    W_.resize(nv_, nh_);
    a_.resize(nv_);
    b_.resize(nh_);

    thetas_.resize(nh_);
    lnthetas_.resize(nh_);
    thetasnew_.resize(nh_);
    lnthetasnew_.resize(nh_);

    npar_ = nv_ * nh_;

    if (usea_) {
      npar_ += nv_;
    } else {
      a_.setZero();
    }

    if (useb_) {
      npar_ += nh_;
    } else {
      b_.setZero();
    }

    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    if (mynode_ == 0) {
      std::cout << "# RBM Initizialized with nvisible = " << nv_
                << " and nhidden = " << nh_ << std::endl;
      std::cout << "# Using visible bias = " << usea_ << std::endl;
      std::cout << "# Using hidden bias  = " << useb_ << std::endl;
    }
  }

  int Nvisible() const override { return nv_; }

  int Nhidden() const { return nh_; }

  int Npar() const override { return npar_; }

  void InitRandomPars(int seed, double sigma) override {
    VectorType par(npar_);

    netket::RandomGaussian(par, seed, sigma);

    SetParameters(par);
  }

  void InitLookup(const Eigen::VectorXd &v, LookupType &lt) override {
    if (lt.VectorSize() == 0) {
      lt.AddVector(b_.size());
    }
    if (lt.V(0).size() != b_.size()) {
      lt.V(0).resize(b_.size());
    }

    lt.V(0) = (W_.transpose() * v + b_);
  }

  void UpdateLookup(const Eigen::VectorXd &v, const std::vector<int> &tochange,
                    const std::vector<double> &newconf,
                    LookupType &lt) override {
    if (tochange.size() != 0) {
      for (std::size_t s = 0; s < tochange.size(); s++) {
        const int sf = tochange[s];
        lt.V(0) += W_.row(sf) * (newconf[s] - v(sf));
      }
    }
  }

  VectorType DerLog(const Eigen::VectorXd &v) override {
    VectorType der(npar_);

    int k = 0;

    if (usea_) {
      for (; k < nv_; k++) {
        der(k) = v(k);
      }
    }

    RbmSpin::tanh(W_.transpose() * v + b_, lnthetas_);

    if (useb_) {
      for (int p = 0; p < nh_; p++) {
        der(k) = lnthetas_(p);
        k++;
      }
    }

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nh_; j++) {
        der(k) = lnthetas_(j) * v(i);
        k++;
      }
    }
    return der;
  }

  VectorType GetParameters() override {
    VectorType pars(npar_);

    int k = 0;

    if (usea_) {
      for (; k < nv_; k++) {
        pars(k) = a_(k);
      }
    }

    if (useb_) {
      for (int p = 0; p < nh_; p++) {
        pars(k) = b_(p);
        k++;
      }
    }

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nh_; j++) {
        pars(k) = W_(i, j);
        k++;
      }
    }

    return pars;
  }

  void SetParameters(const VectorType &pars) override {
    int k = 0;

    if (usea_) {
      for (; k < nv_; k++) {
        a_(k) = pars(k);
      }
    }

    if (useb_) {
      for (int p = 0; p < nh_; p++) {
        b_(p) = pars(k);
        k++;
      }
    }

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nh_; j++) {
        W_(i, j) = pars(k);
        k++;
      }
    }
  }

  // Value of the logarithm of the wave-function
  T LogVal(const Eigen::VectorXd &v) override {
    RbmSpin::lncosh(W_.transpose() * v + b_, lnthetas_);

    return (v.dot(a_) + lnthetas_.sum());
  }

  // Value of the logarithm of the wave-function
  // using pre-computed look-up tables for efficiency
  T LogVal(const Eigen::VectorXd &v, LookupType &lt) override {
    RbmSpin::lncosh(lt.V(0), lnthetas_);

    return (v.dot(a_) + lnthetas_.sum());
  }

  // Difference between logarithms of values, when one or more visible variables
  // are being flipped
  VectorType LogValDiff(
      const Eigen::VectorXd &v, const std::vector<std::vector<int>> &tochange,
      const std::vector<std::vector<double>> &newconf) override {
    const std::size_t nconn = tochange.size();
    VectorType logvaldiffs = VectorType::Zero(nconn);

    thetas_ = (W_.transpose() * v + b_);
    RbmSpin::lncosh(thetas_, lnthetas_);

    T logtsum = lnthetas_.sum();

    for (std::size_t k = 0; k < nconn; k++) {
      if (tochange[k].size() != 0) {
        thetasnew_ = thetas_;

        for (std::size_t s = 0; s < tochange[k].size(); s++) {
          const int sf = tochange[k][s];

          logvaldiffs(k) += a_(sf) * (newconf[k][s] - v(sf));

          thetasnew_ += W_.row(sf) * (newconf[k][s] - v(sf));
        }

        RbmSpin::lncosh(thetasnew_, lnthetasnew_);
        logvaldiffs(k) += lnthetasnew_.sum() - logtsum;
      }
    }
    return logvaldiffs;
  }

  // Difference between logarithms of values, when one or more visible variables
  // are being flipped Version using pre-computed look-up tables for efficiency
  // on a small number of spin flips
  T LogValDiff(const Eigen::VectorXd &v, const std::vector<int> &tochange,
               const std::vector<double> &newconf,
               const LookupType &lt) override {
    T logvaldiff = 0.;

    if (tochange.size() != 0) {
      RbmSpin::lncosh(lt.V(0), lnthetas_);

      thetasnew_ = lt.V(0);

      for (std::size_t s = 0; s < tochange.size(); s++) {
        const int sf = tochange[s];

        logvaldiff += a_(sf) * (newconf[s] - v(sf));

        thetasnew_ += W_.row(sf) * (newconf[s] - v(sf));
      }

      RbmSpin::lncosh(thetasnew_, lnthetasnew_);
      logvaldiff += (lnthetasnew_.sum() - lnthetas_.sum());
    }
    return logvaldiff;
  }

  inline static double lncosh(double x) {
    const double xp = std::abs(x);
    if (xp <= 12.) {
      return std::log(std::cosh(xp));
    } else {
      const static double log2v = std::log(2.);
      return xp - log2v;
    }
  }

  // ln(cos(x)) for std::complex argument
  // the modulus is computed by means of the previously defined function
  // for real argument
  inline static std::complex<double> lncosh(std::complex<double> x) {
    const double xr = x.real();
    const double xi = x.imag();

    std::complex<double> res = RbmSpin::lncosh(xr);
    res += std::log(
        std::complex<double>(std::cos(xi), std::tanh(xr) * std::sin(xi)));

    return res;
  }

  static void tanh(const VectorType &x, VectorType &y) {
    assert(y.size() >= x.size());
    y = Eigen::tanh(x.array());
  }

  static void lncosh(const VectorType &x, VectorType &y) {
    assert(y.size() >= x.size());
    for (int i = 0; i < x.size(); i++) {
      y(i) = lncosh(x(i));
    }
  }

  const Hilbert &GetHilbert() const { return hilbert_; }

  void to_json(json &j) const override {
    j["Machine"]["Name"] = "RbmSpin";
    j["Machine"]["Nvisible"] = nv_;
    j["Machine"]["Nhidden"] = nh_;
    j["Machine"]["UseVisibleBias"] = usea_;
    j["Machine"]["UseHiddenBias"] = useb_;
    j["Machine"]["a"] = a_;
    j["Machine"]["b"] = b_;
    j["Machine"]["W"] = W_;
  }

  void from_json(const json &pars) override {
    if (pars.at("Machine").at("Name") != "RbmSpin") {
      if (mynode_ == 0) {
        std::cerr << "# Error while constructing RbmSpin from Json input"
                  << std::endl;
      }
      std::abort();
    }

    if (FieldExists(pars["Machine"], "Nvisible")) {
      nv_ = pars["Machine"]["Nvisible"];
    }
    if (nv_ != hilbert_.Size()) {
      if (mynode_ == 0) {
        std::cerr << "# Number of visible units is incompatible with given "
                     "Hilbert space"
                  << std::endl;
      }
      std::abort();
    }

    if (FieldExists(pars["Machine"], "Nhidden")) {
      nh_ = FieldVal(pars["Machine"], "Nhidden");
    } else {
      nh_ = nv_ * double(FieldVal(pars["Machine"], "Alpha"));
    }

    usea_ = FieldOrDefaultVal(pars["Machine"], "UseVisibleBias", true);
    useb_ = FieldOrDefaultVal(pars["Machine"], "UseHiddenBias", true);

    Init();

    // Loading parameters, if defined in the input
    if (FieldExists(pars["Machine"], "a")) {
      a_ = pars["Machine"]["a"];
    } else {
      a_.setZero();
    }

    if (FieldExists(pars["Machine"], "b")) {
      b_ = pars["Machine"]["b"];
    } else {
      b_.setZero();
    }
    if (FieldExists(pars["Machine"], "W")) {
      W_ = pars["Machine"]["W"];
    }
  }
};

}  // namespace netket

#endif


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "stuck-dye-hmm.h"

// Standard C++ library headers:
#include <cmath>
#include <vector>

// Local project headers:
#include "common/error-model.h"
#include "common/radiometry.h"
#include "hmm/fit/error-model-fitter.h"
#include "hmm/precomputations/radiometry-precomputations.h"
#include "hmm/precomputations/universal-precomputations.h"
#include "hmm/state-vector/stuck-dye-state-vector.h"
#include "hmm/step/step.h"
#include "hmm/step/stuck-dye-emission.h"
#include "hmm/step/stuck-dye-transition.h"

namespace {
using boost::unit_test::tolerance;
using std::log;
using std::vector;
const double TOL = 0.000000001;
}  // namespace

namespace whatprot {

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(stuck_dye_hmm_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    double p_edman_failure = 0.01;
    double p_detach = 0.02;
    double p_bleach = 0.03;
    double p_dud = 0.04;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = 0.05;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    int num_channels = 2;
    int max_num_dyes = 3;
    UniversalPrecomputations universal_precomputations(em, num_channels);
    universal_precomputations.set_max_num_dyes(max_num_dyes);
    int num_timesteps = 4;
    Radiometry r(num_timesteps, num_channels);
    r(0, 0) = 1.0;
    r(0, 1) = 1.0;
    r(1, 0) = 1.0;
    r(1, 1) = 1.0;
    r(2, 0) = 1.0;
    r(2, 1) = 1.0;
    r(3, 0) = 1.0;
    r(3, 1) = 1.0;
    RadiometryPrecomputations radiometry_precomputations(r, em, max_num_dyes);
    int channel = 0;
    StuckDyeHMM hmm(num_timesteps,
                    num_channels,
                    channel,
                    radiometry_precomputations,
                    universal_precomputations);
    BOOST_ASSERT(hmm.steps.size() == 2 * num_timesteps - 1);
    vector<const Step<StuckDyeStateVector>*>::iterator step = hmm.steps.begin();
    BOOST_TEST(*step
               == &radiometry_precomputations.stuck_dye_emissions[channel]);
    step++;
    BOOST_TEST(*step == &universal_precomputations.stuck_dye_transition);
    step++;
    BOOST_TEST(*step
               == &radiometry_precomputations.stuck_dye_emissions[channel]);
    step++;
    BOOST_TEST(*step == &universal_precomputations.stuck_dye_transition);
    step++;
    BOOST_TEST(*step
               == &radiometry_precomputations.stuck_dye_emissions[channel]);
    step++;
    BOOST_TEST(*step == &universal_precomputations.stuck_dye_transition);
    step++;
    BOOST_TEST(*step
               == &radiometry_precomputations.stuck_dye_emissions[channel]);
}

BOOST_AUTO_TEST_CASE(probability_trivial_test, *tolerance(TOL)) {
    double p_edman_failure = 0.01;
    double p_detach = 0.02;
    double p_bleach = 0.03;
    double p_dud = 0.04;
    DistributionType dist_type = DistributionType::OVERRIDE;
    double mu = 1.0;
    double sigma = 0.05;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    int num_channels = 2;
    int max_num_dyes = 3;
    UniversalPrecomputations universal_precomputations(em, num_channels);
    universal_precomputations.set_max_num_dyes(max_num_dyes);
    int num_timesteps = 1;
    Radiometry r(num_timesteps, num_channels);
    RadiometryPrecomputations radiometry_precomputations(r, em, max_num_dyes);
    int channel = 0;
    StuckDyeHMM hmm(num_timesteps,
                    num_channels,
                    channel,
                    radiometry_precomputations,
                    universal_precomputations);
    BOOST_TEST(hmm.probability() == 1.0);
}

BOOST_AUTO_TEST_CASE(probability_sum_to_one_test, *tolerance(TOL)) {
    // The idea here is that if the emission just causes a multiplication by one
    // no matter what, then the transitions shouldn't matter; they should just
    // be reallocating a probability of one between different states.
    double p_edman_failure = 0.01;
    double p_detach = 0.02;
    double p_bleach = 0.03;
    double p_dud = 0.04;
    DistributionType dist_type = DistributionType::OVERRIDE;
    double mu = 1.0;
    double sigma = 0.05;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    int num_channels = 2;
    int max_num_dyes = 3;
    UniversalPrecomputations universal_precomputations(em, num_channels);
    universal_precomputations.set_max_num_dyes(max_num_dyes);
    int num_timesteps = 3;
    Radiometry r(num_timesteps, num_channels);
    r(0, 0) = 0.0;
    r(0, 1) = 0.1;
    r(1, 0) = 1.0;
    r(1, 1) = 1.1;
    r(2, 0) = 2.0;
    r(2, 1) = 2.1;
    RadiometryPrecomputations radiometry_precomputations(r, em, max_num_dyes);
    int channel = 0;
    StuckDyeHMM hmm(num_timesteps,
                    num_channels,
                    channel,
                    radiometry_precomputations,
                    universal_precomputations);
    BOOST_TEST(hmm.probability() == 1.0);
}

BOOST_AUTO_TEST_CASE(probability_more_involved_test, *tolerance(TOL)) {
    // This is essentially a "no change" test. It assumes that the function was
    // giving the correct result on April 7, 2021.
    double p_edman_failure = 0.01;
    double p_detach = 0.02;
    double p_bleach = 0.03;
    double p_dud = 0.04;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = 0.05;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    int num_channels = 2;
    int max_num_dyes = 3;
    UniversalPrecomputations universal_precomputations(em, num_channels);
    universal_precomputations.set_max_num_dyes(max_num_dyes);
    int num_timesteps = 3;
    Radiometry r(num_timesteps, num_channels);
    r(0, 0) = 1.0;
    r(0, 1) = 0.0;
    r(1, 0) = 1.0;
    r(1, 1) = 0.0;
    r(2, 0) = 0.0;
    r(2, 1) = 0.0;
    RadiometryPrecomputations radiometry_precomputations(r, em, max_num_dyes);
    int channel = 0;
    StuckDyeHMM hmm(num_timesteps,
                    num_channels,
                    channel,
                    radiometry_precomputations,
                    universal_precomputations);
    BOOST_TEST(hmm.probability() == 4.6855215246253996);
}

BOOST_AUTO_TEST_CASE(improve_fit_test, *tolerance(TOL)) {
    // This is essentially a "no change" test. It assumes that the function was
    // giving the correct result on April 7, 2021.
    double p_edman_failure = 0.01;
    double p_detach = 0.02;
    double p_bleach = 0.03;
    double p_dud = 0.04;
    DistributionType dist_type = DistributionType::LOGNORMAL;
    double mu = log(1.0);
    double sigma = 0.05;
    double stuck_dye_ratio = 0.5;
    double p_stuck_dye_loss = 0.08;
    ErrorModel em(p_edman_failure,
                  p_detach,
                  p_bleach,
                  p_dud,
                  dist_type,
                  mu,
                  sigma,
                  stuck_dye_ratio,
                  p_stuck_dye_loss);
    int num_channels = 2;
    int max_num_dyes = 3;
    UniversalPrecomputations universal_precomputations(em, num_channels);
    universal_precomputations.set_max_num_dyes(max_num_dyes);
    int num_timesteps = 3;
    Radiometry r(num_timesteps, num_channels);
    r(0, 0) = 1.0;
    r(0, 1) = 0.0;
    r(1, 0) = 1.0;
    r(1, 1) = 0.0;
    r(2, 0) = 0.0;
    r(2, 1) = 0.0;
    RadiometryPrecomputations radiometry_precomputations(r, em, max_num_dyes);
    int channel = 0;
    StuckDyeHMM hmm(num_timesteps,
                    num_channels,
                    channel,
                    radiometry_precomputations,
                    universal_precomputations);
    ErrorModelFitter emf;
    hmm.improve_fit(&emf);
    // There are no BOOST_TEST statements because setting up a proper test for
    // this function is very difficult. We still have the test though as a no
    // crash test.
}

BOOST_AUTO_TEST_SUITE_END()  // stuck_dye_hmm_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


#include <boost/accumulators/statistics/weighted_tail_variate_means.hpp>


#include "munin/aligned_layout.hpp"
#include "munin/container.hpp"
#include <boost/range/numeric.hpp>
#include <algorithm>

namespace munin {

namespace {

// ==========================================================================
// CALCULATE_COMPONENT_WIDTH
// ==========================================================================
terminalpp::coordinate_type calculate_component_width(
    terminalpp::coordinate_type container_width,
    terminalpp::coordinate_type component_preferred_width,
    horizontal_alignment alignment)
{
    switch (alignment)
    {
    case horizontal_alignment::fill:
        return container_width;

    default:
        return std::min(container_width, component_preferred_width);
    }
}

// ==========================================================================
// CALCULATE_COMPONENT_HEIGHT
// ==========================================================================
terminalpp::coordinate_type calculate_component_height(
    terminalpp::coordinate_type container_height,
    terminalpp::coordinate_type component_preferred_height,
    vertical_alignment alignment)
{
    switch (alignment)
    {
        case vertical_alignment::fill:
            return container_height;

        default:
            return std::min(container_height, component_preferred_height);
    }
}

// ==========================================================================
// CALCULATE_COMPONENT_SIZE
// ==========================================================================
terminalpp::extent calculate_component_size(
    terminalpp::extent container_size,
    terminalpp::extent component_preferred_size,
    alignment alignment)
{
    return {
        calculate_component_width(
            container_size.width_,
            component_preferred_size.width_,
            alignment.horizontal),
        calculate_component_height(
            container_size.height_,
            component_preferred_size.height_,
            alignment.vertical)
    };
}

// ==========================================================================
// CALCULATE_COMPONENT_X_POSITION
// ==========================================================================
terminalpp::coordinate_type calculate_component_x_position(
    terminalpp::coordinate_type container_width,
    terminalpp::coordinate_type component_width,
    horizontal_alignment alignment)
{
    switch(alignment)
    {
    case horizontal_alignment::fill :
        // Fall-through
    case horizontal_alignment::left :
        return 0;

    case horizontal_alignment::right :
        return std::max(container_width - component_width, 0);

    case horizontal_alignment::centre :
    default :
        return std::max(container_width - component_width, 0) / 2;
    }
}

// ==========================================================================
// CALCULATE_COMPONENT_Y_POSITION
// ==========================================================================
terminalpp::coordinate_type calculate_component_y_position(
    terminalpp::coordinate_type container_height,
    terminalpp::coordinate_type component_height,
    vertical_alignment alignment)
{
    switch(alignment)
    {
    case vertical_alignment::fill :
        // Fall-through
    case vertical_alignment::top :
        return 0;

    case vertical_alignment::bottom :
        return std::max(container_height - component_height, 0);

    case vertical_alignment::centre :
    default :
        return std::max(container_height - component_height, 0) / 2;
    }
}

// ==========================================================================
// CALCULATE_COMPONENT_POSITION
// ==========================================================================
terminalpp::point calculate_component_position(
    terminalpp::extent container_size,
    terminalpp::extent component_size,
    alignment component_alignment)
{
    return {
        calculate_component_x_position(
            container_size.width_,
            component_size.width_,
            component_alignment.horizontal),
        calculate_component_y_position(
            container_size.height_,
            component_size.height_,
            component_alignment.vertical),
    };
}

// ==========================================================================
// CALCULATE_ALIGNMENT
// ==========================================================================
alignment calculate_alignment(boost::any const &hint)
{
    auto const *alignment_hint =
        boost::any_cast<alignment>(&hint);

    // By default, components are centre-aligned.
    return alignment_hint != nullptr
         ? *alignment_hint
         : alignment {
               horizontal_alignment::centre,
               vertical_alignment::centre
           };
}

}

// ==========================================================================
// DO_GET_PREFERRED_SIZE
// ==========================================================================
terminalpp::extent aligned_layout::do_get_preferred_size(
    std::vector<std::shared_ptr<component>> const &components,
    std::vector<boost::any>                 const &hints) const
{
    // The preferred size of this component is the largest preferred
    // extents of all components.
    return boost::accumulate(
        components,
        terminalpp::extent{0, 0},
        [](terminalpp::extent max, std::shared_ptr<component> const &comp)
        {
            auto const preferred_size = comp->get_preferred_size();

            return terminalpp::extent {
                std::max(preferred_size.width_, max.width_),
                std::max(preferred_size.height_, max.height_)
            };
        });
}

// ==========================================================================
// DO_LAYOUT
// ==========================================================================
void aligned_layout::do_layout(
    std::vector<std::shared_ptr<component>> const &components,
    std::vector<boost::any>                 const &hints,
    terminalpp::extent                             size) const
{
    for (uint32_t index = 0; index < components.size(); ++index)
    {
        auto const &comp = components[index];
        auto const &hint = hints[index];
        auto const &alignment = calculate_alignment(hint);

        auto const &component_size = calculate_component_size(
            size, comp->get_preferred_size(), alignment);

        auto const &component_position = calculate_component_position(
            size, component_size, alignment);

        comp->set_position(component_position);
        comp->set_size(component_size);
    }
}

// ==========================================================================
// DO_TO_JSON
// ==========================================================================
nlohmann::json aligned_layout::do_to_json() const
{
    return {
        { "type", "aligned_layout" }
    };
}

// ==========================================================================
// MAKE_ALIGNED_LAYOUT
// ==========================================================================
std::unique_ptr<layout> make_aligned_layout()
{
    return std::unique_ptr<layout>(new aligned_layout);
}

}


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

// this program takes matches interest points from the first image in the
// bag to all other interest points. It then prints out the number
// of interest points vs. the difference in position and orientation on all axes

#include <common/init.h>
#include <common/utils.h>
#include <config_reader/config_reader.h>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/PoseStamped.h>
#include <image_transport/image_transport.h>
#include <interest_point/matching.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <gflags/gflags.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>

Eigen::Vector3f QuatToEuler(const Eigen::Quaternionf & q) {
  Eigen::Vector3f euler;
  float q2q2 = q.y() * q.y();
  euler.x() = atan2(2 * (q.x() * q.w() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q2q2));
  float arg = std::max(-1.0f, std::min(1.0f, 2 * (q.y() * q.w() - q.x() * q.z())));
  euler.y() = asin(arg);
  euler.z() = atan2(2 * (q.x() * q.y() + q.z() * q.w()), 1 - 2 * (q2q2 + q.z() * q.z()));
  return euler;
}

void WriteResults(FILE* f, int matches, const Eigen::Vector3f & p, const Eigen::Quaternionf & q) {
  if (f == NULL)
    return;
  Eigen::Vector3f euler = QuatToEuler(q);
  fprintf(f, "%d ", matches);
  fprintf(f, "%g %g %g ", p.x(), p.y(), p.z());
  fprintf(f, "%g %g %g\n", euler.x(), euler.y(), euler.z());
}

void ReadParams(interest_point::FeatureDetector* detector) {
  config_reader::ConfigReader config;
  config.AddFile("localization.config");
  if (!config.ReadFiles()) {
    ROS_ERROR("Failed to read config files.");
    return;
  }

  int min_features, max_features, detection_retries;
  double min_brisk_threshold, default_brisk_threshold, max_brisk_threshold;
  if (!config.GetInt("min_features", &min_features))
    ROS_FATAL("min_features not specified in localization.");
  if (!config.GetInt("max_features", &max_features))
    ROS_FATAL("max_features not specified in localization.");
  if (!config.GetInt("detection_retries", &detection_retries))
    ROS_FATAL("detection_retries not specified in localization.");

  // For the brisk thresholds, quietly assume some defaults
  if (!config.GetReal("min_brisk_threshold", &min_brisk_threshold))
    min_brisk_threshold = 80.0;
  if (!config.GetReal("default_brisk_threshold", &default_brisk_threshold))
    max_brisk_threshold = 100.0;
  if (!config.GetReal("max_brisk_threshold", &max_brisk_threshold))
    max_brisk_threshold = 200.0;

  detector->Reset("ORGBRISK", min_features, max_features, detection_retries,
                  min_brisk_threshold, default_brisk_threshold, max_brisk_threshold);
}

void DetectImageFeatures(interest_point::FeatureDetector & detector, sensor_msgs::ImageConstPtr & image_msg,
                         cv::Mat* description) {
  std::vector<cv::KeyPoint> keypoints;
  cv_bridge::CvImageConstPtr image;
  try {
    image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8);
  } catch (cv_bridge::Exception& e) {
    ROS_FATAL("cv_bridge exception: %s", e.what());
    return;
  }
  detector.Detect(image->image, &keypoints, description);
}

void InterpolateGroundTruth(const geometry_msgs::PoseStampedConstPtr & last_gt,
                            const geometry_msgs::PoseStampedConstPtr & next_gt,
                            const ros::Time & last_gt_time,
                            const ros::Time & next_gt_time, const ros::Time & image_time,
                            Eigen::Quaternionf * q, Eigen::Vector3f * p) {
  Eigen::Quaternionf q_prev(last_gt->pose.orientation.w, last_gt->pose.orientation.x,
                            last_gt->pose.orientation.y, last_gt->pose.orientation.z);
  Eigen::Quaternionf q_next(next_gt->pose.orientation.w, next_gt->pose.orientation.x,
                            next_gt->pose.orientation.y, next_gt->pose.orientation.z);
  Eigen::Vector3f p_prev(last_gt->pose.position.x, last_gt->pose.position.y, last_gt->pose.position.z);
  Eigen::Vector3f p_next(next_gt->pose.position.x, next_gt->pose.position.y, next_gt->pose.position.z);
  float u = (image_time - last_gt_time).toSec() / (next_gt_time - last_gt_time).toSec();
  *q = q_prev.slerp(u, q_next);
  *p = p_prev + u * (p_next - p_prev);
}

int main(int argc, char ** argv) {
  common::InitFreeFlyerApplication(&argc, &argv);

  if (argc < 2) {
    LOG(INFO) << "Usage: " << argv[0] << " bag.bag";
    exit(0);
  }

  rosbag::Bag bag;
  bag.open(argv[1], rosbag::bagmode::Read);

  interest_point::FeatureDetector detector("ORGBRISK");
  ReadParams(&detector);

  FILE* f = NULL;
  std::string name = argv[1];
  name.erase(name.length() - 3);
  name = name + "txt";
  f = fopen(name.c_str(), "w");

  std::vector<std::string> image_topics, truth_topics;
  image_topics.push_back("/hw/cam_nav");
  truth_topics.push_back("/loc/ground_truth");
  rosbag::View image_view(bag, rosbag::TopicQuery(image_topics));
  rosbag::View truth_view(bag, rosbag::TopicQuery(truth_topics));

  rosbag::View::iterator it_gt = truth_view.begin();
  rosbag::View::iterator it_image = image_view.begin();

  geometry_msgs::PoseStampedConstPtr last_gt = (*it_gt).instantiate<geometry_msgs::PoseStamped>();
  ros::Time last_gt_time = (*it_gt).getTime();
  it_gt++;

  bool first = true;

  cv::Mat description_original;

  Eigen::Quaternionf start_q(1, 0, 0, 0);
  Eigen::Vector3f start_p(0, 0, 0);
  int progress = 0;
  while (it_gt != truth_view.end()) {
    geometry_msgs::PoseStampedConstPtr next_gt = (*it_gt).instantiate<geometry_msgs::PoseStamped>();
    ros::Time next_gt_time = (*it_gt).getTime();

    while (it_image != image_view.end() && (*it_image).getTime() < next_gt_time) {
      progress++;
      if (first && (*it_image).getTime() < last_gt_time) {
          it_image++;
          continue;
      }
      sensor_msgs::ImageConstPtr image_msg = (*it_image).instantiate<sensor_msgs::Image>();

      Eigen::Quaternionf q;
      Eigen::Vector3f p;
      InterpolateGroundTruth(last_gt, next_gt, last_gt_time, next_gt_time, (*it_image).getTime(), &q, &p);

      if (first) {
        DetectImageFeatures(detector, image_msg, &description_original);
        start_p = p;
        start_q = q;
        first = false;
      } else {
        cv::Mat description;
        DetectImageFeatures(detector, image_msg, &description);
        std::vector<cv::DMatch> matches;
        interest_point::FindMatches(description, description_original, &matches);
        WriteResults(f, matches.size(), p - start_p, q * start_q.conjugate());
      }

      common::PrintProgressBar(stdout, static_cast<float>(progress) / image_view.size());
      it_image++;
    }

    it_gt++;
    last_gt_time = next_gt_time;
    last_gt = next_gt;
  }
  bag.close();
  if (f != NULL)
    fclose(f);
  common::PrintProgressBar(stdout, 1.0);
  printf("\n");
}