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

#ifdef __EMSCRIPTEN__  // are we actually in a Wasm JS instance?
#include <emscripten.h>
#include <emscripten/html5.h>

#define GL_GLEXT_PROTOTYPES
#define EGL_EGLEXT_PROTOTYPES
#include <GLFW/glfw3.h>

#include <functional>

std::function<void()> loop;
void main_loop() { loop(); }
#endif

#include <mrpt/core/exceptions.h>
#include <mrpt/core/lock_helper.h>
#include <mrpt/gui/CDisplayWindowGUI.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/Lie/SO.h>

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>

// We need to define all variables here to avoid emscripten memory errors
// if they are defined as *locals* in the function.
struct AppData
{
	mrpt::gui::CDisplayWindowGUI::Ptr win;
	mrpt::opengl::CSetOfObjects::Ptr gl_corner_user;
	mrpt::opengl::CAxis::Ptr gl_corner_reference;

	// Input variables (they are bound to the GUI controls):
	mrpt::math::CQuaternionDouble in_quat;
	mrpt::math::CMatrixFixed<double, 3, 3> in_rot;
	std::array<double, 3> in_ypr = {0, 0, 0};
	mrpt::math::TVector3D in_axisangle_ax{0, 0, 0};
	double in_axisangle_ang = 0;
	mrpt::math::CVectorFixed<double, 3> in_lie_log =
		mrpt::math::CVectorFixed<double, 3>::Zero();
	bool units_radians = true;

	// In/out UI control declarations (declared here so we can use them in the
	// lambda below):
	nanogui::TabWidget* tabWidget = nullptr;
	nanogui::TextBox* ed_in_rot[3][3];
	nanogui::TextBox *edOutMatrix = nullptr, *edOutQuat = nullptr,
					 *edOutAxisAngle_Ax = nullptr, *edOutAxisAngle_An = nullptr,
					 *edOutLogSO3 = nullptr;
	nanogui::TextBox* ed_out_rot[3][3];
	nanogui::Slider* sl_in_ypr[3] = {nullptr, nullptr, nullptr};
};

static AppData app;

// The main function: update all calculations:
static void recalcAll()
{
	mrpt::poses::CPose3D userPose;
	switch (app.tabWidget->activeTab())
	{
		// YPR
		case 0:
		{
			const double K = app.units_radians ? 1.0 : (M_PI / 180.0);
			userPose.setFromValues(
				0, 0, 0, app.in_ypr[0] * K, app.in_ypr[1] * K,
				app.in_ypr[2] * K);
		}
		break;
		// Rot matrix:
		case 1:
		{
			// Run a SVD to ensure we take the closest SO(3) matrix to the
			// user input:
			Eigen::Matrix3d M = app.in_rot.asEigen();
			Eigen::JacobiSVD<Eigen::Matrix3d> svd(
				M, Eigen::ComputeFullU | Eigen::ComputeFullV);

			auto R = mrpt::math::CMatrixDouble33(
				(svd.matrixU() * svd.matrixV().transpose()).eval());
			userPose.setRotationMatrix(R);
			for (int r = 0; r < 3; r++)
				for (int c = 0; c < 3; c++)
					app.ed_in_rot[r][c]->setValue(
						mrpt::format("%.05f", R(r, c)));
		}
		break;
		// Quaternion
		case 2:
		{
			userPose = mrpt::poses::CPose3D(app.in_quat, 0, 0, 0);
		}
		break;
		// axis+angle
		case 3:
		{
			const double K = app.units_radians ? 1.0 : (M_PI / 180.0);
			mrpt::math::TVector3D v =
				app.in_axisangle_ax * (app.in_axisangle_ang * K);
			mrpt::math::CVectorFixed<double, 3> vn;
			for (int i = 0; i < 3; i++)
				vn[i] = v[i];

			userPose.setRotationMatrix(mrpt::poses::Lie::SO<3>::exp(vn));
		}
		break;
		// log(SO(3))
		case 4:
		{
			userPose.setRotationMatrix(
				mrpt::poses::Lie::SO<3>::exp(app.in_lie_log));
		}
		break;
	};

	// Set 3D view corner:
	app.gl_corner_user->setPose(userPose);

	// Update text output:
	const mrpt::math::CMatrixDouble33 Rout = userPose.getRotationMatrix();

	// matrix:
	app.edOutMatrix->setValue(Rout.inMatlabFormat());
	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++)
			app.ed_out_rot[r][c]->setValue(mrpt::format("%.05f", Rout(r, c)));

	// quat:
	mrpt::math::CQuaternionDouble q;
	userPose.getAsQuaternion(q);
	app.edOutQuat->setValue(q.asString());

	// SO(3) log:
	const auto log_R = mrpt::poses::Lie::SO<3>::log(Rout);
	app.edOutLogSO3->setValue(
		mrpt::format("[%.05f %.05f %.05f]", log_R[0], log_R[1], log_R[2]));

	// axis-angle:
	mrpt::math::TVector3D axis(log_R[0], log_R[1], log_R[2]);
	if (axis.norm() > 1e-20) axis = axis.unitarize();

	app.edOutAxisAngle_Ax->setValue(
		mrpt::format("[%.05f %.05f %.05f]", axis[0], axis[1], axis[2]));
	app.edOutAxisAngle_An->setValue(mrpt::format(
		"%.05f %s", log_R.norm() * (app.units_radians ? 1.0 : 180.0 / M_PI),
		(app.units_radians ? "rad" : "deg")));
};

static void AppRotationConverter()
{
	nanogui::init();

	// Create main window:
	mrpt::gui::CDisplayWindowGUI_Params cp;
	// cp.fullscreen = true;

	// Init values:
	app.in_rot.setIdentity();

	app.gl_corner_user = mrpt::opengl::stock_objects::CornerXYZ(1.0f);
	app.gl_corner_reference = mrpt::opengl::CAxis::Create(
		-1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 0.2f, 1.0f, true);
	app.gl_corner_reference->setTextScale(0.04);

	// Create GUI:
	app.win = mrpt::gui::CDisplayWindowGUI::Create(
		"3D rotation converter", 900, 700, cp);

	// Add INPUT window:
	// -----------------------------
	nanogui::Window* winInput =
		new nanogui::Window(&(*app.win), "Rotation input");
	winInput->setPosition(nanogui::Vector2i(10, 50));
	winInput->setLayout(new nanogui::GroupLayout());
	winInput->setFixedWidth(350);

	app.tabWidget = winInput->add<nanogui::TabWidget>();

	{  // Yaw/pitch/roll  tab:
		nanogui::Widget* layer = app.tabWidget->createTab("Yaw-pitch-roll");
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		const char* lb[3] = {"yaw (Z)=", "pitch (Y)=", "roll (x)="};

		for (int i = 0; i < 3; i++)
		{
			layer->add<nanogui::Label>(lb[i]);
			nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
				mrpt::format("%.2f", app.in_quat[i]));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([i](const std::string& s) {
				app.in_ypr[i] = std::stod(s);
				app.sl_in_ypr[i]->setValue(
					app.in_ypr[i] / (app.units_radians ? M_PIf : 180.0f));
				recalcAll();
				return true;
			});
			nanogui::Slider* sl = layer->add<nanogui::Slider>();
			app.sl_in_ypr[i] = sl;
			sl->setRange({-1.0f, 1.0f});
			sl->setCallback([&, i, ed](float val) {
				val *= app.units_radians ? M_PIf : 180.0f;
				if (i == 1) val *= 0.5f;  // Pitch
				ed->setValue(mrpt::format("%.03f", val));
				ed->callback()(ed->value());
			});
		}
	}

	{  // rotation matrix tab:
		nanogui::Widget* layer = app.tabWidget->createTab("SO(3) matrix");

		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		for (int r = 0; r < 3; r++)
		{
			for (int c = 0; c < 3; c++)
			{
				nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
					mrpt::format("%.04f", app.in_rot(r, c)));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([r, c](const std::string& s) {
					app.in_rot(r, c) = std::stod(s);
					return true;
				});
				app.ed_in_rot[r][c] = ed;
			}
		}
	}

	{  // Quaternion tab:
		nanogui::Widget* layer = app.tabWidget->createTab("Quaternion");
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		const char* lb[4] = {"w (real)=", "x=", "y=", "z="};

		for (int i = 0; i < 4; i++)
		{
			layer->add<nanogui::Label>(lb[i]);

			nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
				mrpt::format("%.2f", app.in_quat[i]));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([i](const std::string& s) {
				app.in_quat[i] = std::stod(s);
				return true;
			});
		}
	}

	{  // Axis-angle tab:
		nanogui::Widget* layer = app.tabWidget->createTab("Axis-angle");
		layer->setLayout(new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill, 5,
			0));
		layer->add<nanogui::Label>("Axis:");

		{
			nanogui::Widget* panel = layer->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));

			for (int r = 0; r < 3; r++)
			{
				nanogui::TextBox* ed = panel->add<nanogui::TextBox>(
					mrpt::format("%.04f", app.in_axisangle_ax[r]));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([r](const std::string& s) {
					app.in_axisangle_ax[r] = std::stod(s);
					return true;
				});
			}
		}
		{
			layer->add<nanogui::Label>("Angle:");
			nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
				mrpt::format("%.04f", app.in_axisangle_ang));
			ed->setEditable(true);
			ed->setFormat("[-+]?[0-9.e+-]*");
			ed->setCallback([](const std::string& s) {
				app.in_axisangle_ang = std::stod(s);
				return true;
			});
		}
	}

	{  // axis with magnitude = log(R) in Lie group
		nanogui::Widget* layer = app.tabWidget->createTab("log(SO(3))");

		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 5,
			0);
		layer->setLayout(layout);

		layer->add<nanogui::Label>("Axis with magnitude", "sans-bold");
		layer->add<nanogui::Label>("(i.e. vee(log(R)) in SO(3))");
		{
			nanogui::Widget* panel = layer->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));

			for (int r = 0; r < 3; r++)
			{
				nanogui::TextBox* ed = layer->add<nanogui::TextBox>(
					mrpt::format("%.04f", app.in_lie_log[r]));
				ed->setEditable(true);
				ed->setFormat("[-+]?[0-9.e+-]*");
				ed->setCallback([r](const std::string& s) {
					app.in_lie_log[r] = std::stod(s);
					return true;
				});
			}
		}
	}

	app.tabWidget->setActiveTab(0);

	// Apply button:
	winInput->add<nanogui::Button>("Apply", ENTYPO_ICON_CALCULATOR)
		->setCallback(recalcAll);

	// Add top menu subwindow:
	// -----------------------------
	{
		nanogui::ref<nanogui::Window> winMenu =
			new nanogui::Window(&(*app.win), "");
		winMenu->setPosition(nanogui::Vector2i(0, 0));
		winMenu->setLayout(new nanogui::BoxLayout(
			nanogui::Orientation::Horizontal, nanogui::Alignment::Middle, 5));
		nanogui::Theme* modTheme =
			new nanogui::Theme(app.win->screen()->nvgContext());
		modTheme->mWindowHeaderHeight = 1;
		winMenu->setTheme(modTheme);

#ifndef __EMSCRIPTEN__	// are we actually in a Wasm JS instance?
		winMenu->add<nanogui::Button>("Quit", ENTYPO_ICON_ARROW_BOLD_LEFT)
			->setCallback([]() { app.win->setVisible(false); });
#endif

		winMenu->add<nanogui::Label>("      ");	 // separator

		winMenu
			->add<nanogui::CheckBox>(
				"Show reference frame",
				[&](bool b) { app.gl_corner_reference->setVisibility(b); })
			->setChecked(true);

		winMenu
			->add<nanogui::CheckBox>(
				"Show rotated frame",
				[&](bool b) { app.gl_corner_user->setVisibility(b); })
			->setChecked(true);
		winMenu
			->add<nanogui::CheckBox>(
				"Ortho. view",
				[&](bool b) { app.win->camera().setCameraProjective(!b); })
			->setChecked(false);

		winMenu->add<nanogui::Label>("Units:");
		winMenu
			->add<nanogui::ComboBox>(
				std::vector<std::string>({"radians", "degrees"}))
			->setCallback([&](int index) {
				// On units update:
				app.units_radians = (index == 0);
				recalcAll();
			});
	}

	// Add OUTPUT window:
	// -----------------------------
	{
		nanogui::Window* winOutput =
			new nanogui::Window(&(*app.win), "Rotation output");
		winOutput->setPosition(nanogui::Vector2i(10, 320));
		auto layout = new nanogui::GridLayout(
			nanogui::Orientation::Horizontal, 1, nanogui::Alignment::Fill, 5,
			0);
		winOutput->setLayout(layout);
		winOutput->setFixedWidth(350);

		winOutput->add<nanogui::Label>("SO(3) rotation matrix", "sans-bold");
		{
			nanogui::Widget* panel = winOutput->add<nanogui::Widget>();
			panel->setLayout(new nanogui::GridLayout(
				nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill,
				5, 0));
			for (int r = 0; r < 3; r++)
				for (int c = 0; c < 3; c++)
					app.ed_out_rot[r][c] = panel->add<nanogui::TextBox>("");
		}

		winOutput->add<nanogui::Label>("In MATLAB-like notation:");
		app.edOutMatrix = winOutput->add<nanogui::TextBox>("");
		app.edOutMatrix->setEditable(true);

		winOutput->add<nanogui::Label>("Quaternion (r,x,y,z)", "sans-bold");
		app.edOutQuat = winOutput->add<nanogui::TextBox>("");
		app.edOutQuat->setEditable(true);

		winOutput->add<nanogui::Label>("Axis-angle (r,x,y,z)", "sans-bold");
		app.edOutAxisAngle_Ax = winOutput->add<nanogui::TextBox>("");
		app.edOutAxisAngle_Ax->setEditable(true);
		app.edOutAxisAngle_An = winOutput->add<nanogui::TextBox>("");
		app.edOutAxisAngle_An->setEditable(true);

		winOutput->add<nanogui::Label>("Axis with angle (log(R))", "sans-bold");
		app.edOutLogSO3 = winOutput->add<nanogui::TextBox>("");
		app.edOutLogSO3->setEditable(true);
	}

	// Add a background scene:
	// -----------------------------
	{
		auto scene = mrpt::opengl::COpenGLScene::Create();
		scene->insert(mrpt::opengl::CGridPlaneXY::Create());

		scene->insert(app.gl_corner_user);
		scene->insert(app.gl_corner_reference);

		auto lck = mrpt::lockHelper(app.win->background_scene_mtx);
		app.win->background_scene = std::move(scene);
	}

	app.win->performLayout();

	app.win->camera().setZoomDistance(5.0f);

	// Update view and process events:
	app.win->drawAll();
	app.win->setVisible(true);

#if !defined(__EMSCRIPTEN__)  // are we in a Wasm JS instance?
	// No: regular procedure:

	nanogui::mainloop();
	nanogui::shutdown();
#else

	// Yes, we are in a web browser running on JS:
	loop = [] {
		// Check if any events have been activated (key pressed, mouse moved
		// etc.) and call corresponding response functions
		glfwPollEvents();

		glClearColor(0.2f, 0.25f, 0.3f, 1.0f);
		glClear(GL_COLOR_BUFFER_BIT);

		// Draw nanogui
		app.win->drawContents();
		app.win->drawWidgets();

		glfwSwapBuffers(app.win->glfwWindow());
	};
	emscripten_set_main_loop(main_loop, 0, true);

	glfwDestroyWindow(app.win->nanogui_screen()->glfwWindow());

	// Terminate GLFW, clearing any resources allocated by GLFW.
	glfwTerminate();

#endif
}

int main()
{
	try
	{
		AppRotationConverter();
		return 0;
	}
	catch (const std::exception& e)
	{
		std::cerr << mrpt::exception_to_str(e) << std::endl;
		return -1;
	}
}


// Copyright (C) 2009  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include <map>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"
#include "checkerboard.h"
#include <dlib/statistics.h>

#include "tester.h"
#include <dlib/svm_threaded.h>

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.kcentroid");

// ----------------------------------------------------------------------------------------

    template <typename T>
    struct unopt_sparse_linear_kernel
    {
        typedef typename T::value_type::second_type scalar_type;
        typedef T sample_type;
        typedef default_memory_manager mem_manager_type;

        scalar_type operator() (
            const sample_type& a,
            const sample_type& b
        ) const
        {
            return dot(a,b);
        }

        bool operator== (
            const unopt_sparse_linear_kernel&
        ) const
        {
            return true;
        }
    };

    template <typename T>
    struct unopt_linear_kernel
    {
        typedef typename T::type scalar_type;
        typedef T sample_type;
        typedef typename T::mem_manager_type mem_manager_type;

        scalar_type operator() (
            const sample_type& a,
            const sample_type& b
        ) const
        {
            return trans(a)*b;
        }

        bool operator== (
            const unopt_linear_kernel&
        ) const
        {
            return true;
        }
    };

    bool approx_equal(double a, double b)
    {
        return (std::abs(a-b) < 1000*(std::numeric_limits<double>::epsilon()));
    }

    bool approx_equal(double a, double b, double eps)
    {
        return (std::abs(a-b) < eps);
    }

    template <typename K>
    double dist (
        const K& k,
        const matrix<double,4,1>& a,
        const matrix<double,5,1>& b
    )
    /*!
        ensures
            - returns the distance between the a and b vectors in the
              feature space defined by the given kernel k.
    !*/
    {
        const double bias = std::sqrt(k.offset);
        return std::sqrt(length_squared(a-colm(b,0,4)) + std::pow(b(4)-bias,2.0));

    }

    template <typename K>
    double dist (
        const K& k,
        std::map<unsigned long,double> a,
        std::map<unsigned long,double> b
    )
    /*!
        ensures
            - returns the distance between the a and b vectors in the
              feature space defined by the given kernel k.
    !*/
    {
        double temp = 0;
        const double bias = std::sqrt(k.offset);
        temp += std::pow(a[0]-b[0],2.0);
        temp += std::pow(a[1]-b[1],2.0);
        temp += std::pow(a[2]-b[2],2.0);
        temp += std::pow(a[3]-b[3],2.0);
        temp += std::pow(bias-b[4],2.0);

        return std::sqrt(temp);

    }

// ----------------------------------------------------------------------------------------

    template <typename kernel_type>
    void test_kcentroid_with_linear_kernel(
    )
    /*!
        requires
            - kernel_type::sample_type == a matrix<double,5,1>
            - kernel_type == a kernel that just computes a dot product
              between its inputs.  I.e. a linear kernel
        ensures
            - tests the kcentroid object with the given kernel
    !*/
    {
        // Here we declare that our samples will be 2 dimensional column vectors.
        typedef typename kernel_type::sample_type sample_type;

        kernel_type default_kernel;
        kcentroid<kernel_type> test(default_kernel,0.001,20);

        sample_type temp, temp2;

        temp = 2,0,0,0,0;
        dlog << LDEBUG << test(temp) ;
        dlog << LDEBUG << "squared_norm(): " << test.squared_norm() ;

        DLIB_TEST(approx_equal(test(temp), 2));
        DLIB_TEST(approx_equal(test.squared_norm(), 0));

        // make test store the point(2,0,0,0,0)
        test.train(temp, 0, 1);
        dlog << LDEBUG << test(temp) ;
        dlog << LDEBUG << "squared_norm(): " << test.squared_norm() ;
        DLIB_TEST(approx_equal(test(temp), 0));
        DLIB_TEST(approx_equal(test.get_distance_function()(temp), 0));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        temp = 0,2,0,0,0;
        dlog << LDEBUG << test(temp) ;
        DLIB_TEST(approx_equal(test(temp), std::sqrt(2*2 + 2*2.0)));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        // make test store the point(0,2,0,0,0)
        test.train(temp, 0, 1);

        dlog << LDEBUG << test(temp) ;
        DLIB_TEST(approx_equal(test(temp), 0));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        temp = 2,0,0,0,0;
        DLIB_TEST(approx_equal(test(temp), std::sqrt(2*2 + 2*2.0)));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        // make test store the point(1,1,0,0,0)
        test.train(temp, 0.5, 0.5);

        temp = 0;
        DLIB_TEST(approx_equal(test(temp), std::sqrt(2.0)));
        DLIB_TEST(approx_equal(test.squared_norm(), 2));

        // make test store the point(1,1,0,3,0)
        temp = 0,0,0,3,0;
        temp2 = 1,1,0,3,0;
        test.train(temp, 1, 1);

        temp = 0;
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        temp = 1,2,3,4,5;
        DLIB_TEST(approx_equal(test(temp), length(temp2-temp)));
        DLIB_TEST(approx_equal(test.get_distance_function()(temp), length(temp2-temp)));

        // make test store the point(0,1,0,3,-1)
        temp = 1,0,0,0,1;
        test.train(temp, 1, -1);
        temp2 = 0,1,0,3,-1;

        temp = 0;
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        temp = 1,2,3,4,5;
        DLIB_TEST(approx_equal(test(temp), length(temp2-temp)));

        // make test store the -1*point(0,1,0,3,-1)
        temp = 0,0,0,0,0;
        test.train(temp, -1, 0);
        temp2 = -temp2;

        temp = 0;
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        temp = 1,2,-3,4,5;
        DLIB_TEST(approx_equal(test(temp), length(temp2-temp)));

        // make test store the point(0,0,0,0,0)
        temp = 0,0,0,0,0;
        test.train(temp, 0, 0);
        temp2 = 0;

        temp = 0;
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        temp = 1,2,-3,4,5;
        DLIB_TEST(approx_equal(test(temp), length(temp2-temp)));

        // make test store the point(1,0,0,0,0)
        temp = 1,0,0,0,0;
        test.train(temp, 1, 1);
        temp2 = 1,0,0,0,0;

        temp = 0;
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        DLIB_TEST(approx_equal(test.inner_product(test), length_squared(temp2)));
        temp = 1,2,-3,4,5;
        DLIB_TEST(approx_equal(test(temp), length(temp2-temp)));
        DLIB_TEST(approx_equal(test(test), 0));
        DLIB_TEST(approx_equal(test.get_distance_function()(test.get_distance_function()), 0));

    }

// ----------------------------------------------------------------------------------------

    template <typename kernel_type>
    void test_kcentroid_with_offset_linear_kernel(
    )
    /*!
        requires
            - kernel_type::sample_type == a matrix<double,4,1>
            - kernel_type == a kernel that just computes a dot product
              between its inputs + some constant.  I.e. a linear kernel
              wrapped by offset_kernel
        ensures
            - tests the kcentroid object with the given kernel
    !*/
    {
        // Here we declare that our samples will be 2 dimensional column vectors.
        typedef typename kernel_type::sample_type sample_type;

        kernel_type k;
        kcentroid<kernel_type> test(k,0.001,20);

        sample_type temp, temp2, temp3;

        matrix<double,5,1> val, val2;

        const double b = std::sqrt(k.offset);

        temp = 2,0,0,0;
        temp2 = 0;
        val = 0;
        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST(approx_equal(test(temp2), dist(k,temp2,val)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));

        temp2 = 0;

        // make test store the point(0,0,0,0,b)
        val = 0,0,0,0,b;
        test.train(temp2, 0,1);

        temp = 2,0,0,0;
        dlog << LDEBUG << test(temp) ;
        dlog << LDEBUG << "squared_norm(): " << test.squared_norm() ;

        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST(approx_equal(test(temp2), dist(k,temp2,val)));
        DLIB_TEST_MSG(approx_equal(test.get_distance_function()(temp2), dist(k,temp2,val), 1e-6),
                     test.get_distance_function()(temp2) - dist(k,temp2,val) << "  compare to: " <<
                     test(temp2) - dist(k,temp2,val)
        );
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));

        // make test store the point(0,0,0,0,0)
        val = 0,0,0,0,0;
        test.train(temp2, 1,-1);
        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST(approx_equal(test(temp2), dist(k,temp2,val)));
        DLIB_TEST_MSG(approx_equal(test.get_distance_function()(temp2), dist(k,temp2,val)),
                     test.get_distance_function()(temp2) - dist(k,temp2,val) << "  compare to: " <<
                     test(temp2) - dist(k,temp2,val)
        );
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));

        val2 = 0,1,0,0,b;
        val += val2;
        temp2 = 0,1,0,0;
        // make test store the point val
        test.train(temp2, 1,1);

        temp = 1,0,3,0;
        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST_MSG(approx_equal(test(temp2), dist(k,temp2,val), 1e-7),
                     test(temp2) - dist(k,temp2,val));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));
        DLIB_TEST_MSG(approx_equal(test(test), 0, 1e-7), test(test));

        val2 =  0,1,2.6,8,b;
        val += val2;
        temp2 = 0,1,2.6,8;
        // make test store the point val
        test.train(temp2, 1,1);

        temp = 1,1,3,0;
        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST_MSG(approx_equal(test(temp2), dist(k,temp2,val)), test(temp2) - dist(k,temp2,val));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));
        DLIB_TEST(approx_equal(test.inner_product(test), length_squared(val)));
        DLIB_TEST(approx_equal(test(test), 0));
        DLIB_TEST_MSG(approx_equal(test.get_distance_function()(test.get_distance_function()), 0, 1e-6),
                     test.get_distance_function()(test.get_distance_function()));
    }

// ----------------------------------------------------------------------------------------

    template <typename kernel_type>
    void test_kcentroid_with_sparse_linear_kernel(
    )
    /*!
        requires
            - kernel_type::sample_type == a std::map<unsigned long,double>
            - kernel_type == a kernel that just computes a dot product
              between its inputs.  I.e. a linear kernel
        ensures
            - tests the kcentroid object with the given kernel
    !*/
    {
        // Here we declare that our samples will be 2 dimensional column vectors.
        typedef typename kernel_type::sample_type sample_type;

        kernel_type default_kernel;
        kcentroid<kernel_type> test(default_kernel,0.001,20);

        dlog << LDEBUG << "AAAA 1" ;

        sample_type temp, temp2;

        temp[0] = 2;
        dlog << LDEBUG << test(temp) ;
        dlog << LDEBUG << "squared_norm(): " << test.squared_norm() ;

        DLIB_TEST(approx_equal(test(temp), 2));
        DLIB_TEST(approx_equal(test.squared_norm(), 0));

        // make test store the point(2,0,0,0,0)
        test.train(temp, 0, 1);
        dlog << LDEBUG << test(temp) ;
        dlog << LDEBUG << "squared_norm(): " << test.squared_norm() ;
        DLIB_TEST(approx_equal(test(temp), 0));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        dlog << LDEBUG << "AAAA 2" ;
        temp.clear();
        temp[1] = 2;
        dlog << LDEBUG << test(temp) ;
        DLIB_TEST(approx_equal(test(temp), std::sqrt(2*2 + 2*2.0)));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        // make test store the point(0,2,0,0,0)
        test.train(temp, 0, 1);

        dlog << LDEBUG << test(temp) ;
        DLIB_TEST(approx_equal(test(temp), 0));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        temp.clear();
        temp[0] = 2;
        DLIB_TEST(approx_equal(test(temp), std::sqrt(2*2 + 2*2.0)));
        DLIB_TEST(approx_equal(test.squared_norm(), 4));

        // make test store the point(1,1,0,0,0)
        test.train(temp, 0.5, 0.5);

        dlog << LDEBUG << "AAAA 3" ;
        temp.clear();
        DLIB_TEST(approx_equal(test(temp), std::sqrt(2.0)));
        DLIB_TEST(approx_equal(test.squared_norm(), 2));
        DLIB_TEST(approx_equal(test(test), 0));
        DLIB_TEST(approx_equal(test.get_distance_function()(test.get_distance_function()), 0));

        dlog << LDEBUG << "AAAA 3.1" ;
        // make test store the point(1,1,0,3,0)
        temp.clear(); temp[3] = 3;
        temp2.clear();
        temp2[0] = 1;
        temp2[1] = 1;
        temp2[3] = 3;
        test.train(temp, 1, 1);

        dlog << LDEBUG << "AAAA 3.2" ;
        temp.clear();
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        dlog << LDEBUG << "AAAA 3.3" ;
        temp[0] = 1;
        temp[1] = 2;
        temp[2] = 3;
        temp[3] = 4;
        temp[4] = 5;
        dlog << LDEBUG << "AAAA 3.4" ;
        double junk = dlib::distance(temp2,temp);
        dlog << LDEBUG << "AAAA 3.5" ;
        DLIB_TEST(approx_equal(test(temp), junk) );

        dlog << LDEBUG << "AAAA 4" ;
        // make test store the point(0,1,0,3,-1)
        temp.clear();
        temp[0] = 1;
        temp[4] = 1;
        test.train(temp, 1, -1);
        temp2.clear();
        temp2[1] = 1;
        temp2[3] = 3;
        temp2[4] = -1;

        temp.clear();
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        temp[0] = 1;
        temp[1] = 2;
        temp[2] = 3;
        temp[3] = 4;
        temp[4] = 5;
        DLIB_TEST(approx_equal(test(temp), dlib::distance(temp2,temp)));

        // make test store the -1*point(0,1,0,3,-1)
        temp.clear();
        test.train(temp, -1, 0);
        temp2[0] = -temp2[0];
        temp2[1] = -temp2[1];
        temp2[2] = -temp2[2];
        temp2[3] = -temp2[3];
        temp2[4] = -temp2[4];

        dlog << LDEBUG << "AAAA 5" ;
        temp.clear();
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        temp[0] = 1;
        temp[1] = 2;
        temp[2] = -3;
        temp[3] = 4;
        temp[4] = 5;
        DLIB_TEST(approx_equal(test(temp), dlib::distance(temp2,temp)));

        // make test store the point(0,0,0,0,0)
        temp.clear();
        test.train(temp, 0, 0);
        temp2.clear();

        temp.clear();
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        temp[0] = 1;
        temp[1] = 2;
        temp[2] = -3;
        temp[3] = 4;
        temp[4] = 5;
        DLIB_TEST(approx_equal(test(temp), dlib::distance(temp2,temp)));
        DLIB_TEST(approx_equal(test.get_distance_function()(temp), dlib::distance(temp2,temp)));

        dlog << LDEBUG << "AAAA 6" ;

        // make test store the point(1,0,0,0,0)
        temp.clear();
        temp[0] = 1;
        test.train(temp, 1, 1);
        temp2.clear();
        temp2[0] = 1;

        temp.clear();
        DLIB_TEST(approx_equal(test(temp), length(temp2)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(temp2)));
        DLIB_TEST(approx_equal(test.inner_product(test), length_squared(temp2)));
        temp[0] = 1;
        temp[1] = 2;
        temp[2] = -3;
        temp[3] = 4;
        temp[4] = 5;
        DLIB_TEST(approx_equal(test(temp), dlib::distance(temp2,temp)));
        DLIB_TEST(approx_equal(test.get_distance_function()(temp), dlib::distance(temp2,temp)));
        DLIB_TEST(approx_equal(test(test), 0));
        DLIB_TEST(approx_equal(test.get_distance_function()(test.get_distance_function()), 0));

        dlog << LDEBUG << "AAAA 7" ;
    }

// ----------------------------------------------------------------------------------------

    template <typename kernel_type>
    void test_kcentroid_with_offset_sparse_linear_kernel(
    )
    /*!
        requires
            - kernel_type::sample_type == a std::map<unsigned long,double>
            - kernel_type == a kernel that just computes a dot product
              between its inputs + some constant.  I.e. a linear kernel
              wrapped by offset_kernel
        ensures
            - tests the kcentroid object with the given kernel
    !*/
    {
        // Here we declare that our samples will be 2 dimensional column vectors.
        typedef typename kernel_type::sample_type sample_type;

        kernel_type k;
        kcentroid<kernel_type> test(k,0.001,20);

        sample_type temp, temp2, temp3;

        std::map<unsigned long,double> val, val2;

        const double b = std::sqrt(k.offset);

        temp.clear();
        temp[0] = 2;
        temp2.clear();
        val.clear();
        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST(approx_equal(test(temp2), dist(k,temp2,val)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));

        temp2.clear();

        // make test store the point(0,0,0,0,b)
        val.clear();
        val[4] = b;
        test.train(temp2, 0,1);

        temp.clear();
        temp[0] = 2;
        dlog << LDEBUG << test(temp) ;
        dlog << LDEBUG << "squared_norm(): " << test.squared_norm() ;

        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST(approx_equal(test(temp2), dist(k,temp2,val)));
        DLIB_TEST_MSG(approx_equal(test.get_distance_function()(temp2), dist(k,temp2,val), 1e-7),
                     test.get_distance_function()(temp2) - dist(k,temp2,val)
        );
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));
        DLIB_TEST(approx_equal(test(test), 0));
        DLIB_TEST(approx_equal(test.get_distance_function()(test.get_distance_function()), 0, 1e-6));

        // make test store the point(0,0,0,0,0)
        val.clear();
        test.train(temp2, 1,-1);

        temp.clear();
        temp[0] = 2;
        dlog << LDEBUG << test(temp) ;
        dlog << LDEBUG << "squared_norm(): " << test.squared_norm() ;

        DLIB_TEST_MSG(approx_equal(test(temp), dist(k,temp,val)), test(temp) - dist(k,temp,val));
        DLIB_TEST(approx_equal(test(temp2), dist(k,temp2,val)));
        DLIB_TEST(approx_equal(test.get_distance_function()(temp2), dist(k,temp2,val)));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));
        DLIB_TEST(approx_equal(test(test), 0));
        DLIB_TEST(approx_equal(test.get_distance_function()(test.get_distance_function()), 0));

        val2.clear();
        val2[0] = 0;
        val2[1] = 1;
        val2[2] = 0;
        val2[3] = 0;
        val2[4] = b;
        for (unsigned int i = 0; i < 5; ++i) val[i] += val2[i];
        temp2.clear();
        temp2[1] = 1;
        // make test store the point val
        test.train(temp2, 1,1);

        temp.clear();
        temp[0] = 1;
        temp[2] = 3;
        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST_MSG(approx_equal(test(temp2), dist(k,temp2,val), 1e-7),
                     test(temp2) - dist(k,temp2,val));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));

        val2.clear();
        val2[0] = 0;
        val2[1] = 1;
        val2[2] = 2.6;
        val2[3] = 8;
        val2[4] = b;
        for (unsigned int i = 0; i < 5; ++i) val[i] += val2[i];

        temp2.clear();
        temp2[0] = 0;
        temp2[1] = 1;
        temp2[2] = 2.6;
        temp2[3] = 8;
        // make test store the point val
        test.train(temp2, 1,1);

        temp.clear();
        temp[0] = 1;
        temp[1] = 1;
        temp[2] = 3;
        temp[3] = 0;
        DLIB_TEST(approx_equal(test(temp), dist(k,temp,val)));
        DLIB_TEST_MSG(approx_equal(test(temp2), dist(k,temp2,val)), test(temp2) - dist(k,temp2,val));
        DLIB_TEST(approx_equal(test.squared_norm(), length_squared(val)));
        DLIB_TEST(approx_equal(test.inner_product(test), length_squared(val)));
        DLIB_TEST_MSG(approx_equal(test(test), 0, 1e-6), test(test));
        DLIB_TEST(approx_equal(test.get_distance_function()(test.get_distance_function()), 0));
    }

// ----------------------------------------------------------------------------------------

    class kcentroid_tester : public tester
    {
    public:
        kcentroid_tester (
        ) :
            tester ("test_kcentroid",
                    "Runs tests on the kcentroid components.")
        {}

        void perform_test (
        )
        {
            // The idea here is to exercize all the various overloads of the kcentroid object.  We also want
            // to exercize the non-overloaded default version.  That is why we have these unopt_* linear
            // kernels
            test_kcentroid_with_linear_kernel<linear_kernel<matrix<double,5,1> > >();
            test_kcentroid_with_offset_linear_kernel<offset_kernel<linear_kernel<matrix<double,4,1> > > >();
            test_kcentroid_with_linear_kernel<unopt_linear_kernel<matrix<double,5,1> > >();
            test_kcentroid_with_offset_linear_kernel<offset_kernel<unopt_linear_kernel<matrix<double,4,1> > > >();
            test_kcentroid_with_sparse_linear_kernel<sparse_linear_kernel<std::map<unsigned long,double> > >();
            test_kcentroid_with_offset_sparse_linear_kernel<offset_kernel<sparse_linear_kernel<std::map<unsigned long,double> > > >();
            test_kcentroid_with_sparse_linear_kernel<unopt_sparse_linear_kernel<std::map<unsigned long,double> > >();
            test_kcentroid_with_offset_sparse_linear_kernel<offset_kernel<unopt_sparse_linear_kernel<std::map<unsigned long,double> > > >();
        }
    } a;

}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;
using namespace literals;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(
        concat(list(1, '2'), list(3.3, 4_c)) == list(1, '2', 3.3, 4_c)
    );
    //! [main]
}