#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE SIMPLEST_TEST_SUITE
#include <boost/test/unit_test.hpp>
#include "typedefs.h"
#include <cmath>

#ifdef BOOST_VARSET

uint size = 128;

void checkVarsetEmpty(varset a, ulong size)
{
    BOOST_CHECK(size == a.size());
    BOOST_CHECK(true == a.none());
}
void checkVarsetNotEmptyFrom(varset a, uint size, uint from, uint count, ulong value)
{
    BOOST_CHECK(size == a.size());
    BOOST_CHECK(false == a.none());
    BOOST_CHECK(true == a.any());
    BOOST_CHECK(from == a.find_first());
    BOOST_CHECK(count == a.count());
    BOOST_CHECK(value == a.to_ulong());
}
BOOST_AUTO_TEST_CASE(VARSET_CONSTRUCTION)
{
    varset a = VARSET(size);
    checkVarsetEmpty(a,size);
}
BOOST_AUTO_TEST_CASE(VARSET_NEW_CONSTRUCTION)
{
    VARSET_NEW(a,size);
    checkVarsetEmpty(a,size);
}
BOOST_AUTO_TEST_CASE(VARSET_NEW_INIT)
{
    ulong value = 15;
    VARSET_NEW_INIT(a,size,value);

    checkVarsetNotEmptyFrom(a,size,0,4,value);
}
BOOST_AUTO_TEST_CASE(VARSET_SET)
{
    uint index = 10;
    VARSET_NEW(a,size);
    VARSET_SET(a,10);

    BOOST_CHECK(index == a.find_first());
    BOOST_CHECK(true == a.any());
    BOOST_CHECK(1 == a.count());
}
BOOST_AUTO_TEST_CASE(VARSET_SET_VALUE)
{
    ulong value = 15;
    VARSET_NEW(a,10);
    VARSET_NEW_INIT(b,size,value);
    VARSET_SET_VALUE(a,b);

    checkVarsetNotEmptyFrom(a,size,0,4,value);
}
BOOST_AUTO_TEST_CASE(VARSET_SET_ALL)
{
    uint index = 10;
    VARSET_NEW(a,size);
    VARSET_SET_ALL(a,index);

    checkVarsetNotEmptyFrom(a,size,0,index,1023);
}
BOOST_AUTO_TEST_CASE(VARSET_CLEAR)
{
    VARSET_NEW_INIT(a,size,15);
    VARSET_CLEAR(a,0);

    checkVarsetNotEmptyFrom(a,size,1,3,14);
}
BOOST_AUTO_TEST_CASE(VARSET_CLEAR_INTERSECTION)
{
    VARSET_NEW_INIT(a,size,15);
    VARSET_NEW_INIT(b,size,7);

    VARSET_CLEAR_INTERSECTION(a,b);
    checkVarsetNotEmptyFrom(a,size,3,1,8);
}
BOOST_AUTO_TEST_CASE(VARSET_CLEAR_ALL)
{
    VARSET_NEW_INIT(a,size,15);

    VARSET_CLEAR_ALL(a);
    checkVarsetEmpty(a,size);
}
BOOST_AUTO_TEST_CASE(VARSET_GET)
{
    VARSET_NEW_INIT(a,size,15);

    BOOST_CHECK(1 == VARSET_GET(a,0));
    BOOST_CHECK(1 == VARSET_GET(a,1));
    BOOST_CHECK(1 == VARSET_GET(a,2));
    BOOST_CHECK(1 == VARSET_GET(a,3));
    BOOST_CHECK(0 == VARSET_GET(a,4));
    BOOST_CHECK(0 == VARSET_GET(a,5));
}
BOOST_AUTO_TEST_CASE(VARSET_LESS_THAN)
{
    VARSET_NEW_INIT(a,size,15);
    VARSET_NEW_INIT(b,size,31);

    BOOST_CHECK(true == VARSET_LESS_THAN(a,b));
    BOOST_CHECK(false == VARSET_LESS_THAN(b,a));
    BOOST_CHECK(false == VARSET_LESS_THAN(a,a));
}
BOOST_AUTO_TEST_CASE(VARSET_IS_SUBSET_OF)
{
    VARSET_NEW_INIT(a,size,15);
    VARSET_NEW_INIT(b,size,31);

    BOOST_CHECK(true == VARSET_IS_SUBSET_OF(a,b));
    BOOST_CHECK(false == VARSET_IS_SUBSET_OF(b,a));
    BOOST_CHECK(true == VARSET_IS_SUBSET_OF(a,a)); //Note that set is defined to be subset of itself.
}
BOOST_AUTO_TEST_CASE(VARSET_FIND_FIRST_SET)
{
    VARSET_NEW_INIT(a,size,16);

    BOOST_CHECK(4 == VARSET_FIND_FIRST_SET(a));
}
BOOST_AUTO_TEST_CASE(VARSET_FIND_NEXT_SET)
{
    uint indexFirst = 4, indexSecond = 20;
    VARSET_NEW_INIT(a,size,16);
    VARSET_SET(a,indexSecond);

    BOOST_CHECK(indexFirst == VARSET_FIND_FIRST_SET(a));
    BOOST_CHECK(indexSecond == VARSET_FIND_NEXT_SET(a,indexFirst));
}
BOOST_AUTO_TEST_CASE(VARSET_NOT)
{
    VARSET_NEW_INIT(a,16,15);

    a = VARSET_NOT(a);
    BOOST_CHECK(4 == a.find_first());
    BOOST_CHECK(12 == a.count());
}
BOOST_AUTO_TEST_CASE(VARSET_AND)
{
    uint size = 16;
    ulong value = 15;
    VARSET_NEW_INIT(a,size,value);
    varset b = VARSET_NOT(a);

    varset c = VARSET_AND(a,b);
    checkVarsetEmpty(c,size);

    varset d = VARSET_AND(a,a);
    checkVarsetNotEmptyFrom(d,size,0,4,value);
}
BOOST_AUTO_TEST_CASE(VARSET_OR)
{
    uint size = 8;
    ulong value = 15;
    VARSET_NEW_INIT(a,size,value);
    varset b = VARSET_NOT(a);

    varset c = VARSET_OR(a,b);
    checkVarsetNotEmptyFrom(c,size,0,size,255);
}
BOOST_AUTO_TEST_CASE(VARSET_COPY)
{
    ulong value = 15;
    VARSET_NEW_INIT(a,size,value);

    VARSET_COPY(a,b);

    checkVarsetNotEmptyFrom(b,size,0,4,value);
}
BOOST_AUTO_TEST_CASE(VARSET_EQUAL)
{
    ulong value = 15;
    VARSET_NEW_INIT(a,size,value);
    VARSET_NEW_INIT(b,size,value);
    VARSET_NEW_INIT(c,size,value+1);

    BOOST_CHECK(true == VARSET_EQUAL(a,b));
    BOOST_CHECK(false == VARSET_EQUAL(a,c));
}

#endif


///////////////////////////////////////////////////////////////////////////////
// statistics::model::wrap::unary::responses.hpp                             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_MODEL_WRAP_UNARY_RESPONSES_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_WRAP_UNARY_RESPONSES_HPP_ER_2009
#include <boost/statistics/model/wrap/unary/detail/base.hpp>

namespace boost{
namespace statistics{
namespace model{

    template<typename Ry>
    class responses_wrapper : detail::base_<Ry>{
        typedef detail::base_<Ry> base_;
    public:
        typedef Ry                      responses_type;
        typedef responses_wrapper<Ry>   responses_wrapper_type;

        // Construction
        responses_wrapper();
        responses_wrapper(const Ry& x);
        responses_wrapper(const responses_wrapper&);

        // Access
        const Ry& responses()const;
    };

    // Implementation //

    template<typename Ry>
    responses_wrapper<Ry>::responses_wrapper() : base_(){}

    template<typename Ry>
    responses_wrapper<Ry>::responses_wrapper( const Ry& ry ) : base_(ry){}

    template<typename Ry>
    responses_wrapper<Ry>::responses_wrapper(const responses_wrapper& that)
    :base_(that){}

    template<typename Ry>
    const Ry& responses_wrapper<Ry>::responses()const{
        return (this->value);
    }

    // Free functions

    template<typename Ry>
    responses_wrapper<Ry>
    make_responses_wrapper(const Ry& ry){
        typedef responses_wrapper<Ry> result_type;
        return result_type(ry);
    }

}// model
}// statistics
}// boost

#endif


//============================================================================
// Name        : infodlib.cpp
// Author      : Boubacar Diarra
// Version     :
// Copyright   : boubadiarra@gmail.com
// Description : Hello World in C++, Ansi-style
//============================================================================
#include <dlib/gui_widgets.h>
#include <sstream>
#include <string>
/////////////////////////
#include <infodlib/dlib_matrice.h>
//////////////////////////////
#include <infotestdata.h>
//////////////////////////////
#include <global_defs.h>
/////////////////////////////
using namespace std;
using namespace dlib;
using namespace info;
//////////////////////////////
using DrawItemsType = DrawItems<STRINGTYPE, FLOATTYPE>;
using ints_vector = std::vector<IDTYPE>;
using doubles_vector = std::vector<double>;
using strings_vector = std::vector<STRINGTYPE>;
using ints_doubles_map = std::map<IDTYPE, double>;
////////////////////////////////////////
using IntraMatElemType = IntraMatElem<IDTYPE, DISTANCETYPE, STRINGTYPE>;
using SourceType = IIndivSource<IDTYPE, STRINGTYPE>;
using RescritType = std::atomic<DISTANCETYPE>;
using IntraMatElemResultType = IntraMatElemResult<IDTYPE, DISTANCETYPE, STRINGTYPE>;
using IntraMatElemResultPtr = std::shared_ptr<IntraMatElemResultType>;
using queue_type = SharedQueue<IntraMatElemResultPtr>;
////////////////////////
using MatOrdType = IntraMatOrd<IDTYPE, DISTANCETYPE, STRINGTYPE>;
using backgrounder = MatElemResultBackgounder<IDTYPE, DISTANCETYPE, STRINGTYPE>;
////////////////////////////////
using MatriceDataType = MatriceData<IDTYPE, STRINGTYPE, DATATYPE>;
using MatriceWindowType = MatriceDisplayWindow<IDTYPE, DISTANCETYPE, STRINGTYPE, FLOATTYPE, DATATYPE>;
//////////////////////////////////////////
class matrice_win : public drawable_window
{
	MatriceWindowType mat_win;
	label lblVar;
	label lblInd;
	label lblStatus;
	MatriceDataType m_data;
protected:
	void process_result(IntraMatElemResultPtr oRes) {
		STRINGSTREAM os;
		label *pLabel = nullptr;
		IntraMatElemResultType *p = oRes.get();
		if (p != nullptr) {
			StageType stage = p->stage;
			if (stage == StageType::started) {
				os << "STARTED...\t";
			}
			else if (stage == StageType::finished) {
				os << "FINISHED!!!\t";
			}
			STRINGTYPE sx = p->sigle;
			if (!sx.empty()) {
				os << sx << "\t";
			}
			if (p->disposition == DispositionType::indiv) {
				os << "INDS\t";
				pLabel = &(lblInd);
			}
			else if (p->disposition == DispositionType::variable) {
				os << "VARS\t";
				pLabel = &(lblVar);
			}
			DISTANCETYPE crit = p->first;
			os << "Crit: " << crit << std::endl;
		}// p
		STRINGTYPE ss = os.str();
		if (pLabel != nullptr) {
			pLabel->set_text(ss);
		}
	}// process_result
	void my_redim(void) {
		unsigned long h = 0, w = 0;
		this->get_size(w, h);
		long x2 = w / 2;
		x2 += 5;
		lblVar.set_pos(5, 5);
		lblInd.set_pos(x2, 5);
		unsigned long hh = (5 + lblVar.height() + 5);
		unsigned long hy =  h - hh;
		if (hy > 0) {
			this->mat_win.set_pos(0, hh);
			this->mat_win.set_size(w, hy);
			this->mat_win.window_resized();
		}

	}// my_redim
	virtual void on_window_resized() {
		this->my_redim();
	}
public:
	matrice_win() :mat_win(*this),lblVar(*this),lblInd(*this),lblStatus(*this)
	{
		lblVar.set_pos(5, 5);
		size_t nRows = 0, nCols = 0;
		STRINGTYPE name;
		std::vector<STRINGTYPE> rowNames, colNames;
		std::vector<DATATYPE> data;
		InfoTestData::get_test_data(name, nRows, nCols, data, rowNames, colNames);
		this->m_data.initialize(name, nRows, nCols, data, rowNames, colNames);
		this->mat_win.set_callback([this](IntraMatElemResultPtr oRes) {
			this->process_result(oRes);
		});
		this->mat_win.arrange(&this->m_data);
		this->mat_win.show();
		show();
	}
	~matrice_win() {
	}

};
//////////////////////////
int main()
{
	// create our window
	matrice_win my_window;

	// wait until the user closes this window before we let the program
	// terminate.
	my_window.wait_until_closed();

	return 0;
}
////////////////////////////


#include <boost/python.hpp>

#include "../../../def_submodule.hpp"

#include "IceRay/geometry/simple/sphere.hpp"

#include "./simple.hpp"

typedef GS_DDMRM::S_IceRay::S_geometry::GC_sphere         GTs_sphere;

void expose_IceRay_geometry_sphere()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( core );
  MAKE_SUBMODULE( geometry );

  typedef  GTs_coord3D const& (GTs_sphere::*Tf_getCenter  )(void) const;
  Tf_getCenter I_getCenter = &GTs_sphere::F_center;

  typedef  bool (GTs_sphere::*Tf_setCenter  )(GTs_coord3D const&);
  Tf_setCenter I_setCenter = &GTs_sphere::F_center;

  typedef  GTs_scalar const& (GTs_sphere::*Tf_getRadius)(void) const;
  Tf_getRadius I_getRadius = &GTs_sphere::F_radius;

  typedef  bool (GTs_sphere::*Tf_setRadius  )(GTs_scalar const&);
  Tf_setRadius I_setRadius = &GTs_sphere::F_radius;

  boost::python::class_< GTs_sphere, boost::python::bases<GTs__base> >( "GeometrySimpleSphere" )
    .def( boost::python::init<>() )
    .def( boost::python::init<GTs_coord3D>() )
    .def( boost::python::init<GTs_coord3D, GTs_scalar>() )
    .add_property("center", boost::python::make_function( I_getCenter, boost::python::return_value_policy<boost::python::copy_const_reference>() ), I_setCenter )
    .add_property("radius", boost::python::make_function( I_getRadius, boost::python::return_value_policy<boost::python::copy_const_reference>() ), I_setRadius )
    //.def("center",  I_getCenter, boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    //.def("center",  I_setCenter )
    //.def("radius",  I_getRadius, boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    //.def("radius",  I_setRadius      )
  ;

 }


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef CIRCLE_2_COORDINATES_HPP
#define CIRCLE_2_COORDINATES_HPP

#include "coordinates.hpp"
#include "rjmcmc/geometry/Circle_2.hpp"
#include <boost/iterator/iterator_facade.hpp>

template<typename K>
struct circle_coordinates_iterator
        : public boost::iterator_facade<circle_coordinates_iterator<K>, const typename K::FT, boost::forward_traversal_tag>
{
    enum { dimension = 3 };
    circle_coordinates_iterator() : m_i(dimension) {}
    template<typename Engine>
    explicit circle_coordinates_iterator(const geometry::Circle_2<K>& c, Engine& e) : m_i(0) { init(c); }
    explicit circle_coordinates_iterator(const geometry::Circle_2<K>& c           ) : m_i(0) { init(c); }

    typedef typename K::FT FT;

private:
    void init(const geometry::Circle_2<K>& c)
    {
        m_coord[0] = c.center().x();
        m_coord[1] = c.center().y();
        m_coord[2] = c.radius();
    }
    friend class boost::iterator_core_access;
    void increment() { ++m_i; }
    void advance(int i) { m_i+=i; }

    const FT& dereference() const {
        //assert(m_i<dimension);
        return m_coord[m_i];
    }

    FT m_coord[dimension];
    unsigned int m_i;
};

template<typename K>
struct coordinates_iterator< geometry::Circle_2<K> >
{
    typedef circle_coordinates_iterator<K> type;
    enum { dimension = type::dimension };
};

template<typename K>
struct object_from_coordinates< geometry::Circle_2<K> > {
    template<typename Iterator> geometry::Circle_2<K> operator()(Iterator it) const {
        typename K::FT x = *it++;
        typename K::FT y = *it++;
        typename K::Point_2 p(x,y);
        return geometry::Circle_2<K>(p,*it);
    }
};

#endif // CIRCLE_2_COORDINATES_HPP


// Copyright (c) 2015 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "consensus/merkle.h"
#include "test/test_securetag.h"
#include "test/test_random.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(merkle_tests, TestingSetup)

// Older version of the merkle root computation code, for comparison.
static uint256 BlockBuildMerkleTree(const CBlock& block, bool* fMutated, std::vector<uint256>& vMerkleTree)
{
    vMerkleTree.clear();
    vMerkleTree.reserve(block.vtx.size() * 2 + 16); // Safe upper bound for the number of total nodes.
    for (std::vector<CTransactionRef>::const_iterator it(block.vtx.begin()); it != block.vtx.end(); ++it)
        vMerkleTree.push_back((*it)->GetHash());
    int j = 0;
    bool mutated = false;
    for (int nSize = block.vtx.size(); nSize > 1; nSize = (nSize + 1) / 2)
    {
        for (int i = 0; i < nSize; i += 2)
        {
            int i2 = std::min(i+1, nSize-1);
            if (i2 == i + 1 && i2 + 1 == nSize && vMerkleTree[j+i] == vMerkleTree[j+i2]) {
                // Two identical hashes at the end of the list at a particular level.
                mutated = true;
            }
            vMerkleTree.push_back(Hash(vMerkleTree[j+i].begin(), vMerkleTree[j+i].end(),
                                       vMerkleTree[j+i2].begin(), vMerkleTree[j+i2].end()));
        }
        j += nSize;
    }
    if (fMutated) {
        *fMutated = mutated;
    }
    return (vMerkleTree.empty() ? uint256() : vMerkleTree.back());
}

// Older version of the merkle branch computation code, for comparison.
static std::vector<uint256> BlockGetMerkleBranch(const CBlock& block, const std::vector<uint256>& vMerkleTree, int nIndex)
{
    std::vector<uint256> vMerkleBranch;
    int j = 0;
    for (int nSize = block.vtx.size(); nSize > 1; nSize = (nSize + 1) / 2)
    {
        int i = std::min(nIndex^1, nSize-1);
        vMerkleBranch.push_back(vMerkleTree[j+i]);
        nIndex >>= 1;
        j += nSize;
    }
    return vMerkleBranch;
}

static inline int ctz(uint32_t i) {
    if (i == 0) return 0;
    int j = 0;
    while (!(i & 1)) {
        j++;
        i >>= 1;
    }
    return j;
}

BOOST_AUTO_TEST_CASE(merkle_test)
{
    for (int i = 0; i < 32; i++) {
        // Try 32 block sizes: all sizes from 0 to 16 inclusive, and then 15 random sizes.
        int ntx = (i <= 16) ? i : 17 + (insecure_rand() % 4000);
        // Try up to 3 mutations.
        for (int mutate = 0; mutate <= 3; mutate++) {
            int duplicate1 = mutate >= 1 ? 1 << ctz(ntx) : 0; // The last how many transactions to duplicate first.
            if (duplicate1 >= ntx) break; // Duplication of the entire tree results in a different root (it adds a level).
            int ntx1 = ntx + duplicate1; // The resulting number of transactions after the first duplication.
            int duplicate2 = mutate >= 2 ? 1 << ctz(ntx1) : 0; // Likewise for the second mutation.
            if (duplicate2 >= ntx1) break;
            int ntx2 = ntx1 + duplicate2;
            int duplicate3 = mutate >= 3 ? 1 << ctz(ntx2) : 0; // And for the third mutation.
            if (duplicate3 >= ntx2) break;
            int ntx3 = ntx2 + duplicate3;
            // Build a block with ntx different transactions.
            CBlock block;
            block.vtx.resize(ntx);
            for (int j = 0; j < ntx; j++) {
                CMutableTransaction mtx;
                mtx.nLockTime = j;
                block.vtx[j] = MakeTransactionRef(std::move(mtx));
            }
            // Compute the root of the block before mutating it.
            bool unmutatedMutated = false;
            uint256 unmutatedRoot = BlockMerkleRoot(block, &unmutatedMutated);
            BOOST_CHECK(unmutatedMutated == false);
            // Optionally mutate by duplicating the last transactions, resulting in the same merkle root.
            block.vtx.resize(ntx3);
            for (int j = 0; j < duplicate1; j++) {
                block.vtx[ntx + j] = block.vtx[ntx + j - duplicate1];
            }
            for (int j = 0; j < duplicate2; j++) {
                block.vtx[ntx1 + j] = block.vtx[ntx1 + j - duplicate2];
            }
            for (int j = 0; j < duplicate3; j++) {
                block.vtx[ntx2 + j] = block.vtx[ntx2 + j - duplicate3];
            }
            // Compute the merkle root and merkle tree using the old mechanism.
            bool oldMutated = false;
            std::vector<uint256> merkleTree;
            uint256 oldRoot = BlockBuildMerkleTree(block, &oldMutated, merkleTree);
            // Compute the merkle root using the new mechanism.
            bool newMutated = false;
            uint256 newRoot = BlockMerkleRoot(block, &newMutated);
            BOOST_CHECK(oldRoot == newRoot);
            BOOST_CHECK(newRoot == unmutatedRoot);
            BOOST_CHECK((newRoot == uint256()) == (ntx == 0));
            BOOST_CHECK(oldMutated == newMutated);
            BOOST_CHECK(newMutated == !!mutate);
            // If no mutation was done (once for every ntx value), try up to 16 branches.
            if (mutate == 0) {
                for (int loop = 0; loop < std::min(ntx, 16); loop++) {
                    // If ntx <= 16, try all branches. Otherise, try 16 random ones.
                    int mtx = loop;
                    if (ntx > 16) {
                        mtx = insecure_rand() % ntx;
                    }
                    std::vector<uint256> newBranch = BlockMerkleBranch(block, mtx);
                    std::vector<uint256> oldBranch = BlockGetMerkleBranch(block, merkleTree, mtx);
                    BOOST_CHECK(oldBranch == newBranch);
                    BOOST_CHECK(ComputeMerkleRootFromBranch(block.vtx[mtx]->GetHash(), newBranch, mtx) == oldRoot);
                }
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file scheduling.hpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2013-11-19
 */
#ifndef PAAL_SCHEDULING_HPP
#define PAAL_SCHEDULING_HPP

#include "test_utils/logger.hpp"

#include "paal/utils/type_functions.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/functional/hash.hpp>

#include <unordered_map>
#include <utility>
#include <vector>

template <class Machines, class Time, class GetSpeed>
std::vector<long long> generate_job_loads(Machines machines,
                                          double minJobsOnMachine, Time time,
                                          GetSpeed getSpeed) {
    std::vector<long long> loads;
    for (auto const machine : machines) {
        for (Time left = time; left > 0;) {
            Time jobTime = rand() % ((long long)(time / minJobsOnMachine));
            paal::assign_min(jobTime, left);
            loads.push_back(jobTime * getSpeed(machine));
            left -= jobTime;
        }
    }
    return loads;
}

template <class Result, class Job>
void check_jobs(Result result, std::vector<Job> jobs) {
    std::vector<Job> gotJobs;
    for (auto const &it : result) {
        gotJobs.push_back(*it.second);
    }
    boost::sort(gotJobs);
    boost::sort(jobs);
    BOOST_CHECK(jobs == gotJobs);
}

template <class Result, class GetSpeed>
double get_max_time(const Result &result, GetSpeed getSpeed) {
    typedef typename paal::range_to_elem_t<Result>::first_type MachineIter;
    typedef typename std::iterator_traits<MachineIter>::value_type Machine;
    std::unordered_map<Machine, double, boost::hash<Machine>> machineTime;
    for (auto const &machineJobPair : result) {
        Machine machine = *machineJobPair.first;
        auto load = *machineJobPair.second;
        machineTime[machine] += double(load) / getSpeed(machine);
    }
    return *boost::max_element(machineTime | boost::adaptors::map_values);
}

#endif // PAAL_SCHEDULING_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/two_add.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/eps.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/functions/divides.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector_tie.hpp>

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

NT2_TEST_CASE_TPL( two_add, BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::two_add;
  using boost::simd::tag::two_add_;
  using boost::simd::native;

  typedef native<T,BOOST_SIMD_DEFAULT_EXTENSION> vT;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<two_add_(vT,vT)>::type)
                  , (std::pair<vT,vT>)
                  );

  vT inf_    = boost::simd::Inf<vT>();
  vT zero_   = boost::simd::Zero<vT>();
  vT one_    = boost::simd::One<vT>();
  vT half_   = boost::simd::Half<vT>();
  vT eps_    = boost::simd::Eps<vT>();
  vT eps_2_  = eps_/boost::simd::Two<vT>();

  {
    vT s,r;

    two_add(inf_,zero_, s, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    two_add(zero_, inf_, s, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    two_add(half_+ eps_2_, half_, s, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);

    two_add(half_, half_+ eps_2_, s, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);
  }

  {
    vT s,r;

    s = two_add(inf_,zero_, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    s = two_add(zero_, inf_, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    s = two_add(half_+ eps_2_, half_, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);

    s = two_add(half_, half_+ eps_2_, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);
  }

  {
    vT s,r;

    boost::fusion::vector_tie(s,r) = two_add(inf_,zero_);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    boost::fusion::vector_tie(s,r) = two_add(zero_, inf_);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    boost::fusion::vector_tie(s,r) = two_add(half_+ eps_2_, half_);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);

    boost::fusion::vector_tie(s,r) = two_add(half_, half_+ eps_2_);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, eps_2_);
  }

  {
    std::pair<vT,vT> p;

    p = two_add(inf_,zero_);
    NT2_TEST_EQUAL(p.first, inf_);
    NT2_TEST_EQUAL(p.second, zero_);

    p = two_add(zero_, inf_);
    NT2_TEST_EQUAL(p.first, inf_);
    NT2_TEST_EQUAL(p.second, zero_);

    p = two_add(half_+ eps_2_, half_);
    NT2_TEST_EQUAL(p.first, one_);
    NT2_TEST_EQUAL(p.second, eps_2_);

    p = two_add(half_, half_+ eps_2_);
    NT2_TEST_EQUAL(p.first, one_);
    NT2_TEST_EQUAL(p.second, eps_2_);
  }
}


/*
    This file is part of Mitsuba, a physically based rendering system.

    Copyright (c) 2007-2014 by Wenzel Jakob and others.

    Mitsuba is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License Version 3
    as published by the Free Software Foundation.

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

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

#include <mitsuba/render/util.h>
#include <mitsuba/core/quad.h>
#include <mitsuba/core/fstream.h>
#include <mitsuba/core/plugin.h>
#include <boost/bind.hpp>
#include <fstream>
#include <iomanip>
#include <omp.h>

#define RESOLUTION_IOR          50
#define RESOLUTION_ROUGHNESS    30
#define RESOLUTION_THETA        100

#define ROUGHNESS_START         0
#define ROUGHNESS_END           0.5

#define IOR_START               (1 + 1e-4)
#define IOR_END                 4

MTS_NAMESPACE_BEGIN

void transmittanceIntegrand(const BSDF *bsdf, const Vector &wi, size_t nPts, const Float *in, Float *out) {
    Intersection its;

    #pragma omp parallel for
    for (int i=0; i<(int) nPts; ++i) {
        BSDFSamplingRecord bRec(its, wi, Vector(), EImportance);
        bRec.typeMask = BSDF::ETransmission;
        Point2 sample(in[2*i], in[2*i+1]);
        if (sample.x == 1)
            sample.x = 1-Epsilon;
        if (sample.y == 1)
            sample.y = 1-Epsilon;
        out[i] = bsdf->sample(bRec, sample)[0];
        if (std::isnan(out[i]))
            SLog(EError, "%s\n\nNaN!", bRec.toString().c_str());
    }
}

void diffTransmittanceIntegrand(Float *data, size_t resolution, size_t nPts, const Float *in, Float *out) {
    #pragma omp parallel for
    for (int i=0; i<(int) nPts; ++i)
        out[i] = 2 * in[i] * interpCubic1D(std::pow(in[i], (Float) 0.25f), data, 0, 1, resolution);
}

class PrecomputeTransmittance : public Utility {
public:
    Float *computeTransmittance(const char *name, Float ior, Float alpha,
            size_t resolution, Float &diffTrans, int inverted) {
        Properties bsdfProps(alpha == 0 ? "dielectric" : "roughdielectric");
        if (inverted) {
            bsdfProps.setFloat("intIOR", 1.00);
            bsdfProps.setFloat("extIOR", ior);
        } else {
            bsdfProps.setFloat("extIOR", 1.00);
            bsdfProps.setFloat("intIOR", ior);
        }
        bsdfProps.setFloat("alpha", alpha);
        bsdfProps.setString("distribution", name);
        ref<BSDF> bsdf = static_cast<BSDF *>(
                PluginManager::getInstance()->createObject(bsdfProps));

        Float stepSize = 1.0f / (resolution-1);
        Float error;

        NDIntegrator intTransmittance(1, 2, 50000, 0, 1e-6f);
        NDIntegrator intDiffTransmittance(1, 1, 50000, 0, 1e-6f);
        Float *transmittances = new Float[resolution];

        for (size_t i=0; i<resolution; ++i) {
            Float t = i * stepSize;
            if (i == 0) /* Don't go all the way to zero */
                t = stepSize/10;

            Float cosTheta = std::pow(t, (Float) 4.0f);

            Vector wi(math::safe_sqrt(1-cosTheta*cosTheta), 0, cosTheta);

            Float min[2] = {0, 0}, max[2] = {1, 1};
            intTransmittance.integrateVectorized(
                boost::bind(&transmittanceIntegrand, bsdf, wi, _1, _2, _3),
                min, max, &transmittances[i], &error, NULL);
        }

        Float min[1] = { 0 }, max[1] = { 1 };
        intDiffTransmittance.integrateVectorized(
            boost::bind(&diffTransmittanceIntegrand, transmittances, resolution, _1, _2, _3),
            min, max, &diffTrans, &error, NULL);

        if (alpha == 0.0f)
            cout << diffTrans << " vs " << 1-fresnelDiffuseReflectance(inverted ? (1.0f / ior) : ior) << endl;

        return transmittances;
    }

    void fit(const char *name, int inverted) {
        size_t resolutionIOR = RESOLUTION_IOR,
               resolutionAlpha = RESOLUTION_ROUGHNESS,
               resolutionTheta = RESOLUTION_THETA;

        Float alphaStart = ROUGHNESS_START,
              alphaEnd   = ROUGHNESS_END,
              iorStart   = IOR_START,
              iorEnd     = IOR_END;

        std::ofstream os(formatString("%s%i.m", name, inverted).c_str());
        os << std::fixed << std::setprecision(8);

        os << "alphaStart=" << alphaStart << ";" << endl;
        os << "alphaEnd=" << alphaEnd << ";" << endl;
        os << "iorStart=" << iorStart << ";" << endl;
        os << "iorEnd=" << iorEnd << ";" << endl;
        os << "alphaSteps=" << resolutionAlpha << ";" << endl;
        os << "thetaSteps=" << resolutionTheta << ";" << endl;
        os << "iorSteps=" << resolutionIOR << ";" << endl;

        os << "transmittance={" << endl;
        ref<FileStream> fstream = new FileStream(formatString("data/microfacet/%s.dat", name).c_str(),
                inverted ? FileStream::EAppendReadWrite : FileStream::ETruncReadWrite);

        fstream->setByteOrder(Stream::ELittleEndian);

        if (!inverted) {
            fstream->write("MTS_TRANSMITTANCE", 17);
            fstream->writeSize(resolutionIOR);
            fstream->writeSize(resolutionAlpha);
            fstream->writeSize(resolutionTheta);
            fstream->writeSingle(iorStart);
            fstream->writeSingle(iorEnd);
            fstream->writeSingle(alphaStart);
            fstream->writeSingle(alphaEnd);
        }

        Float iorStepSize   = 1.0f / (resolutionIOR-1),
              alphaStepSize = 1.0f / (resolutionAlpha-1);

        for (size_t i=0; i<resolutionIOR; ++i) {
            Float t = i * iorStepSize;
            Float ior = iorStart + (iorEnd-iorStart) * std::pow(t, (Float) 4.0f);
            cout << "ior = " << ior << endl;
            os << "\t{" << endl;
            for (size_t j=0; j<resolutionAlpha; ++j) {
                Float t = j * alphaStepSize;
                Float alpha = alphaStart + (alphaEnd-alphaStart) * std::pow(t, (Float) 4.0f);
                cout << "alpha = " << alpha << endl;
                Float diffTrans;
                Float *transmittance = computeTransmittance(name,
                        ior, alpha, resolutionTheta, diffTrans, inverted);
                os << "\t\t{";
                for (size_t k=0; k<resolutionTheta; ++k) {
                    fstream->writeSingle((float) transmittance[k]);
                    os << transmittance[k];
                    if (k+1 < resolutionTheta)
                        os << ", ";
                }
                os << "}";
                if (j + 1 < resolutionAlpha)
                    os << ",";
                os << endl;
                delete[] transmittance;
                fstream->writeSingle((float) diffTrans);
            }
            os << "\t}";
            if (i + 1 < resolutionIOR)
                os << ",";
            os << endl;
            cout << endl;
        }
        os << "};" << endl;
        os.close();
        fstream->close();
    }

    int run(int argc, char **argv) {
        cout << endl;
        cout << "rdielprec: precompute transmittances through a rough dielectric material" << endl;
        cout << "========================================================================" << endl;
        cout << "The transmittance through a rough dielectric boundary modeled using " << endl;
        cout << "a microfacet model depends on several quantities:" << endl;
        cout << endl;
        cout << "  1. the relative index of refraction" << endl;
        cout << "  2. the angle of incidence (of incoming illumination)" << endl;
        cout << "  3. the used microfacet distribution (beckmann, phong, ggx)" << endl;
        cout << "  4. the roughness parameter of the microfacet distribution" << endl;
        cout << endl;
        cout << "Since a 2D integration is involved, the transmittance is quite expensive" << endl;
        cout << "to evaluate at runtime. This utility therefore precomputes the values on" << endl;
        cout << "a densely spaced grid and stores them on disk so that they can be " << endl;
        cout << "interpolated at runtime (e.g. using cubic splines). The output will be " << endl;
        cout << "written to 'data/microfacet'. This process will take 1-3 hours." << endl;
        cout << "It is recommended that you compile Mitsuba in double precision when running" << endl;
        cout << "this utility." << endl;
        cout << endl;
        cout << "(Press any key to proceed)" << endl;
        std::getchar();
        omp_set_num_threads(4);
        ref<Timer> timer = new Timer();

//      fit("beckmann", 0);
//      fit("beckmann", 1);
        fit("phong", 0);
        fit("phong", 1);
//      fit("ggx", 0);
//      fit("ggx", 1);
        cout << endl;
        cout << "Done, took " << timer->getMilliseconds() / 1000.0f << " seconds" << endl;
        return 0;
    }

    MTS_DECLARE_UTILITY()
};

MTS_EXPORT_UTILITY(PrecomputeTransmittance, "Precompute transmittance data for rough dielectrics")
MTS_NAMESPACE_END


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2014-2015 Samuel Debionne, Grenoble, France.

// This file was modified by Oracle on 2015, 2016, 2017, 2018.
// Modifications copyright (c) 2015-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_BOX_HPP

#include <boost/geometry/algorithms/dispatch/expand.hpp>

#include <boost/geometry/core/tags.hpp>

// For backward compatibility
#include <boost/geometry/strategies/cartesian/expand_box.hpp>
#include <boost/geometry/strategies/spherical/expand_box.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Box + box -> new box containing two input boxes
template
<
    typename BoxOut, typename BoxIn
>
struct expand
    <
        BoxOut, BoxIn,
        box_tag, box_tag
    >
{
    template <typename Strategy>
    static inline void apply(BoxOut& box_out,
                             BoxIn const& box_in,
                             Strategy const& )
    {
        Strategy::apply(box_out, box_in);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_INDEXED_HPP


#include <bio/environment.h>
#include <bio/hidden_markov_model.h>
#include <bio/hmm_gen_sequence.h>
#include <bio/chromosomes_file_set.h>
#include <bio/hmm_forward_backward.h>
#include <bio/hmm_viterbi.h>
#include <bio/hmm_baum_welch.h>
#include <bio/sequence.h>
#include <bio/fasta.h>
#include <bio/hmm_dna.h>
USING_BIO_NS;

#include <boost/test/unit_test.hpp>
#include <boost/test/parameterized_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/assign.hpp>
#include <boost/progress.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/io/ios_state.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost;
using boost::unit_test::test_suite;

#include <gsl/gsl_randist.h>

#include <iostream>
#include <list>
#include <fstream>
using namespace std;

#include <math.h>

//#define VERBOSE_CHECKING

#define ENSEMBL_DIR "C:\\data\\ensembl"
#define CHROMO_DIR ENSEMBL_DIR "\\chromosomes"
#define FASTA_FILE ENSEMBL_DIR "\\test_seq_5.fa"
#define LARGE_FASTA_FILE ENSEMBL_DIR "\\Mus_musculus.NCBIM33.may.dna.contig.fa"
//#define LARGE_FASTA_FILE ENSEMBL_DIR "\\test_seq_1.fa"

#define PROB_TOLERANCE 0.01 //give about 1% tolerance on prob comparisons

#define NUM_BAUM_WELCH_ITERATIONS 10

bool
new_prob_less(
	prob_t last_prob,
	prob_t this_prob)
{
	//make sure we only have small positive log probs (due to rounding errors)
	if (0 < last_prob && last_prob < 1.0e-015) {
		last_prob = 0.0;
	}
	return last_prob <= this_prob + PROB_TOLERANCE * abs(this_prob) / 2;
}

typedef MarkovState<NucleoCode> state_t;
typedef HiddenMarkovModel<NucleoCode> hmm_t;
typedef HmmSequenceGenerator<NucleoCode> seq_gen_t;
BOOST_TEST_DONT_PRINT_LOG_VALUE(hmm_t);

hmm_t hmm;

const SeqList test_seqs = boost::assign::list_of
	("AT")
	("ATC")
	("")
	("A")
	("C")
	("G")
	("T")
	("AA")
	("CC")
	("ACGCTCGT")
	("CTTCTTCTCTCTCTAGAGGAGAGAGA")
	("CTTCTAGAGGAGTCTCTCTCTAGAGA")
	("CTTCTTCTCTCTCT")
	("ATCATCATCATCATCATCATCATCATCATCATCATCATC")
	("CTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCT")
	("CTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCT")
	("CTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTCTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCTAGAGGAGTCTCTCTCTAGAGACTTCT")
	("CGTGGATGGGTCCTC")
	("TTCTTA")
	("ATAATTAAA")
	("TAAATTTTA")
	("TGACG")
	("ATTAATTT")
	("ACTTGGTAACA")
	("TATAAATTT")
	("TAGGAAACAAA")
	("AATAT")
	("GTTCAACAGG")
	("CGCAGGCC")
	("TTAATTTATAAAAAA")
	("ATCGCGGA")
	("TAAAATATAA")
	("CAGAATGCTATGAC")
	("AGTT")
	("CGA")
	("AAATTTTAATTATA")
	("GGAGTGCCAA")
	("CCGACAGC")
	("CCATAATGGG")
	("TGCCAGGGGGTCTGAC")
	("GGTTCTTGATAGT")
	("CCAG")
	("GCTTTCCGT")
	("ACAAAGGAAC")
	("TGCACCCC")
	("ATTTAATATTT")
	("TATTCCGCTG")
	("ACTGCAT")
	("AAATTAAT")
	("GGCCGGGAA")
	("CAGCTGATTTCCTA")
	("ATATATAAT")
	("TTATATTTT")
	("ACCGCC")
	("CACACGCAGG")
	("ATCGGACCA")
	("ATATTTTGGC")
	("CCTGGTTAGAGAG")
	("TAGACTCGATCGAA")
	("ACCGGGAGGGCCGCTCC")
	("TGTCATGTCC")
	("ACACCCGAAGAGTA")
	("AGGACATGTC")
	("AAGGGAGGATTA")
	("AGTTGACCCTA")
	("CGAGGTCGGGC")
	("AGGGTGCTGAG")
	("TGCCCCCG")
	("ATTTATTA")
	("CCTCCTGGCT")
	("GCTTGGGGCC")
	("TAGAGGTAC")
	("GTTGCGCATCG")
	("ATAAAAAATTTA")
	("ATGCAGTC")
	("CAAGATACG")
	("TTTTATTTAATTA")
	("CTAAGCGA")
	("CCATGATCGCT")
	("ATTAATTTT")
	("AGAGAGAGCGAC")
	("TTTCCGTCTC")
	("ATAATTATTTA")
	("TAATTTATTAA")
	("TTTTTGTG")
	("AAGATGGTGTT")
	("TTATTATA")
	("AAATTTATATTTA")
	("TTCTGGCTTA")
	("TCAGATTAC")
	("ATTAATAATTTTTTAA")
	("GCCCAGGCCGGGT")
	("AAATAATT")
	("GGCT")
	("AAAAAAAAATTAA")
	("GCCCGCCGG")
	("CCACAAGA")
	("TGAAGGTTATCG")
	("TCTTGGAGAAAA")
	("TCCTGATTCTATAG")
	("TAAAATATAATTAAA")
	("AAAATA")
	("TTAACC")
	("TATAATAAAAT")
	("TTTAAAATT")
	("GCATGGGGCGT")
	("GTAACCTGTGGCG")
	("GCCCGGGG")
	("ATAAAGACGTCCGTC")
	("AATTCCTGAG")
	("AAAGACACCGAAA")
	("GGGGACCGC")
	("TTTTTAAATAA")
	("TATAAAAATT")
	("CATCGCCACGCAAGC")
	("TCGGGGTG")
	("GTCAGCT")
	;

typedef list<size_t> index_list_t;
BOOST_TEST_DONT_PRINT_LOG_VALUE(index_list_t);

seq_t long_test_seq;

typedef vector<SeqList> seq_vec_vec;
const seq_vec_vec hmm_multiple_seqs = boost::assign::list_of< SeqList >
	(boost::assign::list_of< seq_t >
		(seq_t(""))
		(seq_t(""))
		(seq_t(""))
		)
	(boost::assign::list_of< seq_t >
		(seq_t("A"))
		(seq_t("T"))
		(seq_t(""))
		)
	(boost::assign::list_of< seq_t >
		(seq_t(""))
		(seq_t("T"))
		(seq_t("TC"))
		)
	(boost::assign::list_of< seq_t >
		(seq_t("ACGT"))
		(seq_t("TGCA"))
		)
	(boost::assign::list_of< seq_t >
		(seq_t("ATATATATATATAAATATATAATATATATTTTAA"))
		(seq_t("ACACACCCACCACCCACACACAAAACACACACCC"))
		(seq_t("GTGTGTGTTGTGTGTGGTGTTTTTGTGTGGTGTG"))
		)
	(boost::assign::list_of< seq_t >
		(seq_t("ATCGACTAGCAGCGAGT"))
		(seq_t("GATAGATAGAGAGTA"))
		(seq_t("CTGCTGCTCTTCG"))
		)
	;

/** Functor to check the Baum-Welch algorithm for a model on a sequence. */
struct CheckBaumWelch : std::binary_function<hmm_t, seq_t, void>
{
	void operator()(hmm_t hmm, const seq_t & sequence) const
	{
		const prob_t likelihood_before_training =
			get_per_char_likelihood(hmm, sequence.begin(), sequence.end());

		for (unsigned i = 0; NUM_BAUM_WELCH_ITERATIONS != i; ++i)
		{
			baum_welch_single(
				hmm,
				sequence.begin(),
				sequence.end(),
				sequence.rbegin(),
				sequence.rend());
		}

		const prob_t likelihood_after_training =
			get_per_char_likelihood(hmm, sequence.begin(), sequence.end());

#ifdef VERBOSE_CHECKING
		std::cout
			<< "Size: " << hmm.states.size() << "; "
			<< likelihood_before_training << ", "
			<< likelihood_after_training << '\n';
#endif

		//check we are not more than 5% worse than previously
		BOOST_CHECK(likelihood_after_training >= likelihood_before_training - 0.05);
	}
};

/** Functor to check baum welch on a list of sequences. */
struct CheckBaumWelchOnSequences
{
	const SeqList & sequences;
	CheckBaumWelchOnSequences(const SeqList & sequences)
		: sequences(sequences)
	{ }
	void operator()(hmm_t & hmm) const
	{
		std::for_each(sequences.begin(), sequences.end(), std::bind1st(CheckBaumWelch(), hmm));
	}
};

/** Print some random test sequences to cout. */
void
generate_test_sequences()
{
	cout << "******* generate_test_sequences()" << endl;

	const unsigned mean_length = 10;
	const unsigned num_sequences = 100;

	gsl_rng * r;
	/* create a generator chosen by the
		environment variable GSL_RNG_TYPE */
	{
		const gsl_rng_type * T;

		gsl_rng_env_setup();

		T = gsl_rng_default;
		r = gsl_rng_alloc(T);
	}

	typedef boost::array<prob_t, 3> base_dist_t;

	const base_dist_t uniform_dist		= boost::assign::list_of( 0.25 )( 0.50 )( 0.75 );
	const base_dist_t a_t_dist		= boost::assign::list_of( 0.50 )( 0.50 )( 0.50 );
	const base_dist_t c_g_dist		= boost::assign::list_of( 0.10 )( 0.50 )( 0.90 );

	for (unsigned s = 0; num_sequences != s; ++s)
	{
		//choose a dist
		const prob_t dist_rnd = get_uniform_01();
		const base_dist_t & dist = dist_rnd < .5 ? uniform_dist : (dist_rnd < .75 ? a_t_dist : c_g_dist);

		//choose a length
		const unsigned length = gsl_ran_poisson(r, mean_length);

		//generate a sequence
		seq_t seq;
		for (unsigned i = 0; length != i; ++i)
		{
			const prob_t base_rnd = get_uniform_01();
			if (base_rnd < dist[0])
			{
				seq.push_back('A');
			}
			else if (base_rnd < dist[1])
			{
				seq.push_back('C');
			}
			else if (base_rnd < dist[2])
			{
				seq.push_back('G');
			}
			else
			{
				seq.push_back('T');
			}
		}

		//print the sequence
		cout << "\t(\"" << seq << "\")\n";
	}
	gsl_rng_free(r);
}

void
create_hmm() {

	//1st state
	{
		const prob_t emission_probs [] = { 0.9f, 0.025f, 0.025f, 0.05f };
		prob_vector_t trans_probs;
		trans_probs.push_back(0.1f);
		trans_probs.push_back(0.8f);
		trans_probs.push_back(0.1f);
		hmm.states.push_back(
			state_t(
				0.9f,
				emission_probs,
				trans_probs.begin(),
				trans_probs.end()));
	}

	//2nd state
	{
		const prob_t emission_probs [] = { 0.025f, 0.025f, 0.025f, 0.925f };
		prob_vector_t trans_probs;
		trans_probs.push_back(0.1f);
		trans_probs.push_back(0.1f);
		trans_probs.push_back(0.8f);
		hmm.states.push_back(
			state_t(
				0.05f,
				emission_probs,
				trans_probs.begin(),
				trans_probs.end()));
	}

	//3rd state
	{
		const prob_t emission_probs [] = { 0.025f, 0.9f, 0.025f, 0.05f };
		prob_vector_t trans_probs;
		trans_probs.push_back(0.8f);
		trans_probs.push_back(0.1f);
		trans_probs.push_back(0.1f);
		hmm.states.push_back(
			state_t(
				0.05f,
				emission_probs,
				trans_probs.begin(),
				trans_probs.end()));
	}

#ifdef VERBOSE_CHECKING
	cout << hmm;
#endif
}

void
create_hmm_test_seqs()
{
	ifstream file(FASTA_FILE);
	if (! file.good())
	{
		throw std::logic_error(BIO_MAKE_STRING("Could not open FASTA file: \"" << FASTA_FILE << "\""));
	}

#ifdef VERBOSE_CHECKING
	cout << "Parsing fasta file " FASTA_FILE << endl;
	progress_timer timer;
#endif

	stringstream sequence;
	parse_fasta(file, sequence);

	long_test_seq = sequence.str();
}

void ensure_hmm_built() {
	static bool already_done = false;
	if (! already_done) {
		create_hmm();
		already_done = true;
	}
}

void ensure_hmm_test_seqs_created() {
	static bool already_done = false;
	if (! already_done) {
		create_hmm_test_seqs();
		already_done = true;
	}
}

void check_hmm_overfitting()
{
	ensure_hmm_built();

	cout << "******* check_hmm_overfitting()" << endl;

	//first we overfit the hmm on the first sequence then try to train on the second
	seq_t overfitted_seq("ATCATCATCATCATCATCATCATCATCATCATCATCATC");
	seq_t new_seq("ACTACTACTACTACTACTACTACTACTACTACTACTACT");

	BaumWelchAlgorithm<true> baum_welch;

	//use a new HMM
	hmm_t hmm_2(hmm);

	//first train on the over fitting sequence
	{
		baum_welch.forward(
			hmm_2,
			overfitted_seq.begin(),
			overfitted_seq.end());

		prob_t last_prob = baum_welch.get_log_probability();

#ifdef VERBOSE_CHECKING
		cout
			<< "Overfitted sequence has log probability "
			<< last_prob
			<< " before baum welch "
			<< endl;
#endif

		BOOST_CHECK(0 != BIO_FINITE(last_prob));

		const size_t iterations = NUM_BAUM_WELCH_ITERATIONS;
		for (size_t j = 0; j < iterations; ++j)
		{
			baum_welch.run(
				hmm_2,
				overfitted_seq.begin(),
				overfitted_seq.end(),
				overfitted_seq.rbegin(),
				overfitted_seq.rend());

			BOOST_CHECK(hmm_2.is_consistent());
			//cout << hmm_2;

			baum_welch.forward(
				hmm_2,
				overfitted_seq.begin(),
				overfitted_seq.end());
			const prob_t this_prob = baum_welch.get_log_probability();
			BOOST_CHECK(0 != BIO_FINITE(this_prob));

			BOOST_CHECK(new_prob_less(last_prob, this_prob));
			if (! new_prob_less(last_prob, this_prob))
			{
				cerr << "Iteration: " << j << "; " << last_prob << " > " << this_prob << endl;
			}

			last_prob = this_prob;
		}
	} //training on overfitting seq

	//now train on new sequence
	{
		const size_t iterations = NUM_BAUM_WELCH_ITERATIONS;
		for (size_t j = 0; j < iterations; ++j) {

			baum_welch.run(
				hmm_2,
				new_seq.begin(),
				new_seq.end(),
				new_seq.rbegin(),
				new_seq.rend());

			BOOST_CHECK(hmm_2.is_consistent());
			//cout << hmm_2;
		}
	}
}

void check_forward_backward() {

	ensure_hmm_built();
	ensure_hmm_test_seqs_created();

	cout << "******* check_forward_backward(): " << test_seqs.size() << " artificial sequences" << endl;

	for (SeqList::const_iterator i = test_seqs.begin(); test_seqs.end() != i; ++i) {

		const size_t num_obs = i->end() - i->begin();

		ForwardBackwardAlgorithm<true> fw_with_scaling;
		fw_with_scaling.forward(
			hmm,
			i->begin(),
			i->end());
		fw_with_scaling.backward(
			hmm,
			i->rbegin(),
			i->rend());

		ForwardBackwardAlgorithm<false> fw_without_scaling;
		fw_without_scaling.forward(
			hmm,
			i->begin(),
			i->end());
		fw_without_scaling.backward(
			hmm,
			i->rbegin(),
			i->rend());

#ifdef VERBOSE_CHECKING
		cout
			<< "Sequence " << (i - test_seqs.begin())
			<< " (length=" << i->size()
			<< ") has probability (with    scaling): "
			<< fw_with_scaling.get_probability()
			<< endl;
		cout
			<< "Sequence " << (i - test_seqs.begin())
			<< " (length=" << i->size()
			<< ") has probability (without scaling): "
			<< fw_without_scaling.get_probability()
			<< endl;

		cout
			<< "Sequence " << (i - test_seqs.begin())
			<< " (length=" << i->size()
			<< ") has log probability (with    scaling): "
			<< fw_with_scaling.get_log_probability()
			<< endl;
		cout
			<< "Sequence " << (i - test_seqs.begin())
			<< " (length=" << i->size()
			<< ") has log probability (without scaling): "
			<< fw_without_scaling.get_log_probability()
			<< endl;
		cout << endl;
#endif

		//the log probability may underflow to log(0.0) without scaling - allow this in the tests
		if (BIO_FINITE(fw_without_scaling.get_log_probability())) {
			//only do comparison if no underflow
			BOOST_CHECK_CLOSE(fw_with_scaling.get_log_probability(), fw_without_scaling.get_log_probability(), 1.0);
		}
		//in the normal probability case both or neither underflow
		BOOST_CHECK_CLOSE(fw_with_scaling.get_probability(), fw_without_scaling.get_probability(), 1.0);
	}
}

void
check_viterbi()
{
	ensure_hmm_built();
	ensure_hmm_test_seqs_created();

	cout << "******* check_viterbi(): " << test_seqs.size() << " artificial sequences" << endl;

	for (SeqList::const_iterator i = test_seqs.begin(); test_seqs.end() != i; ++i) {

		const size_t num_obs = i->end() - i->begin();

		//doesn't work well without logs on long sequences
		if (num_obs > 20)
		{
			continue;
		}

		index_list_t state_indices;
		ViterbiAlgorithm<false> viterbi_without_logs;
		viterbi_without_logs.viterbi(
			hmm,
			num_obs,
			i->begin(),
			i->end(),
			front_inserter(state_indices));

#ifdef VERBOSE_CHECKING
		cout
			<< "Sequence " << (i - test_seqs.begin())
			<< " has most likely state sequence (not using logs): ";
		for (index_list_t::const_iterator j = state_indices.begin(); state_indices.end() != j; ++j) {
			cout << *j;
		}
		cout << endl;
#endif

		index_list_t state_indices_using_logs;
		ViterbiAlgorithm<true> viterbi_with_logs;
		viterbi_with_logs.viterbi(
			hmm,
			num_obs,
			i->begin(),
			i->end(),
			front_inserter(state_indices_using_logs));

#ifdef VERBOSE_CHECKING
		cout
			<< "Sequence " << (i - test_seqs.begin())
			<< " has most likely state sequence (    using logs): ";
		for (index_list_t::const_iterator j = state_indices_using_logs.begin(); state_indices_using_logs.end() != j; ++j) {
			cout << *j;
		}
		cout << endl;
#endif

		BOOST_CHECK_EQUAL(state_indices, state_indices_using_logs);

#ifdef VERBOSE_CHECKING
		cout << endl;
#endif
	}
}

/** Check the Baum-Welch algorithm on the long sequence. */
void
check_long_test_seq()
{
	ensure_hmm_built();
	ensure_hmm_test_seqs_created();

	cout << "******* check_long_test_seq(): Checking baum welch algorithm on homo sapiens sequence of length " << long_test_seq.size() << endl;

	BaumWelchAlgorithm<true> baum_welch;

	//use a new HMM for each sequence
	hmm_t hmm_2(hmm);

	baum_welch.forward(
		hmm_2,
		long_test_seq.begin(),
		long_test_seq.end());
	prob_t last_prob = baum_welch.get_log_probability();

#ifdef VERBOSE_CHECKING
	cout
		<< "Long test sequence has log probability "
		<< last_prob
		<< " before baum welch "
		<< endl;
#endif

	BOOST_CHECK(0 != BIO_FINITE(last_prob));

	const size_t iterations = NUM_BAUM_WELCH_ITERATIONS;
	for (size_t j = 0; j < iterations; ++j) {

		baum_welch.run(
			hmm_2,
			long_test_seq.begin(),
			long_test_seq.end(),
			long_test_seq.rbegin(),
			long_test_seq.rend());

		BOOST_CHECK(hmm_2.is_consistent());
		//cout << hmm_2;

		baum_welch.forward(
			hmm_2,
			long_test_seq.begin(),
			long_test_seq.end());
		const prob_t this_prob = baum_welch.get_log_probability();
#ifdef VERBOSE_CHECKING
		cout << "Likelihood " << this_prob << endl;
#endif
		BOOST_CHECK(0 != BIO_FINITE(this_prob));

		if (! new_prob_less(last_prob, this_prob)) {
			cerr << last_prob << " > " << this_prob << endl;
		}
		BOOST_CHECK(new_prob_less(last_prob, this_prob));

		last_prob = this_prob;
	}

#ifdef VERBOSE_CHECKING
	cout
		<< "Long test sequence has log probability "
		<< last_prob
		<< " after baum welch "
		<< endl;
#endif
}

/** Check the Baum-Welch algorithm. */
void
check_baum_welch_multiple(const SeqList & seqs)
{
	ensure_hmm_built();

	cout << "******* check_baum_welch_multiple()" << endl;

	//generate the combined sequence
	const seq_t combined_seq = std::accumulate(seqs.begin(), seqs.end(), string(""));

	//train a hmm on the combined sequence
	hmm_t hmm_single(hmm);
	baum_welch_single(
		hmm_single,
		combined_seq.begin(),
		combined_seq.end(),
		combined_seq.rbegin(),
		combined_seq.rend());
	const prob_t single_log_prob =
		ForwardBackwardAlgorithm<true>()
			.forward(
				hmm_single,
				combined_seq.begin(),
				combined_seq.end()).get_log_probability();

	hmm_t hmm_multiple(hmm);
	prob_t last_multiple_log_prob =
		get_multiple_log_likelihood(hmm_multiple, seqs.begin(), seqs.end());

	//train multiply several times
	for (unsigned i = 0; NUM_BAUM_WELCH_ITERATIONS != i; ++i)
	{
		//train a hmm on all sequences concurrently
		baum_welch_multiple(
			hmm_multiple,
			seqs.begin(),
			seqs.end());

		const prob_t new_log_prob =
			get_multiple_log_likelihood(hmm_multiple, seqs.begin(), seqs.end());

#ifdef VERBOSE_CHECKING
		if (new_log_prob < last_multiple_log_prob)
		{
			cout << new_log_prob << " < " << last_multiple_log_prob << endl;
		}
#endif

		last_multiple_log_prob = new_log_prob;
	}

#ifdef VERBOSE_CHECKING
	cout
		<< "Log prob of combined seq after training one at a time: "
		<< single_log_prob
		<< endl;
	cout
		<< "Log prob of combined seq after training all at once: "
		<< last_multiple_log_prob
		<< endl;
#endif //VERBOSE_CHECKING

}

/** Check the Baum-Welch algorithm. */
void
check_baum_welch(const seq_t & seq)
{
	cout << "******* check_baum_welch(): \"" << seq << "\"\n";

	vector<hmm_t> hmms = boost::assign::list_of
		(hmm_t(1))
		(hmm_t(2))
		(hmm_t(3))
		(hmm_t(4))
		(hmm_t(5))
		(hmm_t(6))
		;

	std::for_each(hmms.begin(), hmms.end(), std::bind2nd(CheckBaumWelch(), seq));
}

/** Check the number of states improves learning. */
void
check_more_states_improves_learning(const seq_t & seq)
{
	cout << "******* check_more_states_improves_learning(): \"" << seq << "\"\n";

	typedef vector<hmm_t> hmm_vec;
	hmm_vec hmms = boost::assign::list_of
		(hmm_t(1))
		(hmm_t(2))
		(hmm_t(3))
		(hmm_t(4))
		(hmm_t(5))
		(hmm_t(6))
		;

	prob_t last_likelihood = 0;
	for (hmm_vec::iterator h = hmms.begin(); hmms.end() != h; ++h)
	{
		for (unsigned i = 0; 20 != i; ++i)
		{
			baum_welch_single(
				*h,
				seq.begin(),
				seq.end(),
				seq.rbegin(),
				seq.rend());
		}

		const prob_t new_likelihood = get_per_char_likelihood(*h, seq.begin(), seq.end());

		cout << h->states.size() << ": " << new_likelihood << endl;
		BOOST_CHECK(new_likelihood >= 0.99 * last_likelihood);
		last_likelihood = new_likelihood;
	}
}

void register_hidden_markov_model_tests(test_suite * test)
{
	ensure_hmm_built();
	ensure_hmm_test_seqs_created();

	test->add(BOOST_TEST_CASE(&check_forward_backward), 0);
	test->add(BOOST_PARAM_TEST_CASE(&check_baum_welch, test_seqs.begin(), test_seqs.end()), 0);
	test->add(BOOST_TEST_CASE(&check_hmm_overfitting), 0);
	test->add(BOOST_TEST_CASE(&check_long_test_seq), 0);
	test->add(BOOST_PARAM_TEST_CASE(&check_baum_welch_multiple, hmm_multiple_seqs.begin(), hmm_multiple_seqs.end()), 0);

	//it is not clear how effective these tests are
	//test->add(BOOST_PARAM_TEST_CASE(&check_more_states_improves_learning, test_seqs.begin(), test_seqs.end()), 0);
	//test->add(BOOST_TEST_CASE(&check_viterbi), 2); //can get 2 underflow problems on long sequences when not using logs
	//test->add(BOOST_TEST_CASE(&generate_test_sequences), 0);
}


#include "fake_anemometer.h"

#include <stdlib.h>     /* srand, rand */
#include <time.h>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

typedef boost::normal_distribution<double> NormalDistribution;
typedef boost::mt19937 RandomGenerator;
typedef boost::variate_generator<RandomGenerator&, \
                       NormalDistribution> GaussianGenerator;

int main( int argc, char** argv )
{
	ros::init(argc, argv, "simulated_anemometer");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	srand (time(NULL));

	//Read parameters
	loadNodeParameters(pn);

	//Publishers
	//ros::Publisher sensor_read_pub = n.advertise<std_msgs::Float32MultiArray>("WindSensor_reading", 500);
	ros::Publisher sensor_read_pub = n.advertise<olfaction_msgs::anemometer>("WindSensor_reading", 500);
	ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("WindSensor_display", 100);

	//Service to request wind values to simulator
    ros::ServiceClient client = n.serviceClient<gaden_player::WindPosition>("/wind_value");
	tf::TransformListener tf_;

	// Init Visualization data (marker)
	//----------------------------------------------------------------
	// sensor = sphere
	// conector = stick from the floor to the sensor
	visualization_msgs::Marker sensor;
	visualization_msgs::Marker connector;
	visualization_msgs::Marker connector_inv;
	visualization_msgs::Marker wind_point;
	visualization_msgs::Marker wind_point_inv;

	sensor.header.frame_id = input_fixed_frame.c_str();
	sensor.ns = "sensor_visualization";
	sensor.action = visualization_msgs::Marker::ADD;
	sensor.type = visualization_msgs::Marker::SPHERE;
	sensor.id = 0;
	sensor.scale.x = 0.1;
	sensor.scale.y = 0.1;
	sensor.scale.z = 0.1;
	sensor.color.r = 0.0f;
	sensor.color.g = 0.0f;
	sensor.color.b = 1.0f;
	sensor.color.a = 1.0;

	connector.header.frame_id = input_fixed_frame.c_str();
	connector.ns  = "sensor_visualization";
	connector.action = visualization_msgs::Marker::ADD;
	connector.type = visualization_msgs::Marker::CYLINDER;
	connector.id = 1;
	connector.scale.x = 0.1;
	connector.scale.y = 0.1;
	connector.color.a  = 1.0;
	connector.color.r = 1.0f;
	connector.color.b = 1.0f;
	connector.color.g = 1.0f;

	// Init Marker: arrow to display the wind direction measured.
	wind_point.header.frame_id = input_sensor_frame.c_str();
	wind_point.action = visualization_msgs::Marker::ADD;
	wind_point.ns = "measured_wind";
	wind_point.type = visualization_msgs::Marker::ARROW;

	// Init Marker: arrow to display the inverted wind direction measured.
	wind_point_inv.header.frame_id = input_sensor_frame.c_str();
	wind_point_inv.action = visualization_msgs::Marker::ADD;
	wind_point_inv.ns = "measured_wind_inverted";
	wind_point_inv.type = visualization_msgs::Marker::ARROW;

	// LOOP
	//----------------------------------------------------------------
	tf::TransformListener listener;
    ros::Rate r(2);
	while (ros::ok())
	{
		//Vars
		tf::StampedTransform transform;
		bool know_sensor_pose = true;

		//Get pose of the sensor in the /map reference
		try
		{
		  listener.lookupTransform(input_fixed_frame.c_str(), input_sensor_frame.c_str(),
								   ros::Time(0), transform);
		}
		catch (tf::TransformException ex)
		{
			ROS_ERROR("%s",ex.what());
			know_sensor_pose = false;
			ros::Duration(1.0).sleep();
		}

		if (know_sensor_pose)
		{
			//Current sensor pose
			float x_pos = transform.getOrigin().x();
			float y_pos = transform.getOrigin().y();
			float z_pos = transform.getOrigin().z();

			// Get Wind vectors (u,v,w) at current position
			// Service request to the simulator
            gaden_player::WindPosition srv;
			srv.request.x = x_pos;
			srv.request.y = y_pos;
			srv.request.z = z_pos;
			float u,v,w;
			olfaction_msgs::anemometer anemo_msg;
			if (client.call(srv))
			{
                double wind_speed;
                double wind_direction;

				//GT Wind vector Value (u,v,w)[m/s]
                //From OpenFoam this is the DownWind direction in the map
				u = (float)srv.response.u;
				v = (float)srv.response.v;
				w = (float)srv.response.w;
                wind_speed = sqrt(pow(u,2)+pow(v,2));

                float downWind_direction_map;
                if (u !=0 || v!=0)
                    downWind_direction_map = atan2(v,u);
                else
                    downWind_direction_map = 0.0;

                if (!use_map_ref_system)
                {
                    // (IMPORTANT) Follow standards on wind measurement (real anemometers):
                    //return the upwind direction in the anemometer reference system
                    //range [-pi,pi]
                    //positive to the right, negative to the left (opposed to ROS poses :s)

                    float upWind_direction_map = angles::normalize_angle(downWind_direction_map + 3.14159);

                    //Transform from map ref_system to the anemometer ref_system using TF
                    geometry_msgs::PoseStamped anemometer_upWind_pose, map_upWind_pose;
                    try
                    {
                        map_upWind_pose.header.frame_id = input_fixed_frame.c_str();
                        map_upWind_pose.pose.position.x = 0.0;
                        map_upWind_pose.pose.position.y = 0.0;
                        map_upWind_pose.pose.position.z = 0.0;
                        map_upWind_pose.pose.orientation = tf::createQuaternionMsgFromYaw(upWind_direction_map);

                        tf_.transformPose(input_sensor_frame.c_str(), map_upWind_pose, anemometer_upWind_pose);
                    }
                    catch(tf::TransformException &ex)
                    {
                        ROS_ERROR("FakeAnemometer - %s - Error: %s", __FUNCTION__, ex.what());
                    }

                    double upwind_direction_anemo = tf::getYaw(anemometer_upWind_pose.pose.orientation);
                    wind_direction = upwind_direction_anemo;
                }
                else
                {
                    // for simulations
                    wind_direction = angles::normalize_angle(downWind_direction_map);
                }

                // Adding Noise
                static RandomGenerator rng(static_cast<unsigned> (time(0)));
                NormalDistribution gaussian_dist(0.0,noise_std);
                GaussianGenerator generator(rng, gaussian_dist);
                wind_direction = wind_direction + generator();
                wind_speed = wind_speed + generator();

                //Publish 2D Anemometer readings
                //------------------------------
				anemo_msg.header.stamp = ros::Time::now();
                if (use_map_ref_system)
                    anemo_msg.header.frame_id = input_fixed_frame.c_str();
                else
                    anemo_msg.header.frame_id = input_sensor_frame.c_str();

				anemo_msg.sensor_label = "Fake_Anemo";
                anemo_msg.wind_direction = wind_direction;  //rad
                anemo_msg.wind_speed = wind_speed;	 //m/s
				//Publish fake_anemometer reading (m/s)
				sensor_read_pub.publish(anemo_msg);

                //Add wind marker ARROW for Rviz (2D) --> Upwind
                /*
				wind_point.header.stamp = ros::Time::now();
				wind_point.points.clear();
				wind_point.id = 1;  //unique identifier for each arrow
				wind_point.pose.position.x = 0.0;
				wind_point.pose.position.y = 0.0;
				wind_point.pose.position.z = 0.0;
				wind_point.pose.orientation = tf::createQuaternionMsgFromYaw(wind_direction_with_noise);
				wind_point.scale.x = 2*sqrt(pow(u,2)+pow(v,2));	  //arrow lenght
				wind_point.scale.y = 0.1;	  //arrow width
				wind_point.scale.z = 0.1;	  //arrow height
				wind_point.color.r = 0.0;
				wind_point.color.g = 1.0;
				wind_point.color.b = 0.0;
				wind_point.color.a = 1.0;
				marker_pub.publish(wind_point);
                */

                //Add inverted wind marker --> DownWind
				wind_point_inv.header.stamp = ros::Time::now();
				wind_point_inv.header.frame_id=anemo_msg.header.frame_id;
				wind_point_inv.points.clear();
				wind_point_inv.id = 1;  //unique identifier for each arrow
				if(use_map_ref_system){
					wind_point_inv.pose.position.x = x_pos;
					wind_point_inv.pose.position.y = y_pos;
					wind_point_inv.pose.position.z = z_pos;
				}else{
					wind_point_inv.pose.position.x = 0.0;
					wind_point_inv.pose.position.y = 0.0;
					wind_point_inv.pose.position.z = 0.0;
				}
                wind_point_inv.pose.orientation = tf::createQuaternionMsgFromYaw(wind_direction+3.1416);
				wind_point_inv.scale.x = 2*sqrt(pow(u,2)+pow(v,2));	  //arrow lenght
				wind_point_inv.scale.y = 0.1;	  //arrow width
				wind_point_inv.scale.z = 0.1;	  //arrow height
				wind_point_inv.color.r = 0.0;
				wind_point_inv.color.g = 1.0;
				wind_point_inv.color.b = 0.0;
				wind_point_inv.color.a = 1.0;
				marker_pub.publish(wind_point_inv);

                notified = false;
			}
			else
			{
                if (!notified)
                {
                    ROS_WARN("[fake_anemometer] Cannot read Wind Vector from simulated data.");
                    notified = true;
                }
			}

            //Publish RVIZ sensor pose (a sphere)
            /*
			sensor.header.stamp = ros::Time::now();
			sensor.pose.position.x = x_pos;
			sensor.pose.position.y = y_pos;
			sensor.pose.position.z = z_pos;
			marker_pub.publish(sensor);
            */

            // PUBLISH ANEMOMETER Stick
            /*
			connector.header.stamp = ros::Time::now();
			connector.scale.z = z_pos;
			connector.pose.position.x = x_pos;
			connector.pose.position.y = y_pos;
			connector.pose.position.z = float(z_pos)/2;
            marker_pub.publish(connector);
            */
		}

		ros::spinOnce();
		r.sleep();
	}
}

//Load Sensor parameters
void loadNodeParameters(ros::NodeHandle private_nh)
{
	//sensor_frame
	private_nh.param<std::string>("sensor_frame", input_sensor_frame, "/anemometer_link");

	//fixed frame
	private_nh.param<std::string>("fixed_frame", input_fixed_frame, "/map");

	//Noise
	private_nh.param<double>("noise_std", noise_std, 0.1);

    //What ref system to use for publishing measurements
    private_nh.param<bool>("use_map_ref_system", use_map_ref_system, false);

	ROS_INFO("[fake anemometer]: wind noise: %f", noise_std);

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_BOOLEAN_FUNCTIONS_SIMD_COMMON_NEGIF_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_BOOLEAN_FUNCTIONS_SIMD_COMMON_NEGIF_HPP_INCLUDED
#include <boost/simd/toolbox/boolean/functions/negif.hpp>
#include <boost/simd/sdk/meta/size.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/constants/properties.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF ( boost::simd::tag::negif_, tag::cpu_, (A0)(A1)(X)
                                , (boost::mpl::equal_to < boost::mpl::sizeof_<A0>
                                                        , boost::mpl::sizeof_<A1>
                                                        >
                                  )
                                , ((simd_<logical_<A0>,X>))
                                  ((simd_<signed_<A1>,X>))
                       )
  {
    typedef A1 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2) { return  if_else(a0,unary_minus(a1),a1); }
  };
} } }

#endif


#include "segmatch/descriptors/cnn.hpp"

#include <algorithm>
#include <math.h>
#include <stdlib.h> /* system, NULL, EXIT_FAILURE */
#include <string>

#include <Eigen/Core>
#include <glog/logging.h>
#include <laser_slam/benchmarker.hpp>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>

#include "segmatch/database.hpp"
#include "segmatch/utilities.hpp"

#include <cstdio>

namespace segmatch {

void CNNDescriptor::describe(const Segment& segment, Features* features) {
  CHECK(false) << "Not implemented";
}

template <typename T>
std::vector<size_t> getIndexesInDecreasingOrdering(const std::vector<T> &v) {

  // initialize original index locations
  std::vector<size_t> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  std::sort(idx.begin(), idx.end(),
            [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});

  return idx;
}

void CNNDescriptor::describe(SegmentedCloud* segmented_cloud_ptr) {

  CHECK_NOTNULL(segmented_cloud_ptr);
  constexpr double kMinChangeBeforeDescription = 0.1; // 0.2

  BENCHMARK_START("SM.Worker.Describe.Preprocess");
  BENCHMARK_RECORD_VALUE("SM.Worker.Describe.NumSegmentsTotal",
                         segmented_cloud_ptr->getNumberOfValidSegments());

  std::vector<tf_graph_executor::Array3D> batch_nn_input;
  std::vector<Id> described_segment_ids;
  std::vector<PclPoint> scales;
  std::vector<PclPoint> thresholded_scales;
  std::vector<std::vector<float> > scales_as_vectors;
  std::vector<PclPoint> rescaled_point_cloud_centroids;
  std::vector<PclPoint> point_mins;
  std::vector<double> alignments_rad;
  std::vector<size_t> nums_occupied_voxels;

  for (std::unordered_map<Id, Segment>::iterator it = segmented_cloud_ptr->begin();
      it != segmented_cloud_ptr->end(); ++it) {

    const PointCloud& point_cloud = it->second.getLastView().point_cloud;
    // const PointICloud& semantic_point_cloud = it->second.getLastView().semantic_point_cloud;
    const size_t num_points = point_cloud.size();

    // Skip describing the segment if it did not change enough.
    if (static_cast<double>(num_points) < static_cast<double>(
        it->second.getLastView().n_points_when_last_described) *
        (1.0 + kMinChangeBeforeDescription)) continue;
    described_segment_ids.push_back(it->second.segment_id);

    // Align with PCA.
    double alignment_rad;
    Eigen::Vector4f pca_centroid;
    pcl::compute3DCentroid(point_cloud, pca_centroid);
    Eigen::Matrix3f covariance_3d;
    computeCovarianceMatrixNormalized(point_cloud, pca_centroid, covariance_3d);
    const Eigen::Matrix2f covariance_2d = covariance_3d.block(0, 0, 2u, 2u);
    Eigen::EigenSolver<Eigen::Matrix2f> eigen_solver(covariance_2d, true);

    alignment_rad = atan2(eigen_solver.eigenvectors()(1,0).real(),
                          eigen_solver.eigenvectors()(0,0).real());

    if (eigen_solver.eigenvalues()(0).real() <
        eigen_solver.eigenvalues()(1).real()) {
      alignment_rad += 0.5*M_PI;
    }

    // Rotate the segment.
    alignment_rad = -alignment_rad;
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.rotate(Eigen::AngleAxisf(alignment_rad, Eigen::Vector3f::UnitZ()));
    PointCloud rotated_point_cloud;
    pcl::transformPointCloud(point_cloud, rotated_point_cloud, transform);

    // Get most points on the lower half of y axis (by rotation).
    PclPoint point_min, point_max;
    pcl::getMinMax3D(rotated_point_cloud, point_min, point_max);
    double centroid_y = point_min.y + (point_max.y - point_min.y) / 2.0;
    unsigned int n_below = 0;
    for (const auto& point : rotated_point_cloud.points) {
      if (point.y < centroid_y) ++n_below;
    }
    if (static_cast<double>(n_below) < static_cast<double>(rotated_point_cloud.size()) / 2.0) {
      alignment_rad += M_PI;
      Eigen::Affine3f adjustment_transform = Eigen::Affine3f::Identity();
      adjustment_transform.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()));
      pcl::transformPointCloud(rotated_point_cloud, rotated_point_cloud, adjustment_transform);
    }

    alignments_rad.push_back(alignment_rad);

    if (save_debug_data_) {
      Segment aligned_segment = it->second;
      aligned_segment.getLastView().point_cloud = rotated_point_cloud;
      aligned_segments_.addValidSegment(aligned_segment);
    }

    PointCloud rescaled_point_cloud;
    pcl::getMinMax3D(rotated_point_cloud, point_min, point_max);
    point_mins.push_back(point_min);

    // "Fit scaling" using the largest dimension as scale.
    PclPoint scale;
    scale.x = point_max.x - point_min.x;
    scale.y = point_max.y - point_min.y;
    scale.z = point_max.z - point_min.z;
    scales.push_back(scale);

    PclPoint thresholded_scale;
    thresholded_scale.x = std::max(scale.x, min_x_scale_m_);
    thresholded_scale.y = std::max(scale.y, min_y_scale_m_);
    thresholded_scale.z = std::max(scale.z, min_z_scale_m_);
    thresholded_scales.push_back(thresholded_scale);

    std::vector<float> scales_as_vector;
    scales_as_vector.push_back(scale.x);
    scales_as_vector.push_back(scale.y);
    scales_as_vector.push_back(scale.z);
    scales_as_vectors.push_back(scales_as_vector);

    for (const auto& point: rotated_point_cloud.points) {
      PclPoint point_new;

      point_new.x = (point.x - point_min.x) / thresholded_scale.x
          * static_cast<float>(n_voxels_x_dim_ - 1u);
      point_new.y = (point.y - point_min.y) / thresholded_scale.y
          * static_cast<float>(n_voxels_y_dim_ - 1u);
      point_new.z = (point.z - point_min.z) / thresholded_scale.z
          * static_cast<float>(n_voxels_z_dim_ - 1u);

      rescaled_point_cloud.points.push_back(point_new);
    }
    rescaled_point_cloud.width = 1;
    rescaled_point_cloud.height = rescaled_point_cloud.points.size();

    PclPoint centroid = calculateCentroid(rescaled_point_cloud);
    rescaled_point_cloud_centroids.push_back(centroid);

    unsigned int n_occupied_voxels = 0;
    tf_graph_executor::Array3D nn_input(n_voxels_x_dim_, n_voxels_y_dim_, n_voxels_z_dim_);
    for (const auto& point: rescaled_point_cloud.points) {
      const unsigned int ind_x = floor(point.x + static_cast<float>(n_voxels_x_dim_ - 1) / 2.0
                                       - centroid.x);
      const unsigned int ind_y = floor(point.y + static_cast<float>(n_voxels_y_dim_ - 1) / 2.0
                                       - centroid.y);
      const unsigned int ind_z = floor(point.z + static_cast<float>(n_voxels_z_dim_ - 1) / 2.0
                                       - centroid.z);

      if (ind_x >= 0 && ind_x < n_voxels_x_dim_ &&
          ind_y >= 0 && ind_y < n_voxels_y_dim_ &&
          ind_z >= 0 && ind_z < n_voxels_z_dim_){

        if (nn_input.container[ind_x][ind_y][ind_z] == 0.0) {
          ++n_occupied_voxels;
        }

        // int semantic_label = point.intensity/1000;
        // if (semantic_label == 0 ||
        //     semantic_label == 1 ||
        //     semantic_label == 2 ||
        //     semantic_label == 3 ||
        //     semantic_label == 4){
        //   nn_input.container[ind_x][ind_y][ind_z] = 1.0;
        // }
        // nn_input.container[ind_x][ind_y][ind_z] = semantic_label+1;
        nn_input.container[ind_x][ind_y][ind_z] = 1;
      }
    }
    nums_occupied_voxels.push_back(n_occupied_voxels);

    it->second.getLastView().n_occupied_voxels = n_occupied_voxels;
    it->second.getLastView().n_points_when_last_described = num_points;

    batch_nn_input.push_back(nn_input);
  }
  BENCHMARK_RECORD_VALUE("SM.Worker.Describe.NumSegmentsDescribed",
                         batch_nn_input.size());
  BENCHMARK_STOP("SM.Worker.Describe.Preprocess");

  if (!batch_nn_input.empty()) {
    BENCHMARK_START("SM.Worker.Describe.ForwardPass");
    std::vector<std::vector<float> > cnn_descriptors;
    std::vector<tf_graph_executor::Array3D> reconstructions;
    std::vector<std::vector<float> > semantics;
    if (batch_nn_input.size() < mini_batch_size_) {
      graph_executor_->batchFullForwardPass(batch_nn_input,
                                            kInputTensorName,
                                            scales_as_vectors,
                                            kScalesTensorName,
                                            kFeaturesTensorName,
                                            kReconstructionTensorName,
                                            cnn_descriptors,
                                            reconstructions);

    } else {
      std::vector<tf_graph_executor::Array3D> mini_batch;
      std::vector<std::vector<float> > mini_batch_scales;
      for (size_t i = 0u; i < batch_nn_input.size(); ++i) {
        mini_batch.push_back(batch_nn_input[i]);
        mini_batch_scales.push_back(scales_as_vectors[i]);
        if (mini_batch.size() == mini_batch_size_) {
          std::vector<std::vector<float> > mini_batch_cnn_descriptors;
          std::vector<tf_graph_executor::Array3D> mini_batch_reconstructions;

          graph_executor_->batchFullForwardPass(mini_batch,
                                                kInputTensorName,
                                                mini_batch_scales,
                                                kScalesTensorName,
                                                kFeaturesTensorName,
                                                kReconstructionTensorName,
                                                mini_batch_cnn_descriptors,
                                                mini_batch_reconstructions);

          cnn_descriptors.insert(cnn_descriptors.end(),
                                 mini_batch_cnn_descriptors.begin(),
                                 mini_batch_cnn_descriptors.end());
          reconstructions.insert(reconstructions.end(),
                                 mini_batch_reconstructions.begin(),
                                 mini_batch_reconstructions.end());
          mini_batch_scales.clear();
          mini_batch.clear();
        }
      }
      if (!mini_batch.empty()) {
        std::vector<std::vector<float> > mini_batch_cnn_descriptors;
        std::vector<tf_graph_executor::Array3D> mini_batch_reconstructions;

        graph_executor_->batchFullForwardPass(mini_batch,
                                              kInputTensorName,
                                              mini_batch_scales,
                                              kScalesTensorName,
                                              kFeaturesTensorName,
                                              kReconstructionTensorName,
                                              mini_batch_cnn_descriptors,
                                              mini_batch_reconstructions);

        cnn_descriptors.insert(cnn_descriptors.end(),
                               mini_batch_cnn_descriptors.begin(),
                               mini_batch_cnn_descriptors.end());
        reconstructions.insert(reconstructions.end(),
                               mini_batch_reconstructions.begin(),
                               mini_batch_reconstructions.end());
      }
    }

    // Execute semantics graph.
    semantics = semantics_graph_executor_->batchExecuteGraph(
        cnn_descriptors, kInputTensorName, kSemanticsOutputName);

    CHECK_EQ(cnn_descriptors.size(), described_segment_ids.size());
    BENCHMARK_STOP("SM.Worker.Describe.ForwardPass");

    BENCHMARK_START("SM.Worker.Describe.SaveFeatures");
    // Write the features.
    for (size_t i = 0u; i < described_segment_ids.size(); ++i) {
      Segment* segment;
      CHECK(segmented_cloud_ptr->findValidSegmentPtrById(described_segment_ids[i], &segment));
      std::vector<float> nn_output = cnn_descriptors[i];

      Feature cnn_feature("cnn");
      for (size_t j = 0u; j < nn_output.size(); ++j) {
        cnn_feature.push_back(FeatureValue("cnn_" + std::to_string(j), nn_output[j]));
      }

      // Push the scales.
      cnn_feature.push_back(FeatureValue("cnn_scale_x", scales[i].x));
      cnn_feature.push_back(FeatureValue("cnn_scale_y", scales[i].y));
      cnn_feature.push_back(FeatureValue("cnn_scale_z", scales[i].z));

      segment->getLastView().features.replaceByName(cnn_feature);

      std::vector<float> semantic_nn_output = semantics[i];
      segment->getLastView().semantic = std::distance(semantic_nn_output.begin(),
                                                      std::max_element(semantic_nn_output.begin(),
                                                                       semantic_nn_output.end()));

      // Generate the reconstructions.
      PointCloud reconstruction;
      const double reconstruction_threshold = 0.75;
      const double ratio_voxels_to_reconstruct = 1.5;
      const bool reconstruct_by_probability = true;

      PclPoint point;
      const PclPoint point_min = point_mins[i];
      const PclPoint scale = thresholded_scales[i];
      const PclPoint centroid = rescaled_point_cloud_centroids[i];

      if (reconstruct_by_probability) {
        for (unsigned int x = 0u; x < n_voxels_x_dim_; ++x) {
          for (unsigned int y = 0u; y < n_voxels_y_dim_; ++y) {
            for (unsigned int z = 0u; z < n_voxels_z_dim_; ++z) {
                if (reconstructions[i].container[x][y][z] >= reconstruction_threshold) {
                  point.x = point_min.x + scale.x *
                    (static_cast<float>(x) - x_dim_min_1_ / 2.0 + centroid.x) / x_dim_min_1_;
                  point.y = point_min.y + scale.y *
                    (static_cast<float>(y) - y_dim_min_1_ / 2.0 + centroid.y) / y_dim_min_1_;
                  point.z = point_min.z + scale.z *
                    (static_cast<float>(z) - z_dim_min_1_ / 2.0 + centroid.z) / z_dim_min_1_;
                  reconstruction.points.push_back(point);
                }
            }
          }
        }
      } else {
                const unsigned int n_voxels_in_original_segment = nums_occupied_voxels[i];
      const unsigned int n_voxels_in_reconstructed_segment =
        floor(double(n_voxels_in_original_segment) * ratio_voxels_to_reconstruct);

      // Order by occupancy probability.
      std::vector<double> probs;
      std::vector<PclPoint> indices;
      for (unsigned int x = 0u; x < n_voxels_x_dim_; ++x) {
        for (unsigned int y = 0u; y < n_voxels_y_dim_; ++y) {
          for (unsigned int z = 0u; z < n_voxels_z_dim_; ++z) {
              probs.push_back(reconstructions[i].container[x][y][z]);
              PclPoint indice;
              indice.x = x;
              indice.y = y;
              indice.z = z;
              indices.push_back(indice);
          }
        }
      }
      std::vector<size_t> indexes_in_decreasing_order = getIndexesInDecreasingOrdering(probs);

      for (unsigned int j = 0u; j < n_voxels_in_reconstructed_segment; ++j){
          point.x = point_min.x + scale.x *
          (static_cast<float>(indices[indexes_in_decreasing_order[j]].x) - x_dim_min_1_ / 2.0 + centroid.x) / x_dim_min_1_;
          point.y = point_min.y + scale.y *
          (static_cast<float>(indices[indexes_in_decreasing_order[j]].y) - y_dim_min_1_ / 2.0 + centroid.y) / y_dim_min_1_;
          point.z = point_min.z + scale.z *
          (static_cast<float>(indices[indexes_in_decreasing_order[j]].z) - z_dim_min_1_ / 2.0 + centroid.z) / z_dim_min_1_;
          reconstruction.points.push_back(point);
      }
      }

      reconstruction.width = 1;
      reconstruction.height = reconstruction.points.size();

      Eigen::Affine3f transform = Eigen::Affine3f::Identity();
      transform.rotate(Eigen::AngleAxisf(-alignments_rad[i], Eigen::Vector3f::UnitZ()));
      pcl::transformPointCloud(reconstruction, reconstruction, transform);
      // segment->getLastView().reconstruction = reconstruction;

      // TODO RD remove if compressing reconstruction not needed.
      /*unsigned int publish_every_x_points = 5;
      segment->getLastView().reconstruction_compressed.clear();
      segment->getLastView().reconstruction_compressed.reserve(reconstruction.points.size() / publish_every_x_points);
      unsigned int z = 0;
      for (const auto& point : reconstruction.points) {
        if (z % publish_every_x_points == 0) {
            segment->getLastView().reconstruction_compressed.points.emplace_back(point.x, point.y, point.z);
        }
        ++z;
      }*/
    }
    BENCHMARK_STOP("SM.Worker.Describe.SaveFeatures");
  }
}

void CNNDescriptor::exportData() const {
  if (save_debug_data_) {
    database::exportSegmentsAndFeatures("/tmp/aligned_segments",
                                        aligned_segments_, true);
  }
}

} // namespace segmatch