/**
 * @file feedforward_network_test.cpp
 * @author Marcus Edel
 *
 * Tests the feed forward network.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>
#include <mlpack/methods/ann/activation_functions/tanh_function.hpp>

#include <mlpack/methods/ann/init_rules/random_init.hpp>

#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/linear_layer.hpp>
#include <mlpack/methods/ann/layer/base_layer.hpp>
#include <mlpack/methods/ann/layer/dropout_layer.hpp>
#include <mlpack/methods/ann/layer/binary_classification_layer.hpp>

#include <mlpack/methods/ann/trainer/trainer.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/performance_functions/mse_function.hpp>
#include <mlpack/methods/ann/optimizer/rmsprop.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(FeedForwardNetworkTest);

/**
 * Train and evaluate a vanilla network with the specified structure.
 */
template<
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void BuildVanillaNetwork(MatType& trainData,
                         MatType& trainLabels,
                         MatType& testData,
                         MatType& testLabels,
                         const size_t hiddenLayerSize,
                         const size_t maxEpochs,
                         const double classificationErrorThreshold,
                         const double ValidationErrorThreshold)
{
  /*
   * Construct a feed forward network with trainData.n_rows input nodes,
   * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
   * network structure looks like:
   *
   *  Input         Hidden        Output
   *  Layer         Layer         Layer
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |     +>|     |     +>|     |
   * +-----+     | +--+--+     | +-----+
   *             |             |
   *  Bias       |  Bias       |
   *  Layer      |  Layer      |
   * +-----+     | +-----+     |
   * |     |     | |     |     |
   * |     +-----+ |     +-----+
   * |     |       |     |
   * +-----+       +-----+
   */

  LinearLayer<> inputLayer(trainData.n_rows, hiddenLayerSize);
  BiasLayer<> inputBiasLayer(hiddenLayerSize);
  BaseLayer<PerformanceFunction> inputBaseLayer;

  LinearLayer<> hiddenLayer1(hiddenLayerSize, trainLabels.n_rows);
  BiasLayer<> hiddenBiasLayer1(trainLabels.n_rows);
  BaseLayer<PerformanceFunction> outputLayer;

  OutputLayerType classOutputLayer;

  auto modules = std::tie(inputLayer, inputBiasLayer, inputBaseLayer,
  						  hiddenLayer1, hiddenBiasLayer1, outputLayer);

  FFN<decltype(modules), decltype(classOutputLayer), PerformanceFunctionType>
      net(modules, classOutputLayer);

  Trainer<decltype(net)> trainer(net, maxEpochs, 1, 0.01);
  trainer.Train(trainData, trainLabels, testData, testLabels);

  MatType prediction;
  size_t error = 0;

  for (size_t i = 0; i < testData.n_cols; i++)
  {
  	MatType predictionInput = testData.unsafe_col(i);
  	MatType targetOutput = testLabels.unsafe_col(i);

    net.Predict(predictionInput, prediction);

    if (arma::sum(arma::sum(arma::abs(prediction - targetOutput))) == 0)
      error++;
  }

  double classificationError = 1 - double(error) / testData.n_cols;

  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
  BOOST_REQUIRE_LE(trainer.ValidationError(), ValidationErrorThreshold);
}

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(VanillaNetworkTest)
{
  // Load the dataset.
  arma::mat dataset;
  data::Load("thyroid_train.csv", dataset, true);

  arma::mat trainData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat trainLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  data::Load("thyroid_test.csv", dataset, true);

  arma::mat testData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat testLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildVanillaNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (trainData, trainLabels, testData, testLabels, 4, 500, 0.1, 60);

  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);

  // Vanilla neural net with logistic activation function.
  BuildVanillaNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (dataset, labels, dataset, labels, 30, 100, 0.6, 10);

  // Vanilla neural net with tanh activation function.
  BuildVanillaNetwork<TanhFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
    (dataset, labels, dataset, labels, 10, 200, 0.6, 20);
}

/**
 * Train and evaluate a Dropout network with the specified structure.
 */
template<
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void BuildDropoutNetwork(MatType& trainData,
                         MatType& trainLabels,
                         MatType& testData,
                         MatType& testLabels,
                         const size_t hiddenLayerSize,
                         const size_t maxEpochs,
                         const double classificationErrorThreshold,
                         const double ValidationErrorThreshold)
{
  /*
   * Construct a feed forward network with trainData.n_rows input nodes,
   * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
   * network structure looks like:
   *
   *  Input         Hidden        Dropout      Output
   *  Layer         Layer         Layer        Layer
   * +-----+       +-----+       +-----+       +-----+
   * |     |       |     |       |     |       |     |
   * |     +------>|     +------>|     +------>|     |
   * |     |     +>|     |       |     |       |     |
   * +-----+     | +--+--+       +-----+       +-----+
   *             |
   *  Bias       |
   *  Layer      |
   * +-----+     |
   * |     |     |
   * |     +-----+
   * |     |
   * +-----+
   */

  LinearLayer<> inputLayer(trainData.n_rows, hiddenLayerSize);
  BiasLayer<> biasLayer(hiddenLayerSize);
  BaseLayer<PerformanceFunction> hiddenLayer0;
  DropoutLayer<> dropoutLayer0;

  LinearLayer<> hiddenLayer1(hiddenLayerSize, trainLabels.n_rows);
  BaseLayer<PerformanceFunction> outputLayer;

  OutputLayerType classOutputLayer;

  auto modules = std::tie(inputLayer, biasLayer, hiddenLayer0, dropoutLayer0,
      					  hiddenLayer1, outputLayer);

  FFN<decltype(modules), decltype(classOutputLayer), PerformanceFunctionType>
      net(modules, classOutputLayer);

  Trainer<decltype(net)> trainer(net, maxEpochs, 1, 0.001);
  trainer.Train(trainData, trainLabels, testData, testLabels);

  MatType prediction;
  size_t error = 0;

  for (size_t i = 0; i < testData.n_cols; i++)
  {
  	MatType input = testData.unsafe_col(i);
    net.Predict(input, prediction);
    if (arma::sum(arma::sum(arma::abs(
    	prediction - testLabels.unsafe_col(i)))) == 0)
      error++;
  }

  double classificationError = 1 - double(error) / testData.n_cols;

  BOOST_REQUIRE_LE(classificationError, classificationErrorThreshold);
  BOOST_REQUIRE_LE(trainer.ValidationError(), ValidationErrorThreshold);
}

/**
 * Train the dropout network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(DropoutNetworkTest)
{
  // Load the dataset.
  arma::mat dataset;
  data::Load("thyroid_train.csv", dataset, true);

  arma::mat trainData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat trainLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  data::Load("thyroid_test.csv", dataset, true);

  arma::mat testData = dataset.submat(0, 0, dataset.n_rows - 4,
      dataset.n_cols - 1);
  arma::mat testLabels = dataset.submat(dataset.n_rows - 3, 0,
      dataset.n_rows - 1, dataset.n_cols - 1);

  // Vanilla neural net with logistic activation function.
  // Because 92 percent of the patients are not hyperthyroid the neural
  // network must be significant better than 92%.
  BuildDropoutNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (trainData, trainLabels, testData, testLabels, 4, 100, 0.1, 60);

  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1).fill(1);

  // Vanilla neural net with logistic activation function.
  BuildVanillaNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (dataset, labels, dataset, labels, 8, 100, 0.6, 10);

  // Vanilla neural net with tanh activation function.
  BuildVanillaNetwork<TanhFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
    (dataset, labels, dataset, labels, 8, 100, 0.6, 20);
}

/**
 * Train the network until the validation error converge.
 */
BOOST_AUTO_TEST_CASE(VanillaNetworkConvergenceTest)
{
  arma::mat input;
  arma::mat labels;

  // Test on a non-linearly separable dataset (XOR).
  input << 0 << 1 << 1 << 0 << arma::endr
        << 1 << 0 << 1 << 0 << arma::endr;
  labels << 0 << 0 << 1 << 1;

  // Vanilla neural net with logistic activation function.
  BuildVanillaNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (input, labels, input, labels, 4, 5000, 0, 0.01);

  // Vanilla neural net with tanh activation function.
  BuildVanillaNetwork<TanhFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (input, labels, input, labels, 4, 5000, 0, 0.01);

  // Test on a linearly separable dataset (AND).
  input << 0 << 1 << 1 << 0 << arma::endr
        << 1 << 0 << 1 << 0 << arma::endr;
  labels << 0 << 0 << 1 << 0;

  // vanilla neural net with sigmoid activation function.
  BuildVanillaNetwork<LogisticFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
    (input, labels, input, labels, 4, 5000, 0, 0.01);

  // Vanilla neural net with tanh activation function.
  BuildVanillaNetwork<TanhFunction,
                      BinaryClassificationLayer,
                      MeanSquaredErrorFunction>
      (input, labels, input, labels, 4, 5000, 0, 0.01);
}

/**
 * Train a vanilla network with the specified structure step by step and
 * evaluate the network.
 */
template<
    typename PerformanceFunction,
    typename OutputLayerType,
    typename PerformanceFunctionType,
    typename MatType = arma::mat
>
void BuildNetworkOptimzer(MatType& trainData,
                          MatType& trainLabels,
                          MatType& testData,
                          MatType& testLabels,
                          size_t hiddenLayerSize,
                          size_t epochs)
{
  /*
   * Construct a feed forward network with trainData.n_rows input nodes,
   * hiddenLayerSize hidden nodes and trainLabels.n_rows output nodes. The
   * network structure looks like:
   *
   *  Input         Hidden        Output
   *  Layer         Layer         Layer
   * +-----+       +-----+       +-----+
   * |     |       |     |       |     |
   * |     +------>|     +------>|     |
   * |     |     +>|     |     +>|     |
   * +-----+     | +--+--+     | +-----+
   *             |             |
   *  Bias       |  Bias       |
   *  Layer      |  Layer      |
   * +-----+     | +-----+     |
   * |     |     | |     |     |
   * |     +-----+ |     +-----+
   * |     |       |     |
   * +-----+       +-----+
   */

  RandomInitialization randInit(0.5, 0.5);

  LinearLayer<RMSPROP, RandomInitialization> inputLayer(trainData.n_rows,
      hiddenLayerSize, randInit);
  BiasLayer<RMSPROP, RandomInitialization> inputBiasLayer(hiddenLayerSize,
      1, randInit);
  BaseLayer<PerformanceFunction> inputBaseLayer;

  LinearLayer<RMSPROP, RandomInitialization> hiddenLayer1(hiddenLayerSize,
      trainLabels.n_rows, randInit);
  BiasLayer<RMSPROP, RandomInitialization> hiddenBiasLayer1(trainLabels.n_rows,
      1, randInit);
  BaseLayer<PerformanceFunction> outputLayer;

  OutputLayerType classOutputLayer;

  auto modules = std::tie(inputLayer, inputBiasLayer, inputBaseLayer,
  						  hiddenLayer1, hiddenBiasLayer1, outputLayer);

  FFN<decltype(modules), OutputLayerType, PerformanceFunctionType>
      net(modules, classOutputLayer);

  Trainer<decltype(net)> trainer(net, epochs, 1, 0.0001, false);

  double error = DBL_MAX;
  for (size_t i = 0; i < 5; i++)
  {
    trainer.Train(trainData, trainLabels, testData, testLabels);
    double validationError = trainer.ValidationError();

    bool b = validationError < error || validationError == 0;
    BOOST_REQUIRE_EQUAL(b, 1);

    error = validationError;
  }
}

/**
 * Train the network with different optimzer and check if the error decreases
 * over time.
 */
BOOST_AUTO_TEST_CASE(NetworkDecreasingErrorTest)
{
  arma::mat dataset;
  dataset.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  for (size_t i = 0; i < dataset.n_cols; ++i)
    dataset.col(i) /= norm(dataset.col(i), 2);

  arma::mat labels = arma::zeros(1, dataset.n_cols);
  labels.submat(0, labels.n_cols / 2, 0, labels.n_cols - 1) += 1;

  // Vanilla neural net with logistic activation function.
  BuildNetworkOptimzer<LogisticFunction,
                       BinaryClassificationLayer,
                       MeanSquaredErrorFunction>
      (dataset, labels, dataset, labels, 20, 15);
}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( eps_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::integer_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()( A0 ) const BOOST_NOEXCEPT
    {
      return static_cast<A0>(1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( eps_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    A0 operator()(A0 a0) const BOOST_NOEXCEPT
    {
      using lim = std::numeric_limits<A0>;

      const A0 a = bs::abs(a0);

            if (is_invalid(a))  return Nan<A0>();
      else  if (a < lim::min()) return Mindenormal<A0>();
      else
      {
        using i_t = bd::as_integer_t<A0, unsigned>;

        i_t e1 = exponent(a)-lim::digits+1;
        return bitwise_cast<A0>(bitwise_cast<i_t>(A0(1))+(e1 << Nbmantissabits<A0>()));
      }
    }
  };
} } }

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing asinpi capabilities

    inverse sine in \f$\pi\f$ multiples.
    @par Semantic:

    For every parameter of floating type T

    @code
    T r = asinpi(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[-0.5, 0.5[\f$ such that <tt>cos(r) == x</tt>.
    If @c x is outside \f$[-1, 1[\f$ the result is Nan.

  **/
  const boost::dispatch::functor<tag::asinpi_> asinpi = {};
} }
#endif

#include <boost/simd/function/scalar/asinpi.hpp>
#include <boost/simd/function/simd/asinpi.hpp>

#endif


/**
 *  .file test/oglplus/angle.cpp
 *  .brief Test case for Angle class and related functionality.
 *
 *  .author Matus Chochlik
 *
 *  Copyright 2011-2015 Matus Chochlik. Distributed under the Boost
 *  Software License, Version 1.0. (See accompanying file
 *  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE OGLPLUS_Angle
#include <boost/test/unit_test.hpp>

#include <oglplus/gl.hpp>
#include <oglplus/math/angle.hpp>

BOOST_AUTO_TEST_SUITE(Angle)

BOOST_AUTO_TEST_CASE(Angle_default_construction)
{
	oglplus::Angle<float> af;
	oglplus::Angle<double> ad;
}

BOOST_AUTO_TEST_CASE(Angle_construction)
{
	typedef oglplus::Angle<float> Anglef;
	Anglef a1 = Anglef::Degrees(90);
	Anglef a2 = Anglef::Radians(oglplus::math::HalfPi());
	Anglef a3 = oglplus::Degrees(90);
	Anglef a4 = oglplus::Radians(oglplus::math::HalfPi());
	Anglef a5 = oglplus::RightAngles(1.0f);
	Anglef a6 = oglplus::FullCircles(1.0f);
	Anglef a7 = a6;
}

BOOST_AUTO_TEST_CASE(Angle_value)
{
	typedef oglplus::Angle<float> Anglef;

	Anglef a1 = Anglef::Radians(oglplus::math::HalfPi());
	Anglef a2 = Anglef::Radians(oglplus::math::Pi());
	Anglef a3 = Anglef::Radians(oglplus::math::TwoPi());

	BOOST_CHECK_EQUAL(a1.ValueInDegrees(), 90);
	BOOST_CHECK_EQUAL(a2.ValueInDegrees(), 180);
	BOOST_CHECK_EQUAL(a3.ValueInDegrees(), 360);
	BOOST_CHECK_EQUAL(a1.Value(), float(oglplus::math::HalfPi()));
	BOOST_CHECK_EQUAL(a2.Value(), float(oglplus::math::Pi()));
	BOOST_CHECK_EQUAL(a3.Value(), float(oglplus::math::TwoPi()));

	Anglef a4 = oglplus::RightAngles(1.0f);
	Anglef a5 = oglplus::FullCircles(1.0f);

	BOOST_CHECK_EQUAL(a1.ValueInDegrees(), a4.ValueInDegrees());
	BOOST_CHECK_EQUAL(a3.ValueInDegrees(), a5.ValueInDegrees());
}

BOOST_AUTO_TEST_CASE(Angle_cmp)
{
	typedef oglplus::Angle<float> Anglef;

	Anglef a1 = Anglef::Radians(oglplus::math::HalfPi());
	Anglef a2 = Anglef::Radians(oglplus::math::TwoPi());

	Anglef a3 = oglplus::RightAngles(1.0f);
	Anglef a4 = oglplus::FullCircles(1.0f);

	BOOST_CHECK(a1 == a1);
	BOOST_CHECK(a2 == a2);

	BOOST_CHECK(a1 != a2);
	BOOST_CHECK(a3 != a4);

	BOOST_CHECK(a1 == a3);
	BOOST_CHECK(a2 == a4);
}

BOOST_AUTO_TEST_CASE(Angle_addition)
{
	typedef oglplus::Angle<double> Angled;

	Angled a0;
	Angled a1 = Angled::Radians(oglplus::math::HalfPi());
	Angled a2 = Angled::Radians(oglplus::math::Pi());
	Angled a3 = Angled::Radians(oglplus::math::TwoPi());

	BOOST_CHECK((a0+a0) == a0);
	BOOST_CHECK((a0+a1) == a1);
	BOOST_CHECK((a1+a1) == a2);
	BOOST_CHECK((a2+a2) == a3);
	BOOST_CHECK((a1+a2+a1) == a3);
	BOOST_CHECK((a1+a1+a1+a1) == a3);
}

BOOST_AUTO_TEST_CASE(Angle_subtraction)
{
	typedef oglplus::Angle<double> Angled;

	Angled a0;
	Angled a1 = Angled::Radians(oglplus::math::HalfPi());
	Angled a2 = Angled::Radians(oglplus::math::Pi());
	Angled a3 = Angled::Radians(oglplus::math::TwoPi());

	BOOST_CHECK((a0-a0) == a0);
	BOOST_CHECK((a1-a1) == a0);
	BOOST_CHECK((a1-a0) == a1);
	BOOST_CHECK((a0-a1) ==-a1);
	BOOST_CHECK((a2-a1) == a1);
	BOOST_CHECK((-a1+a2) == a1);
	BOOST_CHECK((a3-a2-a1) == a1);
	BOOST_CHECK((a3-a2-a1-a1) == a0);
	BOOST_CHECK((-a1-a2+a3) == a1);
	BOOST_CHECK((-a1-a1-a1-a1) == -a3);
	BOOST_CHECK(-(-a1-a1-a2) == a3);
}

BOOST_AUTO_TEST_CASE(Angle_multiplication)
{
	typedef oglplus::Angle<double> Angled;

	Angled a0;
	Angled a1 = Angled::Radians(oglplus::math::HalfPi());
	Angled a2 = Angled::Radians(oglplus::math::Pi());
	Angled a3 = Angled::Radians(oglplus::math::TwoPi());

	BOOST_CHECK((2*a0) == a0);
	BOOST_CHECK((0*a1) == a0);
	BOOST_CHECK((2*a1) == a2);
	BOOST_CHECK((2*a2) == a3);
	BOOST_CHECK(a1 == (a2*0.5));
	BOOST_CHECK((4*a1) == (2*a2));
	BOOST_CHECK((2.1*a1) == (a1*2.1));
	BOOST_CHECK((3.0*a1) == (a2*1.5));
	BOOST_CHECK((2.4*a1) == (a2*1.2));
	BOOST_CHECK((4.4*a1) == (a3*1.1));
	BOOST_CHECK((oglplus::math::TwoPi()*a1) == (a2*oglplus::math::Pi()));
}

BOOST_AUTO_TEST_CASE(Angle_division)
{
	typedef oglplus::Angle<double> Angled;

	Angled a0;
	Angled a1 = Angled::Radians(oglplus::math::HalfPi());
	Angled a2 = Angled::Radians(oglplus::math::Pi());
	Angled a3 = Angled::Radians(oglplus::math::TwoPi());

	BOOST_CHECK(a0 == (a0/2));
	BOOST_CHECK(a1 == (a2/2));
	BOOST_CHECK(a2 == (a3/2));
	BOOST_CHECK(a1 == (a3/4));
	BOOST_CHECK((a1/3) == (a2/6));
	BOOST_CHECK((a1/3) == (a3/12));
	BOOST_CHECK((a1/oglplus::math::Pi()) == (a2/oglplus::math::TwoPi()));
}

BOOST_AUTO_TEST_CASE(Angle_arithmetic)
{
	typedef oglplus::Angle<double> Angled;

	Angled a0;
	Angled a1 = Angled::Radians(oglplus::math::HalfPi());
	Angled a2 = Angled::Radians(oglplus::math::Pi());
	Angled a3 = Angled::Radians(oglplus::math::TwoPi());

	BOOST_CHECK((a1+a1+a1) == (3.0*a2)/2.0);
	BOOST_CHECK((a1+a0) == (a1-a0));
	BOOST_CHECK(2.0*(-a1-a2+a3) == a3/2.0);
	BOOST_CHECK((9*a1-4*a2)*3.0+a1 == a3);
}

BOOST_AUTO_TEST_CASE(Angle_sin_cos)
{
	typedef oglplus::Angle<double> Angled;

	Angled a0;
	Angled a1 = Angled::Radians(oglplus::math::HalfPi());
	Angled a2 = Angled::Radians(oglplus::math::Pi());
	Angled a3 = Angled::Radians(oglplus::math::TwoPi());
	double eps = 1e-9;

	BOOST_CHECK_EQUAL(Sin(a0), 0.0);
	BOOST_CHECK_EQUAL(Cos(a0), 1.0);

	BOOST_CHECK_CLOSE(Sin(a1), 1.0, eps);
	BOOST_CHECK_CLOSE(Sin(a2)+1.0, 1.0, eps);
	BOOST_CHECK_CLOSE(Sin(a2)+1.0, Sin(a0)+1.0, eps);
	BOOST_CHECK_CLOSE(Sin(a2+a1), -1.0, eps);

	BOOST_CHECK_CLOSE(Cos(a1)+1.0, 1.0, eps);
	BOOST_CHECK_CLOSE(Cos(a2), -1.0, eps);
	BOOST_CHECK_CLOSE(Cos(a2+a1)+1.0, 1.0, eps);

	BOOST_CHECK_CLOSE(Sin(a1), Sin(100000*a3+a1), eps);
	BOOST_CHECK_CLOSE(Cos(a0), Cos(100000*a3+a0), eps);

	BOOST_CHECK(Sin(a2+a1) == Sin(a3-a1));
	BOOST_CHECK(Cos(a2+a1) == Cos(a3-a1));
}

BOOST_AUTO_TEST_CASE(Angle_sin_cos_2)
{
	typedef oglplus::Angle<double> Angled;
	Angled a3 = Angled::Radians(oglplus::math::TwoPi());

	double eps = 1e-8;
	double d = 1.6181;

	for(unsigned i=0; i!=1000; ++i)
	{
		Angled a = Angled::Radians(i*i*d);
		BOOST_CHECK_CLOSE(Sin(a)+2.0, Sin(a+a3*i)+2.0, eps);
		BOOST_CHECK_CLOSE(Cos(a)+2.0, Cos(a+a3*i)+2.0, eps);
	}
}

BOOST_AUTO_TEST_CASE(Angle_tan)
{
	typedef oglplus::Angle<double> Angled;

	double eps = 1e-9;
	double d = 1.6181;

	for(unsigned i=0; i!=1000; ++i)
	{
		Angled a = Angled::Radians(i*i*d);
		BOOST_CHECK_CLOSE(Sin(a)/Cos(a)+2.0, Tan(a)+2.0, eps);
	}
}

BOOST_AUTO_TEST_CASE(Angle_arc)
{
	using oglplus::ArcSin;
	using oglplus::ArcCos;

	typedef oglplus::Angle<double> Angled;

	double eps = 2;

	for(unsigned i=1; i!=1000; ++i)
	{
		Angled a = Angled::Radians(oglplus::math::HalfPi()/i);
		BOOST_CHECK_CLOSE(a.Value(), ArcSin(Sin(a)).Value(), eps);
		BOOST_CHECK_CLOSE(a.Value(), ArcCos(Cos(a)).Value(), eps);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include "../../src/worldmap/TrICP.h"
#include "../../src/Util.h"

#include <Eigen/LU>
#include <iostream>

#include <catch2/catch.hpp>

#include "../../src/worldmap/GlobalMap.h"

using namespace navtypes;
using namespace util;

namespace
{
double rand(double low, double high)
{
	return low + (::rand() / (RAND_MAX / (high - low))); // NOLINT(cert-msc50-cpp)
}
} // namespace

TEST_CASE("Trimmed ICP")
{
	points_t map;
	points_t sample;
	points_t truths;
	transform_t trf = toTransformRotateFirst(0.1, -0.25, M_PI / 24);
	srand(time(nullptr)); // NOLINT(cert-msc51-cpp)
	for (int i = 0; i < 150; i++)
	{
		double x1 = rand(-6, 2);
		double y1 = pow(x1, 3);
		map.push_back({x1, y1, 1});

		double x2 = rand(-2, 6);
		double y2 = pow(x2, 3);
		point_t p = {x2, y2, 1};
		truths.push_back(p);
		p = trf * p;
		sample.push_back(p);
	}

	GlobalMap globalMap(1000);
	globalMap.addPoints(transform_t::Identity(), map, 1);

	TrICP icp(25, 0.005, std::bind(&GlobalMap::getClosest, &globalMap, std::placeholders::_1));
	// approximate the inverse of the transform used to create the sample
	transform_t trfApprox = icp.correct(sample, 0.3);
	transform_t trfInv = trf.inverse();

	double mse = (trfInv - trfApprox).array().square().mean();
	std::cout << "TrICP MSE: " << mse << std::endl;
	CHECK(mse == Approx(0).margin(0.01));
}


/**
 * @file hmm_test.cpp
 *
 * Test file for HMMs.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/hmm/hmm.hpp>
#include <mlpack/methods/gmm/gmm.hpp>
#include <mlpack/methods/gmm/diagonal_gmm.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::hmm;
using namespace mlpack::distribution;
using namespace mlpack::gmm;

BOOST_AUTO_TEST_SUITE(HMMTest);

/**
 * We will use the simple case proposed by Russell and Norvig in Artificial
 * Intelligence: A Modern Approach, 2nd Edition, around p.549.
 */
BOOST_AUTO_TEST_CASE(SimpleDiscreteHMMTestViterbi)
{
  // We have two hidden states: rain/dry.  Two emission states: umbrella/no
  // umbrella.
  // In this example, the transition matrix is
  //  rain  dry
  // [[0.7 0.3]  rain
  //  [0.3 0.7]] dry
  // and the emission probability is
  //  rain dry
  // [[0.9 0.2]  umbrella
  //  [0.1 0.8]] no umbrella
  arma::vec initial("1 0"); // Default MATLAB initial states.
  arma::mat transition("0.7 0.3; 0.3 0.7");
  std::vector<DiscreteDistribution> emission(2);
  emission[0] = DiscreteDistribution(std::vector<arma::vec>{"0.9 0.1"});
  emission[1] = DiscreteDistribution(std::vector<arma::vec>{"0.2 0.8"});

  HMM<DiscreteDistribution> hmm(initial, transition, emission);

  // Now let's take a sequence and find what the most likely state is.
  // We'll use the sequence [U U N U U] (U = umbrella, N = no umbrella) like on
  // p. 547.
  arma::mat observation = "0 0 1 0 0";
  arma::Row<size_t> states;
  hmm.Predict(observation, states);

  // Check each state.
  BOOST_REQUIRE_EQUAL(states[0], 0); // Rain.
  BOOST_REQUIRE_EQUAL(states[1], 0); // Rain.
  BOOST_REQUIRE_EQUAL(states[2], 1); // No rain.
  BOOST_REQUIRE_EQUAL(states[3], 0); // Rain.
  BOOST_REQUIRE_EQUAL(states[4], 0); // Rain.
}

/**
 * This example is from Borodovsky & Ekisheva, p. 80-81.  It is just slightly
 * more complex.
 */
BOOST_AUTO_TEST_CASE(BorodovskyHMMTestViterbi)
{
  // Equally probable initial states.
  arma::vec initial(3);
  initial.fill(1.0 / 3.0);

  // Two hidden states: H (high GC content) and L (low GC content), as well as a
  // start state.
  arma::mat transition("0.0 0.0 0.0;"
                       "0.5 0.5 0.4;"
                       "0.5 0.5 0.6");
  // Four emission states: A, C, G, T.  Start state doesn't emit...
  std::vector<DiscreteDistribution> emission(3);
  emission[0] = DiscreteDistribution(
      std::vector<arma::vec>{"0.25 0.25 0.25 0.25"});
  emission[1] = DiscreteDistribution(
      std::vector<arma::vec>{"0.20 0.30 0.30 0.20"});
  emission[2] = DiscreteDistribution(
      std::vector<arma::vec>{"0.30 0.20 0.20 0.30"});

  HMM<DiscreteDistribution> hmm(initial, transition, emission);

  // GGCACTGAA.
  arma::mat observation("2 2 1 0 1 3 2 0 0");
  arma::Row<size_t> states;
  hmm.Predict(observation, states);

  // Most probable path is HHHLLLLLL.
  BOOST_REQUIRE_EQUAL(states[0], 1);
  BOOST_REQUIRE_EQUAL(states[1], 1);
  BOOST_REQUIRE_EQUAL(states[2], 1);
  BOOST_REQUIRE_EQUAL(states[3], 2);
  // This could actually be one of two states (equal probability).
  BOOST_REQUIRE((states[4] == 1) || (states[4] == 2));
  BOOST_REQUIRE_EQUAL(states[5], 2);
  // This could also be one of two states.
  BOOST_REQUIRE((states[6] == 1) || (states[6] == 2));
  BOOST_REQUIRE_EQUAL(states[7], 2);
  BOOST_REQUIRE_EQUAL(states[8], 2);
}

/**
 * Ensure that the forward-backward algorithm is correct.
 */
BOOST_AUTO_TEST_CASE(ForwardBackwardTwoState)
{
  arma::mat obs("3 3 2 1 1 1 1 3 3 1");

  // The values used for the initial distribution here don't entirely make
  // sense.  I am not sure how the output came from hmmdecode(), and the
  // documentation below doesn't completely say.  It seems like maybe the
  // transition matrix needs to be transposed and the results recalculated, but
  // I am not certain.
  arma::vec initial("0.1 0.4");
  arma::mat transition("0.1 0.9; 0.4 0.6");
  std::vector<DiscreteDistribution> emis(2);
  emis[0] = DiscreteDistribution(std::vector<arma::vec>{"0.85 0.15 0.00 0.00"});
  emis[1] = DiscreteDistribution(std::vector<arma::vec>{"0.00 0.00 0.50 0.50"});

  HMM<DiscreteDistribution> hmm(initial, transition, emis);

  // Now check we are getting the same results as MATLAB for this sequence.
  arma::mat stateProb;
  arma::mat forwardProb;
  arma::mat backwardProb;
  arma::vec scales;

  const double log = hmm.Estimate(obs, stateProb, forwardProb, backwardProb,
      scales);

  // All values obtained from MATLAB hmmdecode().
  BOOST_REQUIRE_CLOSE(log, -23.4349, 1e-3);

  BOOST_REQUIRE_SMALL(stateProb(0, 0), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(1, 0), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(0, 1), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(1, 1), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(0, 2), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(1, 2), 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(0, 3), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(1, 3), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(0, 4), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(1, 4), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(0, 5), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(1, 5), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(0, 6), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(1, 6), 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(0, 7), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(1, 7), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(0, 8), 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(1, 8), 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(stateProb(0, 9), 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(stateProb(1, 9), 1e-5);
}

/**
 * In this example we try to estimate the transmission and emission matrices
 * based on some observations.  We use the simplest possible model.
 */
BOOST_AUTO_TEST_CASE(SimplestBaumWelchDiscreteHMM)
{
  // Don't yet require a useful distribution.  1 state, 1 emission.
  HMM<DiscreteDistribution> hmm(1, DiscreteDistribution(1));

  std::vector<arma::mat> observations;
  // Different lengths for each observation sequence.
  observations.push_back("0 0 0 0 0 0 0 0"); // 8 zeros.
  observations.push_back("0 0 0 0 0 0 0"); // 7 zeros.
  observations.push_back("0 0 0 0 0 0 0 0 0 0 0 0"); // 12 zeros.
  observations.push_back("0 0 0 0 0 0 0 0 0 0"); // 10 zeros.

  hmm.Train(observations);

  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.Emission()[0].Probability("0"), 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.Transition()(0, 0), 1.0, 1e-5);
}

/**
 * A slightly more complex model to estimate.
 */
BOOST_AUTO_TEST_CASE(SimpleBaumWelchDiscreteHMM)
{
  HMM<DiscreteDistribution> hmm(1, 2); // 1 state, 2 emissions.
  // Randomize the emission matrix.
  hmm.Emission()[0].Probabilities() = arma::randu<arma::vec>(2);
  hmm.Emission()[0].Probabilities() /= accu(hmm.Emission()[0].Probabilities());

  // P(each emission) = 0.5.
  // I've been careful to make P(first emission = 0) = P(first emission = 1).
  std::vector<arma::mat> observations;
  observations.push_back("0 1 0 1 0 1 0 1 0 1 0 1");
  observations.push_back("0 0 0 0 0 0 1 1 1 1 1 1");
  observations.push_back("1 1 1 1 1 1 0 0 0 0 0 0");
  observations.push_back("1 1 1 0 0 0 1 1 1 0 0 0");
  observations.push_back("0 0 1 1 0 0 0 0 1 1 1 1");
  observations.push_back("1 1 1 0 0 0 1 1 1 0 0 0");
  observations.push_back("0 1 0 1 0 1 0 1 0 1 0 1");
  observations.push_back("0 0 0 0 0 0 1 1 1 1 1 1");
  observations.push_back("1 1 1 1 1 0 1 0 0 0 0 0");
  observations.push_back("1 1 1 0 0 1 0 1 1 0 0 0");
  observations.push_back("0 0 1 1 0 0 0 1 0 1 1 1");
  observations.push_back("1 1 1 0 0 1 0 1 1 0 0 0");

  hmm.Train(observations);

  BOOST_REQUIRE_CLOSE(hmm.Emission()[0].Probability("0"), 0.5, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.Emission()[0].Probability("1"), 0.5, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.Transition()(0, 0), 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 1e-5);
}

/**
 * Increasing complexity, but still simple; 4 emissions, 2 states; the state can
 * be determined directly by the emission.
 */
BOOST_AUTO_TEST_CASE(SimpleBaumWelchDiscreteHMM_2)
{
  HMM<DiscreteDistribution> hmm(2, DiscreteDistribution(4));

  // A little bit of obfuscation to the solution.
  hmm.Transition() = arma::mat("0.1 0.4; 0.9 0.6");
  hmm.Emission()[0].Probabilities() = "0.85 0.15 0.00 0.00";
  hmm.Emission()[1].Probabilities() = "0.00 0.00 0.50 0.50";

  // True emission matrix:
  //  [[0.4 0  ]
  //   [0.6 0  ]
  //   [0   0.2]
  //   [0   0.8]]

  // True transmission matrix:
  //  [[0.5 0.5]
  //   [0.5 0.5]]

  // Generate observations randomly by hand.  This is kinda ugly, but it works.
  std::vector<arma::mat> observations;
  size_t obsNum = 250; // Number of observations.
  size_t obsLen = 500; // Number of elements in each observation.
  size_t stateZeroStarts = 0; // Number of times we start in state 0.
  for (size_t i = 0; i < obsNum; i++)
  {
    arma::mat observation(1, obsLen);

    size_t state = 0;
    size_t emission = 0;

    for (size_t obs = 0; obs < obsLen; obs++)
    {
      // See if state changed.
      double r = math::Random();

      if (r <= 0.5)
      {
        if (obs == 0)
          ++stateZeroStarts;
        state = 0;
      }
      else
      {
        state = 1;
      }

      // Now set the observation.
      r = math::Random();

      switch (state)
      {
        // case 0 is not possible.
        case 0:
          if (r <= 0.4)
            emission = 0;
          else
            emission = 1;
          break;
        case 1:
          if (r <= 0.2)
            emission = 2;
          else
            emission = 3;
          break;
      }

      observation(0, obs) = emission;
    }

    observations.push_back(observation);
  }

  hmm.Train(observations);

  // Calculate true probability of class 0 at the start.
  double prob = double(stateZeroStarts) / observations.size();

  // Only require 2.5% tolerance, because this is a little fuzzier.
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], prob, 2.5);
  BOOST_REQUIRE_CLOSE(hmm.Initial()[1], 1.0 - prob, 2.5);

  BOOST_REQUIRE_CLOSE(hmm.Transition()(0, 0), 0.5, 2.5);
  BOOST_REQUIRE_CLOSE(hmm.Transition()(1, 0), 0.5, 2.5);
  BOOST_REQUIRE_CLOSE(hmm.Transition()(0, 1), 0.5, 2.5);
  BOOST_REQUIRE_CLOSE(hmm.Transition()(1, 1), 0.5, 2.5);

  BOOST_REQUIRE_CLOSE(hmm.Emission()[0].Probability("0"), 0.4, 4.0);
  BOOST_REQUIRE_CLOSE(hmm.Emission()[0].Probability("1"), 0.6, 4.0);
  BOOST_REQUIRE_SMALL(hmm.Emission()[0].Probability("2"), 2.5);
  BOOST_REQUIRE_SMALL(hmm.Emission()[0].Probability("3"), 2.5);
  BOOST_REQUIRE_SMALL(hmm.Emission()[1].Probability("0"), 2.5);
  BOOST_REQUIRE_SMALL(hmm.Emission()[1].Probability("1"), 2.5);
  BOOST_REQUIRE_CLOSE(hmm.Emission()[1].Probability("2"), 0.2, 4.0);
  BOOST_REQUIRE_CLOSE(hmm.Emission()[1].Probability("3"), 0.8, 4.0);
}

BOOST_AUTO_TEST_CASE(DiscreteHMMLabeledTrainTest)
{
  // Generate a random Markov model with 3 hidden states and 6 observations.
  arma::mat transition;
  std::vector<DiscreteDistribution> emission(3);

  transition.randu(3, 3);
  emission[0].Probabilities() = arma::randu<arma::vec>(6);
  emission[0].Probabilities() /= accu(emission[0].Probabilities());
  emission[1].Probabilities() = arma::randu<arma::vec>(6);
  emission[1].Probabilities() /= accu(emission[1].Probabilities());
  emission[2].Probabilities() = arma::randu<arma::vec>(6);
  emission[2].Probabilities() /= accu(emission[2].Probabilities());

  // Normalize so they we have a correct transition matrix.
  for (size_t col = 0; col < 3; col++)
    transition.col(col) /= accu(transition.col(col));

  // Now generate sequences.
  size_t obsNum = 250;
  size_t obsLen = 800;

  std::vector<arma::mat> observations(obsNum);
  std::vector<arma::Row<size_t> > states(obsNum);

  for (size_t n = 0; n < obsNum; n++)
  {
    observations[n].set_size(1, obsLen);
    states[n].set_size(obsLen);

    // Random starting state.
    states[n][0] = math::RandInt(3);

    // Random starting observation.
    observations[n].col(0) = emission[states[n][0]].Random();

    // Now the rest of the observations.
    for (size_t t = 1; t < obsLen; t++)
    {
      // Choose random number for state transition.
      double state = math::Random();

      // Decide next state.
      double sumProb = 0;
      for (size_t st = 0; st < 3; st++)
      {
        sumProb += transition(st, states[n][t - 1]);
        if (sumProb >= state)
        {
          states[n][t] = st;
          break;
        }
      }

      // Decide observation.
      observations[n].col(t) = emission[states[n][t]].Random();
    }
  }

  // Now that our data is generated, we give the HMM the labeled data to train
  // on.
  HMM<DiscreteDistribution> hmm(3, DiscreteDistribution(6));

  hmm.Train(observations, states);

  // Make sure the initial weights are fine.  They should be equal (or close).
  arma::vec initial(3);
  initial.fill(1.0 / 3.0);
  BOOST_REQUIRE_LT(arma::norm(hmm.Initial() - initial), 0.2);

  // Check that the transition matrix is close.
  BOOST_REQUIRE_LT(arma::norm(hmm.Transition() - transition), 0.1);

  for (size_t col = 0; col < hmm.Emission().size(); col++)
  {
    for (size_t row = 0; row < hmm.Emission()[col].Probabilities().n_elem;
        row++)
    {
      arma::vec obs(1);
      obs[0] = row;
      BOOST_REQUIRE_SMALL(hmm.Emission()[col].Probability(obs) -
          emission[col].Probability(obs), 0.07);
    }
  }
}

/**
 * Make sure the Generate() function works for a uniformly distributed HMM;
 * we'll take many samples just to make sure.
 */
BOOST_AUTO_TEST_CASE(DiscreteHMMSimpleGenerateTest)
{
  // Very simple HMM.  4 emissions with equal probability and 2 states with
  // equal probability.
  HMM<DiscreteDistribution> hmm(2, DiscreteDistribution(4));
  hmm.Initial() = arma::ones<arma::vec>(2) / 2.0;
  hmm.Transition() = arma::ones<arma::mat>(2, 2) / 2.0;

  // Now generate a really, really long sequence.
  arma::mat dataSeq;
  arma::Row<size_t> stateSeq;

  hmm.Generate(100000, dataSeq, stateSeq);

  // Now find the empirical probabilities of each state.
  arma::vec emissionProb(4);
  arma::vec stateProb(2);
  emissionProb.zeros();
  stateProb.zeros();
  for (size_t i = 0; i < 100000; i++)
  {
    emissionProb[(size_t) dataSeq.col(i)[0] + 0.5]++;
    stateProb[stateSeq[i]]++;
  }

  // Normalize so these are probabilities.
  emissionProb /= accu(emissionProb);
  stateProb /= accu(stateProb);

  // Now check that the probabilities are right.  3% tolerance.
  BOOST_REQUIRE_CLOSE(emissionProb[0], 0.25, 3.0);
  BOOST_REQUIRE_CLOSE(emissionProb[1], 0.25, 3.0);
  BOOST_REQUIRE_CLOSE(emissionProb[2], 0.25, 3.0);
  BOOST_REQUIRE_CLOSE(emissionProb[3], 0.25, 3.0);

  BOOST_REQUIRE_CLOSE(stateProb[0], 0.50, 3.0);
  BOOST_REQUIRE_CLOSE(stateProb[1], 0.50, 3.0);
}

/**
 * More complex test for Generate().
 */
BOOST_AUTO_TEST_CASE(DiscreteHMMGenerateTest)
{
  // 6 emissions, 4 states.  Random transition and emission probability.
  arma::vec initial("1 0 0 0");
  arma::mat transition(4, 4);
  std::vector<DiscreteDistribution> emission(4);
  emission[0].Probabilities() = arma::randu<arma::vec>(6);
  emission[0].Probabilities() /= accu(emission[0].Probabilities());
  emission[1].Probabilities() = arma::randu<arma::vec>(6);
  emission[1].Probabilities() /= accu(emission[1].Probabilities());
  emission[2].Probabilities() = arma::randu<arma::vec>(6);
  emission[2].Probabilities() /= accu(emission[2].Probabilities());
  emission[3].Probabilities() = arma::randu<arma::vec>(6);
  emission[3].Probabilities() /= accu(emission[3].Probabilities());

  transition.randu();

  // Normalize matrix.
  for (size_t col = 0; col < 4; col++)
    transition.col(col) /= accu(transition.col(col));

  // Create HMM object.
  HMM<DiscreteDistribution> hmm(initial, transition, emission);

  // We'll create a bunch of sequences.
  int numSeq = 400;
  int numObs = 3000;
  std::vector<arma::mat> sequences(numSeq);
  std::vector<arma::Row<size_t> > states(numSeq);
  for (int i = 0; i < numSeq; i++)
  {
    // Random starting state.
    size_t startState = math::RandInt(4);

    hmm.Generate(numObs, sequences[i], states[i], startState);
  }

  // Now we will calculate the full probabilities.
  HMM<DiscreteDistribution> hmm2(4, 6);
  hmm2.Train(sequences, states);

  // Check that training gives the same result.
  BOOST_REQUIRE_LT(arma::norm(hmm.Transition() - hmm2.Transition()), 0.01);

  for (size_t row = 0; row < 6; row++)
  {
    arma::vec obs(1);
    obs[0] = row;
    for (size_t col = 0; col < 4; col++)
    {
      BOOST_REQUIRE_SMALL(hmm.Emission()[col].Probability(obs) -
          hmm2.Emission()[col].Probability(obs), 0.01);
    }
  }
}

BOOST_AUTO_TEST_CASE(DiscreteHMMLogLikelihoodTest)
{
  // Create a simple HMM with three states and four emissions.
  arma::vec initial("0.5 0.2 0.3"); // Default MATLAB initial states.
  arma::mat transition("0.5 0.0 0.1;"
                       "0.2 0.6 0.2;"
                       "0.3 0.4 0.7");
  std::vector<DiscreteDistribution> emission(3);
  emission[0].Probabilities() = "0.75 0.25 0.00 0.00";
  emission[1].Probabilities() = "0.00 0.25 0.25 0.50";
  emission[2].Probabilities() = "0.10 0.40 0.40 0.10";

  HMM<DiscreteDistribution> hmm(initial, transition, emission);

  // Now generate some sequences and check that the log-likelihood is the same
  // as MATLAB gives for this HMM.
  BOOST_REQUIRE_CLOSE(hmm.LogLikelihood("0 1 2 3"), -4.9887223949, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.LogLikelihood("1 2 0 0"), -6.0288487077, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.LogLikelihood("3 3 3 3"), -5.5544000018, 1e-5);
  BOOST_REQUIRE_CLOSE(hmm.LogLikelihood("0 2 2 1 2 3 0 0 1 3 1 0 0 3 1 2 2"),
      -24.51556128368, 1e-5);
}

/**
 * A simple test to make sure HMMs with Gaussian output distributions work.
 */
BOOST_AUTO_TEST_CASE(GaussianHMMSimpleTest)
{
  // We'll have two Gaussians, far away from each other, one corresponding to
  // each state.
  //  E(0) ~ N([ 5.0  5.0], eye(2)).
  //  E(1) ~ N([-5.0 -5.0], eye(2)).
  // The transition matrix is simple:
  //  T = [[0.75 0.25]
  //       [0.25 0.75]]
  GaussianDistribution g1("5.0 5.0", "1.0 0.0; 0.0 1.0");
  GaussianDistribution g2("-5.0 -5.0", "1.0 0.0; 0.0 1.0");

  arma::vec initial("1 0"); // Default MATLAB initial states.
  arma::mat transition("0.75 0.25; 0.25 0.75");

  std::vector<GaussianDistribution> emission;
  emission.push_back(g1);
  emission.push_back(g2);

  HMM<GaussianDistribution> hmm(initial, transition, emission);

  // Now, generate some sequences.
  arma::mat observations(2, 1000);
  arma::Row<size_t> classes(1000);

  // 1000-observations sequence.
  classes[0] = 0;
  observations.col(0) = g1.Random();
  for (size_t i = 1; i < 1000; i++)
  {
    double randValue = math::Random();

    if (randValue > 0.75) // Then we change state.
      classes[i] = (classes[i - 1] + 1) % 2;
    else
      classes[i] = classes[i - 1];

    if (classes[i] == 0)
      observations.col(i) = g1.Random();
    else
      observations.col(i) = g2.Random();
  }

  // Now predict the sequence.
  arma::Row<size_t> predictedClasses;
  arma::mat stateProb;

  hmm.Predict(observations, predictedClasses);
  hmm.Estimate(observations, stateProb);

  // Check that each prediction is right.
  for (size_t i = 0; i < 1000; i++)
  {
    BOOST_REQUIRE_EQUAL(predictedClasses[i], classes[i]);

    // The probability of the wrong class should be infinitesimal.
    BOOST_REQUIRE_SMALL(stateProb((classes[i] + 1) % 2, i), 0.001);
  }
}

/**
 * Ensure that Gaussian HMMs can be trained properly, for the labeled training
 * case and also for the unlabeled training case.
 */
BOOST_AUTO_TEST_CASE(GaussianHMMTrainTest)
{
  // Four emission Gaussians and three internal states.  The goal is to estimate
  // the transition matrix correctly, and each distribution correctly.
  std::vector<GaussianDistribution> emission;
  emission.push_back(GaussianDistribution("0.0 0.0 0.0", "1.0 0.2 0.2;"
                                                         "0.2 1.5 0.0;"
                                                         "0.2 0.0 1.1"));
  emission.push_back(GaussianDistribution("2.0 1.0 5.0", "0.7 0.3 0.0;"
                                                         "0.3 2.6 0.0;"
                                                         "0.0 0.0 1.0"));
  emission.push_back(GaussianDistribution("5.0 0.0 0.5", "1.0 0.0 0.0;"
                                                         "0.0 1.0 0.0;"
                                                         "0.0 0.0 1.0"));

  arma::mat transition("0.3 0.5 0.7;"
                       "0.3 0.4 0.1;"
                       "0.4 0.1 0.2");

  // Now generate observations.
  std::vector<arma::mat> observations(100);
  std::vector<arma::Row<size_t> > states(100);

  for (size_t obs = 0; obs < 100; obs++)
  {
    observations[obs].set_size(3, 1000);
    states[obs].set_size(1000);

    // Always start in state zero.
    states[obs][0] = 0;
    observations[obs].col(0) = emission[0].Random();

    for (size_t t = 1; t < 1000; t++)
    {
      // Choose the state.
      double randValue = math::Random();
      double probSum = 0;
      for (size_t state = 0; state < 3; state++)
      {
        probSum += transition(state, states[obs][t - 1]);
        if (probSum >= randValue)
        {
          states[obs][t] = state;
          break;
        }
      }

      // Now choose the emission.
      observations[obs].col(t) = emission[states[obs][t]].Random();
    }
  }

  // Now that the data is generated, train the HMM.
  HMM<GaussianDistribution> hmm(3, GaussianDistribution(3));

  hmm.Train(observations, states);

  // Check initial weights.
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 1e-5);
  BOOST_REQUIRE_SMALL(hmm.Initial()[1], 1e-3);
  BOOST_REQUIRE_SMALL(hmm.Initial()[2], 1e-3);

  // We use a tolerance of 0.05 for the transition matrices.
  // Check that the transition matrix is correct.
  BOOST_REQUIRE_LT(arma::norm(hmm.Transition() - transition), 0.05);

  // Check that each distribution is correct.
  for (size_t dist = 0; dist < 3; dist++)
  {
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Mean() -
        emission[dist].Mean()), 0.05);
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Covariance() -
        emission[dist].Covariance()), 0.1);
  }

  // Now let's try it all again, but this time, unlabeled.  Everything will fail
  // if we don't have a decent guess at the Gaussians, so we'll take a "poor"
  // guess at it ourselves.  I won't use K-Means because we can't afford to add
  // the instability of that to our test.  We'll leave the covariances as the
  // identity.
  HMM<GaussianDistribution> hmm2(3, GaussianDistribution(3));
  hmm2.Emission()[0].Mean() = "0.3 -0.2 0.1"; // Actual: [0 0 0].
  hmm2.Emission()[1].Mean() = "1.0 1.4 3.2";  // Actual: [2 1 5].
  hmm2.Emission()[2].Mean() = "3.1 -0.2 6.1"; // Actual: [5 0 5].

  // We'll only use 20 observation sequences to try and keep training time
  // shorter.
  observations.resize(20);

  hmm.Train(observations);

  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 0.1);
  BOOST_REQUIRE_SMALL(hmm.Initial()[1], 0.05);
  BOOST_REQUIRE_SMALL(hmm.Initial()[2], 0.05);

  // The tolerances are increased because there is more error in unlabeled
  // training; we use an absolute tolerance of 0.03 for the transition matrices.
  // Check that the transition matrix is correct.
  for (size_t row = 0; row < 3; row++)
    for (size_t col = 0; col < 3; col++)
      BOOST_REQUIRE_SMALL(transition(row, col) - hmm.Transition()(row, col),
          0.03);

  // Check that each distribution is correct.
  for (size_t dist = 0; dist < 3; dist++)
  {
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Mean() -
        emission[dist].Mean()), 0.1);
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Covariance() -
        emission[dist].Covariance()), 0.25);
  }
}

/**
 * Make sure that a random sequence generated by a Gaussian HMM fits the
 * distribution correctly.
 */
BOOST_AUTO_TEST_CASE(GaussianHMMGenerateTest)
{
  // Our distribution will have three two-dimensional output Gaussians.
  HMM<GaussianDistribution> hmm(3, GaussianDistribution(2));
  hmm.Transition() = arma::mat("0.4 0.6 0.8; 0.2 0.2 0.1; 0.4 0.2 0.1");
  hmm.Emission()[0] = GaussianDistribution("0.0 0.0", "1.0 0.0; 0.0 1.0");
  hmm.Emission()[1] = GaussianDistribution("2.0 2.0", "1.0 0.5; 0.5 1.2");
  hmm.Emission()[2] = GaussianDistribution("-2.0 1.0", "2.0 0.1; 0.1 1.0");

  // Now we will generate a long sequence.
  std::vector<arma::mat> observations(1);
  std::vector<arma::Row<size_t> > states(1);

  // Start in state 1 (no reason).
  hmm.Generate(10000, observations[0], states[0], 1);

  HMM<GaussianDistribution> hmm2(3, GaussianDistribution(2));

  // Now estimate the HMM from the generated sequence.
  hmm2.Train(observations, states);

  // Check that the estimated matrices are the same.
  BOOST_REQUIRE_LT(arma::norm(hmm.Transition() - hmm2.Transition()), 0.1);

  // Check that each Gaussian is the same.
  for (size_t dist = 0; dist < 3; dist++)
  {
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Mean() -
        hmm2.Emission()[dist].Mean()), 0.2);
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Covariance() -
        hmm2.Emission()[dist].Covariance()), 0.3);
  }
}

/**
 * Make sure that Predict() is numerically stable.
 */
BOOST_AUTO_TEST_CASE(GaussianHMMPredictTest)
{
  size_t numState = 10;
  size_t obsDimension = 2;
  HMM<GaussianDistribution> hmm(numState, GaussianDistribution(obsDimension));

  arma::vec initial = {1.0000, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  arma::mat transition = {{0.9149, 0, 0, 0, 0, 0, 0, 0, 0, 0},
                          {0.0851, 0.8814, 0, 0, 0, 0, 0, 0, 0, 0},
                          {0, 0.1186, 0.9031, 0, 0, 0, 0, 0, 0, 0},
                          {0, 0, 0.0969, 0.903, 0, 0, 0, 0, 0, 0},
                          {0, 0, 0, 0.097, 0.8941, 0, 0, 0, 0, 0},
                          {0, 0, 0, 0, 0.1059, 0.9024, 0, 0, 0, 0},
                          {0, 0, 0, 0, 0, 0.0976, 0.8902, 0, 0, 0},
                          {0, 0, 0, 0, 0, 0, 0.1098, 0.9107, 0, 0},
                          {0, 0, 0, 0, 0, 0, 0, 0.0893, 0.8964, 0},
                          {0, 0, 0, 0, 0, 0, 0, 0, 0.1036, 1}};

  std::vector<arma::vec> mean = {{0, 0.259},
                                 {0.0372, 0.2063},
                                 {0.1496, -0.3075},
                                 {-0.0366, -0.3255},
                                 {-0.2866, -0.0202},
                                 {0.1804, 0.1385},
                                 {0.1922, -0.0616},
                                 {-0.378, -0.1751},
                                 {-0.1346, 0.1357},
                                 {0.338, 0.183}};

  std::vector<arma::mat> cov = {
      {{3.2837e-07, 0}, {0, 0.032837}},
      {{0.0154, -0.0093}, {-0.0093, 0.0358}},
      {{0.1087, -0.0032}, {-0.0032, 0.0587}},
      {{0.3185, -0.0069}, {-0.0069, 0.0396}},
      {{0.3472, 0.0484}, {0.0484, 0.0706}},
      {{0.39, 0.0406}, {0.0406, 0.0653}},
      {{0.4502, 0.0718}, {0.0718, 0.0705}},
      {{0.3253, 0.0312}, {0.0312, 0.0783}},
      {{0.2355, 0.0195}, {0.0195, 0.0276}},
      {{0.0818, 0.022}, {0.022, 0.0282}}};

  hmm.Initial() = initial;
  hmm.Transition() = transition;

  for (size_t i = 0; i < numState; ++i)
  {
    GaussianDistribution& emission = hmm.Emission().at(i);
    emission.Mean() = mean.at(i);
    emission.Covariance(cov.at(i));
  }

  arma::mat obs = {
      {
          -0.0424, -0.0395, -0.0336, -0.0294, -0.0299, -0.032, -0.0289, -0.0148,
          0.0095, 0.0416, 0.0795, 0.1173, 0.1491, 0.1751, 0.1999, 0.2277,
          0.2586, 0.2858, 0.3019, 0.303, 0.289, 0.2632, 0.2301, 0.1923, 0.1498,
          0.1021, 0.0471, -0.0191, -0.0969, -0.1795, -0.2559, -0.323, -0.3882,
          -0.4582, -0.5334, -0.609, -0.6778999999999999, -0.7278, -0.7481,
          -0.7356, -0.6953, -0.635, -0.5617, -0.478, -0.3833, -0.2721, -0.1365,
          0.0283, 0.217, 0.4148, 0.6028, 0.7664, 0.8937, 0.9737, 1, 0.972,
          0.8972, 0.7891, 0.6613, 0.524, 0.3847, 0.2489, 0.1187, -0.0045,
          -0.1214, -0.2316, -0.3328, -0.4211, -0.4963, -0.5607, -0.6136,
          -0.6532, -0.6777, -0.6867, -0.6807, -0.6612, -0.6345, -0.6075,
          -0.5748, -0.5278, -0.4747, -0.4176, -0.33, -0.2036, -0.0597,
          0.07240000000000001, 0.1754, 0.2471, 0.295, 0.3356, 0.3809, 0.4299,
          0.4737, 0.4987, 0.4958, 0.4676, 0.4253, 0.3802, 0.342, 0.3183
      },
      {
          0.2355, 0.2639, 0.2971, 0.3301, 0.3598, 0.3842, 0.3995, 0.4019, 0.39,
          0.3624, 0.3201, 0.2658, 0.203, 0.1341, 0.06, -0.0179, -0.1006,
          -0.1869, -0.2719, -0.35, -0.4176, -0.4739, -0.52, -0.5584, -0.5913,
          -0.6196, -0.642, -0.6554, -0.6567, -0.6459, -0.6271, -0.6029, -0.5722,
          -0.5318000000000001, -0.4802, -0.4174, -0.3449, -0.2685, -0.1927,
          -0.1201, -0.0532, 0.008699999999999999, 0.0673, 0.1204, 0.1647,
          0.2008, 0.2284, 0.2447, 0.2504, 0.2479, 0.2373, 0.2148, 0.1781,
          0.1283, 0.06710000000000001, -0.0022, -0.0743, -0.1463, -0.2149,
          -0.2784, -0.3362, -0.3867, -0.4297, -0.4651, -0.4924, -0.5101,
          -0.5168, -0.5117, -0.496, -0.4706, -0.4358, -0.3923, -0.3419, -0.2868,
          -0.2289, -0.1702, -0.1094, -0.0421, 0.0311, 0.1047, 0.1732, 0.2257,
          0.254, 0.2532, 0.2308, 0.2017, 0.1724, 0.1425, 0.1195, 0.099, 0.0759,
          0.0521, 0.0313, 0.0188, 0.0113, 0.0068, 0.0042, 0.0026, 0.0018, 0.0014
      }
  };

  arma::Row<size_t> stateSeq;
  auto likelihood = hmm.LogLikelihood(obs);
  hmm.Predict(obs, stateSeq);

  arma::Row<size_t> stateSeqRef = { 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
      1, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, 4, 4,
      4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6, 6, 6, 6, 7, 7, 7,
      7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 8, 8, 8, 8, 8, 8, 8, 8, 9, 9, 9, 9, 9,
      9, 9, 9, 9, 9, 9, 9, 9, 9, 9 };

  BOOST_REQUIRE_CLOSE(likelihood, -2734.43, 1e-3);

  for (size_t i = 0; i < stateSeqRef.n_cols; ++i)
  {
    BOOST_REQUIRE_EQUAL(stateSeqRef.at(i), stateSeq.at(i));
  }
}

/**
 * Test that HMMs work with Gaussian mixture models.  We'll try putting in a
 * simple model by hand and making sure that prediction of observation sequences
 * works correctly.
 */
BOOST_AUTO_TEST_CASE(GMMHMMPredictTest)
{
  // It's possible, but extremely unlikely, that this test can fail.  So we are
  // willing to do three trials in case the first two fail.
  bool success = false;
  for (size_t trial = 0; trial < 3; ++trial)
  {
    // We will use two GMMs; one with two components and one with three.
    std::vector<GMM> gmms(2);
    gmms[0] = GMM(2, 2);
    gmms[0].Weights() = arma::vec("0.75 0.25");

    // N([2.25 3.10], [1.00 0.20; 0.20 0.89])
    gmms[0].Component(0) = GaussianDistribution("4.25 3.10",
                                                "1.00 0.20; 0.20 0.89");

    // N([4.10 1.01], [1.00 0.00; 0.00 1.01])
    gmms[0].Component(1) = GaussianDistribution("7.10 5.01",
                                                "1.00 0.00; 0.00 1.01");

    gmms[1] = GMM(3, 2);
    gmms[1].Weights() = arma::vec("0.4 0.2 0.4");

    gmms[1].Component(0) = GaussianDistribution("-3.00 -6.12",
                                                "1.00 0.00; 0.00 1.00");

    gmms[1].Component(1) = GaussianDistribution("-4.25 -7.12",
                                                "1.50 0.60; 0.60 1.20");

    gmms[1].Component(2) = GaussianDistribution("-6.15 -2.00",
                                                "1.00 0.80; 0.80 1.00");

    // Default MATLAB initial probabilities.
    arma::vec initial("1 0");

    // Transition matrix.
    arma::mat trans("0.30 0.50;"
                    "0.70 0.50");

    // Now build the model.
    HMM<GMM> hmm(initial, trans, gmms);

    // Make a sequence of observations.
    arma::mat observations(2, 1000);
    arma::Row<size_t> states(1000);
    states[0] = 0;
    observations.col(0) = gmms[0].Random();

    for (size_t i = 1; i < 1000; i++)
    {
      double randValue = math::Random();

      if (randValue <= trans(0, states[i - 1]))
        states[i] = 0;
      else
        states[i] = 1;

      observations.col(i) = gmms[states[i]].Random();
    }

    // Run the prediction.
    arma::Row<size_t> predictions;
    hmm.Predict(observations, predictions);

    // Check that the predictions were correct.
    success = true;
    for (size_t i = 0; i < 1000; i++)
    {
      if (predictions[i] != states[i])
      {
        success = false;
        break;
      }
    }

    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

/**
 * Test that GMM-based HMMs can train on models correctly using labeled training
 * data.
 */
BOOST_AUTO_TEST_CASE(GMMHMMLabeledTrainingTest)
{
  // We will use two GMMs; one with two components and one with three.
  std::vector<GMM> gmms(2, GMM(2, 2));
  gmms[0].Weights() = arma::vec("0.3 0.7");

  // N([2.25 3.10], [1.00 0.20; 0.20 0.89])
  gmms[0].Component(0) = GaussianDistribution("4.25 3.10",
                                              "1.00 0.20; 0.20 0.89");

  // N([4.10 1.01], [1.00 0.00; 0.00 1.01])
  gmms[0].Component(1) = GaussianDistribution("7.10 5.01",
                                              "1.00 0.00; 0.00 1.01");

  gmms[1].Weights() = arma::vec("0.20 0.80");

  gmms[1].Component(0) = GaussianDistribution("-3.00 -6.12",
                                              "1.00 0.00; 0.00 1.00");

  gmms[1].Component(1) = GaussianDistribution("-4.25 -2.12",
                                              "1.50 0.60; 0.60 1.20");

  // Transition matrix.
  arma::mat transMat("0.40 0.60;"
                     "0.60 0.40");

  // Make a sequence of observations.
  std::vector<arma::mat> observations(5, arma::mat(2, 2500));
  std::vector<arma::Row<size_t> > states(5, arma::Row<size_t>(2500));
  for (size_t obs = 0; obs < 5; obs++)
  {
    states[obs][0] = 0;
    observations[obs].col(0) = gmms[0].Random();

    for (size_t i = 1; i < 2500; i++)
    {
      double randValue = (double) rand() / (double) RAND_MAX;

      if (randValue <= transMat(0, states[obs][i - 1]))
        states[obs][i] = 0;
      else
        states[obs][i] = 1;

      observations[obs].col(i) = gmms[states[obs][i]].Random();
    }
  }

  // Set up the GMM for training.
  HMM<GMM> hmm(2, GMM(2, 2));

  // Train the HMM.
  hmm.Train(observations, states);

  // Check the initial weights.  The dataset was generated with 100% probability
  // of a sequence starting in state 0.
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 0.01);
  BOOST_REQUIRE_SMALL(hmm.Initial()[1], 0.01);

  // Check the results.  Use absolute tolerances instead of percentages.
  BOOST_REQUIRE_SMALL(hmm.Transition()(0, 0) - transMat(0, 0), 0.03);
  BOOST_REQUIRE_SMALL(hmm.Transition()(0, 1) - transMat(0, 1), 0.03);
  BOOST_REQUIRE_SMALL(hmm.Transition()(1, 0) - transMat(1, 0), 0.03);
  BOOST_REQUIRE_SMALL(hmm.Transition()(1, 1) - transMat(1, 1), 0.03);

  // Now the emission probabilities (the GMMs).
  // We have to sort each GMM for comparison.
  arma::uvec sortedIndices = sort_index(hmm.Emission()[0].Weights());

  BOOST_REQUIRE_SMALL(hmm.Emission()[0].Weights()[sortedIndices[0]] -
      gmms[0].Weights()[0], 0.08);
  BOOST_REQUIRE_SMALL(hmm.Emission()[0].Weights()[sortedIndices[1]] -
      gmms[0].Weights()[1], 0.08);

  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[0]).Mean() -
      gmms[0].Component(0).Mean()), 0.2);
  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[1]).Mean() -
      gmms[0].Component(1).Mean()), 0.2);

  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[0]).Covariance() -
      gmms[0].Component(0).Covariance()), 0.5);
  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[1]).Covariance() -
      gmms[0].Component(0).Covariance()), 0.5);

  // Sort the GMM.
  sortedIndices = sort_index(hmm.Emission()[1].Weights());

  BOOST_REQUIRE_SMALL(hmm.Emission()[1].Weights()[sortedIndices[0]] -
      gmms[1].Weights()[0], 0.08);
  BOOST_REQUIRE_SMALL(hmm.Emission()[1].Weights()[sortedIndices[1]] -
      gmms[1].Weights()[1], 0.08);

  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[0]).Mean() -
      gmms[1].Component(0).Mean()), 0.2);
  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[1]).Mean() -
      gmms[1].Component(1).Mean()), 0.2);

  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[0]).Covariance() -
      gmms[1].Component(0).Covariance()), 0.5);
  BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[1]).Covariance() -
      gmms[1].Component(1).Covariance()), 0.5);
}

/**
 * Test saving and loading of GMM HMMs
 */
BOOST_AUTO_TEST_CASE(GMMHMMLoadSaveTest)
{
  // Create a GMM HMM, save it, and load it.
  HMM<GMM> hmm(3, GMM(4, 3));

  for (size_t j = 0; j < hmm.Emission().size(); ++j)
  {
    hmm.Emission()[j].Weights().randu();
    for (size_t i = 0; i < hmm.Emission()[j].Gaussians(); ++i)
    {
      hmm.Emission()[j].Component(i).Mean().randu();
      arma::mat covariance = arma::randu<arma::mat>(
          hmm.Emission()[j].Component(i).Covariance().n_rows,
          hmm.Emission()[j].Component(i).Covariance().n_cols);
      covariance *= covariance.t();
      covariance += arma::eye<arma::mat>(covariance.n_rows, covariance.n_cols);
      hmm.Emission()[j].Component(i).Covariance(std::move(covariance));
    }
  }

  // Save the HMM.
  {
    std::ofstream ofs("test-hmm-save.xml");
    boost::archive::xml_oarchive ar(ofs);
    ar << BOOST_SERIALIZATION_NVP(hmm);
  }

  // Load the HMM.
  HMM<GMM> hmm2(3, GMM(4, 3));
  {
    std::ifstream ifs("test-hmm-save.xml");
    boost::archive::xml_iarchive ar(ifs);
    ar >> BOOST_SERIALIZATION_NVP(hmm2);
  }

  // Remove clutter.
  remove("test-hmm-save.xml");

  for (size_t j = 0; j < hmm.Emission().size(); ++j)
  {
    BOOST_REQUIRE_EQUAL(hmm.Emission()[j].Gaussians(),
                        hmm2.Emission()[j].Gaussians());
    BOOST_REQUIRE_EQUAL(hmm.Emission()[j].Dimensionality(),
                        hmm2.Emission()[j].Dimensionality());

    for (size_t i = 0; i < hmm.Emission()[j].Dimensionality(); ++i)
      BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Weights()[i],
                          hmm2.Emission()[j].Weights()[i], 1e-3);

    for (size_t i = 0; i < hmm.Emission()[j].Gaussians(); ++i)
    {
      for (size_t l = 0; l < hmm.Emission()[j].Dimensionality(); ++l)
      {
        BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Component(i).Mean()[l],
            hmm2.Emission()[j].Component(i).Mean()[l], 1e-3);

        for (size_t k = 0; k < hmm.Emission()[j].Dimensionality(); ++k)
        {
          BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Component(i).Covariance()(l, k),
              hmm2.Emission()[j].Component(i).Covariance()(l, k), 1e-3);
        }
      }
    }
  }
}

/**
 * Test saving and loading of Gaussian HMMs
 */
BOOST_AUTO_TEST_CASE(GaussianHMMLoadSaveTest)
{
  // Create a Gaussian HMM, save it, and load it.
  HMM<GaussianDistribution> hmm(3, GaussianDistribution(2));

  for (size_t j = 0; j < hmm.Emission().size(); ++j)
  {
    hmm.Emission()[j].Mean().randu();
    arma::mat covariance = arma::randu<arma::mat>(
        hmm.Emission()[j].Covariance().n_rows,
        hmm.Emission()[j].Covariance().n_cols);
    covariance *= covariance.t();
    covariance += arma::eye<arma::mat>(covariance.n_rows, covariance.n_cols);
    hmm.Emission()[j].Covariance(std::move(covariance));
  }

  // Save the HMM.
  {
    std::ofstream ofs("test-hmm-save.xml");
    boost::archive::xml_oarchive ar(ofs);
    ar << BOOST_SERIALIZATION_NVP(hmm);
  }

  // Load the HMM.
  HMM<GaussianDistribution> hmm2(3, GaussianDistribution(2));
  {
    std::ifstream ifs("test-hmm-save.xml");
    boost::archive::xml_iarchive ar(ifs);
    ar >> BOOST_SERIALIZATION_NVP(hmm2);
  }

  // Remove clutter.
  remove("test-hmm-save.xml");

  for (size_t j = 0; j < hmm.Emission().size(); ++j)
  {
    BOOST_REQUIRE_EQUAL(hmm.Emission()[j].Dimensionality(),
                        hmm2.Emission()[j].Dimensionality());

    for (size_t i = 0; i < hmm.Emission()[j].Dimensionality(); ++i)
    {
      BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Mean()[i],
          hmm2.Emission()[j].Mean()[i], 1e-3);
      for (size_t k = 0; k < hmm.Emission()[j].Dimensionality(); ++k)
      {
        BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Covariance()(i, k),
            hmm2.Emission()[j].Covariance()(i, k), 1e-3);
      }
    }
  }
}

/**
 * Test saving and loading of Discrete HMMs
 */
BOOST_AUTO_TEST_CASE(DiscreteHMMLoadSaveTest)
{
  // Create a Discrete HMM, save it, and load it.
  std::vector<DiscreteDistribution> emission(4);
  emission[0].Probabilities() = arma::randu<arma::vec>(6);
  emission[0].Probabilities() /= accu(emission[0].Probabilities());
  emission[1].Probabilities() = arma::randu<arma::vec>(6);
  emission[1].Probabilities() /= accu(emission[1].Probabilities());
  emission[2].Probabilities() = arma::randu<arma::vec>(6);
  emission[2].Probabilities() /= accu(emission[2].Probabilities());
  emission[3].Probabilities() = arma::randu<arma::vec>(6);
  emission[3].Probabilities() /= accu(emission[3].Probabilities());

  // Create HMM object.
  HMM<DiscreteDistribution> hmm(3, DiscreteDistribution(3));

  for (size_t j = 0; j < hmm.Emission().size(); ++j)
  {
    hmm.Emission()[j].Probabilities() = arma::randu<arma::vec>(3);
    hmm.Emission()[j].Probabilities() /= accu(emission[j].Probabilities());
  }

  // Save the HMM.
  {
    std::ofstream ofs("test-hmm-save.xml");
    boost::archive::xml_oarchive ar(ofs);
    ar << BOOST_SERIALIZATION_NVP(hmm);
  }

  // Load the HMM.
  HMM<DiscreteDistribution> hmm2(3, DiscreteDistribution(3));
  {
    std::ifstream ifs("test-hmm-save.xml");
    boost::archive::xml_iarchive ar(ifs);
    ar >> BOOST_SERIALIZATION_NVP(hmm2);
  }

  // Remove clutter.
  remove("test-hmm-save.xml");

  for (size_t j = 0; j < hmm.Emission().size(); ++j)
    for (size_t i = 0; i < hmm.Emission()[j].Probabilities().n_elem; ++i)
      BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Probabilities()[i],
          hmm2.Emission()[j].Probabilities()[i], 1e-3);
}

/********************************************/
/** DiagonalGMM Hidden Markov Models Tests **/
/********************************************/

//! Make sure the prediction of DiagonalGMM HMMs is reasonable.
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMPredictTest)
{
  // This test is probabilistic, so we perform it three times to make it robust.
  bool success = false;
  for (size_t trial = 0; trial < 3; trial++)
  {
    std::vector<DiagonalGMM> gmms(2);
    gmms[0] = DiagonalGMM(2, 2);

    gmms[0].Component(0) = DiagonalGaussianDistribution("3.25 2.10",
        "0.97 1.00");
    gmms[0].Component(1) = DiagonalGaussianDistribution("5.03 7.28",
        "1.20 0.89");

    gmms[1] = DiagonalGMM(3, 2);
    gmms[1].Weights() = arma::vec("0.3 0.2 0.5");
    gmms[1].Component(0) = DiagonalGaussianDistribution("-2.48 -3.02",
        "1.02 0.80");
    gmms[1].Component(1) = DiagonalGaussianDistribution("-1.24 -2.40",
        "0.85 0.78");
    gmms[1].Component(2) = DiagonalGaussianDistribution("-5.68 -4.83",
        "1.42 0.96");

    // Initial probabilities.
    arma::vec initial("1 0");

    // Transition matrix.
    arma::mat transProb("0.40 0.70;"
                        "0.60 0.30");

    // Build the model.
    HMM<DiagonalGMM> hmm(initial, transProb, gmms);

    // Make a sequence of observations according to transition probabilities.
    arma::mat observations(2, 1000);
    arma::Row<size_t> states(1000);

    // Set initial state to zero.
    states[0] = 0;
    observations.col(0) = gmms[0].Random();

    for (size_t i = 1; i < 1000; i++)
    {
      double randValue = math::Random();

      if (randValue <= transProb(0, states[i - 1]))
        states[i] = 0;
      else
        states[i] = 1;

      observations.col(i) = gmms[states[i]].Random();
    }

    // Predict the most probable hidden state sequence.
    arma::Row<size_t> predictions;
    hmm.Predict(observations, predictions);

    // Check them.
    success = true;
    for (size_t i = 0; i < 1000; i++)
    {
      if (predictions[i] != states[i])
      {
        success = false;
        break;
      }
    }

    if (success)
      break;
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

/**
 * Make sure a random data sequence generation is correct when the emission
 * distribution is DiagonalGMM.
 */
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMGenerateTest)
{
  // Build the model.
  HMM<DiagonalGaussianDistribution> hmm(3, DiagonalGaussianDistribution(2));
  hmm.Transition() = arma::mat("0.2 0.3 0.8;"
                               "0.4 0.5 0.1;"
                               "0.4 0.2 0.1");

  hmm.Emission()[0] = DiagonalGaussianDistribution("0.0 0.0", "1.0 0.7");
  hmm.Emission()[1] = DiagonalGaussianDistribution("1.0 1.0", "0.7 0.5");
  hmm.Emission()[2] = DiagonalGaussianDistribution("-3.0 2.0", "2.0 0.3");

  // Now we will generate a long sequence.
  std::vector<arma::mat> observations(1);
  std::vector<arma::Row<size_t> > states(1);

  // Generate a random data sequence.
  hmm.Generate(10000, observations[0], states[0], 1);

  // Build the hmm2.
  HMM<DiagonalGaussianDistribution> hmm2(3, DiagonalGaussianDistribution(2));

  // Now estimate the HMM from the generated sequence.
  hmm2.Train(observations, states);

  // Check that the estimated matrices are the same.
  BOOST_REQUIRE_LT(arma::norm(hmm.Transition() - hmm2.Transition()), 0.05);

  // Check that each Gaussian is the same.
  for (size_t dist = 0; dist < 3; dist++)
  {
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Mean() -
        hmm2.Emission()[dist].Mean()), 0.1);
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[dist].Covariance() -
        hmm2.Emission()[dist].Covariance()), 0.2);
  }
}

/**
 * Make sure the unlabeled 1-state training works reasonably given a single
 * distribution with diagonal covariance.
 */
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMOneGaussianOneStateTrainingTest)
{
  // Create a Gaussian distribution with diagonal covariance.
  DiagonalGaussianDistribution d("2.05 3.45", "0.89 1.05");

  // Make a sequence of observations.
  std::vector<arma::mat> observations(1, arma::mat(2, 5000));
  for (size_t obs = 0; obs < 1; obs++)
  {
    observations[obs].col(0) = d.Random();

    for (size_t i = 1; i < 5000; i++)
    {
      observations[obs].col(i) = d.Random();
    }
  }

  // Build the model.
  HMM<DiagonalGMM> hmm(1, DiagonalGMM(1, 2));

  // Train with observations.
  hmm.Train(observations);

  // Generate the ground truth values.
  arma::vec actualMean = arma::mean(observations[0], 1);
  arma::vec actualCovar = arma::diagvec(
      arma::ccov(observations[0], 1 /* biased estimator */));

  // Check the model to see that it is correct.
  CheckMatrices(hmm.Emission()[0].Component(0).Mean(), actualMean);
  CheckMatrices(hmm.Emission()[0].Component(0).Covariance(), actualCovar);
}

/**
 * Make sure the unlabeled training works reasonably given a single
 * distribution with diagonal covariance.
 */
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMOneGaussianUnlabeledTrainingTest)
{
  // Create a sequence of DiagonalGMMs. Each GMM has one gaussian distribution.
  std::vector<DiagonalGMM> gmms(2, DiagonalGMM(1, 2));
  gmms[0].Component(0) = DiagonalGaussianDistribution("1.25 2.10",
      "0.97 1.00");

  gmms[1].Component(0) = DiagonalGaussianDistribution("-2.48 -3.02",
      "1.02 0.80");

  // Transition matrix.
  arma::mat transProbs("0.30 0.80;"
                       "0.70 0.20");

  arma::vec initialProb("1 0");

  // Make a sequence of observations.
  std::vector<arma::mat> observations(2, arma::mat(2, 500));
  std::vector<arma::Row<size_t>> states(2, arma::Row<size_t>(500));
  for (size_t obs = 0; obs < 2; obs++)
  {
    states[obs][0] = 0;
    observations[obs].col(0) = gmms[0].Random();

    for (size_t i = 1; i < 500; i++)
    {
      double randValue = math::Random();

      if (randValue <= transProbs(0, states[obs][i - 1]))
        states[obs][i] = 0;
      else
        states[obs][i] = 1;

      observations[obs].col(i) = gmms[states[obs][i]].Random();
    }
  }

  // Build the model.
  HMM<DiagonalGMM> hmm(initialProb, transProbs, gmms);

  // Train the model. If labels are not given, when training GMM, the estimated
  // probabilities based on the forward and backward probabilities is used.
  hmm.Train(observations);

  // Check the initial weights.
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 0.01);
  BOOST_REQUIRE_SMALL(hmm.Initial()[1], 0.01);

  // Check the transition probability matrix.
  for (size_t i = 0; i < 2; i++)
    for (size_t j = 0; j < 2; j++)
      BOOST_REQUIRE_SMALL(hmm.Transition()(i, j) - transProbs(i, j), 0.08);

  // Check the estimated weights of the each emission distribution.
  for (size_t i = 0; i < 2; i++)
    BOOST_REQUIRE_SMALL(hmm.Emission()[i].Weights()[0] - gmms[i].Weights()[0],
        0.08);

  // Check the estimated means of the each emission distribution.
  for (size_t i = 0; i < 2; i++)
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[i].Component(0).Mean() -
        gmms[i].Component(0).Mean()), 0.2);

  // Check the estimated covariances of the each emission distribution.
  for (size_t i = 0; i < 2; i++)
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[i].Component(0).Covariance() -
        gmms[i].Component(0).Covariance()), 0.5);
}

/**
 * Make sure the labeled training works reasonably given a single distribution
 * with diagonal covariance.
 */
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMOneGaussianLabeledTrainingTest)
{
  // Create a sequence of DiagonalGMMs.
  std::vector<DiagonalGMM> gmms(3, DiagonalGMM(1, 2));
  gmms[0].Component(0) = DiagonalGaussianDistribution("5.25 7.10",
      "0.97 1.00");

  gmms[1].Component(0) = DiagonalGaussianDistribution("4.48 6.02",
      "1.02 0.80");

  gmms[2].Component(0) = DiagonalGaussianDistribution("-3.28 -5.30",
      "0.87 1.05");

  // Transition matrix.
  arma::mat transProbs("0.2 0.4 0.4;"
                       "0.3 0.4 0.3;"
                       "0.5 0.2 0.3");

  arma::vec initialProb("1 0 0");

  // Make a sequence of observations.
  std::vector<arma::mat> observations(3, arma::mat(2, 5000));
  std::vector<arma::Row<size_t>> states(3, arma::Row<size_t>(5000));
  for (size_t obs = 0; obs < 3; obs++)
  {
    states[obs][0] = 0;
    observations[obs].col(0) = gmms[0].Random();

    for (size_t i = 1; i < 5000; i++)
    {
      double randValue = math::Random();
      double probSum = 0;
      for (size_t state = 0; state < 3; state++)
      {
        probSum += transProbs(state, states[obs][i - 1]);
        if (randValue <= probSum)
        {
          states[obs][i] = state;
          break;
        }
      }

      observations[obs].col(i) = gmms[states[obs][i]].Random();
    }
  }

  // Build the model.
  HMM<DiagonalGMM> hmm(3, DiagonalGMM(1, 2));

  // Train the model.
  hmm.Train(observations, states);

  // Check the initial weights.
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 0.01);
  BOOST_REQUIRE_SMALL(hmm.Initial()[1], 0.01);
  BOOST_REQUIRE_SMALL(hmm.Initial()[2], 0.01);

  // Check the transition probability matrix.
  for (size_t i = 0; i < 3; i++)
    for (size_t j = 0; j < 3; j++)
      BOOST_REQUIRE_SMALL(hmm.Transition()(i, j) - transProbs(i, j), 0.03);

  // Check the estimated weights of the each emission distribution.
  for (size_t i = 0; i < 3; i++)
    BOOST_REQUIRE_SMALL(hmm.Emission()[i].Weights()[0] - gmms[i].Weights()[0],
        0.08);

  // Check the estimated means of the each emission distribution.
  for (size_t i = 0; i < 3; i++)
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[i].Component(0).Mean() -
        gmms[i].Component(0).Mean()), 0.2);

  // Check the estimated covariances of the each emission distribution.
  for (size_t i = 0; i < 3; i++)
    BOOST_REQUIRE_LT(arma::norm(hmm.Emission()[i].Component(0).Covariance() -
        gmms[i].Component(0).Covariance()), 0.5);
}

/**
 * Make sure the unlabeled training works reasonably given multiple
 * distributions with diagonal covariance.
 */
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMMultipleGaussiansUnlabeledTrainingTest)
{
  // Create a sequence of DiagonalGMMs.
  std::vector<DiagonalGMM> gmms(2, DiagonalGMM(2, 2));
  gmms[0].Weights() = arma::vec("0.3 0.7");
  gmms[0].Component(0) = DiagonalGaussianDistribution("8.25 7.10",
      "0.97 1.00");
  gmms[0].Component(1) = DiagonalGaussianDistribution("-3.03 -2.28",
      "1.20 0.89");

  gmms[1].Weights() = arma::vec("0.4 0.6");
  gmms[1].Component(0) = DiagonalGaussianDistribution("4.48 6.02",
        "1.02 0.80");
  gmms[1].Component(1) = DiagonalGaussianDistribution("-9.24 -8.40",
        "0.85 1.58");

  // Transition matrix.
  arma::mat transProbs("0.30 0.40;"
                       "0.70 0.60");

  arma::vec initialProb("1 0");

  // Make a sequence of observations.
  std::vector<arma::mat> observations(2, arma::mat(2, 1000));
  std::vector<arma::Row<size_t>> states(2, arma::Row<size_t>(1000));
  for (size_t obs = 0; obs < 2; obs++)
  {
    states[obs][0] = 0;
    observations[obs].col(0) = gmms[0].Random();

    for (size_t i = 1; i < 1000; i++)
    {
      double randValue = math::Random();

      if (randValue <= transProbs(0, states[obs][i - 1]))
        states[obs][i] = 0;
      else
        states[obs][i] = 1;

      observations[obs].col(i) = gmms[states[obs][i]].Random();
    }
  }

  // Build the model.
  HMM<DiagonalGMM> hmm(initialProb, transProbs, gmms);

  // Train the model. If labels are not given, when training GMM, the estimated
  // probabilities based on the forward and backward probabilities is used.
  hmm.Train(observations);

  // Check the initial weights.
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 0.01);
  BOOST_REQUIRE_SMALL(hmm.Initial()[1], 0.01);

  // Check the transition probability matrix.
  for (size_t i = 0; i < 2; i++)
    for (size_t j = 0; j < 2; j++)
      BOOST_REQUIRE_SMALL(hmm.Transition()(i, j) - transProbs(i, j), 0.08);

  // Sort by the estimated weights of the first emission distribution.
  arma::uvec sortedIndices = sort_index(hmm.Emission()[0].Weights());

  // Check the first emission distribution.
  for (size_t i = 0; i < 2; i++)
  {
    // Check the estimated weights using the first DiagonalGMM.
    BOOST_REQUIRE_SMALL(hmm.Emission()[0].Weights()[sortedIndices[i]] -
        gmms[0].Weights()[i], 0.08);

    // Check the estimated means using the first DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[i]).Mean() -
      gmms[0].Component(i).Mean()), 0.35);

    // Check the estimated covariances using the first DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[i]).Covariance() -
      gmms[0].Component(i).Covariance()), 0.6);
  }

  // Sort by the estimated weights of the second emission distribution.
  sortedIndices = sort_index(hmm.Emission()[1].Weights());

  // Check the second emission distribution.
  for (size_t i = 0; i < 2; i++)
  {
    // Check the estimated weights using the second DiagonalGMM.
    BOOST_REQUIRE_SMALL(hmm.Emission()[1].Weights()[sortedIndices[i]] -
        gmms[1].Weights()[i], 0.08);

    // Check the estimated means using the second DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[i]).Mean() -
      gmms[1].Component(i).Mean()), 0.35);

    // Check the estimated covariances using the second DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[i]).Covariance() -
      gmms[1].Component(i).Covariance()), 0.6);
  }
}

/**
 * Make sure the labeled training works reasonably given multiple distributions
 * with diagonal covariance.
 */
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMMultipleGaussiansLabeledTrainingTest)
{
  math::RandomSeed(std::time(NULL));
  // Create a sequence of DiagonalGMMs.
  std::vector<DiagonalGMM> gmms(2, DiagonalGMM(2, 2));
  gmms[0].Weights() = arma::vec("0.3 0.7");
  gmms[0].Component(0) = DiagonalGaussianDistribution("2.25 5.30",
      "0.97 1.00");
  gmms[0].Component(1) = DiagonalGaussianDistribution("-3.15 -2.50",
      "1.20 0.89");

  gmms[1].Weights() = arma::vec("0.4 0.6");
  gmms[1].Component(0) = DiagonalGaussianDistribution("-4.48 -6.30",
        "1.02 0.80");
  gmms[1].Component(1) = DiagonalGaussianDistribution("5.24 2.40",
        "0.85 1.58");

  // Transition matrix.
  arma::mat transProbs("0.30 0.80;"
                       "0.70 0.20");

  // Make a sequence of observations.
  std::vector<arma::mat> observations(5, arma::mat(2, 2500));
  std::vector<arma::Row<size_t>> states(5, arma::Row<size_t>(2500));
  for (size_t obs = 0; obs < 5; obs++)
  {
    states[obs][0] = 0;
    observations[obs].col(0) = gmms[0].Random();

    for (size_t i = 1; i < 2500; i++)
    {
      double randValue = math::Random();

      if (randValue <= transProbs(0, states[obs][i - 1]))
        states[obs][i] = 0;
      else
        states[obs][i] = 1;

      observations[obs].col(i) = gmms[states[obs][i]].Random();
    }
  }

  // Build the model.
  HMM<DiagonalGMM> hmm(2, DiagonalGMM(2, 2));

  // Train the model.
  hmm.Train(observations, states);

  // Check the initial weights.
  BOOST_REQUIRE_CLOSE(hmm.Initial()[0], 1.0, 0.01);
  BOOST_REQUIRE_SMALL(hmm.Initial()[1], 0.01);

  // Check the transition probability matrix.
  for (size_t i = 0; i < 2; i++)
    for (size_t j = 0; j < 2; j++)
      BOOST_REQUIRE_SMALL(hmm.Transition()(i, j) - transProbs(i, j), 0.03);

  // Sort by the estimated weights of the first emission distribution.
  arma::uvec sortedIndices = sort_index(hmm.Emission()[0].Weights());

  // Check the first emission distribution.
  for (size_t i = 0; i < 2; i++)
  {
    // Check the estimated weights using the first DiagonalGMM.
    BOOST_REQUIRE_SMALL(hmm.Emission()[0].Weights()[sortedIndices[i]] -
        gmms[0].Weights()[i], 0.08);

    // Check the estimated means using the first DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[i]).Mean() -
      gmms[0].Component(i).Mean()), 0.2);

    // Check the estimated covariances using the first DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[0].Component(sortedIndices[i]).Covariance() -
      gmms[0].Component(i).Covariance()), 0.5);
  }

  // Sort by the estimated weights of the second emission distribution.
  sortedIndices = sort_index(hmm.Emission()[1].Weights());

  // Check the second emission distribution.
  for (size_t i = 0; i < 2; i++)
  {
    // Check the estimated weights using the second DiagonalGMM.
    BOOST_REQUIRE_SMALL(hmm.Emission()[1].Weights()[sortedIndices[i]] -
        gmms[1].Weights()[i], 0.08);

    // Check the estimated means using the second DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[i]).Mean() -
      gmms[1].Component(i).Mean()), 0.2);

    // Check the estimated covariances using the second DiagonalGMM.
    BOOST_REQUIRE_LT(arma::norm(
      hmm.Emission()[1].Component(sortedIndices[i]).Covariance() -
      gmms[1].Component(i).Covariance()), 0.5);
  }
}

/**
 * Make sure loading and saving the model is correct.
 */
BOOST_AUTO_TEST_CASE(DiagonalGMMHMMLoadSaveTest)
{
  // Create a GMM HMM, save and load it.
  HMM<DiagonalGMM> hmm(3, DiagonalGMM(4, 3));

  // Generate intial random values.
  for (size_t j = 0; j < hmm.Emission().size(); j++)
  {
    hmm.Emission()[j].Weights().randu();
    for (size_t i = 0; i < hmm.Emission()[j].Gaussians(); i++)
    {
      hmm.Emission()[j].Component(i).Mean().randu();
      arma::vec covariance = arma::randu<arma::vec>(
          hmm.Emission()[j].Component(i).Covariance().n_elem);

      covariance += arma::ones<arma::vec>(covariance.n_elem);
      hmm.Emission()[j].Component(i).Covariance(std::move(covariance));
    }
  }

  // Save the HMM.
  {
    std::ofstream ofs("test-hmm-save.xml");
    boost::archive::xml_oarchive ar(ofs);
    ar << BOOST_SERIALIZATION_NVP(hmm);
  }

  // Load the HMM.
  HMM<DiagonalGMM> hmm2(3, DiagonalGMM(4, 3));
  {
    std::ifstream ifs("test-hmm-save.xml");
    boost::archive::xml_iarchive ar(ifs);
    ar >> BOOST_SERIALIZATION_NVP(hmm2);
  }

  // Remove clutter.
  remove("test-hmm-save.xml");

  for (size_t j = 0; j < hmm.Emission().size(); j++)
  {
    // Check the number of Gaussians.
    BOOST_REQUIRE_EQUAL(hmm.Emission()[j].Gaussians(),
                        hmm2.Emission()[j].Gaussians());

    // Check the dimensionality.
    BOOST_REQUIRE_EQUAL(hmm.Emission()[j].Dimensionality(),
                        hmm2.Emission()[j].Dimensionality());

    for (size_t i = 0; i < hmm.Emission()[j].Dimensionality(); i++)
      // Check the weights.
      BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Weights()[i],
                          hmm2.Emission()[j].Weights()[i], 1e-3);

    for (size_t i = 0; i < hmm.Emission()[j].Gaussians(); i++)
    {
      for (size_t l = 0; l < hmm.Emission()[j].Dimensionality(); l++)
      {
        // Check the means.
        BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Component(i).Mean()[l],
            hmm2.Emission()[j].Component(i).Mean()[l], 1e-3);

        // Check the covariances.
        BOOST_REQUIRE_CLOSE(hmm.Emission()[j].Component(i).Covariance()[l],
            hmm2.Emission()[j].Component(i).Covariance()[l], 1e-3);
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef PA_MATH_TYPES_HPP
#define PA_MATH_TYPES_HPP

#define EIGEN_USE_MKL
#define EIGEN_USE_MKL_ALL
#define MKL_DIRECT_CALL

#include <cstdint>

#include <Eigen/Core>

namespace pa
{
using scalar   = float          ;
using vector2  = Eigen::Vector2f;
using vector3  = Eigen::Vector3f;
using vector4  = Eigen::Vector4f;

using integer  = std::int32_t   ;
using ivector2 = Eigen::Vector2i;
using ivector3 = Eigen::Vector3i;
using ivector4 = Eigen::Vector4i;

using matrix2  = Eigen::Matrix2f;
using matrix3  = Eigen::Matrix3f;
using matrix4  = Eigen::Matrix4f;

using imatrix2 = Eigen::Matrix2i;
using imatrix3 = Eigen::Matrix3i;
using imatrix4 = Eigen::Matrix4i;
}

#endif


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/currencies/europe.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <qle/instruments/payment.hpp>
#include <qle/pricingengines/paymentdiscountingengine.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;
using namespace QuantExt;
using namespace boost::unit_test_framework;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(PaymentTest)

BOOST_AUTO_TEST_CASE(testDomesicPayment) {

    BOOST_TEST_MESSAGE("Testing Domestic Payment NPV...");

    SavedSettings backup;

    Date refDate = Date(8, Dec, 2016);
    Settings::instance().evaluationDate() = refDate;
    Date paymentDate = refDate + 10 * Years;
    Payment payment(100.0, EURCurrency(), paymentDate);
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(0, TARGET(), 0.03, ActualActual()));
    boost::shared_ptr<PricingEngine> engine = boost::make_shared<PaymentDiscountingEngine>(yts);
    payment.setPricingEngine(engine);

    Real expectedNpv = 100.0 * yts->discount(paymentDate);

    BOOST_CHECK_SMALL(payment.NPV() - expectedNpv, 0.000001);
}

BOOST_AUTO_TEST_CASE(testForeignPayment) {

    BOOST_TEST_MESSAGE("Testing Foreign Payment NPV...");

    SavedSettings backup;

    Date refDate = Date(8, Dec, 2016);
    Settings::instance().evaluationDate() = refDate;
    Date paymentDate = refDate + 10 * Years;
    Payment payment(100.0, EURCurrency(), paymentDate);
    Handle<YieldTermStructure> yts(boost::make_shared<FlatForward>(0, TARGET(), 0.03, ActualActual()));
    Handle<Quote> fx(boost::make_shared<SimpleQuote>(0.789));
    boost::shared_ptr<PricingEngine> engine = boost::make_shared<PaymentDiscountingEngine>(yts, fx);
    payment.setPricingEngine(engine);

    Real expectedNpv = 100.0 * yts->discount(paymentDate) * fx->value();

    BOOST_CHECK_SMALL(payment.NPV() - expectedNpv, 0.000001);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


// solving A * X = B
// in two steps -- factor (getrf()) and solve (getrs())

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <boost/numeric/bindings/blas.hpp>
#include <boost/numeric/bindings/lapack/computational/getrf.hpp>
#include <boost/numeric/bindings/lapack/computational/getrs.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace lapack = boost::numeric::bindings::lapack;

using std::size_t;
using std::cout;
using std::endl;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
#endif

int main() {

  cout << endl;

  size_t n = 5;
  m_t a (n, n);   // system matrix

  size_t nrhs = 2;
  m_t x (n, nrhs);
  // b -- right-hand side matrix:
  // .. see leading comments for `gesv()' in clapack.hpp
#ifndef F_ROW_MAJOR
  m_t b (n, nrhs);
#else
  m_t b (nrhs, n);
#endif

  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]
  ublas::matrix_row<m_t> ar1 (a, 0), ar3 (a, 3);
  swap (ar1, ar3);   // swap rows to force pivoting

  ublas::matrix_column<m_t> xc0 (x, 0), xc1 (x, 1);
  blas::set (1., xc0);  // x[.,0] = 1
  blas::set (2., xc1);  // x[.,1] = 2
#ifndef F_ROW_MAJOR
  blas::gemm ( 1.0, a, x, 0.0, b);  // b = a x, so we know the result ;o)
#else
  // see leading comments for `gesv()' in clapack.hpp
  ublas::matrix_row<m_t> br0 (b, 0), br1 (b, 1);
  blas::gemv (a, xc0, br0);  // b[0,.] = a x[.,0]
  blas::gemv (a, xc1, br1);  // b[1,.] = a x[.,1]  =>  b^T = a x
#endif

  print_m (a, "A");
  cout << endl;
  print_m (b, "B");
  cout << endl;

  std::vector<int> ipiv (n);  // pivot vector

  lapack::getrf (a, ipiv);      // factor a
  lapack::getrs (a, ipiv, b);   // solve from factorization
  print_m (b, "X");
  cout << endl;

  print_v (ipiv, "pivots");

  cout << endl;

}


//
// Created by vlad on 14.11.16.
//

#pragma once

#include "route_solver/structs/data_structs.hpp"

#include <Eigen/Core>

#include <map>
#include <vector>
#include <set>

namespace rs {

bool isPointInWorkZone(const point2d& pt, const std::vector<point2d>& workzone);

class VehicleTaskChecker {

public:
	VehicleTaskChecker(const std::vector <Vehicle>& venchiles,
					   const std::vector<Zone>& zones,
					   const std::vector <Task>& tasks);

	bool taskAcceptableForVenchile(std::uint32_t taskId, std::uint32_t venhileId) const;

	const Eigen::MatrixXd& possibilityVehicleToTask() const;

private:
	Eigen::MatrixXd m_possibilityVehicleToTask;

};

}