/*
 * MIT License
 *
 * Copyright (c) 2018-2021 char-lie
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */
#include <boost/test/unit_test.hpp>

#include <constraint_graph.hpp>
#include <disparity_graph.hpp>
#include <image.hpp>
#include <labeling_finder.hpp>
#include <lowest_penalties.hpp>
#include <pgm_io.hpp>

BOOST_AUTO_TEST_SUITE(LabelingFinder)

using sp::graph::constraint::ConstraintGraph;
using sp::graph::disparity::DisparityGraph;
using sp::graph::lowest_penalties::LowestPenalties;
using sp::image::Image;
using sp::image::PGM_IO;
using sp::labeling::finder::calculate_minimal_consistent_threshold;
using sp::labeling::finder::fetch_available_penalties;
using sp::labeling::finder::fetch_edge_available_penalties;
using sp::labeling::finder::fetch_pixel_available_penalties;
using sp::labeling::finder::find_labeling;
using sp::types::FLOAT;

BOOST_AUTO_TEST_CASE(check_black_images)
{
    PGM_IO pgm_io;
    std::istringstream image_content{R"image(
    P2
    3 2
    2
    0 0 0
    0 0 0
    )image"};

    image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{image, image, 2, 1, 1};
    struct LowestPenalties lowest_penalties{&disparity_graph};
    auto pixel_penalties = fetch_pixel_available_penalties(
        &disparity_graph,
        {0, 0},
        0
    );
    BOOST_CHECK_EQUAL(pixel_penalties.size(), 1);
    BOOST_CHECK_CLOSE(pixel_penalties[0], 0, 1);

    auto edge_penalties = fetch_edge_available_penalties(
        &disparity_graph,
        {{{0, 0}, 0}, {{0, 1}, 0}},
        0
    );
    BOOST_CHECK_EQUAL(edge_penalties.size(), 2);
    BOOST_CHECK_CLOSE(edge_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(edge_penalties[1], 1, 1);

    auto available_penalties = fetch_available_penalties(&lowest_penalties);
    BOOST_CHECK_EQUAL(available_penalties.size(), 2);
    BOOST_CHECK_CLOSE(available_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(available_penalties[1], 1, 1);

    FLOAT threshold = calculate_minimal_consistent_threshold(
        &lowest_penalties,
        &disparity_graph,
        available_penalties
    );
    BOOST_CHECK_CLOSE(threshold, 0, 1);

    struct ConstraintGraph constraint_graph{
        &disparity_graph,
        &lowest_penalties,
        threshold
    };
    BOOST_CHECK(solve_csp(&constraint_graph));
    BOOST_CHECK_EQUAL(find_labeling(&constraint_graph), &constraint_graph);
}

BOOST_AUTO_TEST_CASE(check_equal_images)
{
    PGM_IO pgm_io;
    std::istringstream image_content{R"image(
    P2
    3 2
    256
    0   127 256
    256 127 0
    )image"};

    image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{image, image, 3, 1, 1};
    struct LowestPenalties lowest_penalties{&disparity_graph};

    auto pixel_penalties = fetch_pixel_available_penalties(
        &disparity_graph,
        {1, 0},
        0
    );
    BOOST_CHECK_EQUAL(pixel_penalties.size(), 2);
    BOOST_CHECK_CLOSE(pixel_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(pixel_penalties[1], 129 * 129, 1);

    auto edge_penalties = fetch_edge_available_penalties(
        &disparity_graph,
        {{{0, 0}, 0}, {{0, 1}, 0}},
        0
    );
    BOOST_CHECK_EQUAL(edge_penalties.size(), 3);
    BOOST_CHECK_CLOSE(edge_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(edge_penalties[1], 1, 1);
    BOOST_CHECK_CLOSE(edge_penalties[2], 4, 1);

    auto available_penalties = fetch_available_penalties(&lowest_penalties);
    BOOST_CHECK_EQUAL(available_penalties.size(), 6);
    BOOST_CHECK_CLOSE(available_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(available_penalties[1], 1, 1);
    BOOST_CHECK_CLOSE(available_penalties[2], 4, 1);
    BOOST_CHECK_CLOSE(available_penalties[3], 127 * 127, 1);
    BOOST_CHECK_CLOSE(available_penalties[4], 129 * 129, 1);
    BOOST_CHECK_CLOSE(available_penalties[5], 256 * 256, 1);

    FLOAT threshold = calculate_minimal_consistent_threshold(
        &lowest_penalties,
        &disparity_graph,
        available_penalties
    );
    BOOST_CHECK_CLOSE(threshold, 0, 1);

    struct ConstraintGraph constraint_graph{
        &disparity_graph,
        &lowest_penalties,
        threshold
    };
    BOOST_CHECK(solve_csp(&constraint_graph));
    BOOST_CHECK_EQUAL(find_labeling(&constraint_graph), &constraint_graph);
}

BOOST_AUTO_TEST_CASE(basic_check)
{
    PGM_IO pgm_io;
    std::istringstream left_image_content{R"image(
    P2
    3 2
    10
    4 5 10
    0 0 0
    )image"};
    std::istringstream right_image_content{R"image(
    P2
    3 2
    10
    10 7 0
    0 0 0
    )image"};

    left_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image left_image{*pgm_io.get_image()};

    right_image_content >> pgm_io;
    BOOST_REQUIRE(pgm_io.get_image());
    struct Image right_image{*pgm_io.get_image()};

    struct DisparityGraph disparity_graph{left_image, right_image, 3, 1, 1};
    struct LowestPenalties lowest_penalties{&disparity_graph};

    auto pixel_penalties = fetch_pixel_available_penalties(
        &disparity_graph,
        {0, 0},
        0
    );
    BOOST_CHECK_EQUAL(pixel_penalties.size(), 3);
    BOOST_CHECK_CLOSE(pixel_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(pixel_penalties[1], 25, 1);
    BOOST_CHECK_CLOSE(pixel_penalties[2], 36, 1);

    auto edge_penalties = fetch_edge_available_penalties(
        &disparity_graph,
        {{{0, 0}, 0}, {{0, 1}, 0}},
        0
    );
    BOOST_CHECK_EQUAL(edge_penalties.size(), 3);
    BOOST_CHECK_CLOSE(edge_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(edge_penalties[1], 1, 1);
    BOOST_CHECK_CLOSE(edge_penalties[2], 4, 1);

    auto available_penalties = fetch_available_penalties(&lowest_penalties);
    BOOST_CHECK_EQUAL(available_penalties.size(), 6);
    BOOST_CHECK_CLOSE(available_penalties[0], 0, 1);
    BOOST_CHECK_CLOSE(available_penalties[1], 1, 1);
    BOOST_CHECK_CLOSE(available_penalties[2], 4, 1);
    BOOST_CHECK_CLOSE(available_penalties[3], 5, 1);
    BOOST_CHECK_CLOSE(available_penalties[4], 25, 1);
    BOOST_CHECK_CLOSE(available_penalties[5], 36, 1);

    FLOAT threshold = calculate_minimal_consistent_threshold(
        &lowest_penalties,
        &disparity_graph,
        available_penalties
    );
    BOOST_CHECK_CLOSE(threshold, 5, 1);

    struct ConstraintGraph constraint_graph{
        &disparity_graph,
        &lowest_penalties,
        threshold
    };
    BOOST_CHECK(solve_csp(&constraint_graph));
    BOOST_CHECK_EQUAL(find_labeling(&constraint_graph), &constraint_graph);
}

BOOST_AUTO_TEST_SUITE_END()


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"	 // Precompiled header
//
#include <Eigen/Dense>	// First! to avoid conflicts with X.h
//
#include <mrpt/img/color_maps.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/opengl/CMesh.h>
#include <mrpt/opengl/CSetOfTriangles.h>
#include <mrpt/opengl/opengl_api.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;
using mrpt::img::CImage;

IMPLEMENTS_SERIALIZABLE(CMesh, CRenderizable, mrpt::opengl)

CMesh::CMesh(
	bool enableTransparency, float m_xMin_p, float m_xMax_p, float m_yMin_p,
	float m_yMax_p)
	: m_enableTransparency(enableTransparency),
	  Z(0, 0),
	  mask(0, 0),
	  C(0, 0),
	  C_r(0, 0),
	  C_g(0, 0),
	  C_b(0, 0),
	  m_xMin(m_xMin_p),
	  m_xMax(m_xMax_p),
	  m_yMin(m_yMin_p),
	  m_yMax(m_yMax_p)
{
	enableTextureLinearInterpolation(true);
	enableLight(true);

	m_color.A = 255;
	m_color.R = 0;
	m_color.G = 0;
	m_color.B = 150;
}

CMesh::~CMesh() = default;

void CMesh::updateTriangles() const
{
	using mrpt::img::colormap;

	CRenderizable::notifyChange();

	// Remember:
	/** List of triangles in the mesh */
	// mutable
	// std::vector<std::pair<mrpt::opengl::TTriangle,TTriangleVertexIndices>
	// > actualMesh;

	/** The accumulated normals & counts for each vertex, so normals can be
	 * averaged. */
	// mutable std::vector<std::pair<mrpt::math::TPoint3D,size_t> >
	// vertex_normals;

	const auto cols = Z.cols();
	const auto rows = Z.rows();

	actualMesh.clear();
	if (cols == 0 && rows == 0) return;	 // empty mesh

	ASSERT_(cols > 0 && rows > 0);
	ASSERT_GT_(m_xMax, m_xMin);
	ASSERT_GT_(m_yMax, m_yMin);

	// we have 1 more row & col of vertices than of triangles:
	vertex_normals.assign(
		(1 + cols) * (1 + rows),
		std::pair<TPoint3D, size_t>(TPoint3D(0, 0, 0), 0));

	if (m_colorFromZ || m_isImage) updateColorsMatrix();

	bool useMask = false;
	if (mask.cols() != 0 && mask.rows() != 0)
	{
		ASSERT_(mask.cols() == cols && mask.rows() == rows);
		useMask = true;
	}
	const float sCellX = (m_xMax - m_xMin) / (rows - 1);
	const float sCellY = (m_yMax - m_yMin) / (cols - 1);

	mrpt::opengl::TTriangle tri;

	for (int iX = 0; iX < rows - 1; iX++)
		for (int iY = 0; iY < cols - 1; iY++)
		{
			if (useMask && (!mask(iX, iY) || !mask(iX + 1, iY + 1))) continue;
			tri.x(0) = m_xMin + iX * sCellX;
			tri.y(0) = m_yMin + iY * sCellY;
			tri.z(0) = Z(iX, iY);
			tri.x(2) = tri.x(0) + sCellX;
			tri.y(2) = tri.y(0) + sCellY;
			tri.z(2) = Z(iX + 1, iY + 1);

			// Vertex indices:
			TTriangleVertexIndices tvi;
			tvi.vind[0] = iX + rows * iY;
			tvi.vind[2] = (iX + 1) + rows * (iY + 1);

			// Each quadrangle has up to 2 triangles:
			//  [0]
			//   |
			//   |
			//  [1]--[2]
			// Order: 0,1,2
			if (!useMask || mask(iX + 1, iY))
			{
				tri.x(1) = tri.x(2);
				tri.y(1) = tri.y(0);
				tri.z(1) = Z(iX + 1, iY);
				// Assign alpha channel
				for (int i = 0; i < 3; i++)
					tri.a(i) = m_color.A;

				if (m_colorFromZ)
				{
					mrpt::img::TColorf col(0, 0, 0, 1);
					colormap(m_colorMap, C(iX, iY), col.R, col.G, col.B);
					tri.r(0) = f2u8(col.R);
					tri.g(0) = f2u8(col.G);
					tri.b(0) = f2u8(col.B);
					colormap(m_colorMap, C(iX + 1, iY), col.R, col.G, col.B);
					tri.r(1) = f2u8(col.R);
					tri.g(1) = f2u8(col.G);
					tri.b(1) = f2u8(col.B);
					colormap(
						m_colorMap, C(iX + 1, iY + 1), col.R, col.G, col.B);
					tri.r(2) = f2u8(col.R);
					tri.g(2) = f2u8(col.G);
					tri.b(2) = f2u8(col.B);
				}
				else if (m_isImage)
				{
					if (getTextureImage().isColor())
					{
						tri.r(0) = tri.r(1) = tri.r(2) = C_r(iX, iY);
						tri.g(0) = tri.g(1) = tri.g(2) = C_g(iX, iY);
						tri.b(0) = tri.b(1) = tri.b(2) = C_b(iX, iY);
					}
					else
					{
						tri.r(0) = tri.r(1) = tri.r(2) = C(iX, iY);
						tri.g(0) = tri.g(1) = tri.g(2) = C(iX, iY);
						tri.b(0) = tri.b(1) = tri.b(2) = C(iX, iY);
					}
				}
				else
				{
					tri.r(0) = tri.r(1) = tri.r(2) = m_color.R / 255.f;
					tri.g(0) = tri.g(1) = tri.g(2) = m_color.G / 255.f;
					tri.b(0) = tri.b(1) = tri.b(2) = m_color.B / 255.f;
				}

				// Compute normal of this triangle, and add it up to the 3
				// neighboring vertices:
				// A = P1 - P0, B = P2 - P0
				float ax = tri.x(1) - tri.x(0);
				float bx = tri.x(2) - tri.x(0);
				float ay = tri.y(1) - tri.y(0);
				float by = tri.y(2) - tri.y(0);
				float az = tri.z(1) - tri.z(0);
				float bz = tri.z(2) - tri.z(0);
				const TPoint3D this_normal(
					ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);

				// Vertex indices:
				tvi.vind[1] = iX + 1 + rows * iY;

				// Add triangle:
				actualMesh.emplace_back(tri, tvi);

				// For averaging normals:
				for (unsigned long k : tvi.vind)
				{
					vertex_normals[k].first += this_normal;
					vertex_normals[k].second++;
				}
			}
			// 2:
			//  [0]--[1->2]
			//     \  |
			//       \|
			//       [2->1]
			// Order: 0,2,1
			if (!useMask || mask(iX, iY + 1))
			{
				tri.x(1) = tri.x(2);
				tri.y(1) = tri.y(2);
				tri.z(1) = tri.z(2);

				tri.x(2) = tri.x(0);
				// tri.y(2)=tri.y(1);
				tri.z(2) = Z(iX, iY + 1);
				if (m_colorFromZ)
				{
					mrpt::img::TColorf col(0, 0, 0, 1);

					colormap(m_colorMap, C(iX, iY), col.R, col.G, col.B);
					tri.r(0) = f2u8(col.R);
					tri.g(0) = f2u8(col.G);
					tri.b(0) = f2u8(col.B);
					colormap(
						m_colorMap, C(iX + 1, iY + 1), col.R, col.G, col.B);
					tri.r(1) = f2u8(col.R);
					tri.g(1) = f2u8(col.G);
					tri.b(1) = f2u8(col.B);
					colormap(m_colorMap, C(iX, iY + 1), col.R, col.G, col.B);
					tri.r(2) = f2u8(col.R);
					tri.g(2) = f2u8(col.G);
					tri.b(2) = f2u8(col.B);
				}
				else if (m_isImage)
				{
					if (getTextureImage().isColor())
					{
						tri.r(0) = tri.r(1) = tri.r(2) = C_r(iX, iY);
						tri.g(0) = tri.g(1) = tri.g(2) = C_g(iX, iY);
						tri.b(0) = tri.b(1) = tri.b(2) = C_b(iX, iY);
					}
					else
					{
						tri.r(0) = tri.r(1) = tri.r(2) = C(iX, iY);
						tri.g(0) = tri.g(1) = tri.g(2) = C(iX, iY);
						tri.b(0) = tri.b(1) = tri.b(2) = C(iX, iY);
					}
				}
				else
				{
					tri.r(0) = tri.r(1) = tri.r(2) = m_color.R / 255.f;
					tri.g(0) = tri.g(1) = tri.g(2) = m_color.G / 255.f;
					tri.b(0) = tri.b(1) = tri.b(2) = m_color.B / 255.f;
				}

				// Compute normal of this triangle, and add it up to the 3
				// neighboring vertices:
				// A = P1 - P0, B = P2 - P0
				float ax = tri.x(1) - tri.x(0);
				float bx = tri.x(2) - tri.x(0);
				float ay = tri.y(1) - tri.y(0);
				float by = tri.y(2) - tri.y(0);
				float az = tri.z(1) - tri.z(0);
				float bz = tri.z(2) - tri.z(0);
				const TPoint3D this_normal(
					ay * bz - az * by, az * bx - ax * bz, ax * by - ay * bx);

				// Vertex indices:
				tvi.vind[1] = tvi.vind[2];
				tvi.vind[2] = iX + rows * (iY + 1);

				// Add triangle:
				actualMesh.emplace_back(tri, tvi);

				// For averaging normals:
				for (unsigned long k : tvi.vind)
				{
					vertex_normals[k].first += this_normal;
					vertex_normals[k].second++;
				}
			}
		}

	// Average normals:
	for (auto& vertex_normal : vertex_normals)
	{
		const size_t N = vertex_normal.second;
		if (N > 0)
		{
			vertex_normal.first *= 1.0 / N;
			vertex_normal.first = vertex_normal.first.unitarize();
		}
	}

	m_trianglesUpToDate = true;
	m_polygonsUpToDate = false;
}

void CMesh::render(const RenderContext& rc) const
{
	switch (rc.shader_id)
	{
		case DefaultShaderID::TEXTURED_TRIANGLES:
			if (!m_isWireFrame)
				CRenderizableShaderTexturedTriangles::render(rc);
			break;
		case DefaultShaderID::WIREFRAME:
			if (m_isWireFrame) CRenderizableShaderWireFrame::render(rc);
			break;
	};
}
void CMesh::renderUpdateBuffers() const
{
	if (!m_trianglesUpToDate) updateTriangles();

	CRenderizableShaderTexturedTriangles::renderUpdateBuffers();
	CRenderizableShaderWireFrame::renderUpdateBuffers();
}

void CMesh::onUpdateBuffers_Wireframe()
{
	auto& vbd = CRenderizableShaderWireFrame::m_vertex_buffer_data;
	auto& cbd = CRenderizableShaderWireFrame::m_color_buffer_data;
	vbd.clear();
	cbd.clear();

	for (auto& i : actualMesh)
	{
		const mrpt::opengl::TTriangle& t = i.first;

		for (int kk = 0; kk <= 3; kk++)
		{
			int k = kk % 3;
			int k1 = (kk + 1) % 3;

			vbd.emplace_back(t.x(k), t.y(k), t.z(k));
			cbd.emplace_back(t.r(k), t.g(k), t.b(k), t.a(k));

			vbd.emplace_back(t.x(k1), t.y(k1), t.z(k1));
			cbd.emplace_back(t.r(k1), t.g(k1), t.b(k1), t.a(k1));
		}
	}
}

void CMesh::onUpdateBuffers_TexturedTriangles()
{
	auto& tris = CRenderizableShaderTexturedTriangles::m_triangles;
	tris.clear();

	for (auto& i : actualMesh)
	{
		const mrpt::opengl::TTriangle& t = i.first;
		const TTriangleVertexIndices& tvi = i.second;

		const auto& n0 = vertex_normals.at(tvi.vind[0]).first;
		const auto& n1 = vertex_normals.at(tvi.vind[1]).first;
		const auto& n2 = vertex_normals.at(tvi.vind[2]).first;

		auto tri = t;

		tri.vertices[0].normal = n0;
		tri.vertices[1].normal = n1;
		tri.vertices[2].normal = n2;

		for (int k = 0; k < 3; k++)
		{
			tri.vertices[k].uv.x =
				(tri.vertices[k].xyzrgba.pt.x - m_xMin) / (m_xMax - m_xMin);
			tri.vertices[k].uv.y =
				(tri.vertices[k].xyzrgba.pt.y - m_yMin) / (m_yMax - m_yMin);
		}

		tris.emplace_back(std::move(tri));
	}
}

/*---------------------------------------------------------------
							assignImage
  ---------------------------------------------------------------*/
void CMesh::assignImage(const CImage& img)
{
	MRPT_START

	// Make a copy:
	CRenderizableShaderTexturedTriangles::assignImage(img);

	// Delete content in Z
	Z.setZero(img.getHeight(), img.getWidth());

	m_modified_Image = true;
	m_enableTransparency = false;
	m_colorFromZ = false;
	m_isImage = true;
	m_trianglesUpToDate = false;

	CRenderizable::notifyChange();

	MRPT_END
}

void CMesh::assignImageAndZ(
	const CImage& img, const mrpt::math::CMatrixDynamic<float>& in_Z)
{
	MRPT_START

	ASSERT_(
		(img.getWidth() == static_cast<size_t>(in_Z.cols())) &&
		(img.getHeight() == static_cast<size_t>(in_Z.rows())));

	Z = in_Z;

	// Load the texture:
	CRenderizableShaderTexturedTriangles::assignImage(img);

	m_modified_Image = true;
	m_enableTransparency = false;
	m_colorFromZ = false;
	m_isImage = true;
	m_trianglesUpToDate = false;

	CRenderizable::notifyChange();

	MRPT_END
}

uint8_t CMesh::serializeGetVersion() const { return 1; }
void CMesh::serializeTo(mrpt::serialization::CArchive& out) const
{
	writeToStreamRender(out);
	writeToStreamTexturedObject(out);

	// Version 0:
	out << m_xMin << m_xMax << m_yMin << m_yMax;
	out << Z << mask;  // We don't need to serialize C, it's computed
	out << m_enableTransparency;
	out << m_colorFromZ;
	// new in v1
	out << m_isWireFrame;
	out << int16_t(m_colorMap);
}

void CMesh::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		{
			readFromStreamRender(in);
			readFromStreamTexturedObject(in);

			in >> m_xMin;
			in >> m_xMax;
			in >> m_yMin;
			in >> m_yMax;

			in >> Z >> mask;
			in >> m_enableTransparency;
			in >> m_colorFromZ;

			if (version >= 1)
			{
				in >> m_isWireFrame;
				int16_t i;
				in >> i;
				m_colorMap = mrpt::img::TColormap(i);
			}
			else
				m_isWireFrame = false;

			m_modified_Z = true;
		}
			m_trianglesUpToDate = false;
			break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
	m_trianglesUpToDate = false;
	CRenderizable::notifyChange();
}

void CMesh::updateColorsMatrix() const
{
	if ((!m_modified_Z) && (!m_modified_Image)) return;

	CRenderizable::notifyChange();

	if (m_isImage)
	{
		const int cols = getTextureImage().getWidth();
		const int rows = getTextureImage().getHeight();

		if ((cols != Z.cols()) || (rows != Z.rows()))
			printf("\nTexture Image and Z sizes have to be equal");

		else if (getTextureImage().isColor())
		{
			C_r.setSize(rows, cols);
			C_g.setSize(rows, cols);
			C_b.setSize(rows, cols);
			getTextureImage().getAsRGBMatrices(C_r, C_g, C_b);
		}
		else
		{
			C.setSize(rows, cols);
			getTextureImage().getAsMatrix(C);
		}
	}
	else
	{
		const size_t cols = Z.cols();
		const size_t rows = Z.rows();
		C.setSize(rows, cols);

		// Color is proportional to height:
		C = Z;

		// If mask is empty -> Normalize the whole mesh
		if (mask.empty()) mrpt::math::normalize(C, 0.01f, 0.99f);

		// Else -> Normalize color ignoring masked-out cells:
		else
		{
			float val_max = -std::numeric_limits<float>::max(),
				  val_min = std::numeric_limits<float>::max();
			bool any_valid = false;

			for (size_t c = 0; c < cols; c++)
				for (size_t r = 0; r < rows; r++)
				{
					if (!mask(r, c)) continue;
					any_valid = true;
					const float val = C(r, c);
					mrpt::keep_max(val_max, val);
					mrpt::keep_min(val_min, val);
				}

			if (any_valid)
			{
				float minMaxDelta = val_max - val_min;
				if (minMaxDelta == 0) minMaxDelta = 1;
				const float minMaxDelta_ = 1.0f / minMaxDelta;
				C.array() = (C.array() - val_min) * minMaxDelta_;
			}
		}
	}

	m_modified_Image = false;
	m_modified_Z = false;
	m_trianglesUpToDate = false;
}

void CMesh::setZ(const mrpt::math::CMatrixDynamic<float>& in_Z)
{
	Z = in_Z;
	m_modified_Z = true;
	m_trianglesUpToDate = false;

	// Delete previously loaded images
	m_isImage = false;

	CRenderizable::notifyChange();
}

void CMesh::setMask(const mrpt::math::CMatrixDynamic<float>& in_mask)
{
	mask = in_mask;
	m_trianglesUpToDate = false;
	CRenderizable::notifyChange();
}

bool CMesh::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
{
	if (!m_trianglesUpToDate || !m_polygonsUpToDate) updatePolygons();
	return mrpt::math::traceRay(tmpPolys, (o - this->m_pose).asTPose(), dist);
}

static math::TPolygon3D tmpPoly(3);
mrpt::math::TPolygonWithPlane createPolygonFromTriangle(
	const std::pair<mrpt::opengl::TTriangle, CMesh::TTriangleVertexIndices>& p)
{
	const mrpt::opengl::TTriangle& t = p.first;
	for (size_t i = 0; i < 3; i++)
		tmpPoly[i] = t.vertex(i);
	return mrpt::math::TPolygonWithPlane(tmpPoly);
}

void CMesh::updatePolygons() const
{
	if (!m_trianglesUpToDate) updateTriangles();
	size_t N = actualMesh.size();
	tmpPolys.resize(N);
	transform(
		actualMesh.begin(), actualMesh.end(), tmpPolys.begin(),
		createPolygonFromTriangle);
	m_polygonsUpToDate = true;
	CRenderizable::notifyChange();
}

auto CMesh::getBoundingBox() const -> mrpt::math::TBoundingBox
{
	return trianglesBoundingBox().compose(m_pose);
}

void CMesh::adjustGridToImageAR()
{
	ASSERT_(m_isImage);
	const float ycenter = 0.5f * (m_yMin + m_yMax);
	const float xwidth = m_xMax - m_xMin;
	const float newratio = float(getTextureImage().getWidth()) /
		float(getTextureImage().getHeight());
	m_yMax = ycenter + 0.5f * newratio * xwidth;
	m_yMin = ycenter - 0.5f * newratio * xwidth;
	CRenderizable::notifyChange();
}


//
// Created by yche on 1/16/18.
//

#include <fstream>

#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>

#include "boost_serialization_eigen.h"
#include "../util/graph.h"

int main(int argc, char *argv[]) {
    if (file_exists("test.bin")) {
        Eigen::VectorXd D;
        std::ifstream ifs("test.bin");
        boost::archive::binary_iarchive ia(ifs);
        boost::serialization::load(ia, D, 0);
        ifs.close();
        for (auto i = 0; i < D.size(); i++) {
            cout << D[i] << endl;
        }
    } else {
        Eigen::VectorXd D;
        D.resize(10);
        for (auto i = 0; i < 10; i++) {
            D[i] = i * i + 0.1;
        }
        std::ofstream ofs("test.bin");
        boost::archive::binary_oarchive oa(ofs);
        boost::serialization::save(oa, D, 0);
        ofs.close();
        cout << "save finished" << endl;
    }
}


//######################################################################
//#   SDF_Fusion Module
//#
//#   Copyright (C) 2020 Siemens AG
//#   SPDX-License-Identifier: MIT
//#   Author 2020: This module has been developed by
//#                or under supervision of Slobodan Ilic
//#######################################################################

#ifndef POSEVALIDATOR_HPP
#define POSEVALIDATOR_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>

struct PoseValidationPayload
{
    cv::Mat rgb_image;
	cv::Mat depth_image;
	Eigen::Isometry3f pose;
	Eigen::Matrix3f intrinsics;
	std::vector<float> volume;
};

bool validate_pose(const PoseValidationPayload &payload);

#endif


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/min.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>

#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <boost/simd/sdk/config.hpp>
#include <complex>

#include <nt2/include/functions/splat.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/mone.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL ( min_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::min;
  using nt2::tag::min_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef native<cT,ext_t>                vcT;
  typedef typename nt2::dry<T>             dT;
  typedef native<dT,ext_t>                vdT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
    NT2_TEST_ULP_EQUAL(nt2::min(vcT(nt2::Inf<vT>(), nt2::Zero<vT>()), vcT(nt2::Inf<vT>(), nt2::Zero<vT>())), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()), 0);
    NT2_TEST_EQUAL(nt2::min(vdT(nt2::Inf<vdT>()),  vcT(nt2::Inf<vT>(), nt2::Zero<vT>())), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()));
    NT2_TEST_EQUAL(nt2::min(vdT(nt2::Inf<vdT>()),   vdT(nt2::Inf<vdT>())),  vdT(nt2::Inf<vdT>() ));
#endif
    NT2_TEST_EQUAL(nt2::min(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>())), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()));
    NT2_TEST_EQUAL(nt2::min(vcT(nt2::Zero<vT>(),nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>())), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()));
    NT2_TEST_EQUAL(nt2::min(vcT(nt2::Zero<vT>(),nt2::One<vT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>())), vcT(nt2::One<vT>(),nt2::Zero<vT>()));
    NT2_TEST_EQUAL(nt2::min(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>())), vcT(nt2::One<vT>(), nt2::Zero<vT>()));
    NT2_TEST_EQUAL(nt2::min(vdT(nt2::One<vdT>()),  vcT(nt2::Zero<vT>(),nt2::Zero<vT>())), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()));
    NT2_TEST_EQUAL(nt2::min(nt2::Zero<vdT>(), nt2::Zero<vcT>()), nt2::Zero<vcT>());
    NT2_TEST_EQUAL(nt2::min(nt2::Zero<vdT>(), nt2::One<vcT>()),  nt2::Zero<vcT>());
    NT2_TEST_EQUAL(nt2::min(nt2::One<vdT>(),  nt2::One<vcT>()),  nt2::One<vcT>());
    NT2_TEST_EQUAL(nt2::min(nt2::One<vdT>(), nt2::Zero<vdT>()), vdT(nt2::Zero<vdT>()));
    NT2_TEST_EQUAL(nt2::min(nt2::Zero<vdT>(),nt2::Zero<vdT>()), vdT(nt2::Zero<vdT>()));
    NT2_TEST_EQUAL(nt2::min(nt2::Zero<vdT>(),nt2::One<vdT>()),  vdT(nt2::Zero<vdT>()));
    NT2_TEST_EQUAL(nt2::min(nt2::One<vdT>(), nt2::One<vdT>()),  vdT(nt2::One<vdT>() ));
    NT2_TEST_EQUAL(nt2::min(nt2::One<vdT>(), nt2::Mone<vdT>()), vdT(nt2::One<vdT>()));
    NT2_TEST_EQUAL(nt2::min(nt2::One<vT>(),  nt2::Mone<vdT>()), vdT(nt2::One<vdT>()));
    NT2_TEST_EQUAL(nt2::min(nt2::One<vdT>(), nt2::Mone<vT>()), vdT(nt2::One<vdT>()));
}


/******************************************************************************\
* 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                                            *
\******************************************************************************/

// These tests may benefit from using a mocking framework. If a mocking
// framework is added to this code base in the future many of these tests should
// be revisited with that in mind.

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

// File under test:
#include "peptide-emission.h"

// Standard C++ library headers:
#include <functional>

// External headers:
#include "fakeit.hpp"

// Local project headers:
#include "common/error-model.h"
#include "common/radiometry.h"
#include "hmm/fit/error-model-fitter.h"
#include "hmm/fit/log-normal-distribution-fitter.h"
#include "tensor/tensor.h"
#include "test-util/fakeit.h"

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
using fakeit::Fake;
using fakeit::Mock;
using fakeit::Verify;
using fakeit::VerifyNoOtherInvocations;
using std::function;
using whatprot::test_util::Close;
const double TOL = 0.000000001;
}  // namespace

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(step_suite)
BOOST_AUTO_TEST_SUITE(peptide_emission_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 0.5;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    BOOST_TEST(e.num_channels == num_channels);
    BOOST_TEST(e.max_num_dyes == max_num_dyes);
    BOOST_TEST(e.values[0] == 0.5);
}

BOOST_AUTO_TEST_CASE(prob_multiple_timesteps_test, *tolerance(TOL)) {
    int num_timesteps = 3;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(1, 0) = 1.0;
    rad(2, 0) = 2.0;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(e.prob(1, 0, 0) == pdf(1.0, 0));
    BOOST_TEST(e.prob(2, 0, 0) == pdf(2.0, 0));
}

BOOST_AUTO_TEST_CASE(prob_multiple_timesteps_const_test, *tolerance(TOL)) {
    int num_timesteps = 3;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(1, 0) = 1.0;
    rad(2, 0) = 2.0;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    const PeptideEmission& ce = e;
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(ce.prob(1, 0, 0) == pdf(1.0, 0));
    BOOST_TEST(ce.prob(2, 0, 0) == pdf(2.0, 0));
}

BOOST_AUTO_TEST_CASE(prob_multiple_channels_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 3;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(0, 2) = 0.2;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(e.prob(0, 1, 0) == pdf(0.1, 0));
    BOOST_TEST(e.prob(0, 2, 0) == pdf(0.2, 0));
}

BOOST_AUTO_TEST_CASE(prob_multiple_channels_const_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 3;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(0, 2) = 0.2;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    const PeptideEmission& ce = e;
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(ce.prob(0, 1, 0) == pdf(0.1, 0));
    BOOST_TEST(ce.prob(0, 2, 0) == pdf(0.2, 0));
}

BOOST_AUTO_TEST_CASE(prob_multiple_dye_counts_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    int max_num_dyes = 2;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 1.0 / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.max_num_dyes == max_num_dyes);
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(e.prob(0, 0, 1) == pdf(0.0, 1));
    BOOST_TEST(e.prob(0, 0, 2) == pdf(0.0, 2));
}

BOOST_AUTO_TEST_CASE(prob_multiple_dye_counts_const_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    int max_num_dyes = 2;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 1.0 / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.max_num_dyes == max_num_dyes);
    const PeptideEmission& ce = e;
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(ce.prob(0, 0, 1) == pdf(0.0, 1));
    BOOST_TEST(ce.prob(0, 0, 2) == pdf(0.0, 2));
}

BOOST_AUTO_TEST_CASE(prob_multiple_everything_test, *tolerance(TOL)) {
    int num_timesteps = 2;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(1, 0) = 1.0;
    rad(1, 1) = 1.1;
    int max_num_dyes = 1;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return (observed + 0.042) / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(e.prob(0, 0, 1) == pdf(0.0, 1));
    BOOST_TEST(e.prob(0, 1, 0) == pdf(0.1, 0));
    BOOST_TEST(e.prob(0, 1, 1) == pdf(0.1, 1));
    BOOST_TEST(e.prob(1, 0, 0) == pdf(1.0, 0));
    BOOST_TEST(e.prob(1, 0, 1) == pdf(1.0, 1));
    BOOST_TEST(e.prob(1, 1, 0) == pdf(1.1, 0));
    BOOST_TEST(e.prob(1, 1, 1) == pdf(1.1, 1));
}

BOOST_AUTO_TEST_CASE(prob_multiple_everything_const_test, *tolerance(TOL)) {
    int num_timesteps = 2;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(1, 0) = 1.0;
    rad(1, 1) = 1.1;
    int max_num_dyes = 1;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return (observed + 0.042) / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int expected_size = num_timesteps * num_channels * (max_num_dyes + 1);
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    const PeptideEmission& ce = e;
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(ce.prob(0, 0, 1) == pdf(0.0, 1));
    BOOST_TEST(ce.prob(0, 1, 0) == pdf(0.1, 0));
    BOOST_TEST(ce.prob(0, 1, 1) == pdf(0.1, 1));
    BOOST_TEST(ce.prob(1, 0, 0) == pdf(1.0, 0));
    BOOST_TEST(ce.prob(1, 0, 1) == pdf(1.0, 1));
    BOOST_TEST(ce.prob(1, 1, 0) == pdf(1.1, 0));
    BOOST_TEST(ce.prob(1, 1, 1) == pdf(1.1, 1));
}

BOOST_AUTO_TEST_CASE(forward_in_place_trivial_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 0.5;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 3.14;  // loc is {0, 0}
    int edmans = 0;
    e.forward(&edmans, &psv);
    BOOST_TEST(psv.tensor[loc] == 3.14 * pdf(1.0, 0));  // loc is {0, 0}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_trivial_test, *tolerance(TOL)) {
//     int num_timesteps = 1;
//     int num_channels = 1;
//     Radiometry rad(num_timesteps, num_channels);
//     rad(0, 0) = 1.0;
//     int max_num_dyes = 0;
//     function<double(double, int)> pdf = [](double observed,
//                                            int state) -> double {
//         return 0.5;
//     };
//     PeptideEmission e(rad, max_num_dyes, pdf);
//     int order = 1 + num_channels;
//     int* shape = new int[order];
//     shape[0] = num_timesteps;
//     shape[1] = 1;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 3.14;  // loc is {0, 0}
//     int edmans = 0;
//     e.forward(tsr1, &edmans, &psv2);
//     BOOST_TEST(psv2.tensor[loc] == 3.14 * pdf(1.0, 0));  // loc is {0, 0}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_tsr_reuse_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 0.5;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 3.14;  // loc is {0, 0}
    int edmans = 0;
    e.forward(&edmans, &psv);
    e.forward(&edmans, &psv);
    BOOST_TEST(psv.tensor[loc]
               == 3.14 * pdf(1.0, 0) * pdf(1.0, 0));  // loc is {0, 0}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(forward_in_place_multiple_timesteps_test,
                     *tolerance(TOL)) {
    int num_timesteps = 3;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(1, 0) = 1.0;
    rad(2, 0) = 2.0;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 13.0;  // loc is {0, 0}
    loc[0] = 1;
    psv.tensor[loc] = 13.1;  // loc is {1, 0}
    loc[0] = 2;
    psv.tensor[loc] = 13.2;  // loc is {2, 0}
    int edmans = 2;
    e.forward(&edmans, &psv);
    loc[0] = 0;
    BOOST_TEST(psv.tensor[loc] == 13.0 * pdf(2.0, 0));  // loc is {0, 0}
    loc[0] = 1;
    BOOST_TEST(psv.tensor[loc] == 13.1 * pdf(2.0, 0));  // loc is {1, 0}
    loc[0] = 2;
    BOOST_TEST(psv.tensor[loc] == 13.2 * pdf(2.0, 0));  // loc is {2, 0}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_multiple_timesteps_test,
// *tolerance(TOL)) {
//     int num_timesteps = 3;
//     int num_channels = 1;
//     Radiometry rad(num_timesteps, num_channels);
//     rad(0, 0) = 0.0;
//     rad(1, 0) = 1.0;
//     rad(2, 0) = 2.0;
//     int max_num_dyes = 0;
//     function<double(double, int)> pdf = [](double observed,
//                                            int state) -> double {
//         return observed + 0.042;
//     };
//     PeptideEmission e(rad, max_num_dyes, pdf);
//     int order = 1 + num_channels;
//     int* shape = new int[order];
//     shape[0] = num_timesteps;
//     shape[1] = 1;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 13.0;  // loc is {0, 0}
//     loc[0] = 1;
//     psv1.tensor[loc] = 13.1;  // loc is {1, 0}
//     loc[0] = 2;
//     psv1.tensor[loc] = 13.2;  // loc is {2, 0}
//     int edmans = 2;
//     e.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 13.0 * pdf(2.0, 0));  // loc is {0, 0}
//     loc[0] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 13.1 * pdf(2.0, 0));  // loc is {1, 0}
//     loc[0] = 2;
//     BOOST_TEST(psv2.tensor[loc] == 13.2 * pdf(2.0, 0));  // loc is {2, 0}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_multiple_channels_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 3;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(0, 2) = 0.2;
    int max_num_dyes = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = 1;
    shape[2] = 1;
    shape[3] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    loc[3] = 0;
    psv.tensor[loc] = 13.0;  // loc is {0, 0, 0, 0}
    int edmans = 0;
    e.forward(&edmans, &psv);
    // loc is {0, 0, 0, 0}
    BOOST_TEST(psv.tensor[loc]
               == 13.0 * pdf(0.0, 0) * pdf(0.1, 0) * pdf(0.2, 0));
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_multiple_channels_test, *tolerance(TOL))
// {
//     int num_timesteps = 1;
//     int num_channels = 3;
//     Radiometry rad(num_timesteps, num_channels);
//     rad(0, 0) = 0.0;
//     rad(0, 1) = 0.1;
//     rad(0, 2) = 0.2;
//     int max_num_dyes = 0;
//     function<double(double, int)> pdf = [](double observed,
//                                            int state) -> double {
//         return observed + 0.042;
//     };
//     PeptideEmission e(rad, max_num_dyes, pdf);
//     int order = 1 + num_channels;
//     int* shape = new int[order];
//     shape[0] = num_timesteps;
//     shape[1] = 1;
//     shape[2] = 1;
//     shape[3] = 1;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     loc[2] = 0;
//     loc[3] = 0;
//     psv1.tensor[loc] = 13.0;  // loc is {0, 0, 0, 0}
//     int edmans = 0;
//     e.forward(tsr1, &edmans, &psv2);
//     // loc is {0, 0, 0, 0}
//     BOOST_TEST(psv2.tensor[loc]
//                == 13.0 * pdf(0.0, 0) * pdf(0.1, 0) * pdf(0.2, 0));
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_multiple_dye_counts_test,
                     *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    int max_num_dyes = 2;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 1.0 / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = max_num_dyes + 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 13.0;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 13.1;  // loc is {0, 1}
    loc[1] = 2;
    psv.tensor[loc] = 13.2;  // loc is {0, 2}
    int edmans = 0;
    e.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 13.0 * pdf(0.0, 0));  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 13.1 * pdf(0.0, 1));  // loc is {0, 1}
    loc[1] = 2;
    BOOST_TEST(psv.tensor[loc] == 13.2 * pdf(0.0, 2));  // loc is {0, 2}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_multiple_dye_counts_test,
//                      *tolerance(TOL)) {
//     int num_timesteps = 1;
//     int num_channels = 1;
//     Radiometry rad(num_timesteps, num_channels);
//     rad(0, 0) = 0.0;
//     int max_num_dyes = 2;
//     function<double(double, int)> pdf = [](double observed,
//                                            int state) -> double {
//         return 1.0 / (double)(state + 7);
//     };
//     PeptideEmission e(rad, max_num_dyes, pdf);
//     int order = 1 + num_channels;
//     int* shape = new int[order];
//     shape[0] = num_timesteps;
//     shape[1] = max_num_dyes + 1;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 13.0;  // loc is {0, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 13.1;  // loc is {0, 1}
//     loc[1] = 2;
//     psv1.tensor[loc] = 13.2;  // loc is {0, 2}
//     int edmans = 0;
//     e.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 13.0 * pdf(0.0, 0));  // loc is {0, 0}
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 13.1 * pdf(0.0, 1));  // loc is {0, 1}
//     loc[1] = 2;
//     BOOST_TEST(psv2.tensor[loc] == 13.2 * pdf(0.0, 2));  // loc is {0, 2}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_multiple_everything_test,
                     *tolerance(TOL)) {
    int num_timesteps = 2;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(1, 0) = 1.0;
    rad(1, 1) = 1.1;
    int max_num_dyes = 1;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return (observed + 0.042) / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = 2;
    shape[2] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    psv.tensor[loc] = 7.000;  // loc is {0, 0, 0}
    loc[2] = 1;
    psv.tensor[loc] = 7.001;  // loc is {0, 0, 1}
    loc[1] = 1;
    loc[2] = 0;
    psv.tensor[loc] = 7.010;  // loc is {0, 1, 0}
    loc[2] = 1;
    psv.tensor[loc] = 7.011;  // loc is {0, 1, 1}
    loc[0] = 1;
    loc[1] = 0;
    loc[2] = 0;
    psv.tensor[loc] = 7.100;  // loc is {1, 0, 0}
    loc[2] = 1;
    psv.tensor[loc] = 7.101;  // loc is {1, 0, 1}
    loc[1] = 1;
    loc[2] = 0;
    psv.tensor[loc] = 7.110;  // loc is {1, 1, 0}
    loc[2] = 1;
    psv.tensor[loc] = 7.111;  // loc is {1, 1, 1}
    int edmans = 1;
    e.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    // loc is {0, 0, 0}
    BOOST_TEST(psv.tensor[loc] == 7.000 * pdf(1.0, 0) * pdf(1.1, 0));
    loc[2] = 1;
    // loc is {0, 0, 1}
    BOOST_TEST(psv.tensor[loc] == 7.001 * pdf(1.0, 0) * pdf(1.1, 1));
    loc[1] = 1;
    loc[2] = 0;
    // loc is {0, 1, 0}
    BOOST_TEST(psv.tensor[loc] == 7.010 * pdf(1.0, 1) * pdf(1.1, 0));
    loc[2] = 1;
    // loc is {0, 1, 1}
    BOOST_TEST(psv.tensor[loc] == 7.011 * pdf(1.0, 1) * pdf(1.1, 1));
    loc[0] = 1;
    loc[1] = 0;
    loc[2] = 0;
    // loc is {1, 0, 0}
    BOOST_TEST(psv.tensor[loc] == 7.100 * pdf(1.0, 0) * pdf(1.1, 0));
    loc[2] = 1;
    // loc is {1, 0, 1}
    BOOST_TEST(psv.tensor[loc] == 7.101 * pdf(1.0, 0) * pdf(1.1, 1));
    loc[1] = 1;
    loc[2] = 0;
    // loc is {1, 1, 0}
    BOOST_TEST(psv.tensor[loc] == 7.110 * pdf(1.0, 1) * pdf(1.1, 0));
    loc[2] = 1;
    // loc is {1, 1, 1}
    BOOST_TEST(psv.tensor[loc] == 7.111 * pdf(1.0, 1) * pdf(1.1, 1));
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_multiple_everything_test,
//                      *tolerance(TOL)) {
//     int num_timesteps = 2;
//     int num_channels = 2;
//     Radiometry rad(num_timesteps, num_channels);
//     rad(0, 0) = 0.0;
//     rad(0, 1) = 0.1;
//     rad(1, 0) = 1.0;
//     rad(1, 1) = 1.1;
//     int max_num_dyes = 1;
//     function<double(double, int)> pdf = [](double observed,
//                                            int state) -> double {
//         return (observed + 0.042) / (double)(state + 7);
//     };
//     PeptideEmission e(rad, max_num_dyes, pdf);
//     int order = 1 + num_channels;
//     int* shape = new int[order];
//     shape[0] = num_timesteps;
//     shape[1] = 2;
//     shape[2] = 2;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     loc[2] = 0;
//     psv1.tensor[loc] = 7.000;  // loc is {0, 0, 0}
//     loc[2] = 1;
//     psv1.tensor[loc] = 7.001;  // loc is {0, 0, 1}
//     loc[1] = 1;
//     loc[2] = 0;
//     psv1.tensor[loc] = 7.010;  // loc is {0, 1, 0}
//     loc[2] = 1;
//     psv1.tensor[loc] = 7.011;  // loc is {0, 1, 1}
//     loc[0] = 1;
//     loc[1] = 0;
//     loc[2] = 0;
//     psv1.tensor[loc] = 7.100;  // loc is {1, 0, 0}
//     loc[2] = 1;
//     psv1.tensor[loc] = 7.101;  // loc is {1, 0, 1}
//     loc[1] = 1;
//     loc[2] = 0;
//     psv1.tensor[loc] = 7.110;  // loc is {1, 1, 0}
//     loc[2] = 1;
//     psv1.tensor[loc] = 7.111;  // loc is {1, 1, 1}
//     int edmans = 1;
//     e.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     loc[2] = 0;
//     // loc is {0, 0, 0}
//     BOOST_TEST(psv2.tensor[loc] == 7.000 * pdf(1.0, 0) * pdf(1.1, 0));
//     loc[2] = 1;
//     // loc is {0, 0, 1}
//     BOOST_TEST(psv2.tensor[loc] == 7.001 * pdf(1.0, 0) * pdf(1.1, 1));
//     loc[1] = 1;
//     loc[2] = 0;
//     // loc is {0, 1, 0}
//     BOOST_TEST(psv2.tensor[loc] == 7.010 * pdf(1.0, 1) * pdf(1.1, 0));
//     loc[2] = 1;
//     // loc is {0, 1, 1}
//     BOOST_TEST(psv2.tensor[loc] == 7.011 * pdf(1.0, 1) * pdf(1.1, 1));
//     loc[0] = 1;
//     loc[1] = 0;
//     loc[2] = 0;
//     // loc is {1, 0, 0}
//     BOOST_TEST(psv2.tensor[loc] == 7.100 * pdf(1.0, 0) * pdf(1.1, 0));
//     loc[2] = 1;
//     // loc is {1, 0, 1}
//     BOOST_TEST(psv2.tensor[loc] == 7.101 * pdf(1.0, 0) * pdf(1.1, 1));
//     loc[1] = 1;
//     loc[2] = 0;
//     // loc is {1, 1, 0}
//     BOOST_TEST(psv2.tensor[loc] == 7.110 * pdf(1.0, 1) * pdf(1.1, 0));
//     loc[2] = 1;
//     // loc is {1, 1, 1}
//     BOOST_TEST(psv2.tensor[loc] == 7.111 * pdf(1.0, 1) * pdf(1.1, 1));
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(improve_fit_simple_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.009;
    int max_num_dyes = 2;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 1.0 / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = max_num_dyes + 1;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 1.72;  // loc is {0, 0}
    bpsv.tensor[loc] = 2.72;
    nbpsv.tensor[loc] = 3.72;
    loc[1] = 1;
    fpsv.tensor[loc] = 1.36;  // loc is {0, 1}
    bpsv.tensor[loc] = 2.36;
    nbpsv.tensor[loc] = 3.36;
    loc[1] = 2;
    fpsv.tensor[loc] = 1.18;  // loc is {0, 2}
    bpsv.tensor[loc] = 2.18;
    nbpsv.tensor[loc] = 3.18;
    delete[] loc;
    int edmans = 0;
    double probability = 3.14159;
    ErrorModelFitter emf;
    LogNormalDistributionFitter* original_dist_fit = emf.distribution_fit;
    Mock<LogNormalDistributionFitter> df_mock;
    Fake(Method(df_mock, add_sample));
    emf.distribution_fit = &df_mock.get();
    e.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &emf);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.009, TOL),
                          0,
                          Close(1.72 * 2.72 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.009, TOL),
                          1,
                          Close(1.36 * 2.36 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.009, TOL),
                          2,
                          Close(1.18 * 2.18 / 3.14159, TOL)))
            .Exactly(1);
    VerifyNoOtherInvocations(df_mock);
    emf.distribution_fit = original_dist_fit;  // This avoids breaking cleanup.
}

BOOST_AUTO_TEST_CASE(improve_fit_multiple_dye_colors_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.009;
    rad(0, 1) = 1.019;
    int max_num_dyes = 1;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 1.0 / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = max_num_dyes + 1;
    shape[2] = max_num_dyes + 1;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    fpsv.tensor[loc] = 1.72;  // loc is {0, 0, 0}
    bpsv.tensor[loc] = 2.72;
    nbpsv.tensor[loc] = 3.72;
    loc[2] = 1;
    fpsv.tensor[loc] = 1.64;  // loc is {0, 0, 1}
    bpsv.tensor[loc] = 2.64;
    nbpsv.tensor[loc] = 3.64;
    loc[1] = 1;
    loc[2] = 0;
    fpsv.tensor[loc] = 1.36;  // loc is {0, 1, 0}
    bpsv.tensor[loc] = 2.36;
    nbpsv.tensor[loc] = 3.36;
    loc[2] = 1;
    fpsv.tensor[loc] = 1.25;  // loc is {0, 1, 1}
    bpsv.tensor[loc] = 2.25;
    nbpsv.tensor[loc] = 3.25;
    delete[] loc;
    int edmans = 0;
    double probability = 3.14159;
    ErrorModelFitter emf;
    LogNormalDistributionFitter* original_dist_fit = emf.distribution_fit;
    Mock<LogNormalDistributionFitter> df_mock;
    Fake(Method(df_mock, add_sample));
    emf.distribution_fit = &df_mock.get();
    e.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &emf);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.009, TOL),
                          0,
                          Close(1.72 * 2.72 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.019, TOL),
                          0,
                          Close(1.72 * 2.72 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.009, TOL),
                          0,
                          Close(1.64 * 2.64 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.019, TOL),
                          1,
                          Close(1.64 * 2.64 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.009, TOL),
                          1,
                          Close(1.36 * 2.36 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.019, TOL),
                          0,
                          Close(1.36 * 2.36 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.009, TOL),
                          1,
                          Close(1.25 * 2.25 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.019, TOL),
                          1,
                          Close(1.25 * 2.25 / 3.14159, TOL)))
            .Exactly(1);
    VerifyNoOtherInvocations(df_mock);
    emf.distribution_fit = original_dist_fit;  // This avoids breaking cleanup.
}

BOOST_AUTO_TEST_CASE(improve_fit_multiple_edmans_test, *tolerance(TOL)) {
    int num_timesteps = 2;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.009;
    rad(1, 0) = 1.109;
    int max_num_dyes = 1;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 1.0 / (double)(state + 7);
    };
    PeptideEmission e(rad, max_num_dyes, pdf);
    int order = 1 + num_channels;
    int* shape = new int[order];
    shape[0] = num_timesteps;
    shape[1] = max_num_dyes + 1;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 1.72;  // loc is {0, 0}
    bpsv.tensor[loc] = 2.72;
    nbpsv.tensor[loc] = 3.72;
    loc[1] = 1;
    fpsv.tensor[loc] = 1.36;  // loc is {0, 1}
    bpsv.tensor[loc] = 2.36;
    nbpsv.tensor[loc] = 3.36;
    loc[0] = 1;
    loc[1] = 0;
    fpsv.tensor[loc] = 1.64;  // loc is {1, 0}
    bpsv.tensor[loc] = 2.64;
    nbpsv.tensor[loc] = 3.64;
    loc[1] = 1;
    fpsv.tensor[loc] = 1.25;  // loc is {1, 1}
    bpsv.tensor[loc] = 2.25;
    nbpsv.tensor[loc] = 3.25;
    delete[] loc;
    int edmans = 1;
    double probability = 3.14159;
    ErrorModelFitter emf;
    LogNormalDistributionFitter* original_dist_fit = emf.distribution_fit;
    Mock<LogNormalDistributionFitter> df_mock;
    Fake(Method(df_mock, add_sample));
    emf.distribution_fit = &df_mock.get();
    e.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &emf);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.109, TOL),
                          0,
                          Close(1.72 * 2.72 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.109, TOL),
                          1,
                          Close(1.36 * 2.36 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.109, TOL),
                          0,
                          Close(1.64 * 2.64 / 3.14159, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.109, TOL),
                          1,
                          Close(1.25 * 2.25 / 3.14159, TOL)))
            .Exactly(1);
    VerifyNoOtherInvocations(df_mock);
    emf.distribution_fit = original_dist_fit;  // This avoids breaking cleanup.
}

BOOST_AUTO_TEST_SUITE_END()  // peptide_emission_suite
BOOST_AUTO_TEST_SUITE_END()  // step_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
// #include <sensor_msgs/Point_cloud2.h>
#include <jsk_recognition_msgs/BoundingBox.h>

// opencv.
// #include <opencv/cv.h>
#include <opencv2/opencv.hpp>
// #include <opencv2/Core/core.hpp>
#include <cv_bridge/cv_bridge.h>

// pcl.
// #include <pcl/pcl.h>

//! Eigen
#include <Eigen/Dense>

//! user
#include "Utility.hpp"

using namespace std;
using namespace Eigen;

//http://wiki.ros.org/image_transport/Tutorials/SubscribingToImages
//http://wiki.ros.org/image_geometry/Tutorials/ProjectTfFrameToImage

class Registration
{
    ros::NodeHandle node_;

    image_transport::ImageTransport it_;
    image_transport::Subscriber imgSub_;
    image_transport::CameraSubscriber imgCamSub_;
    image_transport::Publisher imgPub_;

    ros::Subscriber cloudSub_;
    ros::Subscriber poseSub_;
    ros::Subscriber bbxSub_;

    image_geometry::PinholeCameraModel camModel_;

public:
    // Registration(ros::NodeHandle& node);
    // virtual ~Registration();
    Registration(ros::NodeHandle& node)
        : node_(node),
        it_(node)
    {
        cout<<"imgCloudRegistration"<<endl;

        string imgTopic = node_.resolveName("/zed/zed_node/left/image_rect_color");
        string cloudTopic = node_.resolveName("/zed/zed_node/point_cloud/cloud_registered");
        string poseTopic = node_.resolveName("/republish_cxx_node/bbxpose");
        string bbxTopic = node_.resolveName("/republish_cxx_node/bbx");
        // cout<<"resolved image topic name: "<<imgTopic<<endl;

        imgCamSub_ = it_.subscribeCamera(imgTopic, 10, &Registration::imgCamCallback, this);
        // imgSub_ = it_.subscribe(imgTopic, 10, &Registration::imgCallback, this);

        // http://wiki.ros.org/image_transport/Tutorials/PublishingImages
        imgPub_ = it_.advertise("image", 10);

        cloudSub_ = node_.subscribe(cloudTopic, 10, &Registration::cloudCallback, this);
        // poseSub_ = node_.subscribe(poseTopic, 10, &Registration::poseCallback, this);
        bbxSub_ = node_.subscribe(bbxTopic, 10, &Registration::bbxCallback, this);
    }

    // bool initilization()
    // {
    //     // ros::NodeHandle node2(node);
    //     cout<<"Registration"<<endl;

    //     string imgTopic = node_.resolveName("/zed/zed_node/left/image_rect_color");
    //     string cloudTopic = node_.resolveName("/zed/zed_node/point_cloud/cloud_registered");
    //     cout<<"resolved image topic name: "<<imgTopic<<endl;

    //     it_(node_);
    //     imgSub_ = it_.subscribeCamera(imgTopic, 10, &Registration::imgCallback, this);
    //     cloudSub_ = node_.subscribe(cloudTopic, 10, &Registration::cloudCallback, this);
    //     // imgSub_ = it_.subscribeCamera(imgTopic, 10, &Registration::imgCallback, this);
    //     // cloudSub_ = node_.subscribe("/zed/zed_node/point_cloud/cloud_registered", 10, &Registration::cloudCallback, this);
    // }

    void imgCamCallback(const sensor_msgs::ImageConstPtr& imgMsg, const sensor_msgs::CameraInfoConstPtr& infoMsg){
        cout<<"image info callback"<<endl;

        //! intrinsic parameters.
        // cout<<infoMsg->K[0]<<endl;
        // cout<<infoMsg->K[1]<<endl;
        // cout<<infoMsg->K[2]<<endl;
        // cout<<infoMsg->K[3]<<endl;
        // cout<<infoMsg->K[4]<<endl;
        // cout<<infoMsg->K[5]<<endl;
        // cout<<infoMsg->K[6]<<endl;
        // cout<<infoMsg->K[7]<<endl;
        // cout<<infoMsg->K[8]<<endl;

        //! 获取相机的内参
        K_ << infoMsg->K[0],0,infoMsg->K[2],
            0,infoMsg->K[4],infoMsg->K[5],
            0,0,1;
        // K_ << 677.393,0,620.749,
        //     0,677.393,350.597,
        //     0,0,1;

        P_.resize(3,4);
        P_ << infoMsg->P[0],0,infoMsg->P[2],infoMsg->P[3],
            0,infoMsg->P[5],infoMsg->P[6],infoMsg->P[7],
            0,0,1,0;

        camModel_.fromCameraInfo(infoMsg);
        // cout<<"camera model: "<<camModel_<<endl;

        //! 获取RGB图像，转成opencv格式，并处理后发布出去
        // http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
        cv::Mat img;
        cv_bridge::CvImagePtr br;
        try{
            br = cv_bridge::toCvCopy(imgMsg, sensor_msgs::image_encodings::BGR8);
            img = br->image;
            // cout<<"input image size: "<<img.cols<<"  "<<img.rows<<endl;
        }catch(cv_bridge::Exception& ex){
            ROS_ERROR("Failed to convert image!");
            return;
        }

        //! draw markers on image. http://wiki.ros.org/image_geometry/Tutorials/ProjectTfFrameToImage
        cv::circle(img, center_, 10, CV_RGB(255,0,0), -1);
        //! BBX四条边的中点
        cv::circle(img, pp1PixelFrame_, 8, CV_RGB(255,0,0), -1);
        cv::circle(img, pp2PixelFrame_, 8, CV_RGB(0,255,0), -1);
        cv::circle(img, pp3PixelFrame_, 8, CV_RGB(0,0,255), -1);
        cv::circle(img, pp4PixelFrame_, 8, CV_RGB(255,255,0), -1);
        //! ROI
        cv::rectangle(img, roiP1_, roiP2_, CV_RGB(0,255,0), 3, 8, 0);

        //! publish drawn image.
        imgPub_.publish(br->toImageMsg());
    }

    /*
    暂停使用
    */
    void imgCallback(const sensor_msgs::ImageConstPtr& imgMsg){
        // cout<<"image callback"<<endl;

        // http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
        cv::Mat img;
        cv_bridge::CvImagePtr br;
        try{
            br = cv_bridge::toCvCopy(imgMsg, sensor_msgs::image_encodings::BGR8);
            img = br->image;
            // cout<<"input image size: "<<img.cols<<"  "<<img.rows<<endl;
        }catch(cv_bridge::Exception& ex){
            ROS_ERROR("Failed to convert image!");
            return;
        }

        //! draw markers on image. http://wiki.ros.org/image_geometry/Tutorials/ProjectTfFrameToImage
        cv::circle(img, center_, 10, CV_RGB(255,0,0), -1);
        //! BBX四条边的中点
        cv::circle(img, pp1PixelFrame_, 8, CV_RGB(255,0,0), -1);
        cv::circle(img, pp2PixelFrame_, 8, CV_RGB(0,255,0), -1);
        cv::circle(img, pp3PixelFrame_, 8, CV_RGB(0,0,255), -1);
        cv::circle(img, pp4PixelFrame_, 8, CV_RGB(255,255,0), -1);
        //! ROI
        cv::rectangle(img, roiP1_, roiP2_, CV_RGB(0,255,0), 3, 8, 0);

        //! publish drawn image.
        imgPub_.publish(br->toImageMsg());
    }

    void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg){
        // cout<<"cloud callback"<<endl;
    }

    void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg){
        cout<<"pose callback"<<endl;

        Vector3f p3;
        // p3 << msg->pose.position.x,msg->pose.position.y,msg->pose.position.z;
        // cloud frame(x front) to camera frame(z front)
        p3 << -msg->pose.position.y,-msg->pose.position.z,msg->pose.position.x;
        cout<<"p3 = "<<p3<<endl;
        // Matrix3f K;
        // K << 702.67,0,636.38,
        //     0,702.67,347.11,
        //     0,0,1;

        Vector3f p2;
        // p2 = utility_.project3dTo2d(p3, K_);
        // cout<<"p2 = "<<p2<<endl<<endl;
        // center_.x = p2(0);
        // center_.y = p2(1);

        p2 = utility_.project3dTo2d(p3, P_);
        cout<<"p2 = "<<p2<<endl<<endl;
        center_.x = p2(0);
        center_.y = p2(1);
    }

    void bbxCallback(const jsk_recognition_msgs::BoundingBoxConstPtr& msg){
        Vector3f p3;
        // cloud frame(x front) to camera frame(z front)
        p3 << -msg->pose.position.y,-msg->pose.position.z,msg->pose.position.x;
        cout<<"p3 = "<<p3<<endl;

        Vector3f p2;
        p2 = utility_.project3dTo2d(p3, P_);
        cout<<"p2 = "<<p2<<endl<<endl;
        center_.x = p2(0);
        center_.y = p2(1);

        //! 计算BBX上下及左右边的中点（点云坐标系）
        float heightLeftCenter_z = msg->pose.position.z;
        float heightLeftCenter_y = msg->pose.position.y + 0.5*msg->dimensions.y;
        float heightLeftCenter_x = msg->pose.position.x;
        Vector3f pp1CamFrame; // 相机坐标系
        pp1CamFrame << -heightLeftCenter_y, -heightLeftCenter_z, heightLeftCenter_x;
        Vector3f pp1PixelFrame; // 图像坐标系
        pp1PixelFrame = utility_.project3dTo2d(pp1CamFrame, P_);
        pp1PixelFrame_.x = pp1PixelFrame(0);
        pp1PixelFrame_.y = pp1PixelFrame(1);

        float heightRightCenter_z = msg->pose.position.z;
        float heightRightCenter_y = msg->pose.position.y - 0.5*msg->dimensions.y;
        float heightRightCenter_x = msg->pose.position.x;
        Vector3f pp2CamFrame; // 相机坐标系
        pp2CamFrame << -heightRightCenter_y, -heightRightCenter_z, heightRightCenter_x;
        Vector3f pp2PixelFrame; // 图像坐标系
        pp2PixelFrame = utility_.project3dTo2d(pp2CamFrame, P_);
        pp2PixelFrame_.x = pp2PixelFrame(0);
        pp2PixelFrame_.y = pp2PixelFrame(1);

        float widthTopCenter_z = msg->pose.position.z + 0.5*msg->dimensions.z;
        float widthTopCenter_y = msg->pose.position.y;
        float widthTopCenter_x = msg->pose.position.x;
        Vector3f pp3CamFrame; // 相机坐标系
        pp3CamFrame << -widthTopCenter_y, -widthTopCenter_z, widthTopCenter_x;
        Vector3f pp3PixelFrame; // 图像坐标系
        pp3PixelFrame = utility_.project3dTo2d(pp3CamFrame, P_);
        pp3PixelFrame_.x = pp3PixelFrame(0);
        pp3PixelFrame_.y = pp3PixelFrame(1);

        float widthBottomCenter_z = msg->pose.position.z - 0.5*msg->dimensions.z;
        float widthBottomCenter_y = msg->pose.position.y;
        float widthBottomCenter_x = msg->pose.position.x;
        Vector3f pp4CamFrame; // 相机坐标系
        pp4CamFrame << -widthBottomCenter_y, -widthBottomCenter_z, widthBottomCenter_x;
        Vector3f pp4PixelFrame; // 图像坐标系
        pp4PixelFrame = utility_.project3dTo2d(pp4CamFrame, P_);
        pp4PixelFrame_.x = pp4PixelFrame(0);
        pp4PixelFrame_.y = pp4PixelFrame(1);

        //! ROI(图像坐标系)
        roiP1_.x = pp1PixelFrame_.x;
        roiP1_.y = pp3PixelFrame_.y;

        roiP2_.x = pp2PixelFrame_.x;
        roiP2_.y = pp4PixelFrame_.y;
    }

private:
    Utility utility_;

    cv::Point2d center_;
    cv::Point2d pp1PixelFrame_, pp2PixelFrame_, pp3PixelFrame_, pp4PixelFrame_;
    cv::Point2i roiP1_, roiP2_;

    // # Intrinsic camera matrix for the raw (distorted) images.
    // #     [fx  0 cx]
    // # K = [ 0 fy cy]
    // #     [ 0  0  1]
    // # Projects 3D points in the camera coordinate frame to 2D pixel
    // # coordinates using the focal lengths (fx, fy) and principal point
    // # (cx, cy).
    Matrix3f K_;

    MatrixXf P_;
};

int main(int argc, char** argv){
    ros::init(argc, argv, "registration");

    ros::NodeHandle node("~");

    cout<<"Registration node started."<<endl;
    Registration registration(node);
    // registration.initilization();

    ros::spin();
    cout<<"Registration node end."<<endl;

    return 0;
}


#ifndef SHASTA_SHORTEST_PATH_HPP
#define SHASTA_SHORTEST_PATH_HPP

// Function to find the shortest weighted path
// between two vertices of an undirected graph.
// We cannot use boost::dijkstra_shortest_paths
// from the Boost Graph library because we want to be
// able to reuse the priority queue, to
// reduce memory allocation activity.

// Requirements on class Graph:
// - Must support a subset of the boost::graph::adjacency_list API.
// - The vertex must have the following data members:
//   vertex_descriptor predecessor;
//   uint64_t distance;
//   uint8_t color;
// - The edge must have a uint64_t weight data member
//   which defines path lengths.

// shasta
#include "orderPairs.hpp"

// Boost Graph library.
#include <boost/graph/iteration_macros.hpp>

// Standard library.
#include "cstddef.hpp"
#include <queue>
#include "vector.hpp"

namespace shasta {

    // The last argument to findShortestPath is a work area with this type.
    template<class Graph> using FindShortestPathQueue  =
        std::priority_queue<
            pair< uint64_t, typename Graph::vertex_descriptor>,
            vector< pair< uint64_t, typename Graph::vertex_descriptor> >,
            OrderPairsByFirstOnlyGreater< uint64_t, typename Graph::vertex_descriptor>
        >;

    template<class Graph> void findShortestPath(
        Graph&,
        typename Graph::vertex_descriptor vSource,
        typename Graph::vertex_descriptor vTarget,
        vector<typename Graph::vertex_descriptor>& path,

        // Work area. Does not need to be initialized.
        // When calling findShortestPath repeatedly,
        // use the same FindShortestPathQueue to reduce memory
        // allocation activity.
        FindShortestPathQueue<Graph>&
        );
}

template<class Graph> inline void shasta::findShortestPath(
    Graph& graph,
    typename Graph::vertex_descriptor vSource,
    typename Graph::vertex_descriptor vTarget,
    vector<typename Graph::vertex_descriptor>& path,
    FindShortestPathQueue<Graph>& q
    )
{
    // Trivial special case.
    if(vTarget == vSource) {
        path.clear();
        path.push_back(vSource);
        return;
    }

    using vertex_descriptor = typename Graph::vertex_descriptor;

    // Initialize.
    BGL_FORALL_VERTICES_T(v, graph, Graph) {
        auto& vertex = graph[v];
        vertex.predecessor = Graph::null_vertex();
        vertex.distance = std::numeric_limits<uint64_t>::max();
        vertex.color = 0;
    }
    graph[vSource].predecessor = vSource;
    graph[vSource].distance = 0;
    while(!q.empty()) {
        q.pop();
    }
    q.push(make_pair(0, vSource));

    // Main loop.
    while(!q.empty()) {

        // Dequeue the closest vertex in the queue.
        const auto p0 = q.top();
        q.pop();
        const uint64_t distance0 = p0.first;
        const vertex_descriptor v0 = p0.second;
        // cout << "Dequeued " << v0.v << " at distance " << distance0 << endl;

        // If already encountered, skip.
        // This can happen because the inner loop uses "lazy deletion".
        // For example, see here:
        // https://stackoverflow.com/questions/9209323/easiest-way-of-using-min-priority-queue-with-key-update-in-c
        auto& vertex0 = graph[v0];
        if(vertex0.color == 1) {
            // cout << "Already encountered, skipped." << endl;
            continue;
        }
        vertex0.color = 1;

        // If we found vLast, construct the path and be done.
        if(v0 == vTarget) {
            path.clear();
            vertex_descriptor v = v0;
            while(true) {
                path.push_back(v);
                if(v == vSource) {
                    break;
                }
                v = graph[v].predecessor;
            }
            std::reverse(path.begin(), path.end());
            while(!q.empty()) {
                q.pop();
            }
            return;
        }

        // Loop over its out-edges.
        BGL_FORALL_OUTEDGES_T(v0, e01, graph, Graph) {
            const vertex_descriptor v1 = target(e01, graph);
            auto& vertex1 = graph[v1];
            if(vertex1.color == 1) {
                // cout << "    " << v1.v << " skipped because of color." << endl;
                continue;
            }
            const uint64_t weight = graph[e01].weight;
            const uint64_t distance1 = distance0 + weight;
            // cout << "    Found " << v1.v << " at distance " << distance1 << endl;

            if(distance1 < vertex1.distance) {
                q.push(make_pair(distance1, v1));
                vertex1.predecessor = v0;
                vertex1.distance = distance1;
                // cout << "        Enqueued." << endl;
            }
        }
    }

    // If getting here, the queue is empty but we have not found vLast.
    // This means that there is no path between vFirst and vlast.
    path.clear();
}

#endif


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


//
// A demonstration of using the KDTREE2 C++ routines, and timing.
// This file is in the public domain.
//

#include "kdtree2.h"

#include <boost/multi_array.hpp>
#include <boost/random.hpp>
#include <iostream>
#include <stdio.h>

static boost::minstd_rand generator(42u);
static boost::uniform_real<> uni_dist(0, 1);
boost::variate_generator<boost::minstd_rand &, boost::uniform_real<> > uni(generator, uni_dist);

float random_variate()
{
  // between [0,1)
  return (uni());
}

//
// define, for convenience a 2d array of floats.
//
typedef boost::multi_array<float, 2> array2dfloat;

#include <ctime>

float time_a_search(kdtree2::KDTree *tree, int nn, int nsearch)
{
  int dim = tree->dim;
  std::vector<float> query(dim);
  kdtree2::KDTreeResultVector result;

  clock_t t0, t1;

  t0 = clock();

  for (int i = 0; i < nsearch; i++)
  {
    for (int j = 0; j < dim; j++)
      query[j] = random_variate();
    tree->n_nearest(query, nn, result);
  }

  t1 = clock();

  return (static_cast<float>(static_cast<double>(t1 - t0) / static_cast<double>(CLOCKS_PER_SEC)));
}

void time_random_searches(kdtree2::KDTree *tree, int nn)
{
  // emit the number of searches per second.
  int nsearch;

  nsearch = 50;
  for (;;)
  {
    float t = time_a_search(tree, nn, nsearch);
    if (t < 1.0)
    {
      nsearch *= 5;
      continue;
    }
    else
    {
      float sps = float(nsearch) / t;
      std::cout << "C++ impl, for nn=" << nn << " searches/sec = " << sps << "\n";
      printf(" %.4f us\n", 1.0/sps*1000*1000);
      return;
    }
  }
}

// Give me N, and dim (e.g. '1000 3').  No commas!1000 2
// I found N=1000,dim=2
//  rearranging
// Tree created, now testing against brute force...
// Testing complete.  Now testing timing...
// C++ impl, for nn=1 searches/sec = 415082
//  2.4092 us
// C++ impl, for nn=5 searches/sec = 161667
//  6.1855 us
// C++ impl, for nn=10 searches/sec = 102175
//  9.7871 us
// C++ impl, for nn=25 searches/sec = 44185.9
//  22.6316 us
// C++ impl, for nn=500 searches/sec = 3195.09

int main()
{
  array2dfloat data(boost::extents[10][3]); // declare a 10000 x 3 array.
  array2dfloat realdata;

  // notice it is in C-standard layout.
  kdtree2::KDTree *tree;
  kdtree2::KDTreeResultVector res;
  int N, dim;

  if (false)
  {
    for (int i = 0; i < 10; i++)
    {
      for (int j = 0; j < 3; j++)
        data[i][j] = static_cast<float>(3 * i + j);
    }
    tree = new kdtree2::KDTree(data, true);

    //    tree->dump_data();
    //data[0][0]=666.0;  // mutate it underneath.  DO NOT DO THIS IN REAL USE
    //tree->dump_data(); // test to see it change there.

    tree->n_nearest_around_point(1, 1, 1, res);
    for (unsigned int i = 0; i < res.size(); i++)
    {
      printf("result[%d]= (%d,%f)\n", i, res[i].idx, res[i].dis);
    }

    delete tree;
  }
  printf("Give me N, and dim (e.g. '1000 3').  No commas!");
  scanf("%d %d", &N, &dim);
  printf("I found N=%d,dim=%d\n", N, dim);
  realdata.resize(boost::extents[N][dim]);

  for (int i = 0; i < N; i++)
  {
    for (int j = 0; j < dim; j++)
      realdata[i][j] = random_variate();
  }

  tree = new kdtree2::KDTree(realdata, true);
  tree->sort_results = true;
  std::cout << "Tree created, now testing against brute force...";
  {
    std::vector<float> query(dim);
    kdtree2::KDTreeResultVector result, resultbrute;
    int nn = 10;

    for (int i = 0; i < 50; i++)
    {
      for (int j = 0; j < dim; j++)
        query[j] = random_variate();

      tree->n_nearest_brute_force(query, nn, resultbrute);
      tree->n_nearest(query, nn, result); // search for 10 of them.

      for (int k = 0; k < nn; k++)
      {
        if ((resultbrute[k].dis != result[k].dis) ||
            (resultbrute[k].idx != result[k].idx))
        {
          std::cout << "Mismatch! nn=" << k << " brute=[" << resultbrute[k].dis << "," << resultbrute[k].idx << "] tree=[" << result[k].dis << "," << result[k].idx << "]\n";
        }
      }
    }
  }
  std::cout << "\nTesting complete.  Now testing timing...\n";
  tree->sort_results = false;

  {

    int nnarray[] = {1, 5, 10, 25, 500};
    for (int i = 0; i < 5; i++)
    {
      time_random_searches(tree, nnarray[i]);
    }
  }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_SSE_AVX_SQRT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

#include <boost/simd/arithmetic/functions/sqrt.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/touint.hpp>
#include <boost/simd/include/functions/simd/is_gez.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/operator/functions/details/assert_utils.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( sqrt_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<double_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm256_sqrt_pd(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<single_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return _mm256_sqrt_ps(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_
                                    , boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<uint64_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::touint(boost::simd::sqrt(boost::simd::tofloat(a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrt_, boost::simd::tag::avx_
                                    , (A0)
                                    , ((simd_<int64_<A0>,boost::simd::tag::avx_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      BOOST_ASSERT_MSG(assert_all(is_gez(a0)), "sqrt input is negative");
      return boost::simd::toint(boost::simd::sqrt(boost::simd::tofloat(a0)));
    }
  };
} } }
#endif
#endif


/*
==== Author:

Relja Arandjelovic (relja@robots.ox.ac.uk)
Visual Geometry Group,
Department of Engineering Science
University of Oxford

==== Copyright:

The library belongs to Relja Arandjelovic and the University of Oxford.
No usage or redistribution is allowed without explicit permission.
*/

#include "holidays_public.h"

#include <string>
#include <stdio.h>

#include <boost/filesystem.hpp>

#include <Eigen/LU>
#include <Eigen/SVD>

#include "desc_to_hell.h"
#include "macros.h"

inline float sqr(float x){ return x*x; }

void holidaysPublic::getFeats( const char fileName[], uint32_t &numFeats, std::vector<ellipse> &regions, float *&descs ) const {

    std::string fn= boost::filesystem::basename(fileName);

    FILE *f= fopen( (descDir_ + fn + ".siftgeo").c_str() ,"rb");
    ASSERT(f!=NULL);

    fseek(f, 0, SEEK_END );
    numFeats= ftell(f)/(9 * 4 + 1 * 4 + 128);
    fseek(f, 0, SEEK_SET);

    float geom[9];
    uint8_t sift[128];
    uint8_t *siftEnd= sift+128;
    descs= new float[numFeats*128];
    float *descsIt= descs;
    regions.resize(numFeats);
    int ret= 0;
    int32_t nDimsTemp;

    for (uint32_t i=0; i<numFeats; ++i){
        ret= fread(geom, sizeof(float), 9, f);

        // Matlab to convert: M=[m11,m12;m21,m22]; where m's are geom[4:7], scale is [2] (0 based indexing)
        // [Vi,D,V]= svd(M);
        // D= D*scale;
        // D= diag(1./(diag(D).^2));
        // V'*D*V
        Eigen::Matrix2f M, V, D2, A;
        float scale= geom[2];
        M(0,0)= geom[4]; M(0,1)= geom[5];
        M(1,0)= geom[6]; M(1,1)= geom[7];
        Eigen::JacobiSVD<Eigen::Matrix2f> svd( M, Eigen::ComputeFullV );
        Eigen::Vector2f D= svd.singularValues();
        V= svd.matrixV();
        D2(0,0)= 1.0/sqr(D(0)*scale);
        D2(0,1)= 0;
        D2(1,0)= 0;
        D2(1,1)= 1.0/sqr(D(1)*scale);
        A.noalias()= V.transpose() * D2 * V;
        ASSERT( fabs(A(0,1)-A(1,0)) < 1e-6 );

        // minus in order to correspond to y being flipped in images
        // also check if det(V)==1 or -1, because if -1 then flipped
        regions[i].set(geom[0], geom[1],
            A(0,0),
            -( V.determinant()>0 ? 1: -1 ) * A(0,1),
            A(1,1));

        ret= fread(&nDimsTemp, sizeof(int32_t), 1, f);
        ASSERT(nDimsTemp==128);

        ret= fread(sift, sizeof(uint8_t), 128, f);
        for (uint8_t *siftIt= sift; siftIt!=siftEnd; ++siftIt, ++descsIt)
            *descsIt= static_cast<float>(*siftIt);
    }
    fclose(f);
    REMOVE_UNUSED_WARNING(ret);

    if (useRootSIFT_)
        descToHell::convertToHell(128, numFeats, descs);
}

std::string
holidaysPublic::getRawDescs(float const *descs, uint32_t numFeats) const {
    throw std::runtime_error( "Don't do this - it is only used when making training descriptors, and you shouldn't be doing it for Holidays (use flickr60k)" );
    std::string res(numFeats*128, '\0');
    uint8_t *resIter= reinterpret_cast<uint8_t*>(&res[0]);
    float const *inIter= descs;
    float const *inIterEnd= descs + numFeats*128;
    for (; inIter!=inIterEnd; ++inIter, ++resIter)
        *resIter= static_cast<uint8_t>(*inIter + 0.1); // +0.1 is to counter numerical issues because cast does floor()
    return res;
}


#include "humanoids/humanoids.hpp"
#include "humanoids/hull2d.hpp"
#include "sco/expr_op_overloads.hpp"
#include "sco/expr_vec_ops.hpp"
#include "sco/modeling_utils.hpp"
#include "osgviewer/osgviewer.hpp"
#include "trajopt/common.hpp"
#include "trajopt/collision_avoidance.hpp"
#include "trajopt/kinematic_terms.hpp"
#include "trajopt/plot_callback.hpp"
#include "trajopt/problem_description.hpp"
#include "trajopt/configuration_space.hpp"
#include "utils/eigen_conversions.hpp"
#include "utils/stl_to_string.hpp"
#include "utils/vector_ops.hpp"
#include <boost/assign.hpp>
#include <boost/foreach.hpp>
#include <openrave-core.h>
#include <openrave/openrave.h>
using namespace boost::assign;
using namespace Eigen;
using namespace OpenRAVE;
using namespace std;
using namespace trajopt;
using namespace util;

bool gEnablePlot = true;
const float DIST_PEN=.02;
const float COLL_COEFF=10;
const float VEL_COEFF = .25;
string data_dir() {
  string out = DATA_DIR;
  return out;
}

/**
import openravepy
env = openravepy.Environment()
env.Load("gfe.xml")
robot = env.GetRobots()[0]
rfoot = robot.GetLink("r_foot")
aabb = rfoot.ComputeLocalAABB(rfoot)
aabb.pos()+aabb.extents()
aabb.pos()-aabb.extents()
 */
MatrixX2d GetLocalAabbPoly() {
  MatrixX2d out(4,2);
  out << 0.17939, 0.062707,
      0.17939, -0.061646,
      -0.08246, -0.061646,
      -0.08246, 0.061646;
  return out;
}
MatrixX2d local_aabb_poly = GetLocalAabbPoly();

MatrixX2d GetLeftPoly(const RobotBase& gfe) {
  OpenRAVE::Vector v = gfe.GetLink("l_foot")->GetTransform().trans;
  v.z = 0;
  return local_aabb_poly.rowwise() + Vector2d(v.x, v.y).transpose();
}
MatrixX2d GetRightPoly(const RobotBase& gfe) {
  OpenRAVE::Vector v = gfe.GetLink("r_foot")->GetTransform().trans;
  v.z = 0;
  return local_aabb_poly.rowwise() + Vector2d(v.x, v.y).transpose();
}
template <typename MatrixT>
MatrixT concat0(const MatrixT& x, const MatrixT& y) {
  MatrixT out(x.rows() + y.rows(), x.cols());
  out.topRows(x.rows()) = x;
  out.bottomRows(y.rows()) = y;
  return out;
}
template <typename MatrixT>
void extend0(MatrixT& x, const MatrixT& y) {
  x.conservativeResize(x.rows()+y.rows(), NoChange);
  x.bottomRows(y.rows()) = y;
}

MatrixX2d GetBothPoly(const RobotBase& gfe) {
  MatrixX2d left = GetLeftPoly(gfe);
  MatrixX2d right = GetRightPoly(gfe);
  MatrixX2d verts = concat0(left, right);
  MatrixX2d hull = hull2d(verts);
  return hull;
}

void SetLeftZMP(TrajOptProb& prob, int t) {
  RobotAndDOFPtr rad = prob.GetRAD();
  prob.addConstraint(ConstraintPtr(new ZMP(rad, GetLeftPoly(*rad->GetRobot()), prob.GetVarRow(t))));
}
void SetRightZMP(TrajOptProb& prob, int t) {
  RobotAndDOFPtr rad = prob.GetRAD();
  prob.addConstraint(ConstraintPtr(new ZMP(rad, GetRightPoly(*rad->GetRobot()), prob.GetVarRow(t))));
}
void SetBothZMP(TrajOptProb& prob, int t) {
  RobotAndDOFPtr rad = prob.GetRAD();
  prob.addConstraint(ConstraintPtr(new ZMP(rad, GetBothPoly(*rad->GetRobot()), prob.GetVarRow(t))));
}
void SetLinkFixed(TrajOptProb& prob, KinBody::LinkPtr link, int t, const OpenRAVE::Transform& tlink) {
//  BoolVec allbutz(6,true);
//  allbutz[5] = false;
  prob.addConstraint(ConstraintPtr(new CartPoseConstraint(prob.GetVarRow(t), tlink, prob.GetRAD(), link)));
}
void SetLinkFixed(TrajOptProb& prob, KinBody::LinkPtr link, int t) {
  OpenRAVE::Transform tlink = link->GetTransform();
  SetLinkFixed(prob, link, t, tlink);
}
void SetStartFixed(TrajOptProb& prob) {
  DblVec cur_dofvals = prob.GetRAD()->GetDOFValues();
  VarArray& vars = prob.GetVars();
  for (int j=0; j < vars.cols(); ++j) {
    prob.addLinearConstraint(exprSub(AffExpr(vars(0,j)), cur_dofvals[j]), EQ);
  }
}

TrajArray Optimize(TrajOptProbPtr prob) {
  // optimize
  BasicTrustRegionSQP opt(prob);
  opt.improve_ratio_threshold_ = .1;
//  opt.min
  TrajArray init(prob->GetNumSteps(), prob->GetNumDOF());
  DblVec cur_dofvals = prob->GetRAD()->GetDOFValues();
  for (int i=0; i < prob->GetNumSteps(); ++i) init.row(i) = toVectorXd(cur_dofvals);
  init += DblMatrix::Random(init.rows(), init.cols()) * .01;
  opt.initialize(DblVec(init.data(), init.data()+init.rows()*init.cols()));
  if (gEnablePlot) opt.addCallback(PlotCallback(*prob));
  opt.optimize();
  return getTraj(opt.x(), prob->GetVars());
}

void LeftRightStep(RobotBasePtr gfe, TrajArray& out) {
  OpenRAVE::KinBody::LinkPtr lfoot = gfe->GetLink("l_foot"),
      rfoot = gfe->GetLink("r_foot");

  CollisionCheckerPtr cc = CollisionChecker::GetOrCreate(*gfe->GetEnv());
  vector<Collision> collisions;
  cc->AllVsAll(collisions);
//  cc->IgnoreLink(lfoot);
//  cc->IgnoreLink(rfoot);

  // determine foot placement pose
  // set up optimization problem

  int n_steps = 5;
  float step_dist = .3;

  RobotAndDOFPtr rad(new RobotAndDOF(gfe, arange(gfe->GetDOF()), OR::DOF_Transform));

  BoolVec xonly(6, false);
  xonly[3] = true;

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));

    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
//      prob->addConstr(ConstraintPtr(new CartPoseConstraint(prob->GetVarRow(i), Tfoottarg, rad, rfoot, xonly)));
      SetLinkFixed(*prob, lfoot, i);
      SetLeftZMP(*prob, i);
    }
    OpenRAVE::Transform Tfootnow = rfoot->GetTransform();
    OpenRAVE::Transform Tfoottarg = Tfootnow;
    Tfoottarg.trans.x = lfoot->GetTransform().trans.x + step_dist; // 30 cm forward
//    if (Tfoottarg.trans.x >= -.1 && Tfoottarg.trans.x < .1) Tfoottarg.trans.z += .05;
    SetLinkFixed(*prob, rfoot, n_steps-1, Tfoottarg);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
      SetLinkFixed(*prob, lfoot, i);
      SetLinkFixed(*prob, rfoot, i);
      SetBothZMP(*prob, i);
    }
    SetRightZMP(*prob, n_steps-1);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
      SetLinkFixed(*prob, rfoot, i);
      SetRightZMP(*prob, i);
    }
    OpenRAVE::Transform Tfootnow = lfoot->GetTransform();
    OpenRAVE::Transform Tfoottarg = Tfootnow;
    Tfoottarg.trans.x = rfoot->GetTransform().trans.x + step_dist; // 30 cm forward
    SetLinkFixed(*prob, lfoot, n_steps-1, Tfoottarg);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

  {
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
      SetLinkFixed(*prob, lfoot, i);
      SetLinkFixed(*prob, rfoot, i);
      SetBothZMP(*prob, i);
    }
    SetLeftZMP(*prob, n_steps-1);

    TrajArray traj = Optimize(prob);
    rad->SetDOFValues(toDblVec(traj.row(n_steps-1)));
    extend0(out,traj);

  }

}

void AnimateTrajectory(RobotAndDOFPtr rad, TrajArray& traj, OSGViewerPtr viewer) {
  for (int i=0; i < traj.rows(); ++i) {
    cout << "step " << i << endl;
    rad->SetDOFValues(toDblVec(traj.row(i)));
    viewer->Idle();
  }
}

int main(int argc, char* argv[]) {
  RaveInitialize(false);
  EnvironmentBasePtr env = RaveCreateEnvironment();
  env->StopSimulation();
  env->Load(string(getenv("HOME")) + "/Proj/darpa-proposal/drclogs.env.xml");
  env->Load(string(getenv("HOME")) + "/Proj/drc/gfe.xml");
  vector<RobotBasePtr> robots; env->GetRobots(robots);
  RobotBasePtr gfe = robots[0];
  OSGViewerPtr viewer(new OSGViewer(env));
  env->AddViewer(viewer);

  RobotAndDOFPtr rad(new RobotAndDOF(gfe, arange(gfe->GetDOF()), OR::DOF_Transform));

  // translate to starting position
  {
    OpenRAVE::Transform T;
    T.trans.y += 1;
    T.trans.z = 0.92712;
    T.trans.x -= .35;
    gfe->SetTransform(T);
  }

  TrajArray traj(0, rad->GetDOF());
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);
  LeftRightStep(gfe, traj);

//  AnimateTrajectory(rad, traj, viewer);

  {
    gfe->SetDOFValues(DblVec(gfe->GetDOF(), 0));
    OpenRAVE::Transform T = gfe->GetTransform();
    T.rot = OR::Vector(1,0,0,.2);
    T.rot.normalize4();
    T.trans = OR::Vector(2.4, 1.4, .92712);
    gfe->SetTransform(T);
    int n_steps = 5;
    TrajOptProbPtr prob(new TrajOptProb(n_steps, rad));
    SetStartFixed(*prob);
    prob->addCost(CostPtr(new JointVelCost(prob->GetVars(), VEL_COEFF*VectorXd::Ones(rad->GetDOF()))));
    for (int i=1; i < n_steps; ++i) {
      SetLinkFixed(*prob, gfe->GetLink("l_foot"), i);
      SetLinkFixed(*prob, gfe->GetLink("r_foot"), i);
      SetLeftZMP(*prob, i);
      SetRightZMP(*prob, i);

//      prob->addCost(CostPtr(new StaticTorqueCost(rad, prob->GetVarRow(i), 1)));
//      prob->addCost(CostPtr(new PECost(rad, prob->GetVarRow(i), .01)));
//      prob->addCost(CostPtr(new CollisionCost(DIST_PEN, COLL_COEFF, rad, prob->GetVarRow(i))));
    }
    OpenRAVE::Transform buttonpose = env->GetKinBody("bigredbutton")->GetTransform();
    buttonpose.trans.z += .1;
    BoolVec justposition(6, false);
    justposition[3] = justposition[4] = justposition[5] = true;
    prob->addConstraint(ConstraintPtr(new CartPoseConstraint(prob->GetVarRow(n_steps-1), buttonpose,
        rad, gfe->GetLink("r_hand"), justposition)));
    TrajArray traj1 = Optimize(prob);
    extend0(traj, traj1);
  }

AnimateTrajectory(rad, traj, viewer);

  viewer->Idle();
ofstream trajfile("/tmp/traj.txt");
trajfile << traj << endl;

  RaveDestroy();
}