#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>

#include "config.h"
#include "visual_odometry.h"
#include "g2o_types.h"

namespace slamar
{
VisualOdometry::VisualOdometry()
	: state_(INITIALIZING), ref_(nullptr), curr_(nullptr), map_(new Map), num_lost_(0), num_inliers_(0), matcher_flann_(new cv::flann::LshIndexParams(5,10,2))
{
	num_of_features_    = Config::get<int>("number_of_features");
	scale_factor_       = Config::get<double>("scale_factor");
	level_pyramid_      = Config::get<int>("level_pyramid");
	match_ratio_        = Config::get<float>("match_ratio");
	max_num_lost_       = Config::get<float>("max_num_lost");
	min_inliers_        = Config::get<int>("min_inliers");
	key_frame_min_rot   = Config::get<double>("keyframe_rotation");
	key_frame_min_trans = Config::get<double>("keyframe_translation");
	map_point_erase_ratio_ = Config::get<double>("map_point_erase_ratio");
	orb_ = cv::ORB::create(num_of_features_, scale_factor_, level_pyramid_);
}

VisualOdometry::~VisualOdometry()
{
}

bool VisualOdometry::addFrame(Frame::Ptr frame)
{
	switch (state_)
	{
	case INITIALIZING:
	{
		state_ = OK;
		curr_ = ref_ = frame;
		// extract features from first frame and add them into map
		extractKeyPoints();
		computeDescriptors();
		// compute the 3d position of features in ref frame
		addKeyFrame();
		break;
	}
	case OK:
	{
		curr_ = frame;
		curr_->T_c_w_ = ref_->T_c_w_;
		extractKeyPoints();
		computeDescriptors();
		featureMatching();
		poseEstimationPnP();
		if (checkEstimatedPose() == true) // a good estimation
		{
			curr_->T_c_w_ = T_c_w_estimated_;
			optimizeMap();
			num_lost_ = 0;
			if (checkKeyFrame() == true) // is a key-frame
			{
				addKeyFrame();
			}
		}
		else // bad estimation due to various reasons
		{
			num_lost_++;
			if (num_lost_ > max_num_lost_)
			{
				state_ = LOST;
			}
			return false;
		}
		break;
	}
	case LOST:
	{
		cout << "vo has lost." << endl;
		break;
	}
	}

	return true;
}

void VisualOdometry::extractKeyPoints()
{
	boost::timer timer;
	orb_->detect(curr_->color_, keypoints_curr_);
	cout << "extract keypoints cost time: " << timer.elapsed() << endl;
}

void VisualOdometry::computeDescriptors()
{
	boost::timer timer;
	orb_->compute(curr_->color_, keypoints_curr_, descriptors_curr_);
	cout << "descriptor computation cost time: " << timer.elapsed() << endl;
}

void VisualOdometry::featureMatching()
{
	boost::timer timer;
	// match desp_ref and desp_curr, use OpenCV's brute force match
	vector<cv::DMatch> matches;
	// select the candidates in map
	Mat desp_map;
	vector<MapPoint::Ptr> candidate;
	for(auto& allpoints: map_->map_points_ )
	{
		MapPoint::Ptr& p = allpoints.second;
		// check if p in curr frame image
		if(curr_->isInFrame(p->pos_))
		{
			// add to candidate
			p->visible_times_++;
			candidate.push_back(p);
			desp_map.push_back(p->descriptor_);
		}
	}
	matcher_flann_.match(desp_map, descriptors_curr_, matches);
	// select the best matches
	float min_dis = std::min_element(
						matches.begin(), matches.end(),
						[](const cv::DMatch &m1, const cv::DMatch &m2) {
							return m1.distance < m2.distance;
						})
						->distance;

	match_3dpts_.clear();
	match_2dkp_index_.clear();
	for (cv::DMatch &m : matches)
	{
		if (m.distance < max<float>(min_dis * match_ratio_, 30.0))
		{
			match_3dpts_.push_back(candidate[m.queryIdx]);
			match_2dkp_index_.push_back(m.trainIdx);
		}
	}
	cout << "good matches: " << match_3dpts_.size() << endl;
	cout << "match cost time: " << timer.elapsed() << endl;
}

void VisualOdometry::poseEstimationPnP()
{
	// construct the 3d 2d observations
	vector<cv::Point3f> pts3d;
	vector<cv::Point2f> pts2d;

	for(int index:match_2dkp_index_)
	{
		pts2d.push_back(keypoints_curr_[index].pt);
	}
	for(MapPoint::Ptr pt:match_3dpts_)
	{
		pts3d.push_back(pt->getPositionCV());
	}

	Mat K = (cv::Mat_<double>(3, 3) <<
	         ref_->camera_->fx_, 0, ref_->camera_->cx_,
			 0, ref_->camera_->fy_, ref_->camera_->cy_,
			 0, 0, 1);

	Mat rvec, tvec, inliers;
	cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
	num_inliers_ = inliers.rows;
	cout << "pnp inliers: " << num_inliers_ << endl;
	T_c_w_estimated_ = SE3(
		SO3(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0)),
		Vector3d(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0)));

	// using bundle adjustment to optimize the pose
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> Block;
	Block::LinearSolverType *linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
	Block* solver_ptr = new Block(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);

	g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
	pose->setId(0);
	pose->setEstimate(g2o::SE3Quat(
		T_c_w_estimated_.rotation_matrix(),
		T_c_w_estimated_.translation()
	));
	optimizer.addVertex(pose);

	// edges
	for (int i = 0; i < inliers.rows; ++i)
	{
		int index = inliers.at<int>(i, 0);
		// 3D -> 2D projection
		EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
		edge->setId(i);
		edge->setVertex(0, pose);
		edge->camera_ = curr_->camera_.get();
		edge->point_ = Vector3d(pts3d[index].x, pts3d[index].y, pts3d[index].z);
		edge->setMeasurement(Vector2d(pts2d[index].x, pts2d[index].y));
		edge->setInformation(Eigen::Matrix2d::Identity());
		optimizer.addEdge(edge);
		// set the inlier map points
		match_3dpts_[index]->matched_times_++;
	}

	optimizer.initializeOptimization();
	optimizer.optimize(10);

	T_c_w_estimated_ = SE3(
		pose->estimate().rotation(),
		pose->estimate().translation()
	);

	cout << "T_c_w_estimated_: " << endl
		 << T_c_w_estimated_.matrix() << endl;
}

bool VisualOdometry::checkEstimatedPose()
{
	// check if the estimated pose is good
	if (num_inliers_ < min_inliers_)
	{
		cout << "reject because inlier is too small: " << num_inliers_ << endl;
		return false;
	}

	// if the motion is too large, it is probably wrong
	SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
	Sophus::Vector6d d = T_r_c.log();
	if (d.norm() > 5.0)
	{
		cout << "reject because motion is too large: " << d.norm() << endl;
		return false;
	}
	return true;
}

bool VisualOdometry::checkKeyFrame()
{
	SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
	Sophus::Vector6d d = T_r_c.log();
	Vector3d trans = d.head<3>();
	Vector3d rot = d.tail<3>();
	if (rot.norm() > key_frame_min_rot || trans.norm() > key_frame_min_trans)
	{
		return true;
	}
	return false;
}

void VisualOdometry::addKeyFrame()
{
	if(map_->keyframes_.empty())
	{
		// first key-frame, add all 3d points into map
		for (size_t i = 0; i < keypoints_curr_.size(); ++i)
		{
			double d = curr_->findDepth(keypoints_curr_[i]);
			if(d < 0)
			{
				continue;
			}
			Vector3d p_world = ref_->camera_->pixel2world(
				Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), curr_->T_c_w_, d);
			Vector3d n = p_world - ref_->getCamCenter();
			n.normalize();
			MapPoint::Ptr map_point = MapPoint::createMapPoint(
				p_world, n, descriptors_curr_.row(i).clone(), curr_.get());
			map_->insertMapPoint(map_point);
		}
	}
	map_->insertKeyFrame(curr_);
	ref_ = curr_;
}

void VisualOdometry::addMapPoints()
{
	// add the new map points into map
	vector<bool> matched(keypoints_curr_.size(), false);
	for(int index:match_2dkp_index_)
	{
		matched[index] = true;
	}
	for (int i = 0; i < keypoints_curr_.size(); ++i)
	{
		if(matched[i] == true)
		{
			continue;
		}
		double d = ref_->findDepth(keypoints_curr_[i]);
		if(d<0)
		{
			continue;
		}
		Vector3d p_world = ref_->camera_->pixel2world(
			Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y),
			curr_->T_c_w_, d);
		Vector3d n = p_world - ref_->getCamCenter();
		n.normalize();
		MapPoint::Ptr map_point = MapPoint::createMapPoint(
			p_world, n, descriptors_curr_.row(i).clone(), curr_.get());
		map_->insertMapPoint(map_point);
	}
}

void VisualOdometry::optimizeMap()
{
	// remove the hardly seen and no visible points
	for (auto iter = map_->map_points_.begin(); iter != map_->map_points_.end();)
	{
		if(!curr_->isInFrame(iter->second->pos_))
		{
			iter = map_->map_points_.erase(iter);
			continue;
		}
		float match_ratio = float(iter->second->matched_times_) / iter->second->visible_times_;
		if(match_ratio < map_point_erase_ratio_)
		{
			iter = map_->map_points_.erase(iter);
			continue;
		}

		double angle = getViewAngle(curr_, iter->second);
		if(angle > M_PI/6.)
		{
			iter = map_->map_points_.erase(iter);
			continue;
		}
		if(iter->second->good_ == false)
		{
			// TODO try triangulate this map point
		}
		iter++;
	}

	if(match_2dkp_index_.size() < 100)
	{
		addMapPoints();
	}
	if(map_->map_points_.size() > 1000)
	{
		// TODO map is too large, remove some one
		map_point_erase_ratio_ += 0.05;
	}
	else
	{
		map_point_erase_ratio_ = 0.1;
	}
	cout << "map points: " << map_->map_points_.size() << endl;
}

double VisualOdometry::getViewAngle(Frame::Ptr frame, MapPoint::Ptr point)
{
	Vector3d n = point->pos_ - frame->getCamCenter();
	n.normalize();
	return acos(n.transpose() * point->norm_);
}

}


/**  \file empiricalbayes.hpp \brief Empirical Bayesian estimator */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt 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 Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef  _EMPIRICALBAYES_HPP_
#define  _EMPIRICALBAYES_HPP_

#include <boost/scoped_ptr.hpp>
#include "criteria_functors.hpp"
#include "posteriormodel.hpp"

namespace bayesopt {

  /** \addtogroup BayesOpt
   *  \brief Main module for Bayesian optimization
   */
  /*@{*/

  class NLOPT_Optimization;

 /**
   * \brief Bayesian optimization using different non-parametric
   * processes as distributions over surrogate functions.
   */
  class EmpiricalBayes: public PosteriorModel
  {
  public:
    /**
     * Constructor
     * @param params set of parameters (see parameters.hpp)
     */
    EmpiricalBayes(size_t dim, Parameters params, randEngine& eng);

    /**
     * Default destructor
     */
    virtual ~EmpiricalBayes();

    void updateHyperParameters();
    void fitSurrogateModel();
    void updateSurrogateModel();

    double evaluateCriteria(const vectord& query);
    void updateCriteria(const vectord& query);

    bool criteriaRequiresComparison();
    void setFirstCriterium();
    bool setNextCriterium(const vectord& prevResult);
    std::string getBestCriteria(vectord& best);

    ProbabilityDistribution* getPrediction(const vectord& query);

  private:
    EmpiricalBayes();

    void setSurrogateModel(randEngine& eng);
    void setCriteria(randEngine& eng);

  private:  // Members
    boost::scoped_ptr<NonParametricProcess> mGP; ///< Pointer to surrogate model
    boost::scoped_ptr<Criteria> mCrit;                   ///< Metacriteria model

    boost::scoped_ptr<NLOPT_Optimization> kOptimizer;
  };

  /**@}*/

  inline void EmpiricalBayes::fitSurrogateModel()
  { mGP->fitSurrogateModel(); };

  inline void EmpiricalBayes::updateSurrogateModel()
  { mGP->updateSurrogateModel(); };

  inline double EmpiricalBayes::evaluateCriteria(const vectord& query)
  { return (*mCrit)(query); };

  inline void EmpiricalBayes::updateCriteria(const vectord& query)
  { return mCrit->update(query); };

  inline bool EmpiricalBayes::criteriaRequiresComparison()
  {return mCrit->requireComparison(); };

  inline void EmpiricalBayes::setFirstCriterium()
  { mCrit->initialCriteria(); };

  inline bool EmpiricalBayes::setNextCriterium(const vectord& prevResult)
  {
    mCrit->pushResult(prevResult);
    return mCrit->rotateCriteria();
  };

  inline std::string EmpiricalBayes::getBestCriteria(vectord& best)
  { return mCrit->getBestCriteria(best); };

  inline  ProbabilityDistribution* EmpiricalBayes::getPrediction(const vectord& query)
  { return mGP->prediction(query); };

} //namespace bayesopt

#endif


#include "adm/elements/time.hpp"
#include <boost/integer/common_factor.hpp>
#include <boost/format.hpp>
#include <iomanip>
#include <regex>
#include <sstream>

namespace adm {

  FractionalTime FractionalTime::normalised() const {
    int64_t gcd = boost::integer::gcd(numerator(), denominator());
    return {numerator() / gcd, denominator() / gcd};
  }

  struct AsNanosecondsVisitor
      : public boost::static_visitor<std::chrono::nanoseconds> {
    std::chrono::nanoseconds operator()(
        const std::chrono::nanoseconds& time) const {
      return time;
    }

    std::chrono::nanoseconds operator()(const FractionalTime& time) const {
      FractionalTime normalised = time.normalised();
      return std::chrono::nanoseconds{(1000000000 * normalised.numerator()) /
                                      normalised.denominator()};
    }
  };

  struct AsFractionalVisitor : public boost::static_visitor<FractionalTime> {
    FractionalTime operator()(const std::chrono::nanoseconds& time) const {
      return {time.count(), 1000000000};
    }

    FractionalTime operator()(const FractionalTime& time) const { return time; }
  };

  std::chrono::nanoseconds Time::asNanoseconds() const {
    return boost::apply_visitor(AsNanosecondsVisitor(), time);
  }

  FractionalTime Time::asFractional() const {
    return boost::apply_visitor(AsFractionalVisitor(), time);
  }

  Time parseTimecode(const std::string& timecode) {
    const static std::regex commonFormat(
        "(\\d{2}):(\\d{2}):(\\d{2}).(\\d{1,9})");
    const static std::regex fractionalFormat(
        "(\\d{2}):(\\d{2}):(\\d{2}).(\\d+)S(\\d+)");

    std::smatch timecodeMatch;
    if (std::regex_match(timecode, timecodeMatch, commonFormat)) {
      // add trailing zeros to string
      auto nanoseconds =
          boost::str(boost::format("%1%") %
                     boost::io::group(std::setw(9), std::left,
                                      std::setfill('0'), timecodeMatch[4]));
      return std::chrono::hours(stoi(timecodeMatch[1])) +
             std::chrono::minutes(stoi(timecodeMatch[2])) +
             std::chrono::seconds(stoi(timecodeMatch[3])) +
             std::chrono::nanoseconds(stoi(nanoseconds));
    } else if (std::regex_match(timecode, timecodeMatch, fractionalFormat)) {
      int64_t seconds = 3600 * stoi(timecodeMatch[1]) +
                        60 * stoi(timecodeMatch[2]) +
                        1 * stoi(timecodeMatch[3]);

      int64_t numerator = stoi(timecodeMatch[4]);
      int64_t denominator = stoi(timecodeMatch[5]);

      if (denominator == 0) {
        std::stringstream errorString;
        errorString << "invalid timecode: " << timecode
                    << " has a zero denominator";
        throw std::runtime_error(errorString.str());
      }

      return FractionalTime{seconds * denominator + numerator, denominator};
    } else {
      std::stringstream errorString;
      errorString << "invalid timecode: " << timecode;
      throw std::runtime_error(errorString.str());
    }
  }

  struct FormatTimeVisitor : public boost::static_visitor<std::string> {
    std::string operator()(const std::chrono::nanoseconds& time) const {
      std::stringstream ss;
      ss << std::setw(2) << std::setfill('0')
         << std::chrono::duration_cast<std::chrono::hours>(time).count();
      ss << ":";
      ss << std::setw(2) << std::setfill('0')
         << std::chrono::duration_cast<std::chrono::minutes>(time).count() % 60;
      ss << ":";
      ss << std::setw(2) << std::setfill('0')
         << std::chrono::duration_cast<std::chrono::seconds>(time).count() % 60;
      ss << ".";
      ss << std::setw(9) << std::setfill('0') << time.count() % 1000000000;
      return ss.str();
    }

    std::string operator()(const FractionalTime& time) const {
      int64_t whole_seconds = time.numerator() / time.denominator();
      int64_t frac_numerator =
          time.numerator() - whole_seconds * time.denominator();

      std::stringstream ss;
      ss << std::setw(2) << std::setfill('0') << whole_seconds / 3600;
      ss << ":";
      ss << std::setw(2) << std::setfill('0') << (whole_seconds / 60) % 60;
      ss << ":";
      ss << std::setw(2) << std::setfill('0') << whole_seconds % 60;
      ss << ".";
      ss << frac_numerator << "S" << time.denominator();
      return ss.str();
    }
  };

  std::string formatTimecode(const Time& time) {
    return boost::apply_visitor(FormatTimeVisitor(), time.asVariant());
  }

}  // namespace adm


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef SNPTEST2_NULL_MODEL_HPP
#define SNPTEST2_NULL_MODEL_HPP

#include <boost/noncopyable.hpp>
#include <vector>
#include "Eigen/Eigen"
#include "snptest/FinitelySupportedFunctionSet.hpp"

namespace snptest {
	namespace case_control {
		struct NullModelLogLikelihood: public boost::noncopyable
		{
		public:
			typedef Eigen::VectorXd Vector ;
			typedef Eigen::RowVectorXd RowVector ;
			typedef Eigen::MatrixXd Matrix ;

			NullModelLogLikelihood() ;

			NullModelLogLikelihood& set_phenotypes( Vector const& phenotypes ) ;
			NullModelLogLikelihood& set_covariates( Matrix const& covariates ) ;
			NullModelLogLikelihood& set_predictor_probs( Matrix const& genotypes, Vector const& levels ) ;
			NullModelLogLikelihood& add_exclusions( std::vector< int > const& exclusions ) ;

			void evaluate_at( Vector const& parameters ) ;
			double get_value_of_function() const ;
			Vector get_value_of_first_derivative() const ;
			Matrix get_value_of_second_derivative() const ;

		private:
			Vector m_phenotypes ;
			Matrix m_covariates ;
			Matrix m_genotype_call_probabilities ;
			Vector m_genotype_levels ;
			std::vector< int > m_exclusions ;

			Matrix m_outcome_probabilities ;
			Matrix m_design_matrix ;
			Matrix m_coefficients ;

			double m_value_of_function ;
			Vector m_value_of_first_derivative ;
			Matrix m_value_of_second_derivative ;

		private:
			void add_exclusions( Vector const& v ) ;
			void add_exclusions( Matrix const& matrix ) ;

			Matrix calculate_design_matrix( Matrix const& covariates ) const ;
			// Calculate the probability of outcome given the genotype, parameters, and covariates.
			Vector evaluate_mean_function( Vector const& linear_combinations, Vector const& outcomes ) const ;
			// Calculate matrix of probabilities of outcome per genotype, given the parameters.
			Matrix calculate_outcome_probabilities( Vector const& parameters, Vector const& phenotypes, Matrix& design_matrix ) const ;
		} ;
	}
}

#endif


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying
//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_QVM_2923BE84E16CD6AE529049F1F1BBE9EB
#define BOOST_QVM_2923BE84E16CD6AE529049F1F1BBE9EB

//This file was generated by a program. Do not edit manually.

#include <boost/qvm/assert.hpp>
#include <boost/qvm/deduce_mat.hpp>
#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/error.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/throw_exception.hpp>

namespace
boost
    {
    namespace
    qvm
        {
        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            deduce_mat2<A,B,2,2> >::type
        operator+( A const & a, B const & b )
            {
            typedef typename deduce_mat2<A,B,2,2>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==2);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==2);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b);
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)+mat_traits<B>::template read_element<0,1>(b);
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)+mat_traits<B>::template read_element<1,0>(b);
            mat_traits<R>::template write_element<1,1>(r)=mat_traits<A>::template read_element<1,1>(a)+mat_traits<B>::template read_element<1,1>(b);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator+;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct plus_mm_defined;

            template <>
            struct
            plus_mm_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
            deduce_mat2<A,B,2,1> >::type
        operator+( A const & a, B const & b )
            {
            typedef typename deduce_mat2<A,B,2,1>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==2);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==1);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b);
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)+mat_traits<B>::template read_element<1,0>(b);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator+;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct plus_mm_defined;

            template <>
            struct
            plus_mm_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            deduce_mat2<A,B,1,2> >::type
        operator+( A const & a, B const & b )
            {
            typedef typename deduce_mat2<A,B,1,2>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==1);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==2);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)+mat_traits<B>::template read_element<0,0>(b);
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)+mat_traits<B>::template read_element<0,1>(b);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator+;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct plus_mm_defined;

            template <>
            struct
            plus_mm_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            deduce_mat2<A,B,2,2> >::type
        operator-( A const & a, B const & b )
            {
            typedef typename deduce_mat2<A,B,2,2>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==2);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==2);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b);
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)-mat_traits<B>::template read_element<0,1>(b);
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)-mat_traits<B>::template read_element<1,0>(b);
            mat_traits<R>::template write_element<1,1>(r)=mat_traits<A>::template read_element<1,1>(a)-mat_traits<B>::template read_element<1,1>(b);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_mm_defined;

            template <>
            struct
            minus_mm_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
            deduce_mat2<A,B,2,1> >::type
        operator-( A const & a, B const & b )
            {
            typedef typename deduce_mat2<A,B,2,1>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==2);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==1);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b);
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)-mat_traits<B>::template read_element<1,0>(b);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_mm_defined;

            template <>
            struct
            minus_mm_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            deduce_mat2<A,B,1,2> >::type
        operator-( A const & a, B const & b )
            {
            typedef typename deduce_mat2<A,B,1,2>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==1);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==2);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)-mat_traits<B>::template read_element<0,0>(b);
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)-mat_traits<B>::template read_element<0,1>(b);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_mm_defined;

            template <>
            struct
            minus_mm_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            A &>::type
        operator+=( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)+=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<0,1>(a)+=mat_traits<B>::template read_element<0,1>(b);
            mat_traits<A>::template write_element<1,0>(a)+=mat_traits<B>::template read_element<1,0>(b);
            mat_traits<A>::template write_element<1,1>(a)+=mat_traits<B>::template read_element<1,1>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator+=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct plus_eq_mm_defined;

            template <>
            struct
            plus_eq_mm_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
            A &>::type
        operator+=( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)+=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<1,0>(a)+=mat_traits<B>::template read_element<1,0>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator+=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct plus_eq_mm_defined;

            template <>
            struct
            plus_eq_mm_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            A &>::type
        operator+=( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)+=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<0,1>(a)+=mat_traits<B>::template read_element<0,1>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator+=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct plus_eq_mm_defined;

            template <>
            struct
            plus_eq_mm_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            A &>::type
        operator-=( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)-=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<0,1>(a)-=mat_traits<B>::template read_element<0,1>(b);
            mat_traits<A>::template write_element<1,0>(a)-=mat_traits<B>::template read_element<1,0>(b);
            mat_traits<A>::template write_element<1,1>(a)-=mat_traits<B>::template read_element<1,1>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_eq_mm_defined;

            template <>
            struct
            minus_eq_mm_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
            A &>::type
        operator-=( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)-=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<1,0>(a)-=mat_traits<B>::template read_element<1,0>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_eq_mm_defined;

            template <>
            struct
            minus_eq_mm_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            A &>::type
        operator-=( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)-=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<0,1>(a)-=mat_traits<B>::template read_element<0,1>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_eq_mm_defined;

            template <>
            struct
            minus_eq_mm_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            deduce_mat<A> >::type
        operator*( A const & a, B b )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)*b;
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)*b;
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)*b;
            mat_traits<R>::template write_element<1,1>(r)=mat_traits<A>::template read_element<1,1>(a)*b;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct mul_ms_defined;

            template <>
            struct
            mul_ms_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==1 && is_scalar<B>::value,
            deduce_mat<A> >::type
        operator*( A const & a, B b )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)*b;
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)*b;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct mul_ms_defined;

            template <>
            struct
            mul_ms_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            deduce_mat<A> >::type
        operator*( A const & a, B b )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)*b;
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)*b;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct mul_ms_defined;

            template <>
            struct
            mul_ms_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            A &>::type
        operator*=( A & a, B b )
            {
            mat_traits<A>::template write_element<0,0>(a)*=b;
            mat_traits<A>::template write_element<0,1>(a)*=b;
            mat_traits<A>::template write_element<1,0>(a)*=b;
            mat_traits<A>::template write_element<1,1>(a)*=b;
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct mul_eq_ms_defined;

            template <>
            struct
            mul_eq_ms_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==1 && is_scalar<B>::value,
            A &>::type
        operator*=( A & a, B b )
            {
            mat_traits<A>::template write_element<0,0>(a)*=b;
            mat_traits<A>::template write_element<1,0>(a)*=b;
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct mul_eq_ms_defined;

            template <>
            struct
            mul_eq_ms_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            A &>::type
        operator*=( A & a, B b )
            {
            mat_traits<A>::template write_element<0,0>(a)*=b;
            mat_traits<A>::template write_element<0,1>(a)*=b;
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct mul_eq_ms_defined;

            template <>
            struct
            mul_eq_ms_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            deduce_mat<A> >::type
        operator/( A const & a, B b )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)/b;
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)/b;
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)/b;
            mat_traits<R>::template write_element<1,1>(r)=mat_traits<A>::template read_element<1,1>(a)/b;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator/;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct div_ms_defined;

            template <>
            struct
            div_ms_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==1 && is_scalar<B>::value,
            deduce_mat<A> >::type
        operator/( A const & a, B b )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)/b;
            mat_traits<R>::template write_element<1,0>(r)=mat_traits<A>::template read_element<1,0>(a)/b;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator/;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct div_ms_defined;

            template <>
            struct
            div_ms_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            deduce_mat<A> >::type
        operator/( A const & a, B b )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=mat_traits<A>::template read_element<0,0>(a)/b;
            mat_traits<R>::template write_element<0,1>(r)=mat_traits<A>::template read_element<0,1>(a)/b;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator/;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct div_ms_defined;

            template <>
            struct
            div_ms_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            A &>::type
        operator/=( A & a, B b )
            {
            mat_traits<A>::template write_element<0,0>(a)/=b;
            mat_traits<A>::template write_element<0,1>(a)/=b;
            mat_traits<A>::template write_element<1,0>(a)/=b;
            mat_traits<A>::template write_element<1,1>(a)/=b;
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator/=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct div_eq_ms_defined;

            template <>
            struct
            div_eq_ms_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==1 && is_scalar<B>::value,
            A &>::type
        operator/=( A & a, B b )
            {
            mat_traits<A>::template write_element<0,0>(a)/=b;
            mat_traits<A>::template write_element<1,0>(a)/=b;
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator/=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct div_eq_ms_defined;

            template <>
            struct
            div_eq_ms_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            A &>::type
        operator/=( A & a, B b )
            {
            mat_traits<A>::template write_element<0,0>(a)/=b;
            mat_traits<A>::template write_element<0,1>(a)/=b;
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator/=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct div_eq_ms_defined;

            template <>
            struct
            div_eq_ms_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            A &>::type
        assign( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<0,1>(a)=mat_traits<B>::template read_element<0,1>(b);
            mat_traits<A>::template write_element<1,0>(a)=mat_traits<B>::template read_element<1,0>(b);
            mat_traits<A>::template write_element<1,1>(a)=mat_traits<B>::template read_element<1,1>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::assign;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct assign_mm_defined;

            template <>
            struct
            assign_mm_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
            A &>::type
        assign( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<1,0>(a)=mat_traits<B>::template read_element<1,0>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::assign;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct assign_mm_defined;

            template <>
            struct
            assign_mm_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            A &>::type
        assign( A & a, B const & b )
            {
            mat_traits<A>::template write_element<0,0>(a)=mat_traits<B>::template read_element<0,0>(b);
            mat_traits<A>::template write_element<0,1>(a)=mat_traits<B>::template read_element<0,1>(b);
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::assign;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct assign_mm_defined;

            template <>
            struct
            assign_mm_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class R,class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<R>::rows==2 && mat_traits<A>::rows==2 &&
            mat_traits<R>::cols==2 && mat_traits<A>::cols==2,
            R>::type
        convert_to( A const & a )
            {
            R r;
            mat_traits<R>::template write_element<0,0>(r) = mat_traits<A>::template read_element<0,0>(a);
            mat_traits<R>::template write_element<0,1>(r) = mat_traits<A>::template read_element<0,1>(a);
            mat_traits<R>::template write_element<1,0>(r) = mat_traits<A>::template read_element<1,0>(a);
            mat_traits<R>::template write_element<1,1>(r) = mat_traits<A>::template read_element<1,1>(a);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::convert_to;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct convert_to_m_defined;

            template <>
            struct
            convert_to_m_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class R,class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<R>::rows==2 && mat_traits<A>::rows==2 &&
            mat_traits<R>::cols==1 && mat_traits<A>::cols==1,
            R>::type
        convert_to( A const & a )
            {
            R r;
            mat_traits<R>::template write_element<0,0>(r) = mat_traits<A>::template read_element<0,0>(a);
            mat_traits<R>::template write_element<1,0>(r) = mat_traits<A>::template read_element<1,0>(a);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::convert_to;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct convert_to_m_defined;

            template <>
            struct
            convert_to_m_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class R,class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<R>::rows==1 && mat_traits<A>::rows==1 &&
            mat_traits<R>::cols==2 && mat_traits<A>::cols==2,
            R>::type
        convert_to( A const & a )
            {
            R r;
            mat_traits<R>::template write_element<0,0>(r) = mat_traits<A>::template read_element<0,0>(a);
            mat_traits<R>::template write_element<0,1>(r) = mat_traits<A>::template read_element<0,1>(a);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::convert_to;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct convert_to_m_defined;

            template <>
            struct
            convert_to_m_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            bool>::type
        operator==( A const & a, B const & b )
            {
            return
                mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b) &&
                mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b) &&
                mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b) &&
                mat_traits<A>::template read_element<1,1>(a)==mat_traits<B>::template read_element<1,1>(b);
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator==;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct eq_mm_defined;

            template <>
            struct
            eq_mm_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
            bool>::type
        operator==( A const & a, B const & b )
            {
            return
                mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b) &&
                mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b);
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator==;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct eq_mm_defined;

            template <>
            struct
            eq_mm_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            bool>::type
        operator==( A const & a, B const & b )
            {
            return
                mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b) &&
                mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b);
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator==;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct eq_mm_defined;

            template <>
            struct
            eq_mm_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            bool>::type
        operator!=( A const & a, B const & b )
            {
            return
                !(mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b)) ||
                !(mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b)) ||
                !(mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b)) ||
                !(mat_traits<A>::template read_element<1,1>(a)==mat_traits<B>::template read_element<1,1>(b));
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator!=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct neq_mm_defined;

            template <>
            struct
            neq_mm_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==1 && mat_traits<B>::cols==1,
            bool>::type
        operator!=( A const & a, B const & b )
            {
            return
                !(mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b)) ||
                !(mat_traits<A>::template read_element<1,0>(a)==mat_traits<B>::template read_element<1,0>(b));
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator!=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct neq_mm_defined;

            template <>
            struct
            neq_mm_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==1 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            bool>::type
        operator!=( A const & a, B const & b )
            {
            return
                !(mat_traits<A>::template read_element<0,0>(a)==mat_traits<B>::template read_element<0,0>(b)) ||
                !(mat_traits<A>::template read_element<0,1>(a)==mat_traits<B>::template read_element<0,1>(b));
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator!=;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct neq_mm_defined;

            template <>
            struct
            neq_mm_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2,
            deduce_mat<A> >::type
        operator-( A const & a )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=-mat_traits<A>::template read_element<0,0>(a);
            mat_traits<R>::template write_element<0,1>(r)=-mat_traits<A>::template read_element<0,1>(a);
            mat_traits<R>::template write_element<1,0>(r)=-mat_traits<A>::template read_element<1,0>(a);
            mat_traits<R>::template write_element<1,1>(r)=-mat_traits<A>::template read_element<1,1>(a);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_m_defined;

            template <>
            struct
            minus_m_defined<2,2>
                {
                static bool const value=true;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==1,
            deduce_mat<A> >::type
        operator-( A const & a )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=-mat_traits<A>::template read_element<0,0>(a);
            mat_traits<R>::template write_element<1,0>(r)=-mat_traits<A>::template read_element<1,0>(a);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_m_defined;

            template <>
            struct
            minus_m_defined<2,1>
                {
                static bool const value=true;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<A>::cols==2,
            deduce_mat<A> >::type
        operator-( A const & a )
            {
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)=-mat_traits<A>::template read_element<0,0>(a);
            mat_traits<R>::template write_element<0,1>(r)=-mat_traits<A>::template read_element<0,1>(a);
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator-;
            }

        namespace
        qvm_detail
            {
            template <int R,int C>
            struct minus_m_defined;

            template <>
            struct
            minus_m_defined<1,2>
                {
                static bool const value=true;
                };
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2,
            typename mat_traits<A>::scalar_type>::type
        determinant( A const & a )
            {
            typedef typename mat_traits<A>::scalar_type T;
            T const a00=mat_traits<A>::template read_element<0,0>(a);
            T const a01=mat_traits<A>::template read_element<0,1>(a);
            T const a10=mat_traits<A>::template read_element<1,0>(a);
            T const a11=mat_traits<A>::template read_element<1,1>(a);
            T det=(a00*a11-a01*a10);
            return det;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::determinant;
            }

        namespace
        qvm_detail
            {
            template <int D>
            struct determinant_defined;

            template <>
            struct
            determinant_defined<2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2 && is_scalar<B>::value,
            deduce_mat<A> >::type
        inverse( A const & a, B det )
            {
            typedef typename mat_traits<A>::scalar_type T;
            BOOST_QVM_ASSERT(det!=scalar_traits<B>::value(0));
            T const a00=mat_traits<A>::template read_element<0,0>(a);
            T const a01=mat_traits<A>::template read_element<0,1>(a);
            T const a10=mat_traits<A>::template read_element<1,0>(a);
            T const a11=mat_traits<A>::template read_element<1,1>(a);
            T const f=scalar_traits<T>::value(1)/det;
            typedef typename deduce_mat<A>::type R;
            R r;
            mat_traits<R>::template write_element<0,0>(r)= f*a11;
            mat_traits<R>::template write_element<0,1>(r)=-f*a01;
            mat_traits<R>::template write_element<1,0>(r)=-f*a10;
            mat_traits<R>::template write_element<1,1>(r)= f*a00;
            return r;
            }

        template <class A>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<A>::cols==2,
            deduce_mat<A> >::type
        inverse( A const & a )
            {
            typedef typename mat_traits<A>::scalar_type T;
            T det=determinant(a);
            if( det==scalar_traits<T>::value(0) )
                BOOST_QVM_THROW_EXCEPTION(zero_determinant_error());
            return inverse(a,det);
            }

        namespace
        sfinae
            {
            using ::boost::qvm::inverse;
            }

        namespace
        qvm_detail
            {
            template <int D>
            struct inverse_m_defined;

            template <>
            struct
            inverse_m_defined<2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            deduce_mat2<A,B,2,2> >::type
        operator*( A const & a, B const & b )
            {
            typedef typename mat_traits<A>::scalar_type Ta;
            typedef typename mat_traits<B>::scalar_type Tb;
            Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
            Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
            Ta const a10 = mat_traits<A>::template read_element<1,0>(a);
            Ta const a11 = mat_traits<A>::template read_element<1,1>(a);
            Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
            Tb const b01 = mat_traits<B>::template read_element<0,1>(b);
            Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
            Tb const b11 = mat_traits<B>::template read_element<1,1>(b);
            typedef typename deduce_mat2<A,B,2,2>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==2);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==2);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=a00*b00+a01*b10;
            mat_traits<R>::template write_element<0,1>(r)=a00*b01+a01*b11;
            mat_traits<R>::template write_element<1,0>(r)=a10*b00+a11*b10;
            mat_traits<R>::template write_element<1,1>(r)=a10*b01+a11*b11;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*;
            }

        namespace
        qvm_detail
            {
            template <int R,int CR,int C>
            struct mul_mm_defined;

            template <>
            struct
            mul_mm_defined<2,2,2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            A &>::type
        operator*=( A & a, B const & b )
            {
            typedef typename mat_traits<A>::scalar_type Ta;
            typedef typename mat_traits<B>::scalar_type Tb;
            Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
            Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
            Ta const a10 = mat_traits<A>::template read_element<1,0>(a);
            Ta const a11 = mat_traits<A>::template read_element<1,1>(a);
            Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
            Tb const b01 = mat_traits<B>::template read_element<0,1>(b);
            Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
            Tb const b11 = mat_traits<B>::template read_element<1,1>(b);
            mat_traits<A>::template write_element<0,0>(a)=a00*b00+a01*b10;
            mat_traits<A>::template write_element<0,1>(a)=a00*b01+a01*b11;
            mat_traits<A>::template write_element<1,0>(a)=a10*b00+a11*b10;
            mat_traits<A>::template write_element<1,1>(a)=a10*b01+a11*b11;
            return a;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*=;
            }

        namespace
        qvm_detail
            {
            template <int D>
            struct mul_eq_mm_defined;

            template <>
            struct
            mul_eq_mm_defined<2>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==2 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==1,
            deduce_mat2<A,B,2,1> >::type
        operator*( A const & a, B const & b )
            {
            typedef typename mat_traits<A>::scalar_type Ta;
            typedef typename mat_traits<B>::scalar_type Tb;
            Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
            Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
            Ta const a10 = mat_traits<A>::template read_element<1,0>(a);
            Ta const a11 = mat_traits<A>::template read_element<1,1>(a);
            Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
            Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
            typedef typename deduce_mat2<A,B,2,1>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==2);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==1);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=a00*b00+a01*b10;
            mat_traits<R>::template write_element<1,0>(r)=a10*b00+a11*b10;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*;
            }

        namespace
        qvm_detail
            {
            template <int R,int CR,int C>
            struct mul_mm_defined;

            template <>
            struct
            mul_mm_defined<2,2,1>
                {
                static bool const value=true;
                };
            }

        template <class A,class B>
        BOOST_QVM_INLINE_OPERATIONS
        typename lazy_enable_if_c<
            mat_traits<A>::rows==1 && mat_traits<B>::rows==2 &&
            mat_traits<A>::cols==2 && mat_traits<B>::cols==2,
            deduce_mat2<A,B,1,2> >::type
        operator*( A const & a, B const & b )
            {
            typedef typename mat_traits<A>::scalar_type Ta;
            typedef typename mat_traits<B>::scalar_type Tb;
            Ta const a00 = mat_traits<A>::template read_element<0,0>(a);
            Ta const a01 = mat_traits<A>::template read_element<0,1>(a);
            Tb const b00 = mat_traits<B>::template read_element<0,0>(b);
            Tb const b01 = mat_traits<B>::template read_element<0,1>(b);
            Tb const b10 = mat_traits<B>::template read_element<1,0>(b);
            Tb const b11 = mat_traits<B>::template read_element<1,1>(b);
            typedef typename deduce_mat2<A,B,1,2>::type R;
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::rows==1);
            BOOST_QVM_STATIC_ASSERT(mat_traits<R>::cols==2);
            R r;
            mat_traits<R>::template write_element<0,0>(r)=a00*b00+a01*b10;
            mat_traits<R>::template write_element<0,1>(r)=a00*b01+a01*b11;
            return r;
            }

        namespace
        sfinae
            {
            using ::boost::qvm::operator*;
            }

        namespace
        qvm_detail
            {
            template <int R,int CR,int C>
            struct mul_mm_defined;

            template <>
            struct
            mul_mm_defined<1,2,2>
                {
                static bool const value=true;
                };
            }

        }
    }

#endif


//==========================================================================
//  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)
//==========================================================================

#ifndef HMM_HPP
#define HMM_HPP

#include <boost/shared_ptr.hpp>

#include <vector>
#include <cmath>
#include <assert.h>
using namespace std;

#include "HMMlib/hmm_matrix.hpp"
#include "HMMlib/hmm_vector.hpp"
#include "HMMlib/sse_operator_traits.hpp"

#include <pmmintrin.h>

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

typedef std::vector<unsigned int> sequence;

namespace hmmlib {

  /**
   * \class HMM
   *
   * \brief Encapsulates the representation of a Hidden Markov
   * Model.
   *
   * This class encapsulates the representation of a Hidden Markov
   * Model. A hidden Markov model is composed by a transition matrix,
   * an emission matrix and a vector of initial state probabilities.
   * Five classical HMM algorithms are supported: Viterbi, forward,
   * backward, Baum-Welch and posterior decoding.  The algorithms are
   * optimized using the SSE instruction set and parallelized using
   * OpenMP as described in <a
   * href="http://birc.au.dk/~asand/papers/HiBi10_hmmlib.pdf">HMMlib:
   * A C++ Library for General Hidden Markov Models Exploiting Modern
   * CPUs</a>.
   *
   * Possible values of \a float_type are: \a double or \a float.
   *
   * Possible values of sse_float_type are: \a double, \a float, \a
   * __m128d or __m128. If \a float_type is \a double, sse_float_type
   * must be either \a __m128d (default) or \a double. If \a
   * float_type is \a float, sse_float_type must be either \a __m128
   * (default) or \a float.
   *
   *
   * \author Andreas Sand (asand@birc.au.dk)
   * \see http://birc.au.dk/~asand/papers/HiBi10_hmmlib.pdf
   */
  template < typename float_type,
	     typename sse_float_type = typename FloatTraits<float_type>::sse_type>
  class HMM {

    typedef SSEOperatorTraits<float_type, sse_float_type> sse_operations_traits;

    boost::shared_ptr< HMMVector<float_type, sse_float_type> > initial_prob;
    boost::shared_ptr< HMMMatrix<float_type, sse_float_type> > trans_prob;
    boost::shared_ptr< HMMMatrix<float_type, sse_float_type> > emission_prob;

    const int no_states;
    const int alphabet_size;

  public :
    /**
     * \brief Constructs an HMM with the indicated initial state
     * probabilities, transition probabilities and emission
     * probabilities.
     *
     * Constructs an HMM with the indicated initial state
     * probabilities, transition probabilities and emission
     * probabilities.
     *
     * \param initial_prob   The vector of initial state probabilities, in which the \f$i\f$th entry must be the probability of the model initially being in state i. The entries in \a initial_prob must sum to 1.
     * \param trans_prob     The matrix of transition probabilities, in which the \f$i,j\f$th entry must be the probability of the transition from state i to state j. Each row in T must sum to 1.
     * \param emission_prob  The matrix of emission probabilities, in which the \f$i,j\f$th entry must be the probabilities of state \f$j\f$ emitting the \f$i\f$th alphabet symbol. Each column must sum to 1.
     *
     * Possible values of \a float_type are: \a double or \a float.
     *
     * Possible values of sse_float_type are: \a double, \a float, \a
     * __m128d or __m128. If \a float_type is \a double, sse_float_type
     * must be either \a __m128d (default) or \a double. If \a
     * float_type is \a float, sse_float_type must be either \a __m128
     * (default) or \a float.
     *
     * \pre The size of \a initial_prob must match the number of rows in \a trans_prob, the number of columns in trans_prob and the number columns in emission_prob.
     */
    HMM(boost::shared_ptr< HMMVector<float_type, sse_float_type> > initial_prob,
	boost::shared_ptr< HMMMatrix<float_type, sse_float_type> > trans_prob,
	boost::shared_ptr< HMMMatrix<float_type, sse_float_type> > emission_prob);

    // field accessors and mutators
    const HMMVector<float_type,sse_float_type> &get_initial_probs() { return *initial_prob; }
    const HMMMatrix<float_type,sse_float_type> &get_trans_probs() { return *trans_prob; }
    const HMMMatrix<float_type,sse_float_type> &get_emission_probs() { return *emission_prob; }
    const int get_no_states() const { return no_states; }
    const int get_alphabet_size() const { return alphabet_size; }

    // algorithms
    /**
     * \brief Runs the forward dynamic programming algorithm.
     *
     * Fills in the forward dynamic programming table, \a F.
     *
     * \param obsseq    The observed sequence of emissions.
     * \param F         The forward dynamic programming table (to be filled in).
     * \param scales    The scaling factors (to be filled in).
     *
     * After running this function, the \f$i,j\f$th entry in \a F will be:
     * \f[f_{ij} = c_i e_{s_i,j} \sum_l f_{i-1,l}t_{l, j},\f]
     * and the \f$i\f$th entry in \a scales, \f$c_i\f$, will be the factor normalizing
     * \f$ \sum_j e_{s_i,j}  \sum_l f_{i-1,l}t_{l, j}\f$.
     *
     * \pre The number of states in the model must match the number of columns in \a F.
     * \pre The number of observations in \a obsseq must match the number of rows in \a F and the size of \a scales.
     */
    void forward(const sequence &obsseq,
		 HMMVector<float_type, sse_float_type> &scales,
		 HMMMatrix<float_type, sse_float_type> &F);

    /**
     * \brief Runs the backward dynamic programming algorithm.
     *
     * Fills in the backward dynamic programming table, \a B.
     *
     * \param obsseq    The observed sequence of emissions.
     * \param B         The backward dynamic programming table (to be filled in).
     * \param scales    The scaling factors from the forward algorithm.
     *
     * After running this function, the \f$i,j\f$th entry in \a B will be:
     * \f[b_{ij} = c_{i+1}\sum_l e_{s_{i+1},l} t_{jl} b_{i+1,l}.\f]
     *
     * \pre The number of states in the model must match the number of columns in \a B.
     * \pre The number of observations in \a obsseq must match the number of rows in \a B and the size of \a scales.
     * \pre \a scales must have been filled out using \a forward().
     */
    void backward(const sequence &obsseq,
		  const HMMVector<float_type, sse_float_type> &scales,
		  HMMMatrix<float_type, sse_float_type> &B);

    /**
     * \brief Computes the loglikelihood of the model generating an observed sequence.
     *
     * Computes the loglikelihood of the model generating an observed sequence, based on the vector of scaling factors computed by the \a forward() function.
     *
     * \param scales    The vector of scaling factors computed by the \a forward() function.
     * \returns         The loglikelihood.
     *
     * \pre \a scales must have been filled out using \a forward().
     */
    float_type likelihood(const HMMVector<float_type, sse_float_type> &scales);

    /**
     * \brief Computes \f$\Theta=(pi_{counts}, T_{counts}, E_{counts})\f$ such that
     * \f$P(obsseq \vert \Theta)\f$ is maximized.
     *
     * Computes \f$\Theta=(pi_{counts}, T_{counts}, E_{counts})\f$ such that
     * \f$P(obsseq \vert \Theta)\f$ is maximized.
     *
     * \param obsseq    The observed sequence of emissions.
     * \param F         The forward dynamic programming table computed by \a forward().
     * \param B         The backward dynamic programming table table computed by \a backward().
     * \param scales    The scalling factors from the forward algorithms computed by \a forward().
     * \param pi_counts The estimated initial state probability vector (to be filled in).
     * \param T_counts  The estimated transition matrix (to be filled in).
     * \param E_counts  The estimated emission matrix (to be filled in).
     *
     * \pre \a F, \a scales and \a B must have been filled in using the \a forward() and \a backward() functions.
     * \pre The number of states in the model must match the number of columns in \a F and \a B.
     * \pre The number of observations in \a obsseq must match the number of rows in \a F and \a B and the size of \a scales.
     * \pre The number of states in the model must match the size of \a pi_counts, the number of rows in \a T_counts, the number of columns in \a T_counts and the number of columns in \a E_counts.
     * \pre The alphabet size of the model must match the number of rows in \a E_counts.
     */
    void baum_welch(const sequence &obsseq,
		    const HMMMatrix<float_type, sse_float_type> &F,
		    const HMMMatrix<float_type, sse_float_type> &B,
		    const HMMVector<float_type, sse_float_type> &scales,
		    HMMVector<float_type, sse_float_type> &pi_counts,
		    HMMMatrix<float_type, sse_float_type> &T_counts,
		    HMMMatrix<float_type, sse_float_type> &E_counts);

    /**
     * \brief Computes the maximum likelihood sequence of hidden states.
     *
     * Computes the maximum likelihood sequence of hidden states.
     *
     * \param obsseq    The observed sequence of emissions.
     * \param hiddenseq The sequence of hidden states (to be filled in).
     *
     * \returns         The loglikelihood of the probability of \a hiddenseq, given that \a obsseq is emitted.
     *
     * \a hiddenseq is filled in using the Viterbi algorithm (computing in log space).
     *
     * \pre The size of hiddenseq must match the size of obsseq.
     */
    float_type viterbi(const sequence &obsseq,
		       sequence &hiddenseq);

    /**
     * \brief Computes the posterior decoding of \a obsseq.
     *
     * Computes the posterior decoding of \a obsseq.
     *
     * \param obsseq    The observed sequence of emissions.
     * \param F         The forward dynamic programming table computed by \a forward().
     * \param B         The backward dynamic programming table computed by \a backward().
     * \param scales    The vector of scaling factors computed by \a forward().
     * \param post      The matrix of posterior decoding probabilities (to be filled in).
     *
     * After running this function the \f$i,j\f$th entry in \a post
     * will be the probability with which the ith observed symbol in
     * obsseq was emitted by the the jth state:
     * \f[P(z_i = j \vert s) = \frac{f_{ij}b_{ij}}{c_i}.\f]
     *
     * \pre \a F, \a B and \a scales must have been fill in using the \a forward() and \a backward() functions.
     * \pre the number of rows in \a post must match the size of obsseq, and the number of columns in \a post must match the number of states in the model.
     */
    void posterior_decoding(const sequence &obsseq,
			    const HMMMatrix<float_type, sse_float_type> &F,
			    const HMMMatrix<float_type, sse_float_type> &B,
			    const HMMVector<float_type, sse_float_type> &scales,
			    HMMMatrix<float_type, sse_float_type> &post);

    friend class AllocatorTraits<float_type, sse_float_type>;
    friend class OperatorTraits<float_type, sse_float_type>;
  };

  // ###############################################
  // ########## Implementation comes here ##########
  // ###############################################
  template <typename float_type, typename sse_float_type>
  HMM<float_type,sse_float_type>::HMM(boost::shared_ptr< HMMVector<float_type, sse_float_type> > initial_prob,
				       boost::shared_ptr< HMMMatrix<float_type, sse_float_type> > trans_prob,
				       boost::shared_ptr< HMMMatrix<float_type, sse_float_type> > emission_prob)
    :
    initial_prob(initial_prob),
    trans_prob(trans_prob),
    emission_prob(emission_prob),
    no_states(initial_prob->get_size()),
    alphabet_size(emission_prob->get_no_rows()) {
    assert(trans_prob->get_no_columns() == no_states);
    assert(trans_prob->get_no_rows() == no_states);
    assert(emission_prob->get_no_columns() == no_states);
    assert(emission_prob->get_no_rows() == alphabet_size);
  }

  template <typename float_type, typename sse_float_type>
  void
  HMM<float_type, sse_float_type>::forward(const sequence &obsseq,
					    HMMVector<float_type, sse_float_type> &scales,
					    HMMMatrix<float_type, sse_float_type> &F) {
    // just making it a bit easier on ourselves...
    const HMMVector<float_type, sse_float_type> &pi = *initial_prob;
    const HMMMatrix<float_type, sse_float_type> &T = *trans_prob;
    const HMMMatrix<float_type, sse_float_type> &E = *emission_prob;

    HMMMatrix<float_type, sse_float_type> &T_t = *(new HMMMatrix<float_type, sse_float_type>(T.get_no_columns(), T.get_no_rows()));
    T.transpose(T_t);

    const int length = obsseq.size();
    const int no_chunks = F.get_no_chunks_per_row();

    // sanity check
    assert(F.get_no_columns() == no_states);
    assert(F.get_no_rows() == length);
    assert(scales.get_size() == length);

    // nice to have
    sse_float_type ones;
    sse_operations_traits::set_all(ones, (float_type) 1.0);

    // initialise
    unsigned int x = obsseq[0];
    sse_float_type scale;
    sse_operations_traits::set_all(scale, (float_type) 0.0);
    int sc;
    for(sc = 0; sc < no_chunks; ++sc) {
      sse_float_type temp = pi.get_chunk(sc) * E.get_chunk(x,sc);
      F.get_chunk(0, sc) = temp;
      scale += temp;
    }

    float_type scale_float = 0;
    sse_operations_traits::sum(scale);
    scale_float = 1 / *((float_type *) &scale);
    scales(0) = scale_float;

    // scale first column of F
    sse_operations_traits::set_all(scale, scale_float);
    int chunk;
    for(chunk = 0; chunk < no_chunks; ++chunk)
      F.get_chunk(0,chunk) *= scale;

    for(int i = 1; i < length; ++i) { // filling in i'th row of F: F(i, -)
      x = obsseq[i];
      #ifdef WITH_OMP
      #pragma omp parallel for
      #endif
      for(int j = 0; j < no_states; ++j) { // filling in F(i,j)
	sse_float_type prob_sum;
	sse_operations_traits::set_all(prob_sum, (float_type) 0.0);
	for(int c = 0; c < no_chunks; ++c)
	  prob_sum += F.get_chunk(i-1, c) * T_t.get_chunk(j, c);
	sse_operations_traits::sum(prob_sum);
	sse_operations_traits::store(F(i,j), prob_sum);
      }

      sse_operations_traits::set_all(scale, (float_type) 0.0);
      for(int c = 0; c < no_chunks; ++c) {
	F.get_chunk(i, c) *= E.get_chunk(x,c);
	scale += F.get_chunk(i, c);
      }
      sse_operations_traits::sum(scale);
      sse_operations_traits::store(scales(i), scale);

      // normalize i'th column
      scales(i) = 1.0 / scales(i);
      sse_operations_traits::set_all(scale, scales(i));
      for(int chunk = 0; chunk < no_chunks; ++chunk)
	F.get_chunk(i,chunk) *= scale;
    }
  }

  template <typename float_type, typename sse_float_type>
  void
  HMM<float_type, sse_float_type>::backward(const sequence &obsseq,
					     const HMMVector<float_type, sse_float_type> &scales,
					     HMMMatrix<float_type, sse_float_type> &B) {
    // just making it a little easier on ourselves...
    const HMMMatrix<float_type, sse_float_type> &T = *trans_prob;
    const HMMMatrix<float_type, sse_float_type> &E = *emission_prob;

    // boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_t_ptr(new HMMMatrix<float_type, sse_float_type>(T.get_no_columns(), T.get_no_rows()));
    // HMMMatrix<float_type, sse_float_type> &T_t = *T_t_ptr;
    // T.transpose(T_t);

    const int length = obsseq.size();
    const int no_chunks = B.get_no_chunks_per_row();

    // sanity check
    assert(B.get_no_columns() == no_states);
    assert(B.get_no_rows() == length);
    assert(scales.get_size() == length);

    // Fill in the last column of B
    sse_float_type scale;
    sse_operations_traits::set_all(scale, scales(length - 1));
    int sc;
    for(sc = 0; sc < no_chunks; ++sc)
      B.get_chunk(length-1,sc) = scale;

    // Recursion
    for(int i = length - 1; i > 0; --i) { // fill in the (i-1)'th row of B
      int x = obsseq[i];
      #ifdef WITH_OMP
      #pragma omp parallel for
      #endif
      for(int s = 0; s < no_states; ++s) { // fill in B(i-1,s)
	sse_float_type prob_sum;
	sse_operations_traits::set_all(prob_sum, 0.0);

	for(int chunk = 0; chunk < no_chunks; ++chunk)
	  prob_sum += T.get_chunk(s, chunk) * E.get_chunk(x, chunk) * B.get_chunk(i, chunk);

	sse_operations_traits::sum(prob_sum);
	float_type sum;
	sse_operations_traits::store(sum, prob_sum);
	B(i-1,s) = sum * scales(i-1);
      }
    }
  }

  template <typename float_type, typename sse_float_type>
  float_type
  HMM<float_type, sse_float_type>::likelihood(const HMMVector<float_type, sse_float_type> &scales) {
    float_type likelihood = 0.0;
    for (int s = 0; s < scales.get_size(); ++s) {
      likelihood -= std::log(scales(s));
    }
    return likelihood;
  }

  template <typename float_type, typename sse_float_type>
  void
  HMM<float_type, sse_float_type>::baum_welch(const sequence &obsseq,
					       const HMMMatrix<float_type, sse_float_type> &F,
					       const HMMMatrix<float_type, sse_float_type> &B,
					       const HMMVector<float_type, sse_float_type> &scales,
					       HMMVector<float_type, sse_float_type> &new_pi,
					       HMMMatrix<float_type, sse_float_type> &new_T,
					       HMMMatrix<float_type, sse_float_type> &new_E) {

    // easier reference
    HMMMatrix<float_type, sse_float_type> &T = *trans_prob;
    HMMMatrix<float_type, sse_float_type> &E = *emission_prob;

    const int length = obsseq.size();
    const int no_chunks = B.get_no_chunks_per_row();

    // sanity check
    assert(new_T.get_no_columns() == no_states);
    assert(new_T.get_no_rows() ==  no_states);
    assert(new_E.get_no_columns() == no_states);
    assert(new_E.get_no_rows() == alphabet_size);
    assert(new_pi.get_size() == no_states);
    assert(scales.get_size() == length);
    assert(F.get_no_rows() == length);
    assert(F.get_no_columns() == no_states);
    assert(B.get_no_rows() == length);
    assert(B.get_no_columns() == no_states);

    // initialise
    new_pi.reset();
    new_T.reset();
    new_E.reset();

    // compute counts
    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);

    // compute new_pi and initialize E_counts
    unsigned x = obsseq[0];
    sse_float_type scale;
    sse_operations_traits::set_all(scale, (float_type) (1.0/scales(0)));
    for (int sc = 0; sc < no_chunks; ++sc) {
      sse_float_type temp = (F.get_chunk(0,sc) * B.get_chunk(0,sc)) * scale;
      new_pi.get_chunk(sc) = temp;
      E_counts.get_chunk(x, sc) += temp;
    }

    // compute transition and emission counts
    for (int i = 1; i < length; ++i) {
      x = obsseq[i];
      #ifdef WITH_OMP
      #pragma omp parallel for
      #endif
      for(int j = 0; j < no_states; ++j) {
	// transition counts
	sse_float_type prev_forward;
	sse_operations_traits::set_all(prev_forward, F(i-1, j));
	for(int chunk = 0; chunk < no_chunks; ++chunk) {
	  T_counts.get_chunk(j, chunk) += prev_forward *
	    T.get_chunk(j,chunk) *
	    E.get_chunk(x,chunk) *
	    B.get_chunk(i,chunk);
	}
      }
      // emission counts
      sse_operations_traits::set_all(scale, (float_type) (1.0/scales(i)));
      for(int chunk = 0; chunk < no_chunks; ++chunk)
	E_counts.get_chunk(x,chunk) += F.get_chunk(i, chunk) *
	  B.get_chunk(i, chunk) * scale;
    }

    // compute new_T and new_E by normalizing counts
    for (int s = 0; s < no_states; ++s) {
      // transition probabilities
      float_type sum = 0.0;
      for (int dst_state = 0; dst_state < no_states; ++dst_state)
        sum += T_counts(s, dst_state);
      for (int dst_state = 0; dst_state < no_states; ++dst_state)
        new_T(s, dst_state) = T_counts(s, dst_state) / sum;

      // emission probabilities
      sum = 0.0;
      for (int sym = 0; sym < alphabet_size; ++sym)
        sum += E_counts(sym, s);
      for (int sym = 0; sym < alphabet_size; ++sym)
        new_E(sym, s) = E_counts(sym, s) / sum;
    }
  }

  template <typename float_type, typename sse_float_type>
  float_type
  HMM<float_type, sse_float_type>::viterbi(const sequence &obsseq, sequence &hiddenseq) {
    // just making it a bit easier on ourselves...
    const HMMVector<float_type, sse_float_type> &pi = *initial_prob;
    const HMMMatrix<float_type, sse_float_type> &T = *trans_prob;
    const HMMMatrix<float_type, sse_float_type> &E = *emission_prob;

    // sanity check
    assert(obsseq.size() == hiddenseq.size());

    // Set up internal data structures
    const int length = obsseq.size();

    HMMMatrix<float_type, sse_float_type> path_probs(length, no_states);

    const int no_chunks = path_probs.get_no_chunks_per_row();

    boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_t_ptr(new HMMMatrix<float_type, sse_float_type>(T.get_no_columns(), T.get_no_rows()));
    boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > T_t_log_ptr(new HMMMatrix<float_type, sse_float_type>(T.get_no_rows(), T.get_no_columns()));
    HMMMatrix<float_type, sse_float_type> &T_t_log = *T_t_log_ptr;

    boost::shared_ptr<HMMMatrix<float_type, sse_float_type> > E_log_ptr(new HMMMatrix<float_type, sse_float_type>(E.get_no_rows(), E.get_no_columns()));
    HMMMatrix<float_type, sse_float_type> &E_log = *E_log_ptr;

    // construct T_t_log and E_log
    T.transpose(*T_t_ptr);
    (*T_t_ptr).log(T_t_log);
    E.log(E_log);

    // Fill first column of path_probs
    int x = obsseq[0];
    for (int i = 0; i < no_states; ++i) {
      path_probs(0, i) = std::log(pi(i)) + E_log(x, i);
    }

    // Recursion
    for(int i = 1; i < length; ++i) { // fill ith row of path_probs
      x = obsseq[i];
      #ifdef WITH_OMP
      #pragma omp parallel for
      #endif
      for(int s = 0; s < no_states; ++s) { // fill path_probs(i,s)
	sse_float_type max, tmp;
	sse_operations_traits::set_all(max, -INFINITY);
	for(int c = 0; c < no_chunks - 1; ++c) { // no_chunks-1 since we don't want to take the max of some garbage.
	  tmp = path_probs.get_chunk(i-1, c) + T_t_log.get_chunk(s, c);
	  sse_operations_traits::max(max, tmp);
	}
	float_type float_max = sse_operations_traits::hmax(max);
	int floats_per_chunk = sizeof(sse_float_type) / sizeof(float_type);
	for(int c = (no_chunks - 1) * floats_per_chunk; c < path_probs.get_no_columns(); ++c)
	  float_max = std::max(float_max, path_probs(i-1,c) + T_t_log(s,c));
	path_probs(i, s) = float_max;
      }

      for(int c = 0; c < no_chunks; ++c) {
	path_probs.get_chunk(i,c) += E_log.get_chunk(x,c);
      }
    }

    // Backtracking - final row
    float_type loglikelihood = -INFINITY;
    int hidden_state = 0;
    for(int i = 0; i < no_states; ++i) {
      if (path_probs(length - 1, i) > loglikelihood) {
	loglikelihood = path_probs(length - 1, i);
	hidden_state = i;
      }
    }
    hiddenseq[length - 1] = hidden_state;

    // Backtracking - recursion
    for (unsigned i = length - 1; i > 0; --i) {
      float_type max = -INFINITY;
      float_type tmp;
      int maxidx = 0;
      for (int s = 0; s < no_states; ++s) {
	tmp = path_probs(i-1, s) + T_t_log(hidden_state, s);
	if(tmp > max) {
	  max = tmp;
	  maxidx = s;
	}
      }
      hidden_state = maxidx;
      hiddenseq[i-1] = hidden_state;
    }

    return loglikelihood;
  }

  template <typename float_type, typename sse_float_type>
  void
  HMM<float_type, sse_float_type>::posterior_decoding(const sequence &obsseq,
						       const HMMMatrix<float_type, sse_float_type> &F,
						       const HMMMatrix<float_type, sse_float_type> &B,
						       const HMMVector<float_type, sse_float_type> &scales,
						       HMMMatrix<float_type, sse_float_type> &post) {
    const int length = obsseq.size();
    const int no_chunks = F.get_no_chunks_per_row();
    #ifdef WITH_OMP
    #pragma omp parallel for
    #endif
    for (int i = 0; i < length; ++i) {
      sse_float_type scale;
      sse_operations_traits::set_all(scale, (float_type) (1.0/scales(i)));
      for (int chunk = 0; chunk < no_chunks; ++chunk) {
	post.get_chunk(i, chunk) = F.get_chunk(i, chunk) * B.get_chunk(i, chunk) * scale;
      }
    }
  }

} // end of namespace

#endif


#ifndef _REGRESSION_TREE_HPP_
#define _REGRESSION_TREE_HPP_

#include <Eigen\Dense>

#include "random_discriminant.hpp"

struct RegressionTreeNode {
  DiscriminantModel model;
  RegressionTreeNode* branch_0;
  RegressionTreeNode* branch_1;
};

struct RegressionTree {
  RegressionTreeNode* root;
};

RegressionTreeNode*
CreateRegressionTreeNode(
  const unsigned dimensionality,
  const Eigen::MatrixXd* train_data,
  const Eigen::VectorXi labels,
  double sufficient_accuracy) {
/**
 *
 */
  RegressionTreeNode* new_node =
    (RegressionTreeNode*) malloc(sizeof(RegressionTreeNode));

  InitDiscriminantModel(&(new_node->model, dimensionality),
}

void
BuildRegressionTree(
  RegressionTree* self,
  unsigned dimensionality,
  const Eigen::MatrixXd* train_data,
  const Eigen::VectorXi labels,
  double sufficient_accuracy) {
/**
 *
 */
  self->root = CreateRegressionTreeNode(train_data, labels);
}

RegressionTreeNode*
CreateRegressionTreeNode(
  const Eigen::MatrixXd* train_data,
  const Eigen::VectorXi* labels) {
/**
 *
 */

}

#endif //#define _REGRESSION_TREE_HPP_