//=========================================================================
// This file is part of HMMlib.
//
// HMMlib is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as
// published by the Free Software Foundation, either version 3 of
// the License, or (at your option) any later version.
//
// HMMlib is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with HMMlib. If not, see
// <http://www.gnu.org/licenses/>.
//
// Copyright (C) 2010  Bioinformatics Research Centre, Aarhus University.
// Author: Andreas Sand (asand@birc.au.dk)
//=========================================================================

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN hmm_test
//#define par

#include <boost/test/included/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>
using namespace boost::unit_test;

#include "HMMlib/hmm_vector.hpp"
#include "HMMlib/hmm_table.hpp"
#include "HMMlib/hmm.hpp"
using namespace hmmlib;

#include <pmmintrin.h>

#ifdef WITH_OMP
#include<omp.h>
#endif

BOOST_AUTO_TEST_SUITE( hmm_test )

template <typename float_type, typename sse_float_type>
static void check_1_table(const HMMMatrix<float_type, sse_float_type> &T) {
  for (int r = 0; r < T.get_no_rows(); ++r)
    for (int c = 0; c < T.get_no_columns(); ++c)
      BOOST_CHECK_CLOSE(T(r,c), (float_type)1.0, 0.001);
}

template <typename float_type, typename sse_float_type>
static void check_matrix_column(const HMMMatrix<float_type, sse_float_type> &T, int c, double val) {
  for (int r = 0; r < T.get_no_rows(); ++r)
    BOOST_CHECK_CLOSE(T(r,c), (float_type)val, 0.001);
}

template <typename float_type>
void construct_non_sse() {
  boost::shared_ptr<HMMVector<float_type,float_type> >
    pi_ptr(new HMMVector<float_type,float_type>(7));
  boost::shared_ptr<HMMMatrix<float_type,float_type> >
    T_ptr(new HMMMatrix<float_type,float_type>(7,7));
  boost::shared_ptr<HMMMatrix<float_type,float_type> >
    E_ptr(new HMMMatrix<float_type,float_type>(9,7));

  HMM<float_type,float_type> hmm(pi_ptr,
		      T_ptr,
		      E_ptr);
}

template <typename float_type, typename sse_float_type>
void construct_sse() {
  boost::shared_ptr<HMMVector<float_type, sse_float_type> >
    pi_ptr(new HMMVector<float_type, sse_float_type>(7));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> >
    T_ptr(new HMMMatrix<float_type, sse_float_type>(7,7));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> >
    E_ptr(new HMMMatrix<float_type, sse_float_type>(9,7));

  HMM<float_type, sse_float_type> hmm(pi_ptr,
		      T_ptr,
		      E_ptr);
}

// ############ FORWARD TESTS ############### //
template <typename float_type, typename sse_float_type>
void forward_1x1() {
  int no_states = 1;
  int alphabet_size = 1;
  int seq_length = 3;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 1.0;
  T(0,0) = 1.0;
  E(0,0) = 1.0;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 0, 0, 0 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(obs.size(), no_states);
  HMMVector<float_type, sse_float_type> scales(obs.size());
  hmm.forward(obs, scales, F);

  BOOST_CHECK_CLOSE(hmm.likelihood(scales), (float_type) 0.0, 0.001); // log-likelihood
  check_1_table(F);
}

template <typename float_type, typename sse_float_type>
void forward_1x2_no_transitions() {
  int no_states = 2;
  int alphabet_size = 1;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 1.0; pi(1) = 0.0;
  T(0,0) = 1.0; T(0,1) = 0.0;
  T(1,0) = 0.0; T(1,1) = 1.0;
  E(0,0) = 1.0; E(0,1) = 0.0;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 0, 0, 0 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);
  hmm.forward(obs, scales, F);

  BOOST_CHECK_CLOSE(hmm.likelihood(scales), (float_type)0.0, 0.001);

  check_matrix_column(F, 0, 1.0);
  check_matrix_column(F, 1, 0.0);
}

template <typename float_type, typename sse_float_type>
void forward_2x2_no_transitions() {
  int no_states = 2;
  int alphabet_size = 2;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 1.0; pi(1) = 0.0;
  T(0,0) = 1.0; T(0,1) = 0.0;
  T(1,0) = 0.0; T(1,1) = 1.0;

  // emissions from state 0
  E(0,0) = 0.25;
  E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.25;
  E(1,1) = 0.75;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 1, 0, 1 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);

  hmm.forward(obs, scales, F);

  BOOST_CHECK_CLOSE(hmm.likelihood(scales), (float_type)std::log(0.25*0.75*0.25*0.75), 0.01);

  BOOST_CHECK_CLOSE(F(0,0), (float_type)1, 0.001);
  BOOST_CHECK_CLOSE(F(1,0), (float_type)1, 0.001);
  BOOST_CHECK_CLOSE(F(2,0), (float_type)1, 0.001);
  BOOST_CHECK_CLOSE(F(3,0), (float_type)1, 0.001);

  check_matrix_column(F, 1, 0.0);
}

template <typename float_type, typename sse_float_type>
void forward_2x2_with_transitions() {
  int no_states = 2;
  int alphabet_size = 2;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 0.2; pi(1) = 0.8;
  T(0,0) = 0.1; T(0,1) = 0.9;
  T(1,0) = 0.9; T(1,1) = 0.1;

  // emissions from state 0
  E(0,0) = 0.25;
  E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.25;
  E(1,1) = 0.75;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 1, 0, 1 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);

  hmm.forward(obs, scales, F);

  BOOST_CHECK_CLOSE(hmm.likelihood(scales), (float_type)std::log(0.25*0.75*0.25*0.75), 0.001);

  BOOST_CHECK_CLOSE((float_type)F(0,0), (float_type)(0.2*0.25 * scales(0)), 0.001);
  BOOST_CHECK_CLOSE((float_type)F(0,1), (float_type)(0.8*0.25 * scales(0)), 0.001);

  BOOST_CHECK_CLOSE((float_type)F(1,0), (float_type)((F(0,0)*0.1+F(0,1)*0.9)*0.75 * scales(1)), 0.001);
  BOOST_CHECK_CLOSE((float_type)F(1,1), (float_type)((F(0,0)*0.9+F(0,1)*0.1)*0.75 * scales(1)), 0.001);

  BOOST_CHECK_CLOSE((float_type)F(2,0), (float_type)((F(1,0)*0.1+F(1,1)*0.9)*0.25 * scales(2)), 0.001);
  BOOST_CHECK_CLOSE((float_type)F(2,1), (float_type)((F(1,0)*0.9+F(1,1)*0.1)*0.25 * scales(2)), 0.001);

  BOOST_CHECK_CLOSE((float_type)F(3,0), (float_type)((F(2,0)*0.1+F(2,1)*0.9)*0.75 * scales(3)), 0.001);
  BOOST_CHECK_CLOSE((float_type)F(3,1), (float_type)((F(2,0)*0.9+F(2,1)*0.1)*0.75 * scales(3)), 0.001);
}

#ifdef WITH_OMP
template <typename float_type, typename sse_float_type>
void parallel_forward() {
  int obsseq_length = 100;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  for(int i = 0; i < no_states; ++i)
    pi(i) = random();
  double sum = pi.sum();
  pi /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c)
      T(r,c) = random();
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c)
      T(r,c) /= sum;
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) = random();
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) /= sum;
  }

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales = *scales_ptr;

  omp_set_num_threads(1);
  hmm.forward(obs, scales, F);
  float_type likelihood = hmm.likelihood(scales);
  //  std::cout << likelihood << std::endl;

  omp_set_num_threads(2);
  hmm.forward(obs, scales, F);
  float_type likelihood2 = hmm.likelihood(scales);
  //  std::cout << likelihood2 << std::endl;

  omp_set_num_threads(4);
  hmm.forward(obs, scales, F);
  float_type likelihood4 = hmm.likelihood(scales);
  //  std::cout << likelihood4 << std::endl;

  omp_set_num_threads(8);
  hmm.forward(obs, scales, F);
  float_type likelihood8 = hmm.likelihood(scales);
  //  std::cout << likelihood8 << std::endl;

  BOOST_CHECK_CLOSE(likelihood, likelihood2, 0.01);
  BOOST_CHECK_CLOSE(likelihood, likelihood4, 0.01);
  BOOST_CHECK_CLOSE(likelihood, likelihood8, 0.01);
}
#endif

template <typename float_type>
void sse_vs_nonSse_forward() {

  typedef typename FloatTraits<float_type>::sse_type sse_float_type;

  int obsseq_length = 133;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, float_type> > pi_ptr(new HMMVector<float_type, float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > T_ptr(new HMMMatrix<float_type, float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > E_ptr(new HMMMatrix<float_type, float_type>(alphabet_size, no_states));

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr_sse(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr_sse(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr_sse(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, float_type> &T = *T_ptr;
  HMMMatrix<float_type, float_type> &E = *E_ptr;

  HMMVector<float_type, sse_float_type> &pi_sse = *pi_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &T_sse = *T_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &E_sse = *E_ptr_sse;

  for(int i = 0; i < no_states; ++i) {
    pi(i) = random();
    pi_sse(i) = pi(i);
  }
  double sum = pi.sum();
  pi /= sum;
  pi_sse /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c) {
      T(r,c) = random();
    }
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c) {
      T(r,c) /= sum;
      T_sse(r,c) = T(r,c);
    }
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) = random();
    }
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) /= sum;
      E_sse(r,c) = E(r,c);
    }
  }

  HMM<float_type, float_type> hmm(pi_ptr, T_ptr, E_ptr);
  HMM<float_type, sse_float_type> hmm_sse(pi_ptr_sse, T_ptr_sse, E_ptr_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > F_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, float_type> > scales_ptr(new HMMVector<float_type, float_type>(obsseq_length));
  HMMVector<float_type, float_type> &scales = *scales_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F_sse = *F_ptr_sse;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr_sse(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales_sse = *scales_ptr_sse;

  hmm.forward(obs, scales, F);
  hmm_sse.forward(obs, scales_sse, F_sse);

  float_type likelihood = hmm.likelihood(scales);
  float_type likelihood_sse = hmm_sse.likelihood(scales_sse);

  BOOST_CHECK_CLOSE(F(132,127), F_sse(132,127), 0.01);
  BOOST_CHECK_CLOSE(F(100,25), F_sse(100,25), 0.01);
  BOOST_CHECK_CLOSE(F(87,3), F_sse(87,3), 0.01);
  BOOST_CHECK_CLOSE(likelihood, likelihood_sse, 0.01);
}

// ############ BACKWARD TESTS ############### //
template<typename float_type, typename sse_float_type>
void backward_1x1() {
  int no_states = 1;
  int alphabet_size = 1;
  int seq_length = 3;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 1.0;
  T(0,0) = 1.0;
  E(0,0) = 1.0;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 0, 0, 0 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);
  hmm.forward(obs, scales, F);
  HMMMatrix<float_type, sse_float_type> B(seq_length, no_states);
  hmm.backward(obs, scales, B);

  check_1_table(B);
}

template <typename float_type, typename sse_float_type>
void backward_1x2_no_transitions() {
  int no_states = 2;
  int alphabet_size = 1;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 1.0; pi(1) = 0.0;
  T(0,0) = 1.0; T(0,1) = 0.0;
  T(1,0) = 0.0; T(1,1) = 1.0;
  E(0,0) = 1.0; E(0,1) = 0.0;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 0, 0, 0 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states);
  HMMMatrix<float_type, sse_float_type> B(seq_length, no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);

  hmm.forward(obs, scales, F);
  hmm.backward(obs, scales, B);

  check_matrix_column(B, 0, 1.0);

  BOOST_WARN_CLOSE(B(3,1), scales(3), 0.001);

  BOOST_WARN_CLOSE(B(2,1), (B(3,0)*T(1,0)*E(1,0) + B(3,1)*T(1,1)*E(1,1))*scales(2), 0.001);

  BOOST_WARN_CLOSE(B(1,1), (B(2,0)*T(1,0)*E(0,0) + B(2,1)*T(1,1)*E(0,1))*scales(1), 0.001);

  BOOST_WARN_CLOSE(B(0,1), (B(1,0)*T(1,0)*E(1,0) + B(1,1)*T(1,1)*E(1,1))*scales(0), 0.001);
}

template <typename float_type, typename sse_float_type>
void backward_2x2_no_transitions() {
  int no_states = 2;
  int alphabet_size = 2;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 1.0; pi(1) = 0.0;
  T(0,0) = 1.0; T(0,1) = 0.0;
  T(1,0) = 0.0; T(1,1) = 1.0;

  // emissions from state 0
  E(0,0) = 0.25;
  E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.25;
  E(1,1) = 0.75;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 1, 0, 1 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states);
  HMMMatrix<float_type, sse_float_type> B(seq_length, no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);

  hmm.forward(obs, scales, F);
  hmm.backward(obs, scales, B);

  BOOST_CHECK_CLOSE(B(3,0), scales(3), 0.001);
  BOOST_CHECK_CLOSE(B(3,1), scales(3), 0.001);

  BOOST_CHECK_CLOSE(B(2,0), (B(3,0)*T(0,0)*E(1,0) + B(3,1)*T(0,1)*E(1,1))*scales(2), 0.001);
  BOOST_CHECK_CLOSE(B(2,1), (B(3,0)*T(1,0)*E(1,0) + B(3,1)*T(1,1)*E(1,1))*scales(2), 0.001);

  BOOST_CHECK_CLOSE(B(1,0), (B(2,0)*T(0,0)*E(0,0) + B(2,1)*T(0,1)*E(0,1))*scales(1), 0.001);
  BOOST_CHECK_CLOSE(B(1,1), (B(2,0)*T(1,0)*E(0,0) + B(2,1)*T(1,1)*E(0,1))*scales(1), 0.001);

  BOOST_CHECK_CLOSE(B(0,0), (B(1,0)*T(0,0)*E(1,0) + B(1,1)*T(0,1)*E(1,1))*scales(0), 0.001);
  BOOST_CHECK_CLOSE(B(0,1), (B(1,0)*T(1,0)*E(1,0) + B(1,1)*T(1,1)*E(1,1))*scales(0), 0.001);
}

template <typename float_type, typename sse_float_type>
void backward_2x2_with_transitions() {
  int no_states = 2;
  int alphabet_size = 2;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 0.2; pi(1) = 0.8;
  T(0,0) = 0.1; T(0,1) = 0.9;
  T(1,0) = 0.9; T(1,1) = 0.1;

  // emissions from state 0
  E(0,0) = 0.25;
  E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.25;
  E(1,1) = 0.75;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 1, 0, 1 };
  sequence obs(obs_array, obs_array+seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states);
  HMMMatrix<float_type, sse_float_type> B(seq_length, no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);

  hmm.forward(obs, scales, F);
  hmm.backward(obs, scales, B);

  BOOST_CHECK_CLOSE(B(3,0), scales(3), 0.01);
  BOOST_CHECK_CLOSE(B(3,1), scales(3), 0.01);

  BOOST_CHECK_CLOSE(B(2,0), (B(3,0)*T(0,0)*E(1,0) + B(3,1)*T(0,1)*E(1,1))*scales(2), 0.001);
  BOOST_CHECK_CLOSE(B(2,1), (B(3,0)*T(1,0)*E(1,0) + B(3,1)*T(1,1)*E(1,1))*scales(2), 0.001);

  BOOST_CHECK_CLOSE(B(1,0), (B(2,0)*T(0,0)*E(0,0) + B(2,1)*T(0,1)*E(0,1))*scales(1), 0.001);
  BOOST_CHECK_CLOSE(B(1,1), (B(2,0)*T(1,0)*E(0,0) + B(2,1)*T(1,1)*E(0,1))*scales(1), 0.001);

  BOOST_CHECK_CLOSE(B(0,0), (B(1,0)*T(0,0)*E(1,0) + B(1,1)*T(0,1)*E(1,1))*scales(0), 0.001);
  BOOST_CHECK_CLOSE(B(0,1), (B(1,0)*T(1,0)*E(1,0) + B(1,1)*T(1,1)*E(1,1))*scales(0), 0.001);
}

#ifdef WITH_OMP
template <typename float_type, typename sse_float_type>
void parallel_backward() {
  int obsseq_length = 100;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  for(int i = 0; i < no_states; ++i)
    pi(i) = random();
  double sum = pi.sum();
  pi /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c)
      T(r,c) = random();
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c)
      T(r,c) /= sum;
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) = random();
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) /= sum;
  }

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales = *scales_ptr;

  hmm.forward(obs, scales, F);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > B_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &B = *B_ptr;

  omp_set_num_threads(1);
  hmm.backward(obs, scales, B);
  float_type v11 = B(obsseq_length-1, 3);
  float_type v12 = B(obsseq_length-1, 64);

  omp_set_num_threads(2);
  hmm.backward(obs, scales, B);
  float_type v21 = B(obsseq_length-1, 3);
  float_type v22 = B(obsseq_length-1, 64);

  omp_set_num_threads(4);
  hmm.backward(obs, scales, B);
  float_type v41 = B(obsseq_length-1, 3);
  float_type v42 = B(obsseq_length-1, 64);

  omp_set_num_threads(8);
  hmm.backward(obs, scales, B);
  float_type v81 = B(obsseq_length-1, 3);
  float_type v82 = B(obsseq_length-1, 64);

  BOOST_CHECK_CLOSE(v11, v21, 0.01);
  BOOST_CHECK_CLOSE(v11, v41, 0.01);
  BOOST_CHECK_CLOSE(v11, v81, 0.01);
  BOOST_CHECK_CLOSE(v12, v22, 0.01);
  BOOST_CHECK_CLOSE(v12, v42, 0.01);
  BOOST_CHECK_CLOSE(v12, v82, 0.01);
}
#endif

template <typename float_type>
void sse_vs_nonSse_backward() {

  typedef typename FloatTraits<float_type>::sse_type sse_float_type;

  int obsseq_length = 133;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, float_type> > pi_ptr(new HMMVector<float_type, float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > T_ptr(new HMMMatrix<float_type, float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > E_ptr(new HMMMatrix<float_type, float_type>(alphabet_size, no_states));

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr_sse(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr_sse(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr_sse(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, float_type> &T = *T_ptr;
  HMMMatrix<float_type, float_type> &E = *E_ptr;

  HMMVector<float_type, sse_float_type> &pi_sse = *pi_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &T_sse = *T_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &E_sse = *E_ptr_sse;

  for(int i = 0; i < no_states; ++i) {
    pi(i) = random();
    pi_sse(i) = pi(i);
  }
  double sum = pi.sum();
  pi /= sum;
  pi_sse /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c) {
      T(r,c) = random();
    }
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c) {
      T(r,c) /= sum;
      T_sse(r,c) = T(r,c);
    }
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) = random();
    }
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) /= sum;
      E_sse(r,c) = E(r,c);
    }
  }

  HMM<float_type, float_type> hmm(pi_ptr, T_ptr, E_ptr);

  HMM<float_type, sse_float_type> hmm_sse(pi_ptr_sse, T_ptr_sse, E_ptr_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > F_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, float_type> > scales_ptr(new HMMVector<float_type, float_type>(obsseq_length));
  HMMVector<float_type, float_type> &scales = *scales_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F_sse = *F_ptr_sse;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr_sse(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales_sse = *scales_ptr_sse;

  hmm.forward(obs, scales, F);
  hmm_sse.forward(obs, scales_sse, F_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > B_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &B = *B_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > B_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &B_sse = *B_ptr_sse;

  hmm.backward(obs, scales, B);
  hmm_sse.backward(obs, scales_sse, B_sse);

  float_type v1 = B(obsseq_length-1, 3);
  float_type v2 = B(obsseq_length-1, 64);
  float_type v1_sse = B_sse(obsseq_length-1, 3);
  float_type v2_sse = B_sse(obsseq_length-1, 64);

  BOOST_CHECK_CLOSE(v1, v1_sse, 0.01);
  BOOST_CHECK_CLOSE(v2, v2_sse, 0.01);
}

// ############ BAUM WELCH TESTS ############### //
template <typename float_type, typename sse_float_type>
void baum_welch_2x2_with_transitions() {
  int no_states = 2;
  int alphabet_size = 2;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  // initial state probabilities
  pi(0)  = 0.2; pi(1) = 0.8;

  // transitions from state 0
  T(0,0) = 0.1; T(0,1) = 0.9;
  // transitions from state 1
  T(1,0) = 0.9; T(1,1) = 0.1;

  // emissions from state 0
  E(0,0) = 0.25; E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.25; E(1,1) = 0.75;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 1, 0, 1 };
  sequence obsseq(obs_array, obs_array + seq_length);

  HMMMatrix<float_type, sse_float_type> F(seq_length, no_states,seq_length);
  HMMMatrix<float_type, sse_float_type> B(seq_length,no_states);
  HMMVector<float_type, sse_float_type> scales(seq_length);

  hmm.forward(obsseq, scales, F);
  hmm.backward(obsseq, scales, B);

  HMMVector<float_type, sse_float_type> new_pi(no_states);
  HMMMatrix<float_type, sse_float_type> new_T(no_states,no_states);
  HMMMatrix<float_type, sse_float_type> new_E(alphabet_size,no_states);

  hmm.baum_welch(obsseq, F, B, scales, new_pi, new_T, new_E);

  // check that new_pi sums to 1
  double sum = 0;
  for (int s = 0; s < new_pi.get_size(); ++s)
    sum += new_pi(s);
  BOOST_CHECK_CLOSE(sum, 1.0, 0.0001);

  // check that all rows of new_T sums to 1
  for (int r = 0; r < new_T.get_no_rows(); ++r) {
    sum = 0.0;
    for (int c = 0; c < new_T.get_no_columns(); ++c) {
      sum += new_T(r,c);
    }
    BOOST_CHECK_CLOSE(sum, 1.0, 0.0001);
  }

  // check that all columns of new_E sums to 1
  for (int c = 0; c < new_E.get_no_columns(); ++c) {
    sum = 0;
    for (int r = 0; r < new_E.get_no_rows(); ++r) {
      sum += new_E(r,c);
    }
    BOOST_CHECK_CLOSE(sum, 1.0, 0.0001);
  }
}

#ifdef WITH_OMP
template <typename float_type, typename sse_float_type>
void parallel_bw() {
  int obsseq_length = 100;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  for(int i = 0; i < no_states; ++i)
    pi(i) = random();
  double sum = pi.sum();
  pi /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c)
      T(r,c) = random();
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c)
      T(r,c) /= sum;
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) = random();
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) /= sum;
  }

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales = *scales_ptr;

  hmm.forward(obs, scales, F);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > B_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &B = *B_ptr;

  hmm.backward(obs, scales, B);

  HMMVector<float_type, sse_float_type> pi_counts(no_states);
  HMMMatrix<float_type, sse_float_type> T_counts(no_states,no_states);
  HMMMatrix<float_type, sse_float_type> E_counts(alphabet_size,no_states);

  omp_set_num_threads(1);
  hmm.baum_welch(obs, F, B, scales, pi_counts, T_counts, E_counts);
  float_type pi11 = pi_counts(5);
  float_type pi12 = pi_counts(120);
  float_type T11 = T_counts(64, 45);
  float_type T12 = T_counts(1, 120);
  float_type E11 = E_counts(8, 45);
  float_type E12 = E_counts(1, 120);

  omp_set_num_threads(2);
  hmm.baum_welch(obs, F, B, scales, pi_counts, T_counts, E_counts);
  float_type pi21 = pi_counts(5);
  float_type pi22 = pi_counts(120);
  float_type T21 = T_counts(64, 45);
  float_type T22 = T_counts(1, 120);
  float_type E21 = E_counts(8, 45);
  float_type E22 = E_counts(1, 120);

  omp_set_num_threads(4);
  hmm.baum_welch(obs, F, B, scales, pi_counts, T_counts, E_counts);
  float_type pi41 = pi_counts(5);
  float_type pi42 = pi_counts(120);
  float_type T41 = T_counts(64, 45);
  float_type T42 = T_counts(1, 120);
  float_type E41 = E_counts(8, 45);
  float_type E42 = E_counts(1, 120);

  omp_set_num_threads(8);
  hmm.baum_welch(obs, F, B, scales, pi_counts, T_counts, E_counts);
  float_type pi81 = pi_counts(5);
  float_type pi82 = pi_counts(120);
  float_type T81 = T_counts(64, 45);
  float_type T82 = T_counts(1, 120);
  float_type E81 = E_counts(8, 45);
  float_type E82 = E_counts(1, 120);

  BOOST_CHECK_CLOSE(pi11, pi21, 0.01);
  BOOST_CHECK_CLOSE(pi11, pi41, 0.01);
  BOOST_CHECK_CLOSE(pi11, pi81, 0.01);

  BOOST_CHECK_CLOSE(pi12, pi22, 0.01);
  BOOST_CHECK_CLOSE(pi12, pi42, 0.01);
  BOOST_CHECK_CLOSE(pi12, pi82, 0.01);

  BOOST_CHECK_CLOSE(T11, T21, 0.01);
  BOOST_CHECK_CLOSE(T11, T41, 0.01);
  BOOST_CHECK_CLOSE(T11, T81, 0.01);

  BOOST_CHECK_CLOSE(T12, T22, 0.01);
  BOOST_CHECK_CLOSE(T12, T42, 0.01);
  BOOST_CHECK_CLOSE(T12, T82, 0.01);

  BOOST_CHECK_CLOSE(E11, E21, 0.01);
  BOOST_CHECK_CLOSE(E11, E41, 0.01);
  BOOST_CHECK_CLOSE(E11, E81, 0.01);

  BOOST_CHECK_CLOSE(E12, E22, 0.01);
  BOOST_CHECK_CLOSE(E12, E42, 0.01);
  BOOST_CHECK_CLOSE(E12, E82, 0.01);
}
#endif

template <typename float_type>
void sse_vs_nonSse_bw() {

  typedef typename FloatTraits<float_type>::sse_type sse_float_type;

  int obsseq_length = 133;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, float_type> > pi_ptr(new HMMVector<float_type, float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > T_ptr(new HMMMatrix<float_type, float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > E_ptr(new HMMMatrix<float_type, float_type>(alphabet_size, no_states));

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr_sse(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr_sse(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr_sse(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, float_type> &T = *T_ptr;
  HMMMatrix<float_type, float_type> &E = *E_ptr;

  HMMVector<float_type, sse_float_type> &pi_sse = *pi_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &T_sse = *T_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &E_sse = *E_ptr_sse;

  for(int i = 0; i < no_states; ++i) {
    pi(i) = random();
    pi_sse(i) = pi(i);
  }
  double sum = pi.sum();
  pi /= sum;
  pi_sse /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c) {
      T(r,c) = random();
    }
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c) {
      T(r,c) /= sum;
      T_sse(r,c) = T(r,c);
    }
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) = random();
    }
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) /= sum;
      E_sse(r,c) = E(r,c);
    }
  }

  HMM<float_type, float_type> hmm(pi_ptr, T_ptr, E_ptr);

  HMM<float_type, sse_float_type> hmm_sse(pi_ptr_sse, T_ptr_sse, E_ptr_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > F_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, float_type> > scales_ptr(new HMMVector<float_type, float_type>(obsseq_length));
  HMMVector<float_type, float_type> &scales = *scales_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F_sse = *F_ptr_sse;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr_sse(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales_sse = *scales_ptr_sse;

  hmm.forward(obs, scales, F);
  hmm_sse.forward(obs, scales_sse, F_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > B_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &B = *B_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > B_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &B_sse = *B_ptr_sse;

  hmm.backward(obs, scales, B);
  hmm_sse.backward(obs, scales_sse, B_sse);

  HMMVector<float_type, float_type> pi_counts(no_states);
  HMMMatrix<float_type, float_type> T_counts(no_states,no_states);
  HMMMatrix<float_type, float_type> E_counts(alphabet_size,no_states);

  HMMVector<float_type, sse_float_type> pi_counts_sse(no_states);
  HMMMatrix<float_type, sse_float_type> T_counts_sse(no_states,no_states);
  HMMMatrix<float_type, sse_float_type> E_counts_sse(alphabet_size,no_states);

  hmm.baum_welch(obs, F, B, scales, pi_counts, T_counts, E_counts);
  hmm_sse.baum_welch(obs, F_sse, B_sse, scales_sse, pi_counts_sse, T_counts_sse, E_counts_sse);

  float_type pi1 = pi_counts(5);
  float_type pi2 = pi_counts(120);
  float_type T1 = T_counts(64, 45);
  float_type T2 = T_counts(1, 120);
  float_type E1 = E_counts(8, 45);
  float_type E2 = E_counts(1, 120);

  float_type pi1_sse = pi_counts_sse(5);
  float_type pi2_sse = pi_counts_sse(120);
  float_type T1_sse = T_counts_sse(64, 45);
  float_type T2_sse = T_counts_sse(1, 120);
  float_type E1_sse = E_counts_sse(8, 45);
  float_type E2_sse = E_counts_sse(1, 120);

  BOOST_CHECK_CLOSE(pi1, pi1_sse, 0.01);
  BOOST_CHECK_CLOSE(pi2, pi2_sse, 0.01);
  BOOST_CHECK_CLOSE(T1, T1_sse, 0.01);
  BOOST_CHECK_CLOSE(T2, T2_sse, 0.01);
  BOOST_CHECK_CLOSE(E1, E1_sse, 0.01);
  BOOST_CHECK_CLOSE(E2, E2_sse, 0.01);
}

// ############ VITERBI TESTS ############### //
template <typename float_type, typename sse_float_type>
void viterbi_2x2_with_transitions() {
  int no_states = 2;
  int alphabet_size = 2;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 0.2; pi(1) = 0.8;

  // transitions from state 0
  T(0,0) = 0.1; T(0,1) = 0.9;
  // transitions from state 1
  T(1,0) = 0.9; T(1,1) = 0.1;

  // emissions from state 0
  E(0,0) = 0.25; E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.25; E(1,1) = 0.75;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 1, 0, 1 };
  sequence obsseq(obs_array, obs_array + seq_length);
  sequence hiddenseq(seq_length);

  hmm.viterbi(obsseq, hiddenseq);

  BOOST_CHECK_EQUAL(hiddenseq[0], 1u);
  BOOST_CHECK_EQUAL(hiddenseq[1], 0u);
  BOOST_CHECK_EQUAL(hiddenseq[2], 1u);
  BOOST_CHECK_EQUAL(hiddenseq[3], 0u);
}

#ifdef WITH_OMP
template <typename float_type, typename sse_float_type>
void parallel_viterbi() {
  int obsseq_length = 100;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  for(int i = 0; i < no_states; ++i)
    pi(i) = random();
  double sum = pi.sum();
  pi /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c)
      T(r,c) = random();
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c)
      T(r,c) /= sum;
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) = random();
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) /= sum;
  }

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  sequence hidden_seq(obsseq_length);
  omp_set_num_threads(1);
  hmm.viterbi(obs, hidden_seq);
  unsigned s11 = hidden_seq[obsseq_length -1];
  unsigned s12 = hidden_seq[64];

  omp_set_num_threads(2);
  hmm.viterbi(obs, hidden_seq);
  unsigned s21 = hidden_seq[obsseq_length -1];
  unsigned s22 = hidden_seq[64];

  omp_set_num_threads(4);
  hmm.viterbi(obs, hidden_seq);
  unsigned s41 = hidden_seq[obsseq_length -1];
  unsigned s42 = hidden_seq[64];

  omp_set_num_threads(8);
  hmm.viterbi(obs, hidden_seq);
  unsigned s81 = hidden_seq[obsseq_length -1];
  unsigned s82 = hidden_seq[64];

  BOOST_CHECK_EQUAL(s11, s21);
  BOOST_CHECK_EQUAL(s11, s41);
  BOOST_CHECK_EQUAL(s11, s81);
  BOOST_CHECK_EQUAL(s12, s22);
  BOOST_CHECK_EQUAL(s12, s42);
  BOOST_CHECK_EQUAL(s12, s82);
}
#endif

template <typename float_type>
void sse_vs_nonSse_viterbi() {

  typedef typename FloatTraits<float_type>::sse_type sse_float_type;

  int obsseq_length = 175;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, float_type> > pi_ptr(new HMMVector<float_type, float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > T_ptr(new HMMMatrix<float_type, float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > E_ptr(new HMMMatrix<float_type, float_type>(alphabet_size, no_states));

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr_sse(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr_sse(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr_sse(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, float_type> &T = *T_ptr;
  HMMMatrix<float_type, float_type> &E = *E_ptr;

  HMMVector<float_type, sse_float_type> &pi_sse = *pi_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &T_sse = *T_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &E_sse = *E_ptr_sse;

  for(int i = 0; i < no_states; ++i) {
    pi(i) = random();
    pi_sse(i) = pi(i);
  }
  double sum = pi.sum();
  pi /= sum;
  pi_sse /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c) {
      T(r,c) = random();
    }
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c) {
      T(r,c) /= sum;
      T_sse(r,c) = T(r,c);
    }
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) = random();
    }
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) /= sum;
      E_sse(r,c) = E(r,c);
    }
  }

  HMM<float_type, float_type> hmm(pi_ptr, T_ptr, E_ptr);
  HMM<float_type, sse_float_type> hmm_sse(pi_ptr_sse, T_ptr_sse, E_ptr_sse);

  sequence hidden_seq(obsseq_length);
  sequence hidden_seq_sse(obsseq_length);

  hmm.viterbi(obs, hidden_seq);
  hmm_sse.viterbi(obs, hidden_seq_sse);

  unsigned s1 = hidden_seq[obsseq_length -1];
  unsigned s2 = hidden_seq[64];
  unsigned s3 = hidden_seq[86];
  unsigned s4 = hidden_seq[24];

  unsigned s1_sse = hidden_seq_sse[obsseq_length -1];
  unsigned s2_sse = hidden_seq_sse[64];
  unsigned s3_sse = hidden_seq_sse[86];
  unsigned s4_sse = hidden_seq_sse[24];

  BOOST_CHECK_EQUAL(s1, s1_sse);
  BOOST_CHECK_EQUAL(s2, s2_sse);
  BOOST_CHECK_EQUAL(s3, s3_sse);
  BOOST_CHECK_EQUAL(s4, s4_sse);
}

// ############ POSTERIOR DECODING TESTS ############### //
#ifdef WITH_OMP
template <typename float_type, typename sse_float_type>
void parallel_pd() {
  int obsseq_length = 100;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  for(int i = 0; i < no_states; ++i)
    pi(i) = random();
  double sum = pi.sum();
  pi /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c)
      T(r,c) = random();
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c)
      T(r,c) /= sum;
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) = random();
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r)
      E(r,c) /= sum;
  }

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales = *scales_ptr;

  hmm.forward(obs, scales, F);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > B_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &B = *B_ptr;

  hmm.backward(obs, scales, B);

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > pd_ptr(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &pd = *pd_ptr;

  omp_set_num_threads(1);
  hmm.posterior_decoding(obs, F, B, scales, pd);
  float_type pd11 = pd(120, 8);
  float_type pd12 = pd(127, 56);

  omp_set_num_threads(2);
  hmm.posterior_decoding(obs, F, B, scales, pd);
  float_type pd21 = pd(120, 8);
  float_type pd22 = pd(127, 56);

  omp_set_num_threads(4);
  hmm.posterior_decoding(obs, F, B, scales, pd);
  float_type pd41 = pd(120, 8);
  float_type pd42 = pd(127, 56);

  omp_set_num_threads(8);
  hmm.posterior_decoding(obs, F, B, scales, pd);
  float_type pd81 = pd(120, 8);
  float_type pd82 = pd(127, 56);

  BOOST_CHECK_CLOSE(pd11, pd21, 0.01);
  BOOST_CHECK_CLOSE(pd11, pd41, 0.01);
  BOOST_CHECK_CLOSE(pd11, pd81, 0.01);

  BOOST_CHECK_CLOSE(pd12, pd22, 0.01);
  BOOST_CHECK_CLOSE(pd12, pd42, 0.01);
  BOOST_CHECK_CLOSE(pd12, pd82, 0.01);
}
#endif

template <typename float_type>
void sse_vs_nonSse_pd() {

  typedef typename FloatTraits<float_type>::sse_type sse_float_type;

  int obsseq_length = 133;
  int no_states = 128;
  int alphabet_size = 16;

  vector<unsigned int> obs;
  for(int i = 0; i < obsseq_length; ++i) {
    unsigned r = random() % alphabet_size;
    obs.push_back(r);
  }

  boost::shared_ptr<HMMVector<float_type, float_type> > pi_ptr(new HMMVector<float_type, float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > T_ptr(new HMMMatrix<float_type, float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, float_type> > E_ptr(new HMMMatrix<float_type, float_type>(alphabet_size, no_states));

  boost::shared_ptr<HMMVector<float_type, sse_float_type> > pi_ptr_sse(new HMMVector<float_type, sse_float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_ptr_sse(new HMMMatrix<float_type, sse_float_type>(no_states, no_states));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_ptr_sse(new HMMMatrix<float_type, sse_float_type>(alphabet_size, no_states));

  HMMVector<float_type, float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, float_type> &T = *T_ptr;
  HMMMatrix<float_type, float_type> &E = *E_ptr;

  HMMVector<float_type, sse_float_type> &pi_sse = *pi_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &T_sse = *T_ptr_sse;
  HMMMatrix<float_type, sse_float_type> &E_sse = *E_ptr_sse;

  for(int i = 0; i < no_states; ++i) {
    pi(i) = random();
    pi_sse(i) = pi(i);
  }
  double sum = pi.sum();
  pi /= sum;
  pi_sse /= sum;

  for(int r = 0; r < no_states; ++r) {
    for(int c = 0; c < no_states; ++c) {
      T(r,c) = random();
    }
    sum = T.row_sum(r);
    for(int c = 0; c < no_states; ++c) {
      T(r,c) /= sum;
      T_sse(r,c) = T(r,c);
    }
  }

  for(int c = 0; c < no_states; ++c) {
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) = random();
    }
    sum = T.column_sum(c);
    for(int r = 0; r < alphabet_size; ++r) {
      E(r,c) /= sum;
      E_sse(r,c) = E(r,c);
    }
  }

  HMM<float_type, float_type> hmm(pi_ptr, T_ptr, E_ptr);

  HMM<float_type, sse_float_type> hmm_sse(pi_ptr_sse, T_ptr_sse, E_ptr_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > F_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &F = *F_ptr;
  boost::shared_ptr<HMMVector<float_type, float_type> > scales_ptr(new HMMVector<float_type, float_type>(obsseq_length));
  HMMVector<float_type, float_type> &scales = *scales_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > F_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &F_sse = *F_ptr_sse;
  boost::shared_ptr<HMMVector<float_type, sse_float_type> > scales_ptr_sse(new HMMVector<float_type, sse_float_type>(obsseq_length));
  HMMVector<float_type, sse_float_type> &scales_sse = *scales_ptr_sse;

  hmm.forward(obs, scales, F);
  hmm_sse.forward(obs, scales_sse, F_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > B_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &B = *B_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > B_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &B_sse = *B_ptr_sse;

  hmm.backward(obs, scales, B);
  hmm_sse.backward(obs, scales_sse, B_sse);

  boost::shared_ptr<HMMMatrix<float_type, float_type> > pd_ptr(new HMMMatrix<float_type, float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, float_type> &pd = *pd_ptr;

  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > pd_ptr_sse(new HMMMatrix<float_type, sse_float_type>(obsseq_length, no_states));
  HMMMatrix<float_type, sse_float_type> &pd_sse = *pd_ptr_sse;

  hmm.posterior_decoding(obs, F, B, scales, pd);
  hmm_sse.posterior_decoding(obs, F_sse, B_sse, scales_sse, pd_sse);

  float_type pd1 = pd(120, 8);
  float_type pd2 = pd(127, 56);
  float_type pd3 = pd(50, 76);
  float_type pd4 = pd(87, 12);

  float_type pd1_sse = pd_sse(120, 8);
  float_type pd2_sse = pd_sse(127, 56);
  float_type pd3_sse = pd_sse(50, 76);
  float_type pd4_sse = pd_sse(87, 12);

  BOOST_CHECK_CLOSE(pd1, pd1_sse, 0.01);
  BOOST_CHECK_CLOSE(pd2, pd2_sse, 0.01);
  BOOST_CHECK_CLOSE(pd3, pd3_sse, 0.01);
  BOOST_CHECK_CLOSE(pd4, pd4_sse, 0.01);
}

// ############ TEST ALL ############### //
template <typename float_type, typename sse_float_type>
void all_sse() {
  int K = 2;
  int M = 2;
  int n = 4;

  boost::shared_ptr<HMMVector<float_type, sse_float_type> >
    pi_ptr(new HMMVector<float_type, sse_float_type>(K));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> >
    T_ptr(new HMMMatrix<float_type, sse_float_type>(K,K));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> >
    E_ptr(new HMMMatrix<float_type, sse_float_type>(M, K));

  HMMVector<float_type, sse_float_type> &pi = *pi_ptr;
  HMMMatrix<float_type, sse_float_type> &T = *T_ptr;
  HMMMatrix<float_type, sse_float_type> &E = *E_ptr;

  pi(0)  = 0.2; pi(1) = 0.8;

  // transitions from state 0
  T(0,0) = 0.1; T(0,1) = 0.9;
  // transitions from state 1
  T(1,0) = 0.9; T(1,1) = 0.1;

  // emissions from state 0
  E(0,0) = 0.25;
  E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.75;
  E(1,1) = 0.25;

  HMM<float_type, sse_float_type> hmm(pi_ptr, T_ptr, E_ptr);

  sequence obsseq(n);
  obsseq[0] = 0;
  obsseq[1] = 1;
  obsseq[2] = 0;
  obsseq[3] = 1;

  sequence hiddenseq(n);
  hmm.viterbi(obsseq, hiddenseq);

  HMMMatrix<float_type, sse_float_type>
    F(n, K);
  HMMVector<float_type, sse_float_type>
    scales(n);
  hmm.forward(obsseq, scales, F);

  float_type log_likelihood = hmm.likelihood(scales);
  log_likelihood += 1; // to avoid warnings about unused variable.

  HMMMatrix<float_type, sse_float_type>
    B(n, K);
  hmm.backward(obsseq, scales, B);

  HMMMatrix<float_type, sse_float_type>
    pd(n, K);
  hmm.posterior_decoding(obsseq, F, B, scales, pd);

  boost::shared_ptr<HMMVector<float_type, sse_float_type> >
    pi_counts_ptr(new HMMVector<float_type, sse_float_type>(K));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> >
    T_counts_ptr(new HMMMatrix<float_type, sse_float_type>(K,K));
  boost::shared_ptr<HMMMatrix<float_type, sse_float_type> >
    E_counts_ptr(new HMMMatrix<float_type, sse_float_type>(M,K));
  hmm.baum_welch(obsseq,
	     F,
	     B,
	     scales,
	     *pi_counts_ptr,
	     *T_counts_ptr,
	     *E_counts_ptr);

  HMM<float_type, sse_float_type> new_hmm(pi_counts_ptr,
			  T_counts_ptr,
			  E_counts_ptr);
}

template <typename float_type>
void all_non_sse() {
  int no_states = 2;
  int alphabet_size = 2;
  int seq_length = 4;

  boost::shared_ptr<HMMVector<float_type,float_type> >
    pi_ptr(new HMMVector<float_type,float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type,float_type> >
    T_ptr(new HMMMatrix<float_type,float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type,float_type> >
    E_ptr(new HMMMatrix<float_type,float_type>(alphabet_size, no_states));

  HMMVector<float_type,float_type> &pi = *pi_ptr;
  HMMMatrix<float_type,float_type> &T = *T_ptr;
  HMMMatrix<float_type,float_type> &E = *E_ptr;

  pi(0)  = 0.2; pi(1) = 0.8;

  // transitions from state 0
  T(0,0) = 0.1; T(0,1) = 0.9;
  // transitions from state 1
  T(1,0) = 0.9; T(1,1) = 0.1;

  // emissions from state 0
  E(0,0) = 0.25;
  E(1,0) = 0.75;
  // emissions from state 1
  E(0,1) = 0.25;
  E(1,1) = 0.75;

  HMM<float_type,float_type> hmm(pi_ptr, T_ptr, E_ptr);

  unsigned obs_array[] = { 0, 1, 0, 1 };
  sequence obsseq(obs_array, obs_array + seq_length);

  sequence hiddenseq(obsseq.size());
  hmm.viterbi(obsseq, hiddenseq);

  HMMMatrix<float_type,float_type> F(obsseq.size(), no_states);
  HMMVector<float_type,float_type> scales(obsseq.size());
  hmm.forward(obsseq, scales, F);

  float_type log_likelihood = hmm.likelihood(scales);
  log_likelihood += 1;

  HMMMatrix<float_type,float_type> B(obsseq.size(), no_states);
  hmm.backward(obsseq, scales, B);

  HMMMatrix<float_type,float_type> pd(obsseq.size(), no_states);
  hmm.posterior_decoding(obsseq, F, B, scales, pd);

  boost::shared_ptr<HMMVector<float_type,float_type> >
    pi_counts_ptr(new HMMVector<float_type,float_type>(no_states));
  boost::shared_ptr<HMMMatrix<float_type,float_type> >
    T_counts_ptr(new HMMMatrix<float_type,float_type>(no_states,no_states));
  boost::shared_ptr<HMMMatrix<float_type,float_type> >
    E_counts_ptr(new HMMMatrix<float_type,float_type>(alphabet_size,no_states));
  hmm.baum_welch(obsseq,
	     F,
	     B,
	     scales,
	     *pi_counts_ptr,
	     *T_counts_ptr,
	     *E_counts_ptr);

  HMM<float_type,float_type> new_hmm(pi_counts_ptr,
				     T_counts_ptr,
				     E_counts_ptr);
}

#define TEST(test)					\
  BOOST_AUTO_TEST_CASE( hmm_double_double_##test ) {	\
    test<double, double>();				\
  }							\
  BOOST_AUTO_TEST_CASE( hmm_float_float_##test ) {	\
    test<float, float>();				\
  }							\
  BOOST_AUTO_TEST_CASE( hmm_double_m128d_##test ) {	\
    test<double, __m128d>();				\
  }							\
  BOOST_AUTO_TEST_CASE( hmm_float_m128_##test ) {	\
    test<float, __m128>();				\
  }							\

#define TEST_NON_SSE(test)				\
  BOOST_AUTO_TEST_CASE( hmm_double_double_##test ) {	\
    test<double>();	        			\
  }							\
  BOOST_AUTO_TEST_CASE( hmm_float_float_##test ) {	\
    test<float>();        				\
  }							\

TEST(forward_1x1)
TEST(forward_1x2_no_transitions)
TEST(forward_2x2_no_transitions)
TEST(forward_2x2_with_transitions)

TEST(backward_1x1)
TEST(backward_1x2_no_transitions)
TEST(backward_2x2_no_transitions)
TEST(backward_2x2_with_transitions)

TEST(baum_welch_2x2_with_transitions)

TEST(viterbi_2x2_with_transitions)

TEST(construct_sse)
TEST(all_sse)

TEST_NON_SSE(construct_non_sse)
TEST_NON_SSE(all_non_sse)

#ifdef WITH_OMP
TEST(parallel_forward)
TEST(parallel_backward)
TEST(parallel_viterbi)
TEST(parallel_bw)
TEST(parallel_pd)
#endif

TEST_NON_SSE(sse_vs_nonSse_forward)
TEST_NON_SSE(sse_vs_nonSse_backward)
TEST_NON_SSE(sse_vs_nonSse_bw)
TEST_NON_SSE(sse_vs_nonSse_viterbi)
TEST_NON_SSE(sse_vs_nonSse_pd)

BOOST_AUTO_TEST_SUITE_END()


#ifndef AIKIDO_STATESPACE_SE2STATESPACE_HPP_
#define AIKIDO_STATESPACE_SE2STATESPACE_HPP_
#include <Eigen/Geometry>

#include "aikido/statespace/ScopedState.hpp"
#include "aikido/statespace/StateSpace.hpp"

namespace aikido {
namespace statespace {

/// Defined in detail/SE2-impl.hpp
template <class>
class SE2StateHandle;

/// The two-dimensional special Euclidean group SE(2), i.e. the space of planar
/// rigid body transformations. Note that the group operation for SE(2) differs
/// from the group operation of the Cartesian product space R^2 x SO(2) because
/// it is constructed through the semi-direct product.
class SE2 : public virtual StateSpace
{
public:
  class State : public StateSpace::State
  {
  public:
    using Isometry2d
        = Eigen::Transform<double, 2, Eigen::Isometry, Eigen::DontAlign>;

    /// Constructs the identity element.
    State();

    ~State() = default;

    /// Constructs the state from an Eigen transformation object.
    ///
    /// \param _transform Eigen transformation
    explicit State(const Isometry2d& _transform);

    /// Sets value to an Eigen transfomation object.
    ///
    /// \param _transform Eigen transformation
    void setIsometry(const Isometry2d& _transform);

    /// Gets value as an Eigen transformation object.
    ///
    /// \return Eigen trasnformation
    const Isometry2d& getIsometry() const;

  private:
    Isometry2d mTransform;

    friend class SE2;
  };

  using StateHandle = SE2StateHandle<State>;
  using StateHandleConst = SE2StateHandle<const State>;

  using ScopedState = statespace::ScopedState<StateHandle>;
  using ScopedStateConst = statespace::ScopedState<StateHandleConst>;

  using StateSpace::compose;

  using Isometry2d = State::Isometry2d;

  /// Constructs a state space representing SE(2).
  SE2() = default;

  /// Helper function to create a \c ScopedState.
  ///
  /// \return new \c ScopedState
  ScopedState createState() const;

  /// Creates an identical clone of \c stateIn.
  ScopedState cloneState(const StateSpace::State* stateIn) const;

  /// Gets value as an Eigen transformation object.
  ///
  /// \param _state a \c State in this state space
  /// \return Eigen transformation
  const Isometry2d& getIsometry(const State* _state) const;

  /// Sets value to an Eigen transfomation object.
  ///
  /// \param _state a \c State in this state space
  /// \param _transform Eigen transformation
  void setIsometry(State* _state, const Isometry2d& _transform) const;

  // Documentation inherited.
  std::size_t getStateSizeInBytes() const override;

  // Documentation inherited.
  StateSpace::State* allocateStateInBuffer(void* _buffer) const override;

  // Documentation inherited.
  void freeStateInBuffer(StateSpace::State* _state) const override;

  // Documentation inherited.
  void compose(
      const StateSpace::State* _state1,
      const StateSpace::State* _state2,
      StateSpace::State* _out) const override;

  // Documentation inherited
  void getIdentity(StateSpace::State* _out) const override;

  // Documentation inherited
  void getInverse(
      const StateSpace::State* _in, StateSpace::State* _out) const override;

  // Documentation inherited
  std::size_t getDimension() const override;

  // Documentation inherited
  void copyState(
      const StateSpace::State* _source,
      StateSpace::State* _destination) const override;

  /// Exponential mapping of Lie algebra element to a Lie group element. The
  /// tangent space is parameterized a planar twist of the form (translation,
  /// translation, rotation).
  ///
  /// \param _tangent element of the tangent space
  /// \param[out] _out corresponding element of the Lie group
  void expMap(
      const Eigen::VectorXd& _tangent, StateSpace::State* _out) const override;

  /// Log mapping of Lie group element to a Lie algebra element. The tangent
  /// space is parameterized as a planar twist of the form (translation,
  /// translation, rotation).
  ///
  /// \param _state element of this Lie group
  /// \param[out] _tangent corresponding element of the tangent space
  void logMap(const StateSpace::State* _state, Eigen::VectorXd& _tangent)
      const override;

  /// Print the state. Format: [x, y, theta]
  void print(const StateSpace::State* _state, std::ostream& _os) const override;
};

} // namespace statespace
} // namespace aikido

#include "detail/SE2-impl.hpp"

#endif // ifndef AIKIDO_STATESPACE_SE2STATESPACE_HPP_


#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <ctime>
using namespace std;

#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <vw/Core.h>
#include <vw/Image.h>
#include <vw/FileIO.h>

#include "../../../core/format_helper.h"

#include "../../base/stereo_cvsgbm.h"
#include "../../base/stereo_sgm.h"
#include "semi_global_matching.h"

void computeSGM();

cv::StereoSGBM sgbm;
cv::Mat img_left, img_right;
cv::Mat img_disp, img_disp_save, mat_disp_save;

vw::ImageView<vw::PixelGray<vw::uint8>> left_image, right_image;

//int frame_num = 391;
int frame_num = 102;
int frame_num_max = 1000;
int pre_filter_cap = 40;                  // 70
int pre_filter_cap_max = 200;
int sad_window_size = 3;                  // 3 - 11
int sad_window_size_max = 20;
int P1 = 40;                              // 70
int P1_max = 200;
int P2 = 1800;                             // 800
int P2_max = 4000;
int min_disparity = 0;
int min_disparity_max = 20;
int number_of_disparities = 128;          // or 256 max
int number_of_disparities_max = 512;
int uniqueness_ratio = 0;                 // 0 - 15
int uniqueness_ratio_max = 15;
int speckle_window_size = 200;            // 100
int speckle_window_size_max = 1000;
int speckle_range = 32;
int speckle_range_max = 128;
int disp12_max_diff = 1;                 // 1
int disp12_max_diff_max = 20;
int full_dp = 1;
int full_dp_max = 1;

void updateParameters()
{
  if(P2 <= P1) {
    P2 = P1 + 1;
    std::cout << "Warning: P2 too small! Using P2 = " << P2 << "\n";
  }
  int residue = number_of_disparities % 16;
  if(residue != 0) {
    number_of_disparities += (16 - residue);
    std::cout << "Warning: number_of_disparities \% 16 != 0! Using number_of_disparities = " << number_of_disparities << "\n";
  }
  sgbm.preFilterCap = pre_filter_cap;
  sgbm.SADWindowSize = sad_window_size;
  sgbm.P1 = P1;
  sgbm.P2 = P2;
  sgbm.minDisparity = min_disparity;
  sgbm.numberOfDisparities = number_of_disparities;
  sgbm.uniquenessRatio = uniqueness_ratio;
  sgbm.speckleWindowSize = speckle_window_size;
  sgbm.speckleRange = speckle_range;
  sgbm.disp12MaxDiff = disp12_max_diff;
  sgbm.fullDP = full_dp;
}

void on_trackbar(int, void*)
{
  updateParameters();
}

void computeSGM()
{
  recon::StereoSGMParams sgm_params;
  sgm_params.disp_range = 128;
  //sgm_params.window_sz = 1;
  sgm_params.window_sz = 3;
  //sgm_params.penalty1 = 15;
  sgm_params.penalty1 = 40;
  //sgm_params.penalty2 = 100;
  sgm_params.penalty2 = 1000;
  recon::StereoSGM sgm(sgm_params);
  img_left = cv::imread("/home/kivan/Projects/datasets/Middlebury/im2.png", CV_LOAD_IMAGE_GRAYSCALE);
  img_right = cv::imread("/home/kivan/Projects/datasets/Middlebury/im6.png", CV_LOAD_IMAGE_GRAYSCALE);
  //img_left = cv::imread("/home/kivan/Projects/datasets/KITTI/sequences_gray/01/image_0/000102.png", CV_LOAD_IMAGE_GRAYSCALE);
  //img_right = cv::imread("/home/kivan/Projects/datasets/KITTI/sequences_gray/01/image_1/000102.png", CV_LOAD_IMAGE_GRAYSCALE);
  //double sigma = 0.0;
  //cv::GaussianBlur(img_left, img_left, cv::Size(3,3), sigma);
  //cv::GaussianBlur(img_right, img_right, cv::Size(3,3), sigma);

  struct timeval start, end;
  gettimeofday(&start, NULL);

  sgm.compute(img_left, img_right, img_disp);

  gettimeofday(&end, NULL);
  float delta = ((end.tv_sec  - start.tv_sec) * 1000000u + end.tv_usec - start.tv_usec) / 1.e6;
  std::cout << "My SGM time spent = " << delta << "s\n";
  cv::equalizeHist(img_disp, img_disp);
  cv::imwrite("disp_image.png", img_disp);

  //vw::read_image(left_image, limg_path);
  //vw::read_image(right_image, rimg_path);
  vw::read_image(left_image, "/home/kivan/Projects/datasets/Middlebury/im2.png");
  vw::read_image(right_image, "/home/kivan/Projects/datasets/Middlebury/im6.png");
  gettimeofday(&start, NULL);
  vw::write_image("disp_vw.png", vw::semi_global_matching_func(left_image, right_image));
  gettimeofday(&end, NULL);
  delta = ((end.tv_sec  - start.tv_sec) * 1000000u + end.tv_usec - start.tv_usec) / 1.e6;
  std::cout << "vw_SGM time spent = " << delta << "s\n";

  //recon::StereoSGBMParams params(min_disparity, number_of_disparities, sad_window_size, P1, P2, disp12_max_diff,
  //                               pre_filter_cap, uniqueness_ratio, speckle_window_size, speckle_range, full_dp);
  //cv::Mat buffer;
  //recon::computeDisparitySGBM(img_left, img_right, img_disp, params, buffer);
  //recon::computeSGM(img_left, img_right, params, img_disp);
  //sgbm(img_left, img_right, img_disp);

  //img_disp.convertTo(img_disp_save, CV_8U, 1.0/16.0);
  //img_disp.convertTo(mat_disp_save, CV_32F, 1.0/16.0);
  //for(int j = 0; j < img_disp.rows; j++) {
  //  for(int k = 0; k < img_disp.cols; k++) {
  //    std::cout << (int)img_disp_save.at<uint8_t>(j,k) << " - CV_8U\n";
  //    std::cout << img_disp.at<int16_t>(j,k) / 16.0 << " - CV_16S\n";
  //    std::cout << mat_disp_save.at<float>(j,k) << " - CV_32F\n";
  //  }
  //}
  //img_disp.convertTo(img_disp, CV_8U, (255.0/sgbm.numberOfDisparities) * (1.0/16.0));
  //cv::equalizeHist(img_disp, img_disp);
  //cv::imshow("Disparity image", img_disp);

  //string img_fname = output_folder + "img/" + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.png";
  //string mat_fname = output_folder + "mat/" + imagelist[i].substr(9, imagelist[i].size()-13) + "_disp.yml";
  //cout << "Saving: " << img_fname << "\n" << mat_fname << "\n----------------------\n";
  //cv::imwrite(img_fname, img_disp_save, compression_params);
  //cv::FileStorage mat_file(mat_fname, cv::FileStorage::WRITE);
  //mat_file << "disparity_matrix" << mat_disp_save;

  //cout << img_disp << "\n\n";
  //imshow("disparity", img_disp);
  //waitKey(0);
}

void fineTuneSGM(const vector<string>& imagelist, const string& source_folder)
{
  // http://www.cvlibs.net/datasets/kitti/eval_stereo_flow_detail.php?benchmark=stereo&error=3&eval=all&result=3ae300a3a3b3ed3e48a63ecb665dffcc127cf8ab
  //sgbm.preFilterCap = 63;
  //sgbm.SADWindowSize = 3;       // 3, 5
  //int cn = 1;                   // image channel size
  ////sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
  //sgbm.P1 = 4*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;       // 50
  //sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;      // 800
  //sgbm.minDisparity = 0;
  //sgbm.numberOfDisparities = 128;     // 128, 256
  //sgbm.uniquenessRatio = 10;          // 10, 0
  //sgbm.speckleWindowSize = 100;       // 100
  //sgbm.speckleRange = 32;             // 32
  //sgbm.disp12MaxDiff = 1;
  //sgbm.fullDP = 1;

  // better for KITTI
  // http://www.cvlibs.net/datasets/kitti/eval_stereo_flow_detail.php?benchmark=stereo&error=3&eval=all&result=0d8c42f3fb90ff87bf89c1f8f7977cf309a0f49d
  //sgbm.preFilterCap = 15;
  //sgbm.SADWindowSize = 5;
  //sgbm.P1 = 50;
  //sgbm.P2 = 800;
  //sgbm.minDisparity = 0;
  //sgbm.numberOfDisparities = 256;
  //sgbm.uniquenessRatio = 0;
  //sgbm.speckleWindowSize = 100;
  //sgbm.speckleRange = 32;
  //sgbm.disp12MaxDiff = 1;
  //sgbm.fullDP = 1;
  updateParameters();

  cv::namedWindow("Disparity image");
  cv::namedWindow("Left image");
  cv::namedWindow("Right image");
  cv::namedWindow("Parameters");
  cv::createTrackbar("Frame number", "Parameters", &frame_num, frame_num_max, on_trackbar);
  cv::createTrackbar("preFilterCap", "Parameters", &pre_filter_cap, pre_filter_cap_max, on_trackbar);
  cv::createTrackbar("SADWindowSize", "Parameters", &sad_window_size, sad_window_size_max, on_trackbar);
  cv::createTrackbar("P1", "Parameters", &P1, P1_max, on_trackbar);
  cv::createTrackbar("P2", "Parameters", &P2, P2_max, on_trackbar);
  cv::createTrackbar("minDisparity", "Parameters", &min_disparity, min_disparity_max, on_trackbar);
  cv::createTrackbar("numberOfDisparities", "Parameters", &number_of_disparities, number_of_disparities_max, on_trackbar);
  cv::createTrackbar("uniquenessRatio", "Parameters", &uniqueness_ratio, uniqueness_ratio_max, on_trackbar);
  cv::createTrackbar("speckleWindowSize", "Parameters", &speckle_window_size, speckle_window_size_max, on_trackbar);
  cv::createTrackbar("speckleRange", "Parameters", &speckle_range, speckle_range_max, on_trackbar);
  cv::createTrackbar("disp12MaxDiff", "Parameters", &disp12_max_diff, disp12_max_diff_max, on_trackbar);
  cv::createTrackbar("fullDP", "Parameters", &full_dp, full_dp_max, on_trackbar);
  cv::imshow("Parameters", cv::Mat::zeros(1, 1400, CV_8U));

  //while(true) {
  //  //cout << source_folder + imagelist[frame_num] << "\n";
  //  std::string limg_path = source_folder + imagelist[frame_num*2];
  //  std::string rimg_path = source_folder + imagelist[(frame_num*2)+1];
  //  img_left = cv::imread(limg_path, CV_LOAD_IMAGE_GRAYSCALE);
  //  img_right = cv::imread(rimg_path, CV_LOAD_IMAGE_GRAYSCALE);
  //  cv::imshow("Left image", img_left);
  //  cv::imshow("Right image", img_right);

  //  cv::Mat img_left_eq, img_right_eq;
  //  cv::equalizeHist(img_left, img_left_eq);
  //  cv::equalizeHist(img_right, img_right_eq);
  //  img_left = img_left_eq;
  //  img_right = img_right_eq;
  //  cv::imshow("left_eq", img_left_eq);
  //  cv::imshow("right_eq", img_right_eq);

  //  computeSGM();
  //  cv::waitKey(0);
  //}
  //cv::waitKey(0);
  computeSGM();
}

int main(int argc, char** argv)
{
  string config_file;
  string imagelistfn;
  string cam_params_file;
  string source_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help", "produce help message")
      ("config,c", po::value<string>(&config_file)->default_value("config.txt"), "config filename")
      ;
    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<string>(&cam_params_file)->default_value("camera_params.txt"),
       "camera params file")
      ("source_folder,s", po::value<string>(&source_folder), "folder with source")
      ("imglist,l", po::value<string>(&imagelistfn), "file with image list")
      ;

    po::options_description cmdline_options;
    cmdline_options.add(generic).add(config);

    po::options_description config_file_options;
    config_file_options.add(config);
    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).allow_unregistered().run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if(vm.count("help")) {
      cout << generic;
      cout << config;
      return 0;
    }

    ifstream ifs(config_file.c_str());
    if (!ifs) {
      cout << "can not open config file: " << config_file << "\n";
      cout << generic;
      cout << config;
      return 0;
    }
    else {
      po::store(parse_config_file(ifs, config_file_options, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    cout << e.what() << "\n";
    return 1;
  }

  std::vector<std::string> imagelist;
  bool ok = core::FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
  {
    cout << "can not open " << imagelistfn << " or the string list is empty" << endl;
    return -1;
  }

  fineTuneSGM(imagelist, source_folder);

  return 0;
}


#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>
#include <array>
#include <iterator>
#include <iostream>

int main()
{
  std::array<int, 6> a{{0, 1, 2, 3, 4, 5}};
  boost::random_shuffle(a);
  boost::copy(a, std::ostream_iterator<int>{std::cout, ","});
  std::cout << "\n" << *boost::max_element(a) << '\n';
  std::cout << boost::accumulate(a, 0) << '\n';
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_POWM1
#define BOOST_MATH_POWM1

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/assert.hpp>

namespace boost{ namespace math{ namespace detail{

template <class T, class Policy>
inline T powm1_imp(const T x, const T y, const Policy& pol)
{
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::powm1<%1%>(%1%, %1%)";

   if (x > 0)
   {
      if ((fabs(y * (x - 1)) < 0.5) || (fabs(y) < 0.2))
      {
         // We don't have any good/quick approximation for log(x) * y
         // so just try it and see:
         T l = y * log(x);
         if (l < 0.5)
            return boost::math::expm1(l);
         if (l > boost::math::tools::log_max_value<T>())
            return boost::math::policies::raise_overflow_error<T>(function, 0, pol);
         // fall through....
      }
   }
   else
   {
      // y had better be an integer:
      if (boost::math::trunc(y) != y)
         return boost::math::policies::raise_domain_error<T>(function, "For non-integral exponent, expected base > 0 but got %1%", x, pol);
      if (boost::math::trunc(y / 2) == y / 2)
         return powm1_imp(T(-x), y, pol);
   }
   return pow(x, y) - 1;
}

} // detail

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   powm1(const T1 a, const T2 z)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::powm1_imp(static_cast<result_type>(a), static_cast<result_type>(z), policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   powm1(const T1 a, const T2 z, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   return detail::powm1_imp(static_cast<result_type>(a), static_cast<result_type>(z), pol);
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_POWM1


#include "dataset.h"
#include "stereo_algorithm.h"
#include "calibration.h"
#include "utils.h"
#include "imgproc.h"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/contrib/contrib.hpp>

#include <fstream>
#include <Eigen/LU>

void colorizeDisparity(const cv::Mat& src, cv::Mat& dst, double min_d, double num_d)
{
  THROW_ERROR_IF( src.type() != cv::DataType<float>::type, "disparity must be float" );

  double scale = 0.0;
  if(num_d > 0) {
    scale = 255.0 / num_d;
  } else {
    double max_val = 0;
    cv::minMaxLoc(src, nullptr, &max_val);
    scale = 255.0 / max_val;
  }

  src.convertTo(dst, CV_8U, scale);
  cv::applyColorMap(dst, dst, cv::COLORMAP_JET);

  for(int y = 0; y < src.rows; ++y)
    for(int x = 0; x < src.cols; ++x)
      if(src.at<float>(y,x) <= min_d)
        dst.at<cv::Vec3b>(y,x) = cv::Vec3b(0,0,0);
}

inline cv::Mat colorizeDisparity(const cv::Mat& src, double min_d = 0, double num_d = -1)
{
  cv::Mat ret;
  colorizeDisparity(src, ret, min_d, num_d);
  return ret;
}

void overlayDisparity(const cv::Mat& I, const cv::Mat& D, cv::Mat& dst,
                      double alpha, double min_d = 0, double num_d = 128)
{
  cv::Mat image;
  switch( I.type() )
  {
    case CV_8UC1:
      cv::cvtColor(I, image, CV_GRAY2BGR);
      break;
    case CV_8UC3:
      image = I;
      break;
    case CV_8UC4:
      cv::cvtColor(I, image, CV_BGRA2BGR);
      break;
    default:
      THROW_ERROR("unsupported image type");
  }

  cv::addWeighted(image, alpha, colorizeDisparity(D, min_d, num_d), 1.0-alpha, 0.0, dst);
}

int main(int argc, char** argv)
{
  utils::ProgramOptions options;
  options("config,c", "../config/kitti_stereo.cfg", "config file").parse(argc, argv);

  auto dataset = Dataset::Create(options.get<std::string>("config"));

  cv::Mat dmap;
  UniquePointer<DatasetFrame> frame;
  int f_i = 0;
  while( (frame = dataset->getFrame(f_i++)) ) {
    fprintf(stdout, "Frame %06d\r", f_i-1); fflush(stdout);
    overlayDisparity(frame->image(), frame->disparity(), dmap, 0.7);
    cv::imshow("dmap", dmap);
    if('q' == (0xff & cv::waitKey(5)))
      break;
  }

  auto calib = dataset->calibration();

  std::vector<float> z( dataset->imageSize().numel() );

  disparityToDepth(frame->disparity().ptr<float>(), dataset->imageSize(),
                   calib.baseline() * calib.fx(), z.data());

  Mat33 K_inv = calib.K().inverse();
  std::ofstream ofs("X");
  for(int y = 0, i = 0; y < frame->disparity().rows; ++y) {
    for(int x = 0; x < frame->disparity().cols; ++x, ++i) {
      if(z[i] > 0.0 && z[i] < 10.0) {
        Vec_<double,3> X = z[i] * K_inv * Vec_<double,3>(x, y, 1.0);
        ofs << X.transpose() << "\n";
      }
    }
  }

  return 0;
}


#include <iostream>
#include <vector>
#include <fstream>
#include <sstream>
#include <string>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <boost/filesystem.hpp>
#include <glog/logging.h>

#include "FileSystemTools.h"
#include "YamlFileIO.h"

struct CameraParam
{
    int image_width;
    int image_height;

    cv::Mat camera_matrix;
    cv::Mat distortion_coefficients;
};

bool readIntrinsic(const std::string &file_name, CameraParam &cam_param)
{
    if (file_name.empty())
    {
        std::cout << "[readFileStorage] Empty input file name " << file_name << "\n";
        return false;
    }

    cv::FileStorage fsCamera(file_name, cv::FileStorage::READ);
    if (!fsCamera.isOpened())
    {
        std::cout << "invalid camera calibration file: " << file_name << "\n";
        return false;
    }

    cam_param.image_width = (int)fsCamera["image_width"];
    cam_param.image_height = (int)fsCamera["image_height"];

    fsCamera["camera_matrix"] >> cam_param.camera_matrix;
    fsCamera["distortion_coefficients"] >> cam_param.distortion_coefficients;
    std::cout << "camera_matrix : \n"
              << cam_param.camera_matrix << "\n";
    std::cout << "distortion_coefficients : \n"
              << cam_param.distortion_coefficients << "\n";

    fsCamera.release();

    return true;
}

int main(int argc, char **argv)
{
    if (argc < 4)
    {
        std::cerr << "Useage : test_undist_img <raw_img_folder> <intrinsic.yaml> <output_folder> <focal_scale>\n";
        return -1;
    }

    //! image path relative parameters
    std::string raw_img_folder(argv[1]);
    std::string intrin_filepath(argv[2]);
    std::string output_folder(argv[3]);
    double f_scale = 0.72;
    if (argc >= 5)
    {
        f_scale = std::stod(argv[4]);
    }

    if (!common::pathExists(raw_img_folder))
    {
        LOG(FATAL) << " Image folder " << raw_img_folder << " does not exit, quit...\n";
        return -1;
    }
    //! check and create undistorted_img image folder
    if (!common::pathExists(output_folder))
    {
        if(!common::createPath(output_folder))
        {
            LOG(FATAL) << "Fail to create " << output_folder << "\n";
            return -1;
        }
    }

    std::vector<std::string> v_img_paths;
    std::vector<std::string> paths;
    paths.push_back(raw_img_folder);
    common::getFileLists(paths, true, "jpg", &v_img_paths);

    //! read intrisnic file
    std::string distort_type;
    cv::Mat K, D;
    int img_width = -1, img_height = -1;
    // bool ret = common::loadIntrinFileKalibr(intrin_filepath, K, D, distort_type, img_width, img_height);
    bool ret = common::loadIntrinFileOpencv(intrin_filepath, K, D, distort_type, img_width, img_height);
    if (!ret)
    {
        return -1;
    }
    LOG(INFO) << " Total distorted image number is " << v_img_paths.size() << "\n";

    cv::Mat new_K = cv::Mat_<double>::eye(3,3);
    cv::Mat new_D = cv::Mat_<double>::zeros(1, 4);
    for (size_t i = 0; i < v_img_paths.size(); i++)
    {
        std::string img_path = v_img_paths[i];
        cv::Mat image = cv::imread(img_path, cv::IMREAD_UNCHANGED);
        if (image.empty())
        {
            LOG(FATAL) << "Fail to load the image: " << img_path << "\n";
            return -1;
        }

        cv::Mat undistorted_img;

        if (distort_type == "equi-distant")
        {
            cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1), map1, map2;
            // new_K.at<double>(0,0) = K.at<double>(0,0) * f_scale;
            // new_K.at<double>(1,1) = K.at<double>(0,0) * f_scale;
            // new_K.at<double>(0,2) = img_width / 2;
            // new_K.at<double>(1,2) = img_height / 2;
            new_K = K;
            cv::fisheye::initUndistortRectifyMap(K, D, R, new_K, cv::Size(img_width, img_height), CV_32FC1, map1, map2);
            cv::remap(image, undistorted_img, map1, map2, cv::InterpolationFlags::INTER_CUBIC);
        }
        if (distort_type == "radtan")
        {
            cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1), map1, map2;
            new_K = K;
            cv::initUndistortRectifyMap(K, D, R, new_K, cv::Size(img_width, img_height), CV_32FC1, map1, map2);
            cv::remap(image, undistorted_img, map1, map2, cv::InterpolationFlags::INTER_CUBIC);
        }

        std::string filepath, img_name;
        common::splitPathAndFilename(img_path, &filepath, &img_name);
        std::string undist_img_filepath = common::concatenateFolderAndFileName(output_folder, img_name);
        LOG(INFO) << " Save undistorted_img image " << undist_img_filepath << "\n";

        // std::vector<int> compression_params;
        // compression_params.push_back(cv::ImwriteFlags::IMWRITE_JPEG_QUALITY);
        // compression_params.push_back(100);
        // cv::imwrite(undist_img_filepath, undistorted_img, compression_params);
        cv::imwrite(undist_img_filepath, undistorted_img);
    }

    // save new intrinsic file
    std::string intrin_file_name, filepath;
    common::splitPathAndFilename(intrin_filepath, &filepath, &intrin_file_name);
    std::string save_new_intrin_filepath = common::concatenateFolderAndFileName(output_folder, intrin_file_name);
    ret = common::saveIntrinFileOpencv(save_new_intrin_filepath, new_K, new_D, distort_type, img_width, img_height);
    if (!ret)
    {
        return -1;
    }
    return 0;
}


#ifndef SAMPML_INCLUDE_DATA_HPP
#define SAMPML_INCLUDE_DATA_HPP

#include <vector>
#include <fstream>
#include <sstream>
#include <string_view>

#include <dlib/svm.h>

#include "common.hpp"
#include "exception.hpp"
#include "feature_vector.hpp"

namespace SAMPML_NAMESPACE {
    namespace data {
        template <class sample_type>
        class reader {
        public:
            using value_type = sample_type;
            using reference = std::add_lvalue_reference_t<value_type>;
            using const_reference = std::add_lvalue_reference_t<std::add_const_t<value_type>>;
            using container_type = std::vector<value_type>;
            using iterator = typename container_type::iterator;
            using const_iterator = typename container_type::const_iterator;
            using size_type = typename container_type::size_type;
            using difference_type = typename container_type::difference_type;

            reader() = default;
            reader(std::string_view path) { open(path); }
            reader(const reader& other) : samples(other.samples) { }
            reader(reader&& other) : samples(std::move(other.samples)) { }

            reader& operator=(const reader& other) {
                if (&other != this) {
                    samples = other.samples;
                }
                return *this;
            }

            reader& operator=(reader&& other) {
                if (&other != this) {
                    samples = std::move(other.samples);
                }
                return *this;
            }

            void open(std::string_view path) {
                process(path);
            }

            size_type size() const { return samples.size(); }
            size_type max_size() const { return samples.max_size(); }
            bool empty() const { return samples.empty(); }

            iterator begin() { return samples.begin(); }
            iterator end() { return samples.end(); }
            const_iterator begin() const { return samples.begin(); }
            const_iterator end() const { return samples.end(); }
            const_iterator cbegin() const { return samples.cbegin(); }
            const_iterator cend() const { return samples.cend(); }

            reference operator[](int index) { return samples[index]; }
            const_reference operator[](int index) const { return samples[index]; }

            reference at(int index) { return samples.at(index); }
            const_reference at(int index) const { return samples.at(index); }

            void swap(reader& other) {
                samples.swap(other.samples);
            }

        protected:
            container_type samples;

        private:
            void process(std::string_view path) {
                std::ifstream input_file(std::string(path.begin(), path.end()));
                if(!input_file.is_open()) { throw io_error(); }

                using T = typename value_type::type;

                samples.clear();

                std::string line;
                while (std::getline(input_file, line)) {
                    std::istringstream input(line);
                    std::istream_iterator<T> itr(input);
                    samples.push_back({});
                    for (int i = 0; i < samples.back().size(); i++) {
                        if(itr == std::istream_iterator<T>()) {
                            if(i == 0)
                                break;
                            throw bad_format("Bad Input Format: less features than expected");
                        }

                        samples.back()(i) = *itr++;

                    }
                    if(itr != std::istream_iterator<T>()) {
                        throw bad_format("Bad Input Format: more features than expected");
                    }
                }
            }
        };

        template<class sample_type>
        void swap(reader<sample_type>& lhs, reader<sample_type>& rhs) {
            lhs.swap(rhs);
        }

        template<class sample_type>
        bool operator==(const reader<sample_type>& lhs, const reader<sample_type>& rhs) {
            return lhs.samples == rhs.samples;
        }

        template<class sample_type>
        bool operator!=(const reader<sample_type>& lhs, const reader<sample_type>& rhs) {
            return lhs.samples != rhs.samples;
        }

        template<class container_type>
        void write(std::string_view path, const container_type& samples) {
            std::ofstream output_file(std::string(path.begin(), path.end()));
            if(!output_file.is_open()) { throw io_error(); }

            output_file << std::setprecision(6);
            for(const auto& sample : samples) {
                for(auto value : sample)
                    output_file << value << ' ';
                output_file << std::endl;
            }
        }

        template<class ForwardItr>
        void write(std::string_view path, ForwardItr first, ForwardItr last) {
            std::ofstream output_file(std::string(path.begin(), path.end()));
            if(!output_file.is_open()) { throw io_error(); }

            output_file << std::setprecision(6);
            while(first != last) {
                for(auto value : *first)
                    output_file << value << ' ';
                output_file << std::endl;
                first++;
            }
        }
    }
}
#endif /* SAMPML_INCLUDE_DATA_HPP */


#include <boost/integer/static_min_max.hpp>


//
// Created by samjjx on 2018/12/20.
//

#ifndef CSCI5570_SVM_HPP
#define CSCI5570_SVM_HPP

#include "base/magic.hpp"
#include <vector>
#include <Eigen/Dense>
#include <cmath>
#include "lib/svm_sample.hpp"

using namespace Eigen;
using namespace csci5570;
using DataStore = std::vector<lib::SVMSample>;

template <typename T>
class SVM {

public:
    SVM(DataStore* data_store)
    :data_store_(data_store) {
      for(auto& row : (*data_store_)) {
        for(auto& col : row.x_) {
          theta_[col.first] = 0.0;
          grad_[col.first] = 0.0;
        }
      }
    }
    std::vector<double> compute_gradients(const std::vector<lib::SVMSample>& samples, const std::vector<Key>& keys,
                                          const std::vector<double>& vals, double alpha) {
      std::vector<double> deltas(keys.size(), 0.);
      for (auto sample : samples) {
        auto& x = sample.x_;
        double y = sample.y_;
        double predict = 0.;
        int idx = 0;
        for (auto& field : x) {
          while (keys[idx] < field.first)
            ++idx;
          predict += vals[idx] * field.second;
        }
        predict += vals.back();
        int predictLabel;
        if (predict >= 0) {
          predictLabel = 1;
        } else {
          predictLabel = -1;
        }

        idx = 0;
        for (auto& field : x) {
          while (keys[idx] < field.first)
            ++idx;
          deltas[idx] += alpha * field.second * (y - predictLabel);
        }
        deltas[deltas.size() - 1] += alpha * (y - predictLabel);
      }
      return deltas;
    }

    double correct_rate(const std::vector<lib::SVMSample>& samples, const std::vector<Key>& keys,
                        const std::vector<double>& vals) {
      int total = samples.size();
      double n = 0;
      for (auto sample : samples) {
        auto& x = sample.x_;
        double y = sample.y_;
        double predict = 0.;
        int idx = 0;
        for (auto& field : x) {
          while (keys[idx] < field.first)
            ++idx;
          predict += vals[idx] * field.second;
        }
        predict += vals.back();
        int predict_;
        if (predict >= 0) {
          predict_ = 1;
        } else {
          predict_ = -1;
        }
        if (predict_ == y) {
          n++;
        }
      }
      double result = n / total;
      return result;
    }

    void get_keys(std::vector<Key>& keys) {
      for(auto& kv : theta_) {
        keys.push_back(kv.first);
      }
    }

private:
    DataStore* data_store_;
    std::map<Key, T> theta_;
    std::map<Key, T> grad_;
};

#endif //CSCI5570_SVM_HPP