//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_DETAIL_IMPLODER_HPP
#define CRYPTO3_DETAIL_IMPLODER_HPP

#include <nil/crypto3/detail/stream_endian.hpp>
#include <nil/crypto3/detail/unbounded_shift.hpp>

#include <boost/static_assert.hpp>

#include <climits>
#include <cstring>

namespace nil {
    namespace crypto3 {
        namespace detail {

            // By definition, for all imploders, InputBits < OutputBits,
            // so we're taking many smaller values and combining them into one value

            template<typename Endianness, int InputBits, int OutputBits, int k>
            struct imploder_step;

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_step<stream_endian::big_unit_big_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutputValue>
                static void step(InputValue z, OutputValue &x) {
                    int const shift = OutputBits - (InputBits + k);
                    OutputValue y = low_bits<InputBits>(OutputValue(z));
                    x |= unbounded_shl<shift>(y);
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_step<stream_endian::little_unit_big_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutputValue>
                static void step(InputValue z, OutputValue &x) {
                    int const kb = (k % UnitBits);
                    int const ku = k - kb;
                    int const shift = InputBits >= UnitBits  ? k :
                                      OutputBits >= UnitBits ? ku + (UnitBits - (InputBits + kb)) :
                                                               OutputBits - (InputBits + kb);
                    OutputValue y = low_bits<InputBits>(OutputValue(z));
                    x |= unbounded_shl<shift>(y);
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_step<stream_endian::big_unit_little_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutputValue>
                static void step(InputValue z, OutputValue &x) {
                    int const kb = (k % UnitBits);
                    int const ku = k - kb;
                    int const shift = InputBits >= UnitBits  ? OutputBits - (InputBits + k) :
                                      OutputBits >= UnitBits ? OutputBits - (UnitBits + ku) + kb :
                                                               kb;
                    OutputValue y = low_bits<InputBits>(OutputValue(z));
                    x |= unbounded_shl<shift>(y);
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_step<stream_endian::little_unit_little_bit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutputValue>
                static void step(InputValue z, OutputValue &x) {
                    int const shift = k;
                    OutputValue y = low_bits<InputBits>(OutputValue(z));
                    x |= unbounded_shl<shift>(y);
                }
            };

            template<int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder_step<stream_endian::host_unit<UnitBits>, InputBits, OutputBits, k> {
                template<typename InputValue, typename OutputValue>
                static void step(InputValue z, OutputValue &x) {
                    BOOST_STATIC_ASSERT(sizeof(InputValue) * CHAR_BIT == InputBits);
                    BOOST_STATIC_ASSERT(sizeof(OutputValue) * CHAR_BIT == OutputBits);
                    std::memcpy((char *)&x + k / CHAR_BIT, &z, InputBits / CHAR_BIT);
                }
            };

            template<typename Endianness, int InputBits, int OutputBits, int k = 0>
            struct imploder;

            template<template<int> class Endian, int UnitBits, int InputBits, int OutputBits, int k>
            struct imploder<Endian<UnitBits>, InputBits, OutputBits, k> {

                // To keep the implementation managable, input and output sizes must
                // be multiples or factors of the unit size.
                // If one of these is firing, you may want a bit-only stream_endian
                // rather than one that mentions bytes or octets.
                BOOST_STATIC_ASSERT(!(InputBits % UnitBits && UnitBits % InputBits));
                BOOST_STATIC_ASSERT(!(OutputBits % UnitBits && UnitBits % OutputBits));

                typedef Endian<UnitBits> Endianness;
                typedef imploder_step<Endianness, InputBits, OutputBits, k> step_type;
                typedef imploder<Endianness, InputBits, OutputBits, k + InputBits> next_type;

                template<typename InIter, typename OutputValue>
                static void implode(InIter &in, OutputValue &x) {
                    step_type::step(*in++, x);
                    next_type::implode(in, x);
                }
            };

            template<template<int> class Endian, int UnitBits, int InputBits, int OutputBits>
            struct imploder<Endian<UnitBits>, InputBits, OutputBits, OutputBits> {
                template<typename InIter, typename OutputValue>
                static void implode(InIter &, OutputValue &) {
                }
            };

        }    // namespace detail
    }        // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_BLOCK_DETAIL_IMPLODER_HPP


// Dynamics used for integrating
#ifndef DYNAMICS_H
#define DYNAMICS_H

#include "segway_sim/common.hpp"
#include <Eigen/Dense>
#include <math.h>

static const double model[15] = {44.798,            //mb
                                 2.485,             //mw
                                 0.055936595310797, //Jw
                                 -0.02322718759275, //a2
                                 0.166845864363019, //c2
                                 3.604960049044268, //A2
                                 3.836289730154863, //B2
                                 1.069672194414735, //C2
                                 1.261650363363571, //K
                                 0.195,             //r
                                 0.5,               //L
                                 9.81,              //gGravity
                                 0.,                //FricCoeff 3.185188257847262
                                 1.0e-3,            //velEps
                                 1.225479467549329  //FricCoeff 1.225479467549329
                                 };

void dynamics(const double t,
              const double X[STATE_LENGTH],
              const double U[INPUT_LENGTH],
                    double xDot[STATE_LENGTH])
{
	double g[STATE_LENGTH*INPUT_LENGTH];
	double Fric;
	double a_tmp;
	double b_a_tmp;
	double f_tmp;
	double b_f_tmp;
	double c_f_tmp;
	double d_f_tmp;
	double e_f_tmp;
	double f_f_tmp;
	double g_f_tmp;
	double h_f_tmp;
	double i_f_tmp;
	double j_f_tmp;
	double k_f_tmp;
	double l_f_tmp;
	double m_f_tmp;
	double n_f_tmp;
	double o_f_tmp;
	double p_f_tmp;
	double q_f_tmp;
	double r_f_tmp;
	double s_f_tmp;
	double t_f_tmp;
	double u_f_tmp;
	double v_f_tmp;
	double f_tmp_tmp;
	double b_f_tmp_tmp;
	double w_f_tmp;
	double x_f_tmp;

/*  */
	Fric = X[3] - X[6] * model[9];
	Fric = model[12] * tanh(Fric / model[13]) + model[14] * Fric;
	a_tmp = cos(X[5]);
	b_a_tmp = sin(X[5]);
	xDot[0] = X[3] * cos(X[2]);
	xDot[1] = X[3] * sin(X[2]);
	xDot[2] = X[4];
	f_tmp = model[3] * model[3];
	b_f_tmp = model[9] * model[9];
	c_f_tmp = model[4] * model[4];
	d_f_tmp = model[0] * model[0];
	e_f_tmp = 4.0 * f_tmp;
	f_f_tmp = 4.0 * c_f_tmp;
	g_f_tmp = X[4] * X[4];
	h_f_tmp = X[6] * X[6];
	i_f_tmp = 4.0 * h_f_tmp + 3.0 * g_f_tmp;
	j_f_tmp = cos(2.0 * X[5]);
	k_f_tmp = cos(3.0 * X[5]);
	l_f_tmp = pow(model[3], 3.0);
	m_f_tmp = 4.0 * model[6] * model[4] * model[0];
	n_f_tmp = pow(model[4], 3.0);
	o_f_tmp = sin(2.0 * X[5]);
	p_f_tmp = model[5] * model[4] * model[0] * model[9] * g_f_tmp;
	q_f_tmp = -model[4] * model[7] * model[0] * model[9] * g_f_tmp;
	r_f_tmp = sin(3.0 * X[5]);
	s_f_tmp = 3.0 * f_tmp * model[4] * d_f_tmp * model[9] * g_f_tmp;
	t_f_tmp = -4.0 * model[3] * model[4];
	u_f_tmp = 2.0 * model[3] * model[4];
	v_f_tmp = f_tmp * d_f_tmp;
	f_tmp_tmp = v_f_tmp * b_f_tmp;
	b_f_tmp_tmp = c_f_tmp * d_f_tmp * b_f_tmp;
	f_f_tmp = 1.0 / ((((((((((4.0 * model[6] * model[2] + e_f_tmp * model[2] *
		model[0]) + f_f_tmp * model[2] * model[0]) + 2.0 * model[6] * model[0] *
	b_f_tmp) + f_tmp_tmp) + b_f_tmp_tmp) + 4.0 * model[6] * model[1] * b_f_tmp)
	+ e_f_tmp * model[0] * model[1] * b_f_tmp) + f_f_tmp *
	model[0] * model[1] * b_f_tmp) + (f_tmp + -c_f_tmp) *
	d_f_tmp * b_f_tmp * j_f_tmp) + u_f_tmp * d_f_tmp * b_f_tmp *
	o_f_tmp);
	w_f_tmp = 2.0 * f_tmp;
	xDot[3] = 0.5 * model[9] * f_f_tmp * (((((((((((((((((((((((-8.0 * model[6] *
		Fric + -8.0 * f_tmp * Fric * model[0]) + -8.0 * c_f_tmp * Fric * model[0]) +
	model[0] * model[9] * ((((-8.0 * model[4] * Fric + model[3] * (-model[5] +
		model[7]) * g_f_tmp) + 4.0 * model[3] * model[6] * (h_f_tmp + g_f_tmp)) +
	l_f_tmp * model[0] * i_f_tmp) + model[3] * c_f_tmp * model[0] * i_f_tmp) *
	a_tmp) + t_f_tmp * model[11] * d_f_tmp * model[9] * j_f_tmp) + model[3] *
	model[5] * model[0] * model[9] * g_f_tmp * k_f_tmp) + -model[3] * model[7] *
	model[0] * model[9] * g_f_tmp * k_f_tmp) + l_f_tmp * d_f_tmp * model[9] *
	g_f_tmp * k_f_tmp) + -3.0 * model[3] * c_f_tmp * d_f_tmp * model[9] *
	g_f_tmp * k_f_tmp) + 8.0 * model[3] * Fric * model[0] * model[9] * b_a_tmp)
	+ m_f_tmp * h_f_tmp * model[9] * b_a_tmp) + e_f_tmp * model[4] * d_f_tmp *
	h_f_tmp * model[9] * b_a_tmp) + 4.0 * n_f_tmp * d_f_tmp * h_f_tmp * model[9]
	* b_a_tmp) + p_f_tmp * b_a_tmp) + m_f_tmp * model[9] * g_f_tmp * b_a_tmp) +
	q_f_tmp * b_a_tmp) + s_f_tmp * b_a_tmp) + 3.0 * n_f_tmp * d_f_tmp * model[9]
	* g_f_tmp * b_a_tmp) + w_f_tmp * model[11] * d_f_tmp * model[9] * o_f_tmp) +
	-2.0 * c_f_tmp * model[11] * d_f_tmp * model[9] * o_f_tmp) + p_f_tmp *
	r_f_tmp) + q_f_tmp * r_f_tmp) + s_f_tmp * r_f_tmp) + -n_f_tmp * d_f_tmp *
	model[9] * g_f_tmp * r_f_tmp);
	e_f_tmp = model[10] * model[10];
	i_f_tmp = -2.0 * f_tmp;
	k_f_tmp = 2.0 * c_f_tmp;
	l_f_tmp = i_f_tmp * model[0];
	m_f_tmp = k_f_tmp * model[0];
	n_f_tmp = f_tmp * model[0];
	c_f_tmp *= model[0];
	p_f_tmp = model[4] * model[0];
	q_f_tmp = model[2] * e_f_tmp;
	e_f_tmp *= model[1];
	r_f_tmp = 2.0 * (model[7] + n_f_tmp);
	s_f_tmp = 2.0 * (model[5] + c_f_tmp);
	u_f_tmp *= model[0];
	xDot[4] = b_f_tmp * X[4] * ((-2.0 * model[3] * model[0] * X[3] * a_tmp + t_f_tmp *
		model[0] * X[6] * j_f_tmp) + -2.0 * (p_f_tmp * X[3] + (((model[5] + -model[7])
			+ l_f_tmp) + m_f_tmp) * X[6] * a_tmp) * b_a_tmp) * (1.0 / ((((q_f_tmp +
				e_f_tmp * b_f_tmp) + r_f_tmp * b_f_tmp * (a_tmp * a_tmp)) + s_f_tmp *
			b_f_tmp * (b_a_tmp * b_a_tmp)) + u_f_tmp * b_f_tmp * o_f_tmp));
			xDot[5] = X[6];
			t_f_tmp = 4.0 * model[4] * model[11];
			k_f_tmp = k_f_tmp * model[2] * model[0];
			m_f_tmp = m_f_tmp * model[1] * b_f_tmp;
			x_f_tmp = -(model[4] * model[4]) * d_f_tmp;
			xDot[6] = f_f_tmp * ((((((((((((((((((((8.0 * Fric * model[2] + 4.0 * Fric *
				model[0] * b_f_tmp) + 8.0 * Fric * model[1] * b_f_tmp) + 2.0 * model[0] *
			(2.0 * model[4] * Fric * model[9] + model[3] * model[11] * (2.0 * model[2] +
				(model[0] + 2.0 * model[1]) * b_f_tmp)) * a_tmp) + -2.0 * model[3] * model[4]
			* model[0] * (model[0] * h_f_tmp * b_f_tmp + -2.0 * (model[2] + model[1] *
				b_f_tmp) * g_f_tmp) * j_f_tmp) + t_f_tmp * model[2] * model[0] * b_a_tmp) +
			-4.0 * model[3] * Fric * model[0] * model[9] * b_a_tmp) + 2.0 * model[4] *
			model[11] * d_f_tmp * b_f_tmp * b_a_tmp) + t_f_tmp * model[0] * model[1] *
			b_f_tmp * b_a_tmp) + v_f_tmp * h_f_tmp * b_f_tmp * o_f_tmp) + x_f_tmp *
			h_f_tmp * b_f_tmp * o_f_tmp) + -2.0 * model[5] * model[2] * g_f_tmp *
			o_f_tmp) + 2.0 * model[7] * model[2] * g_f_tmp * o_f_tmp) + i_f_tmp * model
			[2] * model[0] * g_f_tmp * o_f_tmp) + k_f_tmp * g_f_tmp * o_f_tmp) + -model
			[5] * model[0] * b_f_tmp * g_f_tmp * o_f_tmp) + model[7] * model[0] *
			b_f_tmp * g_f_tmp * o_f_tmp) + -2.0 * model[5] * model[1]
			* b_f_tmp * g_f_tmp * o_f_tmp) + 2.0 * model[7] * model[1]
			* b_f_tmp * g_f_tmp * o_f_tmp) + l_f_tmp * model[1] *
			b_f_tmp * g_f_tmp * o_f_tmp) + m_f_tmp * g_f_tmp * o_f_tmp);
			t_f_tmp = x_f_tmp * b_f_tmp;
			l_f_tmp = (((((((2.0 * model[6] * model[2] + w_f_tmp * model[2] * model[0]) +
				k_f_tmp) + model[6] * model[0] * b_f_tmp) + f_tmp_tmp) +
			b_f_tmp_tmp) + 2.0 * model[6] * model[1] * b_f_tmp) + w_f_tmp *
			model[0] * model[1] * b_f_tmp) + m_f_tmp;
			j_f_tmp = -f_tmp * d_f_tmp * b_f_tmp;
			i_f_tmp = model[3] * model[4] * d_f_tmp * b_f_tmp * o_f_tmp;
			g_f_tmp = p_f_tmp * model[9] * a_tmp;
			h_f_tmp = -model[3] * model[0] * model[9] * b_a_tmp;
			Fric = model[8] * model[9] * ((((model[6] + n_f_tmp) + c_f_tmp) + g_f_tmp) +
				h_f_tmp);
			g[3] = Fric * (1.0 / (((l_f_tmp + t_f_tmp * (a_tmp * a_tmp)) + j_f_tmp *
				(b_a_tmp * b_a_tmp)) + i_f_tmp));
			g[10] = Fric * (1.0 / (((l_f_tmp + t_f_tmp * (a_tmp * a_tmp)) + j_f_tmp *
				(b_a_tmp * b_a_tmp)) + i_f_tmp));
			t_f_tmp = r_f_tmp * model[9];
			l_f_tmp = q_f_tmp * (1.0 / model[9]) + e_f_tmp * model[9];
			j_f_tmp = s_f_tmp * model[9];
			i_f_tmp = u_f_tmp * model[9] * o_f_tmp;
			g[4] = -model[8] * model[10] * (1.0 / (((l_f_tmp + t_f_tmp * (a_tmp * a_tmp))
				+ j_f_tmp * (b_a_tmp * b_a_tmp)) + i_f_tmp));
			g[11] = model[8] * model[10] * (1.0 / (((l_f_tmp + t_f_tmp * (a_tmp * a_tmp))
				+ j_f_tmp * (b_a_tmp * b_a_tmp)) + i_f_tmp));
			g[0] = 0.0;
			g[1] = 0.0;
			g[2] = 0.0;
			g[5] = 0.0;
			g[7] = 0.0;
			g[8] = 0.0;
			g[9] = 0.0;
			g[12] = 0.0;
			t_f_tmp = -2.0 * model[8] * ((((2.0 * model[2] + model[0] * b_f_tmp) + 2.0 *
				model[1] * b_f_tmp) + g_f_tmp) + h_f_tmp) * f_f_tmp;
			g[6] = t_f_tmp;
			g[13] = t_f_tmp;

			for(int i=0; i<STATE_LENGTH; i++)
			{
				for(int j=0; j<INPUT_LENGTH; j++)
				{
					xDot[i]+=g[i+j*STATE_LENGTH]*U[j];
				}
			}
		}

#endif


#include <string>
#include <vector>
#include <map>
#include <stdexcept>
#include <math.h>
#include <algorithm>
#include <cctype>

#include <fstream>
#include <iostream>
#include <sstream>
#include <iterator>

#include "thermodynamics.h"
#include "unitsets.h"

#include <Eigen/Dense>

using namespace std;
using namespace Thermodynamics::Types;

using namespace Thermodynamics::UOM::SI;

namespace Thermodynamics
{

    void Database::scan_database_file()
    {
        ifstream infile;
        infile.open(this->filename);
        if (infile.fail())
        { // oops the file did not exist for reading?
            cout << "Opening Property database file " << this->filename << " failed." << endl;
            exit(1);
        }

        string line;
        while (getline(infile, line))
        { //read data from file object and put it into string.
            std::istringstream iss(line);
            std::vector<std::string> results((std::istream_iterator<std::string>(iss)),
                                             std::istream_iterator<std::string>());

            if (results[0] == "SYST")
            {
                auto maxComp = stoi(results[3]);
                this->NC = maxComp;
                for (int i = 0; i < maxComp; i++)
                {
                    this->known_components.push_back(Substance());
                }
            }
            if (results[0] == "SHOR")
            {
                auto index = stoi(results[1]) - 1;
                this->known_components[index].identifier = results[2];
                this->component_names.push_back(results[2]);
            }
            if (results[0] == "NAME")
            {
                auto index = stoi(results[1]) - 1;
                this->known_components[index].name = results[2];
            }
            if (results[0] == "CASN")
            {
                auto index = stoi(results[1]) - 1;
                this->known_components[index].casNo = results[2];
            }

            if (results[0] == "MOLW")
            {
                auto index = stoi(results[1]) - 1;
                auto value = stod(results[3]);
                this->known_components[index].constants.insert(
                    {MolecularProperties::MolarWeight,
                     Quantity(results[0], results[0], value, kg / kmol)});
            }

            if (results[0] == "PC")
            {
                auto index = stoi(results[1]) - 1;
                auto value = stod(results[3]);
                this->known_components[index].constants.insert(
                    {MolecularProperties::CriticalPressure,
                     Quantity(results[0], results[0], value, Pa)});
            }
            if (results[0] == "TC")
            {
                auto index = stoi(results[1]) - 1;
                auto value = stod(results[3]);
                this->known_components[index].constants.insert(
                    {MolecularProperties::CriticalTemperature,
                     Quantity(results[0], results[0], value, K)});
            }
            if (results[0] == "AC")
            {
                auto index = stoi(results[1]) - 1;
                auto value = stod(results[3]);
                this->known_components[index].constants.insert(
                    {MolecularProperties::AcentricFactor,
                     Quantity(results[0], results[0], value, none)});
            }

            if (results[0] == "VP")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::VaporPressure;
                this->known_components[index].functions.insert(
                    {PureProperties::VaporPressure,
                     function});
            }

            if (results[0] == "CPID")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::IdealGasHeatCapacity;
                this->known_components[index].functions.insert(
                    {PureProperties::IdealGasHeatCapacity,
                     function});
            }
            if (results[0] == "HVAP")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::HeatOfVaporization;
                this->known_components[index].functions.insert(
                    {PureProperties::HeatOfVaporization,
                     function});
            }
            if (results[0] == "DENL")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::LiquidDensity;
                this->known_components[index].functions.insert(
                    {PureProperties::LiquidDensity,
                     function});
            }
            if (results[0] == "ST")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::SurfaceTension;
                this->known_components[index].functions.insert(
                    {PureProperties::SurfaceTension,
                     function});
            }
            if (results[0] == "CL")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::LiquidHeatCapacity;
                this->known_components[index].functions.insert(
                    {PureProperties::LiquidHeatCapacity,
                     function});
            }

            if (results[0] == "KLIQ")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::LiquidHeatConductivity;
                this->known_components[index].functions.insert(
                    {PureProperties::LiquidHeatConductivity,
                     function});
            }
            if (results[0] == "KVAP")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::VaporHeatConductivity;
                this->known_components[index].functions.insert(
                    {PureProperties::VaporHeatConductivity,
                     function});
            }
            if (results[0] == "VISL")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::LiquidViscosity;
                this->known_components[index].functions.insert(
                    {PureProperties::LiquidViscosity,
                     function});
            }
            if (results[0] == "VISV")
            {
                auto index = stoi(results[1]) - 1;
                auto function = this->parse_function(infile, results);
                function.property = PureProperties::VaporViscosity;
                this->known_components[index].functions.insert(
                    {PureProperties::VaporViscosity,
                     function});
            }
            if (results[0] == "LVEQ")
            {
                if (results[2] == "NRTL")
                {
                    this->binaryparameters.insert({"NRTL", BinaryParameterSet("NRTL", this->NC)});
                }
            }

            if (results[0] == "NRTL")
            {
                auto matrix = results[1];
                auto i = stoi(results[2]) - 1;
                auto j = stoi(results[3]) - 1;
                auto kij = stod(results[4]);
                auto kji = stod(results[5]);

                this->binaryparameters.at("NRTL").set_value(matrix, i, j, kij);
                this->binaryparameters.at("NRTL").set_value(matrix, j, i, kji);
            }
        }
        std::cout.flush();
        infile.close();
    }

    PureFunction Database::parse_function(ifstream &infile, vector<string> &results)
    {
        auto function = PureFunction();

        function.tmin = stod(results[5]);
        function.tmax = stod(results[6]);

        string paramline;
        getline(infile, paramline);
        std::istringstream iss2(paramline);
        std::vector<std::string> parameters((std::istream_iterator<std::string>(iss2)), std::istream_iterator<std::string>());

        function.c = Eigen::VectorXd(parameters.size());

        for (size_t i = 0; i < parameters.size(); i++)
        {
            /* code */
            function.c(i) = stod(parameters[i]);
        }

        function.xUnit= SI::K;
        if (Thermodynamics::Types::NameToCorrelation.count(results[3]) > 0)
            function.correlation = Thermodynamics::Types::NameToCorrelation[results[3]];
        else
            function.correlation = Thermodynamics::Types::PureCorrelations::None;

        return function;
    }

    Substance Database::find_component(std::string name)
    {

        for (auto comp : this->known_components)
        {
            if (comp.identifier == name || comp.name == name)
                return comp;
        }

        Substance comp;
        comp.name = "ERROR";
        comp.identifier = "ERROR";
        return comp;
    }

    void Database::fill_binary_parameters(ThermodynamicSystem *system)
    {
        auto nrtlsys = BinaryParameterSet("NRTL", system->NC);
        auto nrtldb = this->binaryparameters.at("NRTL");
        auto matrices = {"A", "B", "C", "D", "E", "F"};

        for (int i = 0; i < system->NC; i++)
        {
            for (int j = 0; j < system->NC; j++)
            {
                if (i == j)
                    continue;

                auto name_i = system->substances[i].identifier;
                auto name_j = system->substances[j].identifier;

                auto idb = find(this->component_names.begin(), this->component_names.end(), name_i);
                int ii = distance(this->component_names.begin(), idb);
                auto jdb = find(this->component_names.begin(), this->component_names.end(), name_j);
                int jj = distance(this->component_names.begin(), jdb);

                for (auto matrix : matrices)
                {
                    auto aij = nrtldb.get_value(matrix, ii, jj);
                    auto aji = nrtldb.get_value(matrix, jj, ii);
                    nrtlsys.set_value(matrix, i, j, aij);
                    nrtlsys.set_value(matrix, j, i, aji);
                }
            }
            system->binaryparameters.insert({"NRTL", nrtlsys});
        }
    }

    std::vector<string> Database::get_component_list()
    {
        return this->component_names;
    }

} // namespace Thermodynamics


/*=============================================================================
    Phoenix V1.2.1
    Copyright (c) 2001-2003 Joel de Guzman

    Use, modification and distribution is subject to the Boost Software
    License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt)
==============================================================================*/
#include <iostream>
#include <cmath>

#define PHOENIX_LIMIT 15
#include <boost/spirit/include/phoenix1_primitives.hpp>
#include <boost/spirit/include/phoenix1_composite.hpp>
#include <boost/spirit/include/phoenix1_functions.hpp>
#include <boost/spirit/include/phoenix1_operators.hpp>

#include <boost/core/lightweight_test.hpp>

using namespace phoenix;
using namespace std;

    ///////////////////////////////////////////////////////////////////////////////
    struct test_ {

        typedef void result_type;
        void operator()() const { cout << "TEST LAZY FUNCTION\n"; }
    };

    function<test_> test;

    ///////////////////////////////////////////////////////////////////////////////
    struct sqr_ {

        template <typename ArgT>
        struct result { typedef ArgT type; };

        template <typename ArgT>
        ArgT operator()(ArgT n) const { return n * n; }
    };

    function<sqr_> sqr;

    ///////////////////////////////////////////////////////////////////////////////
    struct fact_ {

        template <typename ArgT>
        struct result { typedef ArgT type; };

        template <typename ArgT>
        ArgT operator()(ArgT n) const
        { return (n <= 0) ? 1 : n * this->operator()(n-1); }
    };

    function<fact_> fact;

    ///////////////////////////////////////////////////////////////////////////////
    struct pow_ {

        template <typename Arg1T, typename Arg2T>
        struct result { typedef Arg1T type; };

        template <typename Arg1T, typename Arg2T>
        Arg1T operator()(Arg1T a, Arg2T b) const { return pow(a, b); }
    };

    function<pow_> power;

///////////////////////////////////////////////////////////////////////////////
int
main()
{
    int     i5 = 5;
    double  d5 = 5, d3 = 3;

///////////////////////////////////////////////////////////////////////////////
//
//  Lazy functors
//
///////////////////////////////////////////////////////////////////////////////

    test()();
    BOOST_TEST(sqr(arg1)(i5) == (i5*i5));
    BOOST_TEST(fact(4)() == 24);
    BOOST_TEST(fact(arg1)(i5) == 120);
    BOOST_TEST((int)power(arg1, arg2)(d5, d3) == (int)pow(d5, d3));
    BOOST_TEST((sqr(arg1) + 5)(i5) == ((i5*i5)+5));

///////////////////////////////////////////////////////////////////////////////
//
//  End asserts
//
///////////////////////////////////////////////////////////////////////////////

    return boost::report_errors();
}


#include <cassert>
#include <exception>
#include <iostream>
#include <cstdint>

#include <boost/safe_numerics/safe_integer.hpp>

int main()
{
	std::cout << "Using safe numerics" << std::endl;
    try{
        using namespace boost::safe_numerics;
        safe<int> x = INT_MAX - 5;
        ++x;
    }
    catch(const std::exception & e){
        std::cout << e.what() << std::endl;
        std::cout << "error detected!" << std::endl;
    }

    return 0;
}


// It is extremely important to use this header
// if you are using the numpy_eigen interface
#include <sparse_block_matrix/sparse_block_matrix.h>
#include <Eigen/Dense>
#include <numpy_eigen/boost_python_headers.hpp>

using namespace boost::python;
using namespace sparse_block_matrix;

template <typename SBM>
void clearSBM(SBM* sbm) {
    sbm->clear();
}

template <typename SBM>
typename SBM::SparseMatrixBlock blockSBM(const SBM* sbm, int r, int c) {
    if (r < 0 || r >= sbm->bRows()) throw std::runtime_error("Row index out of bounds");
    if (c < 0 || c >= sbm->bCols()) throw std::runtime_error("Column index out of bounds");

    const typename SBM::SparseMatrixBlock* mat = sbm->block(r, c);
    if (mat) {
        return *mat;
    } else {
        typename SBM::SparseMatrixBlock rval;
        rval.resize(sbm->rowsOfBlock(r), sbm->colsOfBlock(c));
        rval.setZero();
        return rval;
    }
}

template <typename SBM>
void setBlockSBM(SBM* sbm, int r, int c, const typename SBM::SparseMatrixBlock& mat) {
    if (r < 0 || r >= sbm->bRows()) throw std::runtime_error("Row index out of bounds");
    if (c < 0 || c >= sbm->bCols()) throw std::runtime_error("Column index out of bounds");

    typename SBM::SparseMatrixBlock* blk = sbm->block(r, c, true);
    if (!blk) throw std::runtime_error("Unexpected error: block is null");

    *blk = mat;
}

template <typename SBM>
Eigen::MatrixXd toDenseSymmetric(SBM* sbm) {
    // return sbm->toDense().selfadjointView<Eigen::Upper>();
    return sbm->toDense().template selfadjointView<Eigen::Upper>();
}

template <typename SBM>
double at(SBM* sbm, int r, int c) {
    return (*sbm)(r, c);
}

void exportSBM() {
    typedef SparseBlockMatrix<Eigen::MatrixXd> mat_t;

    boost::python::class_<mat_t>("SparseBlockMatrixXd", init<>())
        .def(init<const Eigen::VectorXi&, const Eigen::VectorXi&, bool>(
            ("SparseBlockMatrixXd(rbi, cbi, hasStorage)\n\nrbi:  a vector of ints containing the row layout of the "
             "blocks. The component i of the array should contain the index of the first row of the block i+1\ncbi:  a "
             "vector of ints containing the column layout of the blocks. The component i of the array should contain "
             "the index of the first column of the block i+1")))
        .def("rows", &mat_t::rows)
        .def("cols", &mat_t::cols)
        .def("bRows", &mat_t::bRows)
        .def("bCols", &mat_t::bCols)
        .def("clear", &clearSBM<mat_t>)
        .def("clear", &mat_t::clear)
        .def("isBlockSet", &mat_t::isBlockSet)
        .def("getBlock", &blockSBM<mat_t>)
        .def("setBlock", &setBlockSBM<mat_t>)
        .def("rowsOfBlock", &mat_t::rowsOfBlock)
        .def("colsOfBlock", &mat_t::colsOfBlock)
        .def("rowBaseOfBlock", &mat_t::rowBaseOfBlock)
        .def("colBaseOfBlock", &mat_t::colBaseOfBlock)
        .def("nonZeros", &mat_t::nonZeros)
        .def("nonZeroBlocks", &mat_t::nonZeroBlocks)
        .def("clone", &mat_t::clone, return_value_policy<manage_new_object>())
        .def("toDense", &mat_t::toDense)
        .def("toDenseSymmetric", &toDenseSymmetric<mat_t>)
        .def("at", &at<mat_t>)
        // Still more to do here, but this is the easy stuff that will allow us to look
        // at one of these things in Python.
        ;
}


#include "DataSetGenerator.h"
#include <fstream>
#include <Eigen/Dense>

std::vector<LineWrapper> LoadFromFile(const std::string& filename)
{
    std::vector<LineWrapper> result;
    std::ifstream in(filename);
    std::string line;
    while (std::getline(in, line))
    {
        std::istringstream iss(line);
        char sep;
        Eigen::Vector3f m, d;
        if (!(iss >> m.x() >> sep >> m.y()>> sep >> m.z() >> sep >> d.x() >> sep >> d.y() >> sep >> d.z())) { break; } // error

        result.emplace_back(Line(d, m));
    }
    return result;
}

std::vector<LineWrapper> GenerateRandomLines(unsigned int seed, unsigned int lineCount, float maxDist)
{
    std::vector<LineWrapper> lines{};
    lines.reserve(lineCount);

    std::default_random_engine rng{seed};
    std::uniform_real_distribution<float> dist(-maxDist, maxDist);
    for (int i = 0; i < lineCount; ++i) {
        Line l(Vector3f::Zero(), Vector3f::Zero());
        do {
            l = Line::FromTwoPoints(
                    Vector3f(dist(rng), dist(rng), dist(rng)),
                    Vector3f(dist(rng), dist(rng), dist(rng))
            );
        }while(l.m.norm() >= 150);
        lines.emplace_back(l);
    }
    return lines;
}

std::vector<LineWrapper> SampleRandomLines(const std::vector<LineWrapper>& all_lines, unsigned int seed, unsigned int lineCount)
{
    if(lineCount > all_lines.size())
    {
        throw std::runtime_error("Not enough lines in dataset");
    }
    std::vector<LineWrapper> lines = all_lines;
    auto lines_new_end = lines.end();

    std::default_random_engine rng{seed};
    auto nbLinesToRemove = all_lines.size() - lineCount;
    for (std::vector<LineWrapper>::size_type i = 0; i < nbLinesToRemove; ++i) {
        std::uniform_int_distribution<std::vector<LineWrapper>::size_type> rand_i(0, std::distance(lines.begin(), lines_new_end)-1);
        std::iter_swap(lines.begin() + rand_i(rng), lines_new_end-1);
        lines_new_end--;
    }
    lines.erase(lines_new_end, lines.end());
    return lines;
}

std::vector<LineWrapper> GenerateParallelLines(unsigned int seed, unsigned int lineCount, float maxDist, const Vector3f& direction)
{
    std::vector<LineWrapper> lines{};
    lines.reserve(lineCount);

    std::default_random_engine rng{seed};
    std::uniform_real_distribution<float> dist(0, maxDist);
    for (int i = 0; i < lineCount; ++i) {
        Line l(Vector3f::Zero(), Vector3f::Zero());
        do {
            l = Line::FromPointAndDirection(
                    Vector3f(dist(rng), dist(rng), dist(rng)),
                    direction
            );
        }while(l.m.norm() >= 150);
        lines.emplace_back(l);
    }
    return lines;
}

std::vector<LineWrapper> GenerateEquiDistantLines(unsigned int seed, unsigned int lineCount, float distance)
{
    std::vector<LineWrapper> lines{};
    lines.reserve(lineCount);

    std::default_random_engine rng{seed};
    std::uniform_real_distribution<float> dist(0, 1.0);
    for (int i = 0; i < lineCount; ++i) {
        // Generate point p which line runs through
        Vector3f p_hat = Vector3f(dist(rng), dist(rng), dist(rng));
        p_hat.normalize();

        // Generate random directional vector, orthogonal to p
        Vector3f d = Vector3f(dist(rng), dist(rng), dist(rng));
        d -= p_hat * p_hat.dot(d);
        d.normalize();

        Vector3f m = p_hat.cross(d) * distance;
        lines.emplace_back(Line(d, m));
    }
    return lines;
}

std::vector<LineWrapper> GenerateEqualMomentLines(unsigned int seed, unsigned int lineCount, const Vector3f& moment)
{
    std::vector<LineWrapper> lines{};
    lines.reserve(lineCount);

    std::default_random_engine rng{seed};
    std::uniform_real_distribution<float> unitDist(0, 1.0f);

    Vector3f moment_d = moment.normalized();
    Vector3f baseU = Vector3f(unitDist(rng), unitDist(rng), unitDist(rng));
    baseU -= moment_d * moment_d.dot(baseU);
    baseU.normalize();
    Vector3f baseV = baseU.cross(moment_d);

    for (int i = 0; i < lineCount; ++i) {
        float angle = unitDist(rng) * 2.0f * M_PI;
        Vector3f d = std::sin(angle) * baseU + std::cos(angle) * baseV;
        lines.emplace_back(Line(d, moment));
    }
    return lines;
}

std::vector<LineSegmentWrapper> GenerateRandomLineSegments(unsigned int seed, unsigned int lineCount, float maxDist, float minT, float maxT)
{
    std::vector<LineSegmentWrapper> lines{};
    lines.reserve(lineCount);

    std::default_random_engine rng{seed};
    std::uniform_real_distribution<float> dist(0, maxDist);
    std::uniform_real_distribution<float> distT(minT, maxT);
    for (int i = 0; i < lineCount; ++i) {
        LineSegment l(Line(Vector3f::Zero(), Vector3f::Zero()), 0, 0);
        do {
            float t1 = distT(rng);
            float t2 = distT(rng);
            if(t1 > t2)
            {
                std::swap(t1, t2);
            }

            l = LineSegment(Line::FromTwoPoints(
                    Vector3f(dist(rng), dist(rng), dist(rng)),
                    Vector3f(dist(rng), dist(rng), dist(rng))
            ), t1, t2);
        }while(l.l.m.norm() >= 150);
        lines.emplace_back(l);
    }

    return lines;
}

void ModifyLineSegmentLength(std::vector<LineSegmentWrapper>& l, float length)
{
    float half_length = length/2.0f;
    for(auto& s : l)
    {
        float mid = (s.l.t2 - s.l.t1)/2.0f;
        s.l.t1 = mid - half_length;
        s.l.t2 = mid + half_length;
    }
}

std::vector<Vector3f> GenerateRandomPoints(unsigned int seed, unsigned int pointCount, float maxDist)
{
    std::vector<Vector3f> queryPoints{};
    queryPoints.reserve(pointCount);
    std::default_random_engine rng{seed};
    std::uniform_real_distribution<float> dist(0, maxDist);

    for (int query_i = 0; query_i < pointCount; ++query_i) {
        queryPoints.emplace_back(dist(rng), dist(rng), dist(rng));
    }
    return queryPoints;
}

std::vector<Vector3f> GenerateRandomNormals(unsigned int seed, unsigned int normalCount)
{
    std::vector<Vector3f> query_point_normals {};
    query_point_normals.reserve(normalCount);

    std::default_random_engine rng {seed+1};
    std::uniform_real_distribution<float> dist(0, 1);

    for(int i = 0; i < normalCount; ++i)
    {
        query_point_normals.emplace_back(dist(rng), dist(rng), dist(rng));
        query_point_normals.back().normalize();
    }

    return query_point_normals;
}


#include <memory>
#include <array>
#include <bitset>
#include <sstream>
#include <boost/dynamic_bitset.hpp>

#include <gtest/gtest.h>

#include "boost/archive/binary_oarchive.hpp"
#include "boost/archive/binary_iarchive.hpp"

#include "boost/serialization/array.h"
#include <boost/serialization/bitset.hpp>
#include "boost/serialization/dynamic_bitset.h"
#include "boost/serialization/multi_array.h"
#include "boost/serialization/tuple.h"
#include "boost/serialization/continuous_interval.h"

namespace ba = boost::archive;
namespace bs = boost::serialization;
namespace bi = boost::icl;
using namespace std;

template<typename T>
void test_random_accessable_serialization(T& a)
{
	for (size_t ii = 0; ii<a.size(); ++ii)
		a[ii] = ii;

	stringstream os;
	ba::binary_oarchive out(os);

	out << a;
	os.flush();

	ba::binary_iarchive in(os);
	T b;
	in >> b;

	ASSERT_EQ(a, b);
}

TEST(Serialization, Array)
{
	array<int, 42> a;
	test_random_accessable_serialization(a);
}

TEST(Serialization, StdBitset)
{
	bitset<256> a;
	test_random_accessable_serialization(a);
}

TEST(Serialization, DynamicBitset)
{
	boost::dynamic_bitset<> a(256);
	test_random_accessable_serialization(a);
}

TEST(Serialization, Tuple)
{
	typedef std::tuple<int, double, char, std::string> type;
	type a;

	get<0>(a) = 42;
	get<1>(a) = 3.141;
	get<2>(a) = 'D';
	get<3>(a) = std::string("fancy test string");

	stringstream os;
	ba::binary_oarchive out(os);

	out << a;
	os.flush();

	ba::binary_iarchive in(os);
	type b;
	in >> b;

	ASSERT_EQ(a, b);
}

TEST(Serialization, ContinuousInterval)
{
	typedef bi::continuous_interval<double> type;
	type a = bi::construct<type>(3.14, 1024.2048, bi::interval_bounds::closed());

	stringstream os;
	ba::binary_oarchive out(os);

	out << a;
	os.flush();

	ba::binary_iarchive in(os);
	type b;
	in >> b;

	// hacky conversion, avoiding ambiguous overload for 'operator<<'
	std::stringstream str_a, str_b;
	str_a << a;
	str_b << b;
	ASSERT_EQ(str_a.str(), str_b.str());
}


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#define TEST_NAME "estimators/homography_matrix"
#include "estimators/homography_matrix.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "util/testing.h"

using namespace colmap;

BOOST_AUTO_TEST_CASE(TestEstimate) {
  for (double x = 0; x < 10; ++x) {
    Eigen::Matrix3d H0;
    H0 << x, 0.2, 0.3, 30, 0.2, 0.1, 0.3, 20, 1;

    std::vector<Eigen::Vector2d> src;
    src.emplace_back(x, 0);
    src.emplace_back(1, 0);
    src.emplace_back(2, 1);
    src.emplace_back(10, 30);

    std::vector<Eigen::Vector2d> dst;

    for (size_t i = 0; i < 4; ++i) {
      const Eigen::Vector3d dsth = H0 * src[i].homogeneous();
      dst.push_back(dsth.hnormalized());
    }

    HomographyMatrixEstimator est_tform;
    const auto models = est_tform.Estimate(src, dst);

    std::vector<double> residuals;
    est_tform.Residuals(src, dst, models[0], &residuals);

    for (size_t i = 0; i < 4; ++i) {
      BOOST_CHECK(residuals[i] < 1e-6);
    }
  }
}


#include <iostream>
#include <boost/atomic.hpp>
#include <boost/thread.hpp>
#include <boost/interprocess/managed_shared_memory.hpp>
#include <boost/interprocess/sync/named_mutex.hpp>
#include "opencv2/core.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include <opencv2/opencv.hpp>

#include <cuda_runtime.h>

#include "cudaImg.cuh"
#include "cudaUtils.hpp"

#include <cvUtils.hpp>

#include <time.h>

#include <algorithm>

using namespace std;

#define IMAGE_ROWS 480
#define IMAGE_COLS 640

int main() {

    if(!getGPUConfig())
    {
        return 0;
    }

    clocker myTimer;

    cv::Mat gray(IMAGE_ROWS,IMAGE_COLS,CV_8UC1,cv::Scalar(0));
    cv::Mat erode_img(IMAGE_ROWS,IMAGE_COLS,CV_8UC1,cv::Scalar(0));
    cv::Mat hsvRange_img(IMAGE_ROWS,IMAGE_COLS,CV_8UC3,cv::Scalar(0));
    cv::Mat histImg;
    cv::Mat histGray(IMAGE_ROWS,IMAGE_COLS,CV_8UC1,cv::Scalar(0));

    unsigned int hist_cpu[256];
    memset(hist_cpu,0,256* sizeof(unsigned int));

    uchar3* d_in;
    unsigned char* gray_gpu;
    uchar3* hsvRange_gpu;
    unsigned char* erode_img_gpu;

    unsigned int* hist_gpu;

    unsigned int* sum_ni;
    unsigned char* histGray_gpu;

    cudaMalloc((void**)&sum_ni,256* sizeof(unsigned int));
    cudaMalloc((void**)&histGray_gpu,IMAGE_ROWS*IMAGE_COLS*sizeof(unsigned char));

    cudaMalloc((void**)&d_in,IMAGE_ROWS*IMAGE_COLS* sizeof(uchar3));
    cudaMalloc((void**)&gray_gpu,IMAGE_ROWS*IMAGE_COLS* sizeof(unsigned char));

    cudaMalloc((void**)&erode_img_gpu,IMAGE_ROWS*IMAGE_COLS* sizeof(unsigned char));
    cudaMalloc((void**)&hsvRange_gpu,IMAGE_ROWS*IMAGE_COLS* sizeof(uchar3));

    cudaMalloc((void**)&hist_gpu,256* sizeof(unsigned int));

    dim3 threadsPerBlock(32,32);
    dim3 blockPerGrid((IMAGE_COLS+threadsPerBlock.x-1)/threadsPerBlock.x,(IMAGE_ROWS+threadsPerBlock.y-1)/threadsPerBlock.y);

//    cv::VideoCapture cap0(0);

    cv::Mat frame = cv::imread("/home/dasuda/david/cudaCV/imgs/img0.jpg");
    cv::Mat bina_test;

    cv::Mat frame0,frame1;
    while(true)
    {
        //cap>>frame;

        cudaMemcpy(d_in,frame.data,IMAGE_ROWS*IMAGE_COLS * sizeof(uchar3),cudaMemcpyHostToDevice);

//        uchar3 min_hsv={0,0,0},max_hsv={180,255,255};
//        RGB2HSV_gpu(d_in,hsvRange_gpu,IMAGE_ROWS,IMAGE_COLS,min_hsv,max_hsv,threadsPerBlock,blockPerGrid);
//        cudaMemcpy(hsvRange_img.data,hsvRange_gpu,IMAGE_ROWS*IMAGE_COLS * sizeof(uchar3),cudaMemcpyDeviceToHost);

//        myTimer.start();
        RGB2GRAY_gpu(d_in,gray_gpu,IMAGE_ROWS,IMAGE_COLS,threadsPerBlock,blockPerGrid);
//        cudaDeviceSynchronize();
//        myTimer.print_ms_slideTimer("01");
        cudaMemcpy(gray.data,gray_gpu,IMAGE_ROWS*IMAGE_COLS * sizeof(unsigned char),cudaMemcpyDeviceToHost);

//        thresholdBinarization_gpu(gray_gpu,erode_img_gpu,IMAGE_ROWS,IMAGE_COLS,180,200,0,255,threadsPerBlock,blockPerGrid);
//        cudaMemcpy(erode_img.data,erode_img_gpu,IMAGE_ROWS*IMAGE_COLS * sizeof(unsigned char),cudaMemcpyDeviceToHost);

//        sobel_gpu(gray_gpu,erode_img_gpu,IMAGE_ROWS,IMAGE_COLS,threadsPerBlock,blockPerGrid);
//        erode_gpu(gray_gpu,erode_img_gpu,IMAGE_ROWS,IMAGE_COLS,cv::Size(3,3),threadsPerBlock,blockPerGrid);
        cudaDeviceSynchronize();
//        myTimer.start();
//        getHist_gpu(gray_gpu,hist_gpu,threadsPerBlock,blockPerGrid);
//        cudaMemcpy(hist_cpu,hist_gpu,256 * sizeof(unsigned int),cudaMemcpyDeviceToHost);
//        showHistImage(histImg,hist_cpu,256);
        equalizeHist_gpu(gray_gpu,hist_gpu,sum_ni,histGray_gpu,IMAGE_ROWS,IMAGE_COLS,threadsPerBlock,blockPerGrid);
        cudaDeviceSynchronize();
//        myTimer.print_ms_slideTimer("01",1);
//        cudaMemcpy(hist_cpu,sum_ni,256 * sizeof(unsigned int),cudaMemcpyDeviceToHost);
//        cout<<hist_cpu[0]<<endl;
//        cout<<hist_cpu[1]<<endl;
//        cout<<"----------"<<endl;
//        showHistImage(histImg,hist_cpu,256);
        cudaMemcpy(histGray.data,histGray_gpu,IMAGE_ROWS*IMAGE_COLS * sizeof(unsigned char),cudaMemcpyDeviceToHost);

//        ostu_gpu(gray_gpu,erode_img_gpu,hist_gpu,host_sum_Pi,host_sum_i_Pi,host_u_0,host_varance,host_thres,IMAGE_ROWS,IMAGE_COLS);

//        cudaMemcpy(erode_img.data,erode_img_gpu,IMAGE_ROWS*IMAGE_COLS * sizeof(unsigned char),cudaMemcpyDeviceToHost);
//        cudaMemcpy(hist_cpu,hist_gpu,256 * sizeof(unsigned int),cudaMemcpyDeviceToHost);
//        showHistImage(histImg,hist_cpu,256);

//        cudaMemcpy(erode_img.data,erode_img_gpu,IMAGE_ROWS*IMAGE_COLS * sizeof(unsigned char),cudaMemcpyDeviceToHost);

//        myTimer.start();
//        cv::threshold(gray,bina_test,180,255,cv::THRESH_BINARY);
//        cv::threshold(gray,bina_test,180,255,cv::THRESH_BINARY);
//        cv::threshold(gray,bina_test,180,255,cv::THRESH_BINARY);
//        cv::threshold(gray,bina_test,180,255,cv::THRESH_BINARY);
//        cv::threshold(gray,bina_test,180,255,cv::THRESH_BINARY);
//
//        myTimer.print_ms_slideTimer("01");

//        thresholdBinarization_gpu(gray_gpu,erode_img_gpu,IMAGE_ROWS,IMAGE_COLS,180,200,0,255,threadsPerBlock,blockPerGrid);
//        cudaMemcpy(erode_img.data,erode_img_gpu,IMAGE_ROWS*IMAGE_COLS * sizeof(unsigned char),cudaMemcpyDeviceToHost);

//        showHistImage(histImg,hist_cpu,256);

//        cv::cvtColor(frame,gray,cv::COLOR_RGB2GRAY);
//        named_mtx.lock();
//        for (int i = 0; i < 480; ++i) {
//            uchar* ptr_data = gray.ptr<uchar>(i);
//            for (int j = 0; j < 640; ++j) {
//                *(shm_image+i*640+j) = ptr_data[j];
//            }
//        }
//        named_mtx.unlock();

        myTimer.start();
        cv::equalizeHist(gray, bina_test);
        cv::equalizeHist(gray, bina_test);
        cv::equalizeHist(gray, bina_test);
        cv::equalizeHist(gray, bina_test);
        cv::equalizeHist(gray, bina_test);
        myTimer.print_ms_slideTimer("01",5);

        cv::imshow("frame",histGray);
        cv::imshow("bina_test",bina_test);
//        cv::imshow("gray",gray);
//        cv::imshow("hsvRange",hsvRange_img);
//        cv::imshow("erode",erode_img);

        if(cv::waitKey(3)>0)
        {
//            cv::imwrite("/home/dasuda/david/cudaCV/imgs/hist00.jpg",histImg);
//            boost::interprocess::named_mutex::remove("mtx");
            cudaFree(d_in);
            cudaFree(gray_gpu);
            cudaFree(erode_img_gpu);
            break;
        }
    }

}


// GlobalRegistration.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
#include <pcl/common/distances.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include "helper.h"
#include "RansacCurvature.h"

// Types
typedef pcl::PointXYZ PointT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;

std::string dir_name = "";

void do_all( int num )
{
	Configuration config;

	RGBDTrajectory traj;
	RGBDInformation info;
#pragma omp parallel for num_threads( 8 ) schedule( dynamic )
	for ( int i = 0; i < num; i++ ) {
		for ( int j = i + 1; j < num; j++ ) {
			// Load object and scene
			pcl::console::print_highlight( "Betweeen fragments %d and %d.\n", i, j );
			bool smart_swapped = false;

			PointCloudT::Ptr object (new PointCloudT);
			PointCloudT::Ptr object_aligned (new PointCloudT);
			PointCloudT::Ptr scene (new PointCloudT);
			FeatureCloudT::Ptr object_features (new FeatureCloudT);
			FeatureCloudT::Ptr scene_features (new FeatureCloudT);
			pcl::console::print_highlight ("Loading point clouds...\n");

			char filename[ 1024 ];
			sprintf( filename, "%scloud_bin_%d.pcd", dir_name.c_str(), i );
			pcl::io::loadPCDFile<PointNT>( filename, *scene );

			sprintf( filename, "%scloud_bin_%d.pcd", dir_name.c_str(), j );
			pcl::io::loadPCDFile<PointNT>( filename, *object );

			const float leaf = config.resample_leaf_;

			// Downsample
			pcl::console::print_highlight ("Downsampling...\n");
			pcl::VoxelGrid<PointNT> grid;
			grid.setLeafSize (leaf, leaf, leaf);
			grid.setInputCloud (object);
			grid.filter (*object);
			grid.setInputCloud (scene);
			grid.filter (*scene);

			if ( config.smart_swap_ ) {
				if ( object->size() > scene->size() ) {
					PointCloudT::Ptr temp = object;
					object = scene;
					scene = temp;
					smart_swapped = true;
				} else {
					smart_swapped = false;
				}
			}

			if ( config.estimate_normal_ ) {
				PointCloudT::Ptr scene_bak (new PointCloudT);
				pcl::copyPointCloud( *scene, *scene_bak );
				PointCloudT::Ptr object_bak (new PointCloudT);
				pcl::copyPointCloud( *object, *object_bak );

				// Estimate normals for scene
				pcl::console::print_highlight ("Estimating scene normals...\n");
				pcl::NormalEstimationOMP<PointNT,PointNT> nest;
				nest.setRadiusSearch( config.normal_radius_ );
				nest.setInputCloud (scene);
				nest.compute (*scene);
				for ( int i = 0; i < scene->size(); i++ ) {
					if ( scene->points[ i ].normal_x * scene_bak->points[ i ].normal_x
						+ scene->points[ i ].normal_y * scene_bak->points[ i ].normal_y
						+ scene->points[ i ].normal_z * scene_bak->points[ i ].normal_z < 0.0 ) {
						scene->points[ i ].normal_x *= -1;
						scene->points[ i ].normal_y *= -1;
						scene->points[ i ].normal_z *= -1;
					}
				}

				// Estimate normals for object
				pcl::console::print_highlight ("Estimating object normals...\n");
				nest.setRadiusSearch( config.normal_radius_ );
				nest.setInputCloud (object);
				nest.compute (*object);
				for ( int i = 0; i < object->size(); i++ ) {
					if ( object->points[ i ].normal_x * object_bak->points[ i ].normal_x
						+ object->points[ i ].normal_y * object_bak->points[ i ].normal_y
						+ object->points[ i ].normal_z * object_bak->points[ i ].normal_z < 0.0 ) {
						object->points[ i ].normal_x *= -1;
						object->points[ i ].normal_y *= -1;
						object->points[ i ].normal_z *= -1;
					}
				}
			}

			// Estimate features
			pcl::console::print_highlight ("Estimating features...\n");
			FeatureEstimationT fest;
			fest.setRadiusSearch ( config.feature_radius_ );
			fest.setInputCloud (object);
			fest.setInputNormals (object);
			fest.compute (*object_features);
			fest.setInputCloud (scene);
			fest.setInputNormals (scene);
			fest.compute (*scene_features);

			pcl::console::print_highlight ("Starting alignment...\n");
			RansacCurvature<PointNT,PointNT,FeatureT> align;
			align.setInputCloud (object);
			align.setSourceFeatures (object_features);
			align.setInputTarget (scene);
			align.setTargetFeatures (scene_features);
			align.setMaximumIterations ( config.max_iteration_ ); // Number of RANSAC iterations
			align.setNumberOfSamples ( config.num_of_samples_ ); // Number of points to sample for generating/prerejecting a pose
			align.setCorrespondenceRandomness ( config.correspondence_randomness_ ); // Number of nearest features to use
			align.setSimilarityThreshold ( config.edge_similarity_ ); // Polygonal edge length similarity threshold
			align.setMaxCorrespondenceDistance ( config.max_correspondence_distance_ ); // Inlier threshold
			align.setInlierFraction ( config.inlier_fraction_ ); // Required inlier fraction for accepting a pose hypothesis
			align.setInlierNumber( config.inlier_number_ );
			align.setAngleDiff( config.angle_difference_ );
			{
				pcl::ScopeTime t("Alignment");
				align.align (*object_aligned);
			}

			if (align.hasConverged ())
				//if (true)
			{
				align.getInformation();
				if ( config.aux_data_ ) {
					char test[ 1024 ];
					sprintf( test, "test_%d_%d.txt", i, j );
					align.writeAuxData( std::string( test ) );
				}
				Eigen::Matrix4f transformation;
				Eigen::Matrix< double, 6, 6 > information;
				if ( smart_swapped ) {
					transformation = align.getFinalTransformation ().inverse();
					information = align.information_target_;
				} else {
					transformation = align.getFinalTransformation ();
					information = align.information_source_;
				}
#pragma omp critical
				{
					// Print results
					pcl::console::print_info ("\n");
					pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
					pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
					pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
					pcl::console::print_info ("\n");
					pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
					pcl::console::print_info ("\n");
					pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
					traj.data_.push_back( FramedTransformation( i, j, num, transformation.cast< double >() ) );
					info.data_.push_back( FramedInformation( i, j, num, information ) );
				}
			} else {
				pcl::console::print_error ("Alignment failed!\n");
			}
		}
	}
	traj.SaveToFile( "result.txt" );
	info.SaveToFile( "result.info" );
}

int fragment;
RGBDTrajectory segment_traj;
RGBDTrajectory init_traj;
RGBDTrajectory pose_traj;
RGBDTrajectory odometry_traj;
RGBDInformation odometry_info;

void create_init_traj()
{
	init_traj.data_.clear();
	Eigen::Matrix4d base_mat = Eigen::Matrix4d::Identity();
	for ( size_t i = 0; i < segment_traj.data_.size(); i++ ) {
		if ( i % fragment == 0 && i > 0 ) {
			base_mat = init_traj.data_[ i - 1 ].transformation_ * segment_traj.data_[ i ].transformation_.inverse();
		}
		init_traj.data_.push_back( FramedTransformation( i, i, i + 1, base_mat * segment_traj.data_[ i ].transformation_ ) );
	}
	init_traj.SaveToFile( "init.log" );
}

void create_pose_traj()
{
	pose_traj.data_.clear();
	for ( size_t i = 0; i < init_traj.data_.size(); i += fragment ) {
		pose_traj.data_.push_back( FramedTransformation( i / fragment, i / fragment, i / fragment + 1, init_traj.data_[ i ].transformation_ * segment_traj.data_[ 0 ].transformation_.inverse() ) );
	}
	pose_traj.SaveToFile( "pose.log" );
}

void create_odometry( int num )
{
	Configuration config;
	odometry_traj.data_.clear();
	odometry_info.data_.clear();
	for ( int i = 1; i < num; i++ ) {
		odometry_traj.data_.push_back( FramedTransformation( i - 1, i, num, pose_traj.data_[ i - 1 ].transformation_.inverse() * pose_traj.data_[ i ].transformation_ ) );
		pcl::console::print_highlight( "Betweeen fragments %d and %d.\n", i - 1, i );
		PointCloudT::Ptr object (new PointCloudT);
		PointCloudT::Ptr object_aligned (new PointCloudT);
		PointCloudT::Ptr scene (new PointCloudT);
		FeatureCloudT::Ptr object_features (new FeatureCloudT);
		FeatureCloudT::Ptr scene_features (new FeatureCloudT);
		pcl::console::print_highlight ("Loading point clouds...\n");

		char filename[ 1024 ];
		sprintf( filename, "%scloud_bin_%d.pcd", dir_name.c_str(), i - 1 );
		pcl::io::loadPCDFile<PointNT>( filename, *scene );

		sprintf( filename, "%scloud_bin_%d.pcd", dir_name.c_str(), i );
		pcl::io::loadPCDFile<PointNT>( filename, *object );

		const float leaf = config.resample_leaf_;

		// Downsample
		pcl::console::print_highlight ("Downsampling...\n");
		pcl::VoxelGrid<PointNT> grid;
		grid.setLeafSize (leaf, leaf, leaf);
		grid.setInputCloud (object);
		grid.filter (*object);
		grid.setInputCloud (scene);
		grid.filter (*scene);

		if ( config.estimate_normal_ ) {
			PointCloudT::Ptr scene_bak (new PointCloudT);
			pcl::copyPointCloud( *scene, *scene_bak );
			PointCloudT::Ptr object_bak (new PointCloudT);
			pcl::copyPointCloud( *object, *object_bak );

			// Estimate normals for scene
			pcl::console::print_highlight ("Estimating scene normals...\n");
			pcl::NormalEstimationOMP<PointNT,PointNT> nest;
			nest.setRadiusSearch( config.normal_radius_ );
			nest.setInputCloud (scene);
			nest.compute (*scene);
			for ( int i = 0; i < scene->size(); i++ ) {
				if ( scene->points[ i ].normal_x * scene_bak->points[ i ].normal_x
					+ scene->points[ i ].normal_y * scene_bak->points[ i ].normal_y
					+ scene->points[ i ].normal_z * scene_bak->points[ i ].normal_z < 0.0 ) {
					scene->points[ i ].normal_x *= -1;
					scene->points[ i ].normal_y *= -1;
					scene->points[ i ].normal_z *= -1;
				}
			}

			// Estimate normals for object
			pcl::console::print_highlight ("Estimating object normals...\n");
			nest.setRadiusSearch( config.normal_radius_ );
			nest.setInputCloud (object);
			nest.compute (*object);
			for ( int i = 0; i < object->size(); i++ ) {
				if ( object->points[ i ].normal_x * object_bak->points[ i ].normal_x
					+ object->points[ i ].normal_y * object_bak->points[ i ].normal_y
					+ object->points[ i ].normal_z * object_bak->points[ i ].normal_z < 0.0 ) {
					object->points[ i ].normal_x *= -1;
					object->points[ i ].normal_y *= -1;
					object->points[ i ].normal_z *= -1;
				}
			}
		}

		// Estimate features
		pcl::console::print_highlight ("Estimating features...\n");
		FeatureEstimationT fest;
		fest.setRadiusSearch ( config.feature_radius_ );
		fest.setInputCloud (object);
		fest.setInputNormals (object);
		fest.compute (*object_features);
		fest.setInputCloud (scene);
		fest.setInputNormals (scene);
		fest.compute (*scene_features);

		pcl::console::print_highlight ("Starting alignment...\n");
		RansacCurvature<PointNT,PointNT,FeatureT> align;
		align.setInputCloud (object);
		align.setSourceFeatures (object_features);
		align.setInputTarget (scene);
		align.setTargetFeatures (scene_features);
		align.setMaximumIterations ( config.max_iteration_ ); // Number of RANSAC iterations
		align.setNumberOfSamples ( config.num_of_samples_ ); // Number of points to sample for generating/prerejecting a pose
		align.setCorrespondenceRandomness ( config.correspondence_randomness_ ); // Number of nearest features to use
		align.setSimilarityThreshold ( config.edge_similarity_ ); // Polygonal edge length similarity threshold
		align.setMaxCorrespondenceDistance ( config.max_correspondence_distance_ ); // Inlier threshold
		align.setInlierFraction ( config.inlier_fraction_ ); // Required inlier fraction for accepting a pose hypothesis
		align.setInlierNumber( config.inlier_number_ );
		align.setAngleDiff( config.angle_difference_ );
		{
			pcl::ScopeTime t("Alignment");
			//align.align (*object_aligned);
			align.align_redux( *object_aligned, odometry_traj.data_[ i - 1 ].transformation_.cast< float >() );
		}

		align.getInformation();
		odometry_info.data_.push_back( FramedInformation( i - 1, i, num, align.information_source_ ) );
	}
	odometry_traj.SaveToFile( "odometry.log" );
	odometry_info.SaveToFile( "odometry.info" );
}

int main(int argc, char * argv[])
{
	if ( argc < 2 ) {
		cout << "Usage : " << endl;
		cout << "    GlobalRegistration.exe <dir>" << endl;
		cout << "    GlobalRegistration.exe <dir> <100-0.log> <segment_length>" << endl;
		return 0;
	}
	dir_name = std::string( argv[ 1 ] );
	int num_of_pcds =  std::count_if( boost::filesystem::directory_iterator( boost::filesystem::path( dir_name ) ),
		boost::filesystem::directory_iterator(),
		[](const boost::filesystem::directory_entry& e) {
			return e.path().extension() == ".pcd";  }
	);
	cout << num_of_pcds << " detected." << endl << endl;

	if ( argc == 2 ) {
		do_all( num_of_pcds );
	}

	if ( argc == 4 ) {
		segment_traj.LoadFromFile( argv[ 2 ] );
		sscanf( argv[ 3 ], "%d", &fragment );

		create_init_traj();
		create_pose_traj();
		create_odometry( num_of_pcds );

		if ( boost::filesystem::exists( "result.txt" ) ) {
			cout << "result.txt detected. skip global registration." << endl;
		} else {
			do_all( num_of_pcds );
		}
	}

	return 0;
}


#ifndef BOOST_PREPROCESSOR_MIN_HPP
#define BOOST_PREPROCESSOR_MIN_HPP

/* Copyright (C) 2001
 * Housemarque Oy
 * http://www.housemarque.com
 *
 * Permission to copy, use, modify, sell and distribute this software is
 * granted provided this copyright notice appears in all copies. This
 * software is provided "as is" without express or implied warranty, and
 * with no claim as to its suitability for any purpose.
 *
 * See http://www.boost.org for most recent version.
 */

#include <boost/preprocessor/comparison/less_equal.hpp>
#include <boost/preprocessor/if.hpp>

/**
Expands to the minimum of X and Y.

Both X and Y must expand to integer literals
in the range [0, BOOST_PP_LIMIT_MAG].

For example, BOOST_PP_MIN(5,7) expands to 5 (a
single token).

Uses

BOOST_PP_WHILE()

Test

arithmetic_test.cpp

*/
#define BOOST_PP_MIN(X,Y) BOOST_PP_MIN_D(0,X,Y)

/**
Can be used inside BOOST_PP_WHILE().
 */
#define BOOST_PP_MIN_D(D,X,Y) BOOST_PP_IF(BOOST_PP_LESS_EQUAL_D(D,Y,X),Y,X)

/*
Obsolete. Use BOOST_PP_MIN().
 */
#define BOOST_PREPROCESSOR_MIN(X,Y) BOOST_PP_MIN(X,Y)
#endif


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

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

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

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

 /*!

    @ingroup group-arithmetic
    Function object implementing neg capabilities

    return the elementwise unary minus of the parameter

    Infix notation can be used with operator '-'

    This is a convenient alias of @ref unary_minus
  **/
  const boost::dispatch::functor<tag::neg_> neg = {};
} }
#endif

#include <boost/simd/function/scalar/unary_minus.hpp>
#include <boost/simd/function/simd/neg.hpp>

#endif


/*-----------------------------------------------------------------------------+
Author: Joachim Faulhaber
Copyright (c) 2009-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_LIBS_ICL_EXAMPLE_LARGE_BITSET_BITS_HPP_JOFA_091019
#define BOOST_LIBS_ICL_EXAMPLE_LARGE_BITSET_BITS_HPP_JOFA_091019
//[mini_bits_includes
                                               // These includes are needed ...
#include <string>                              // for conversion to output and to
#include <boost/icl/type_traits/has_set_semantics.hpp>//declare that bits has the
                                               // behavior of a set.
//]

namespace mini
{
//[mini_bits_class_bits
template<class NaturalT> class bits
{
public:
    typedef NaturalT word_type;
    static const int       digits = std::numeric_limits<NaturalT>::digits;
    static const word_type w1     = static_cast<NaturalT>(1) ;

    bits():_bits(){}
    explicit bits(word_type value):_bits(value){}

    word_type word()const{ return _bits; }
    bits& operator |= (const bits& value){_bits |= value._bits; return *this;}
    bits& operator &= (const bits& value){_bits &= value._bits; return *this;}
    bits& operator ^= (const bits& value){_bits ^= value._bits; return *this;}
    bits  operator ~  ()const { return bits(~_bits); }
    bool operator  <  (const bits& value)const{return _bits < value._bits;}
    bool operator  == (const bits& value)const{return _bits == value._bits;}

    bool contains(word_type element)const{ return ((w1 << element) & _bits) != 0; }
    std::string as_string(const char off_on[2] = " 1")const;

private:
    word_type _bits;
};
//]

template<class NaturalT>
std::string bits<NaturalT>::as_string(const char off_on[2])const
{
    std::string sequence;
    for(int bit=0; bit < digits; bit++)
        sequence += contains(bit) ? off_on[1] : off_on[0];
    return sequence;
}

} // mini

//[mini_bits_is_set
namespace boost { namespace icl
{
    template<class NaturalT>
    struct is_set<mini::bits<NaturalT> >
    {
        typedef is_set<mini::bits<NaturalT> > type;
        BOOST_STATIC_CONSTANT(bool, value = true);
    };

    template<class NaturalT>
    struct has_set_semantics<mini::bits<NaturalT> >
    {
        typedef has_set_semantics<mini::bits<NaturalT> > type;
        BOOST_STATIC_CONSTANT(bool, value = true);
    };
}}
//]

#endif


// test_rand.cc
#include "menon/utility.hh"
#include <boost/core/lightweight_test.hpp>
#include <vector>
#include <algorithm>

int main()
{
  std::vector<int> v1, v2;
  constexpr int n = 0x8000;

  // 生成した乱数列を2組作る。
  for (int i = 0; i < n; i++)
    v1.push_back(menon::rand());
  for (int i = 0; i < n; i++)
    v2.push_back(menon::rand());

  // 各欄数列のサイズを確認
  BOOST_TEST_EQ(v1.size(), n);
  BOOST_TEST_EQ(v2.size(), n);

  // 2つの乱数列が異なることを確認
  BOOST_TEST_NOT(std::equal(v1.cbegin(), v1.cend(), v2.cbegin()));

  return boost::report_errors();
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/nsm.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Generate a mask used to compute the mantissa of a floating point value

    @par Semantic:

    @code
    as_integer<T> r = Mantissamask<T>();
    @endcode

    @code
    if T is double
      r =  -2.225073858507200889e-308;
    else if T is float
      r =  -1.1754942106924410755e-38;
    @endcode

    @return The Mantissamask constant for the proper type
  */

namespace boost { namespace simd
{
  namespace tag
  {
    struct mantissamask_ : boost::dispatch::constant_value_<mantissamask_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,mantissamask_,boost::dispatch::constant_value_<mantissamask_>);
      BOOST_SIMD_REGISTER_CONSTANT(0,0x807FFFFFUL,0x800FFFFFFFFFFFFFULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, mantissamask_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::mantissamask_,mantissamask);
  }

  template<typename T> BOOST_FORCEINLINE auto Mantissamask()
  BOOST_NOEXCEPT_DECLTYPE(detail::mantissamask( boost::dispatch::as_<T>{}))
  {
    return detail::mantissamask( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/*
Copyright (c) 2009 Yahoo! Inc.  All rights reserved.  The copyrights
embodied in the content of this file are licensed under the BSD
(revised) open source license
 */
#include <fstream>
#include <vector>
#include <float.h>
#include <netdb.h>
#include <string.h>
#include <stdio.h>
#include <assert.h>
#include "parse_example.h"
#include "constant.h"
#include "sparse_dense.h"
#include "gd.h"
#include "lda_core.h"
#include "cache.h"
#include "multisource.h"
#include "simple_label.h"
#include "delay_ring.h"

#define MINEIRO_SPECIAL
#ifdef MINEIRO_SPECIAL

namespace {

inline float
fastlog2 (float x)
{
  union { float f; uint32_t i; } vx = { x };
  union { uint32_t i; float f; } mx = { (vx.i & 0x007FFFFF) | (0x7e << 23) };
  float y = vx.i;
  y *= 1.0f / (1 << 23);

  return
    y - 124.22544637f - 1.498030302f * mx.f - 1.72587999f / (0.3520887068f + mx.f);
}

inline float
fastlog (float x)
{
  return 0.69314718f * fastlog2 (x);
}

inline float
fastpow2 (float p)
{
  float offset = (p < 0) ? 1.0f : 0.0f;
  float clipp = (p < -126) ? -126.0f : p;
  int w = clipp;
  float z = clipp - w + offset;
  union { uint32_t i; float f; } v = { (uint32_t)((1 << 23) * (clipp + 121.2740838f + 27.7280233f / (4.84252568f - z) - 1.49012907f * z)) };

  return v.f;
}

inline float
fastexp (float p)
{
  return fastpow2 (1.442695040f * p);
}

inline float
fastpow (float x,
         float p)
{
  return fastpow2 (p * fastlog2 (x));
}

inline float
fastlgamma (float x)
{
  float logterm = fastlog (x * (1.0f + x) * (2.0f + x));
  float xp3 = 3.0f + x;

  return
    -2.081061466f - x + 0.0833333f / xp3 - logterm + (2.5f + x) * fastlog (xp3);
}

inline float
fastdigamma (float x)
{
  float twopx = 2.0f + x;
  float logterm = fastlog (twopx);

  return - (1.0f + 2.0f * x) / (x * (1.0f + x))
         - (13.0f + 6.0f * x) / (12.0f * twopx * twopx)
         + logterm;
}

#define log fastlog
#define exp fastexp
#define powf fastpow
#define mydigamma fastdigamma
#define mylgamma fastlgamma

#if defined(__SSE2__) && !defined(VW_LDA_NO_SSE)

#include <emmintrin.h>

typedef __m128 v4sf;
typedef __m128i v4si;

#define v4si_to_v4sf _mm_cvtepi32_ps
#define v4sf_to_v4si _mm_cvttps_epi32

static inline float
v4sf_index (const v4sf x,
            unsigned int i)
{
  union { v4sf f; float array[4]; } tmp = { x };

  return tmp.array[i];
}

static inline const v4sf
v4sfl (float x)
{
  union { float array[4]; v4sf f; } tmp = { { x, x, x, x } };

  return tmp.f;
}

static inline const v4si
v4sil (uint32_t x)
{
  uint64_t wide = (((uint64_t) x) << 32) | x;
  union { uint64_t array[2]; v4si f; } tmp = { { wide, wide } };

  return tmp.f;
}

static inline v4sf
vfastpow2 (const v4sf p)
{
  v4sf ltzero = _mm_cmplt_ps (p, v4sfl (0.0f));
  v4sf offset = _mm_and_ps (ltzero, v4sfl (1.0f));
  v4sf lt126 = _mm_cmplt_ps (p, v4sfl (-126.0f));
  v4sf clipp = _mm_andnot_ps (lt126, p) + _mm_and_ps (lt126, v4sfl (-126.0f));
  v4si w = v4sf_to_v4si (clipp);
  v4sf z = clipp - v4si_to_v4sf (w) + offset;

  const v4sf c_121_2740838 = v4sfl (121.2740838f);
  const v4sf c_27_7280233 = v4sfl (27.7280233f);
  const v4sf c_4_84252568 = v4sfl (4.84252568f);
  const v4sf c_1_49012907 = v4sfl (1.49012907f);
  union { v4si i; v4sf f; } v = {
    v4sf_to_v4si (
      v4sfl (1 << 23) *
      (clipp + c_121_2740838 + c_27_7280233 / (c_4_84252568 - z) - c_1_49012907 * z)
    )
  };

  return v.f;
}

inline v4sf
vfastexp (const v4sf p)
{
  const v4sf c_invlog_2 = v4sfl (1.442695040f);

  return vfastpow2 (c_invlog_2 * p);
}

inline v4sf
vfastlog2 (v4sf x)
{
  union { v4sf f; v4si i; } vx = { x };
  union { v4si i; v4sf f; } mx = { (vx.i & v4sil (0x007FFFFF)) | v4sil (0x3f000000) };
  v4sf y = v4si_to_v4sf (vx.i);
  y *= v4sfl (1.1920928955078125e-7f);

  const v4sf c_124_22551499 = v4sfl (124.22551499f);
  const v4sf c_1_498030302 = v4sfl (1.498030302f);
  const v4sf c_1_725877999 = v4sfl (1.72587999f);
  const v4sf c_0_3520087068 = v4sfl (0.3520887068f);

  return y - c_124_22551499
           - c_1_498030302 * mx.f
           - c_1_725877999 / (c_0_3520087068 + mx.f);
}

inline v4sf
vfastlog (v4sf x)
{
  const v4sf c_0_69314718 = v4sfl (0.69314718f);

  return c_0_69314718 * vfastlog2 (x);
}

inline v4sf
vfastdigamma (v4sf x)
{
  v4sf twopx = v4sfl (2.0f) + x;
  v4sf logterm = vfastlog (twopx);

  return (v4sfl (-48.0f) + x * (v4sfl (-157.0f) + x * (v4sfl (-127.0f) - v4sfl (30.0f) * x))) /
         (v4sfl (12.0f) * x * (v4sfl (1.0f) + x) * twopx * twopx)
         + logterm;
}

void
vexpdigammify (float* gamma)
{
  unsigned int n = global.lda;
  float extra_sum = 0.0f;
  v4sf sum = v4sfl (0.0f);
  size_t i;

  for (i = 0; i < n && ((uintptr_t) (gamma + i)) % 16 > 0; ++i)
    {
      extra_sum += gamma[i];
      gamma[i] = fastdigamma (gamma[i]);
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (gamma + i);
      sum += arg;
      arg = vfastdigamma (arg);
      _mm_store_ps (gamma + i, arg);
    }

  for (; i < n; ++i)
    {
      extra_sum += gamma[i];
      gamma[i] = fastdigamma (gamma[i]);
    }

  extra_sum += v4sf_index (sum, 0) + v4sf_index (sum, 1) +
               v4sf_index (sum, 2) + v4sf_index (sum, 3);
  extra_sum = fastdigamma (extra_sum);
  sum = v4sfl (extra_sum);

  for (i = 0; i < n && ((uintptr_t) (gamma + i)) % 16 > 0; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (gamma[i] - v4sf_index (sum, 0)));
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (gamma + i);
      arg -= sum;
      arg = vfastexp (arg);
      arg = _mm_max_ps (v4sfl (1e-10f), arg);
      _mm_store_ps (gamma + i, arg);
    }

  for (; i < n; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (gamma[i] - v4sf_index (sum, 0)));
    }
}

void
vexpdigammify_2(float*       gamma,
                const float* norm)
{
  size_t n = global.lda;
  size_t i;

  for (i = 0; i < n && ((uintptr_t) (gamma + i)) % 16 > 0; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (fastdigamma (gamma[i]) - norm[i]));
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (gamma + i);
      arg = vfastdigamma (arg);
      v4sf vnorm = _mm_loadu_ps (norm + i);
      arg -= vnorm;
      arg = vfastexp (arg);
      arg = _mm_max_ps (v4sfl (1e-10f), arg);
      _mm_store_ps (gamma + i, arg);
    }

  for (; i < n; ++i)
    {
      gamma[i] = fmaxf (1e-10f, fastexp (fastdigamma (gamma[i]) - norm[i]));
    }
}

#define myexpdigammify vexpdigammify
#define myexpdigammify_2 vexpdigammify_2

#else
#warning "lda IS NOT using sse instructions"
#define myexpdigammify expdigammify
#define myexpdigammify_2 expdigammify_2

#endif // __SSE2__

} // end anonymous namespace

#else

#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>

using namespace boost::math::policies;

#define mydigamma boost::math::digamma
#define mylgamma boost::math::lgamma
#define myexpdigammify expdigammify
#define myexpdigammify_2 expdigammify_2

#endif // MINEIRO_SPECIAL

size_t max_w = 0;

float decayfunc(float t, float old_t, float power_t) {
  float result = 1;
  for (float i = old_t+1; i <= t; i += 1)
    result *= (1-powf(i, -power_t));
  return result;
}

float decayfunc2(float t, float old_t, float power_t)
{
  float power_t_plus_one = 1. - power_t;
  float arg =  - ( powf(t, power_t_plus_one) -
                   powf(old_t, power_t_plus_one));
  return exp ( arg
               / power_t_plus_one);
}

float decayfunc3(double t, double old_t, double power_t)
{
  double power_t_plus_one = 1. - power_t;
  double logt = log(t);
  double logoldt = log(old_t);
  return (old_t / t) * exp(0.5*power_t_plus_one*(-logt*logt + logoldt*logoldt));
}

float decayfunc4(double t, double old_t, double power_t)
{
  if (power_t > 0.99)
    return decayfunc3(t, old_t, power_t);
  else
    return decayfunc2(t, old_t, power_t);
}

void expdigammify(float* gamma)
{
  float sum=0;
  for (size_t i = 0; i<global.lda; i++)
    {
      sum += gamma[i];
      gamma[i] = mydigamma(gamma[i]);
    }
  sum = mydigamma(sum);
  for (size_t i = 0; i<global.lda; i++)
    gamma[i] = fmax(1e-10, exp(gamma[i] - sum));
}

void expdigammify_2(float* gamma, float* norm)
{
  for (size_t i = 0; i<global.lda; i++)
    {
      gamma[i] = fmax(1e-10, exp(mydigamma(gamma[i]) - norm[i]));
    }
}

float average_diff(float* oldgamma, float* newgamma)
{
  float sum = 0.;
  float normalizer = 0.;
  for (size_t i = 0; i<global.lda; i++) {
    sum += fabsf(oldgamma[i] - newgamma[i]);
    normalizer += newgamma[i];
  }
  return sum / normalizer;
}

v_array<float> Elogtheta;

// Returns E_q[log p(\theta)] - E_q[log q(\theta)].
float theta_kl(float* gamma)
{
  float gammasum = 0;
  Elogtheta.erase();
  for (size_t k = 0; k < global.lda; k++) {
    push(Elogtheta, mydigamma(gamma[k]));
    gammasum += gamma[k];
  }
  float digammasum = mydigamma(gammasum);
  gammasum = mylgamma(gammasum);
  float kl = -(global.lda*mylgamma(global.lda_alpha));
  kl += mylgamma(global.lda_alpha*global.lda) - gammasum;
  for (size_t k = 0; k < global.lda; k++) {
    Elogtheta[k] -= digammasum;
    kl += (global.lda_alpha - gamma[k]) * Elogtheta[k];
    kl += mylgamma(gamma[k]);
  }

  return kl;
}

float find_cw(float* u_for_w, float* v)
{
  float c_w = 0;
  for (size_t k =0; k<global.lda; k++)
    c_w += u_for_w[k]*v[k];

  return 1.f / c_w;
}

v_array<float> new_gamma;
v_array<float> old_gamma;
// Returns an estimate of the part of the variational bound that
// doesn't have to do with beta for the entire corpus for the current
// setting of lambda based on the document passed in. The value is
// divided by the total number of words in the document This can be
// used as a (possibly very noisy) estimate of held-out likelihood.
float lda_loop(float* v,weight* weights,example* ec, float power_t)
{
  new_gamma.erase();
  old_gamma.erase();

  for (size_t i = 0; i < global.lda; i++)
    {
      push(new_gamma, 1.f);
      push(old_gamma, 0.f);
    }
  size_t num_words =0;
  for (size_t* i = ec->indices.begin; i != ec->indices.end; i++)
    num_words += ec->subsets[*i][1] - ec->subsets[*i][0];

  float xc_w = 0;
  float score = 0;
  float doc_length = 0;
  do
    {
      memcpy(v,new_gamma.begin,sizeof(float)*global.lda);
      myexpdigammify(v);

      memcpy(old_gamma.begin,new_gamma.begin,sizeof(float)*global.lda);
      memset(new_gamma.begin,0,sizeof(float)*global.lda);

      score = 0;
      size_t word_count = 0;
      doc_length = 0;
      for (size_t* i = ec->indices.begin; i != ec->indices.end; i++)
	{
	  feature *f = ec->subsets[*i][0];
	  for (; f != ec->subsets[*i][1]; f++)
	    {
	      float* u_for_w = &weights[(f->weight_index&global.thread_mask)+global.lda+1];
	      float c_w = find_cw(u_for_w,v);
	      xc_w = c_w * f->x;
              score += -f->x*log(c_w);
	      size_t max_k = global.lda;
	      for (size_t k =0; k<max_k; k++) {
		new_gamma[k] += xc_w*u_for_w[k];
	      }
	      word_count++;
              doc_length += f->x;
	    }
	}
      for (size_t k =0; k<global.lda; k++)
	new_gamma[k] = new_gamma[k]*v[k]+global.lda_alpha;
    }
  while (average_diff(old_gamma.begin, new_gamma.begin) > 0.001);

  ec->topic_predictions.erase();
  if (ec->topic_predictions.end_array - ec->topic_predictions.begin < (int)global.lda)
    reserve(ec->topic_predictions,global.lda);
  memcpy(ec->topic_predictions.begin,new_gamma.begin,global.lda*sizeof(float));

  score += theta_kl(new_gamma.begin);

  return score / doc_length;
}

class index_feature {
public:
  uint32_t document;
  feature f;
  bool operator<(const index_feature b) const { return f.weight_index < b.f.weight_index; }
};

std::vector<index_feature> sorted_features;

void start_lda(gd_thread_params t)
{
  regressor reg = t.reg;
  example* ec = NULL;

  v_array<float> total_lambda;
  v_array<float> total_new;
  v_array<example* > examples;
  v_array<int> doc_lengths;
  v_array<float> digammas;
  v_array<float> v;
  reserve(v, global.lda*global.minibatch);

  total_lambda.erase();

  for (size_t k = 0; k < global.lda; k++)
    push(total_lambda, 0.f);
  size_t stride = global.stride;
  weight* weights = reg.weight_vectors[0];

  for (size_t i =0; i <= global.thread_mask;i+=stride)
    for (size_t k = 0; k < global.lda; k++)
      total_lambda[k] += weights[i+k];

  v_array<float> decay_levels;
  push(decay_levels, 0.f);
  double example_t = global.initial_t;
  while ( true )
    {
      example_t++;
      total_new.erase();
      for (size_t k = 0; k < global.lda; k++)
	push(total_new, 0.f);

      sorted_features.resize(0);

      float eta = -1;
      float minuseta = -1;
      examples.erase();
      doc_lengths.erase();
      size_t batch_size = global.minibatch;
      for (size_t d = 0; d < batch_size; d++)
	{
          push(doc_lengths, 0);
	  if ((ec = get_example(0)) != NULL)//semiblocking operation.
	    {
	      push(examples, ec);
              for (size_t* i = ec->indices.begin; i != ec->indices.end; i++) {
                feature* f = ec->subsets[*i][0];
                for (; f != ec->subsets[*i][1]; f++) {
                  index_feature temp = {(uint32_t)d, *f};
                  sorted_features.push_back(temp);
                  doc_lengths[d] += f->x;
                }
              }
	    }
	  else if (thread_done(0))
	    batch_size = d;
	  else
	    d--;
	}

      sort(sorted_features.begin(), sorted_features.end());

      eta = global.eta * powf(example_t, -t.vars->power_t);
      minuseta = 1.0 - eta;
      eta *= global.lda_D / batch_size;
      push(decay_levels, decay_levels.last() + log(minuseta));

      digammas.erase();
      float additional = (float)(global.length()) * global.lda_rho;
      for (size_t i = 0; i<global.lda; i++) {
	push(digammas,mydigamma(total_lambda[i] + additional));
      }

      size_t last_weight_index = -1;
      for (index_feature* s = &sorted_features[0]; s <= &sorted_features.back(); s++)
	{
	  if (last_weight_index == s->f.weight_index)
	    continue;
	  last_weight_index = s->f.weight_index;
	  float* weights_for_w = &(weights[s->f.weight_index & global.thread_mask]);
          float decay = fmin(1.0, exp(decay_levels.end[-2] - decay_levels.end[(int)(-1-example_t+weights_for_w[global.lda])]));
	  float* u_for_w = weights_for_w + global.lda+1;

	  weights_for_w[global.lda] = example_t;
	  for (size_t k = 0; k < global.lda; k++)
	    {
	      weights_for_w[k] *= decay;
	      u_for_w[k] = weights_for_w[k] + global.lda_rho;
	    }
	  myexpdigammify_2(u_for_w, digammas.begin);
	}

      v.erase();

      for (size_t d = 0; d < batch_size; d++)
	{
          float score = lda_loop(&v[d*global.lda], weights, examples[d],t.vars->power_t);
          if (global.audit)
	    print_audit_features(reg, examples[d]);
          // If the doc is empty, give it loss of 0.
          if (doc_lengths[d] > 0) {
            global.sd->sum_loss -= score;
            global.sd->sum_loss_since_last_dump -= score;
          }
          finish_example(examples[d]);
	}

      for (index_feature* s = &sorted_features[0]; s <= &sorted_features.back();)
	{
	  index_feature* next = s+1;
	  while(next <= &sorted_features.back() && next->f.weight_index == s->f.weight_index)
	    next++;

	  float* word_weights = &(weights[s->f.weight_index & global.thread_mask]);
	  for (size_t k = 0; k < global.lda; k++) {
	    float new_value = minuseta*word_weights[k];
	    word_weights[k] = new_value;
	  }

	  for (; s != next; s++) {
	    float* v_s = &v[s->document*global.lda];
	    float* u_for_w = &weights[(s->f.weight_index & global.thread_mask) + global.lda + 1];
	    float c_w = eta*find_cw(u_for_w, v_s)*s->f.x;
	    for (size_t k = 0; k < global.lda; k++) {
	      float new_value = u_for_w[k]*v_s[k]*c_w;
	      total_new[k] += new_value;
 	      word_weights[k] += new_value;
	    }
	  }
	}
      for (size_t k = 0; k < global.lda; k++) {
	total_lambda[k] *= minuseta;
	total_lambda[k] += total_new[k];
      }

      if (thread_done(0))
	{
	  for (size_t i = 0; i < global.length(); i++) {
	    weight* weights_for_w = & (weights[i*global.stride]);
            float decay = fmin(1.0, exp(decay_levels.last() - decay_levels.end[(int)(-1-example_t+weights_for_w[global.lda])]));
	    for (size_t k = 0; k < global.lda; k++) {
	      weights_for_w[k] *= decay;
            }
	  }

	  if (global.local_prediction > 0)
	    shutdown(global.local_prediction, SHUT_WR);

	  return;
	}
    }
}

void end_lda()
{

}