#include <boost/random.hpp>

namespace boost{ namespace random{ namespace pyimpl{

struct generators{
    static void instantiate(){
        sizeof( ecuyer1988            );
        sizeof( hellekalek1995        );
        sizeof( kreutzer1986          );
        sizeof( lagged_fibonacci1279  );
        sizeof( lagged_fibonacci19937 );
        sizeof( lagged_fibonacci2281  );
        sizeof( lagged_fibonacci23209 );
        sizeof( lagged_fibonacci3217  );
        sizeof( lagged_fibonacci4423  );
        sizeof( lagged_fibonacci44497 );
        sizeof( lagged_fibonacci607   );
        sizeof( lagged_fibonacci9689  );
        sizeof( minstd_rand           );
        sizeof( minstd_rand0          );
        sizeof( mt11213b              );
        sizeof( mt19937               );
        sizeof( ranlux3               );
        sizeof( ranlux3_01            );
        sizeof( ranlux4               );
        sizeof( ranlux4_01            );
        sizeof( ranlux64_3_01         );
        sizeof( ranlux64_4_01         );
        sizeof( taus88                );
    }
};

#define CREATE_INSTANTIATION( GENERATOR_TYPE )                                                      \
    static result_type                                                                              \
    inline call( distribution& dist, GENERATOR_TYPE& gen ){                                         \
        return dist( gen );                                                                         \
    }                                                                                \
                                                                                \
    typedef variate_generator< GENERATOR_TYPE, distribution > variate_generator_##GENERATOR_TYPE;   \
                                                                                \
    variate_generator< GENERATOR_TYPE, distribution >                                               \
    create_vg(distribution dist, GENERATOR_TYPE gen ){                                              \
        return variate_generator< GENERATOR_TYPE, distribution >(gen, dist);                        \
    }

struct py_uniform_smallint{

    typedef uniform_smallint<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    //CREATE_INSTANTIATION( lagged_fibonacci1279  );
    //CREATE_INSTANTIATION( lagged_fibonacci19937 );
    //CREATE_INSTANTIATION( lagged_fibonacci2281  );
    //CREATE_INSTANTIATION( lagged_fibonacci23209 );
    //CREATE_INSTANTIATION( lagged_fibonacci3217  );
    //CREATE_INSTANTIATION( lagged_fibonacci4423  );
    //CREATE_INSTANTIATION( lagged_fibonacci44497 );
    //CREATE_INSTANTIATION( lagged_fibonacci607   );
    //CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    //CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    //CREATE_INSTANTIATION( ranlux4_01          );
    //CREATE_INSTANTIATION( ranlux64_3_01       );
    //CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_uniform_int{

    typedef uniform_int<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    //CREATE_INSTANTIATION( lagged_fibonacci1279  );
    //CREATE_INSTANTIATION( lagged_fibonacci19937 );
    //CREATE_INSTANTIATION( lagged_fibonacci2281  );
    //CREATE_INSTANTIATION( lagged_fibonacci23209 );
    //CREATE_INSTANTIATION( lagged_fibonacci3217  );
    //CREATE_INSTANTIATION( lagged_fibonacci4423  );
    //CREATE_INSTANTIATION( lagged_fibonacci44497 );
    //CREATE_INSTANTIATION( lagged_fibonacci607   );
    //CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    //CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    //CREATE_INSTANTIATION( ranlux4_01          );
    //CREATE_INSTANTIATION( ranlux64_3_01       );
    //CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_uniform_real{

    typedef uniform_real<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_bernoulli_distribution{

    typedef bernoulli_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_geometric_distribution{

    typedef geometric_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_triangle_distribution{

    typedef triangle_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_exponential_distribution{

    typedef exponential_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_normal_distribution{

    typedef normal_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_uniform_on_sphere{

    typedef uniform_on_sphere<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_lognormal_distribution{

    typedef lognormal_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_gamma_distribution{

    typedef gamma_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_binomial_distribution{

    typedef binomial_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_poisson_distribution{

    typedef poisson_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

struct py_cauchy_distribution{

    typedef cauchy_distribution<> distribution;
    typedef distribution::result_type result_type;

    CREATE_INSTANTIATION( ecuyer1988          );
    CREATE_INSTANTIATION( hellekalek1995      );
    CREATE_INSTANTIATION( kreutzer1986        );
    CREATE_INSTANTIATION( lagged_fibonacci1279  );
    CREATE_INSTANTIATION( lagged_fibonacci19937 );
    CREATE_INSTANTIATION( lagged_fibonacci2281  );
    CREATE_INSTANTIATION( lagged_fibonacci23209 );
    CREATE_INSTANTIATION( lagged_fibonacci3217  );
    CREATE_INSTANTIATION( lagged_fibonacci4423  );
    CREATE_INSTANTIATION( lagged_fibonacci44497 );
    CREATE_INSTANTIATION( lagged_fibonacci607   );
    CREATE_INSTANTIATION( lagged_fibonacci9689  );
    CREATE_INSTANTIATION( minstd_rand         );
    CREATE_INSTANTIATION( minstd_rand0        );
    CREATE_INSTANTIATION( mt11213b            );
    CREATE_INSTANTIATION( mt19937             );
    CREATE_INSTANTIATION( ranlux3             );
    CREATE_INSTANTIATION( ranlux3_01          );
    CREATE_INSTANTIATION( ranlux4             );
    CREATE_INSTANTIATION( ranlux4_01          );
    CREATE_INSTANTIATION( ranlux64_3_01       );
    CREATE_INSTANTIATION( ranlux64_4_01       );
    CREATE_INSTANTIATION( taus88              );
};

} } }


#include "CDefDataProvider.h"

#include <chrono>

#include <boost/random/random_device.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/lexical_cast.hpp>

namespace services
{
    CDefDataProvider::CDefDataProvider()
    {}

    CDefDataProvider::~CDefDataProvider()
    {
        stop();
        wait();
    }

    IDataProvider::Connection CDefDataProvider::onNewData(const FNewDataHandler& handler)
    {
        return m_newDataReady.connect(handler);
    }

    void CDefDataProvider::setNewData(const Data& data)
    {
        if (!m_newDataReady.empty())
        {
            m_newDataReady(data);
        }
    }

    void CDefDataProvider::start()
    {
        m_isStopped.store(false, std::memory_order_relaxed);
        m_worker = std::thread([this] {
            boost::random::random_device rng;

            boost::random::uniform_real_distribution<> dist(-100.0, 100.0);
            boost::random::uniform_int_distribution<> errDist(1, 100);

            while (!m_isStopped.load(std::memory_order_relaxed))
            {
                using namespace std::chrono_literals;

                auto isError = errDist(rng) % 5 == 0;

                if (isError)
                {
                    setNewData("an error");
                }
                else {
                    auto newValue = dist(rng);
                    setNewData(boost::lexical_cast<std::string>(newValue));
                }

                std::this_thread::sleep_for(1s);
            }
        });
    }

    void CDefDataProvider::stop()
    {
        m_isStopped.store(true, std::memory_order_relaxed);
    }

    void CDefDataProvider::wait()
    {
        if (m_worker.joinable())
        {
            m_worker.join();
        }
    }

} // end namepsace services


/* Copyright 2018 Nils Bore (nbore@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name of the copyright holder 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 HOLDER 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.
 */

#include <data_tools/xtf_data.h>
extern "C" {
#include <libxtf/xtf_reader.h>
}
#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/date_time.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#undef BOOST_NO_CXX11_SCOPED_ENUMS

//#include <stdio.h>
#include <fcntl.h>
#ifdef _MSC_VER
  #include <io.h>
#else
  #include <unistd.h>
#endif
//#include <stdlib.h>
//#include <string.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <data_tools/lat_long_utm.h>

using namespace std;

namespace xtf_data {

cv::Mat make_waterfall_image(const xtf_sss_ping::PingsT& pings)
{
    int rows = pings.size();
    int cols = pings[0].port.pings.size() + pings[0].stbd.pings.size();
    cv::Mat swath_img = cv::Mat(rows, cols, CV_8UC3, cv::Scalar(255, 255, 255));
    for (int i = 0; i < pings.size(); ++i) {
        for (int j = 0; j < pings[i].port.pings.size(); ++j) {
            cv::Point3_<uchar>* p = swath_img.ptr<cv::Point3_<uchar> >(i, pings[0].stbd.pings.size()+j);
            p->z = uchar(255.*(float(pings[i].port.pings[j]) + 32767.)/(2.*32767.));
            p->y = uchar(255.*(float(pings[i].port.pings[j]) + 32767.)/(2.*32767.));
            p->x = uchar(255.*(float(pings[i].port.pings[j]) + 32767.)/(2.*32767.));
        }
        for (int j = 0; j < pings[i].stbd.pings.size(); ++j) {
            cv::Point3_<uchar>* p = swath_img.ptr<cv::Point3_<uchar> >(i, pings[0].stbd.pings.size()-j-1);
            //tie(p->z, p->y, p->x) = uchar(255.*(float(pings[i].port_pings[j]) + 32767.)/(2.*65536));
            p->z = uchar(255.*(float(pings[i].stbd.pings[j]) + 32767.)/(2.*32767.));
            p->y = uchar(255.*(float(pings[i].stbd.pings[j]) + 32767.)/(2.*32767.));
            p->x = uchar(255.*(float(pings[i].stbd.pings[j]) + 32767.)/(2.*32767.));
        }
    }
    cv::Mat resized_swath_img;//dst image
    cv::resize(swath_img, resized_swath_img, cv::Size(512, rows));//resize image

    return resized_swath_img;
}

Eigen::MatrixXd make_eigen_waterfall_image(const xtf_sss_ping::PingsT& pings)
{
    int rows = pings.size();
    int cols = pings[0].port.pings.size() + pings[0].stbd.pings.size();
    //cv::Mat swath_img = cv::Mat(rows, cols, CV_8UC3, cv::Scalar(255, 255, 255));
    Eigen::MatrixXd swath_img = Eigen::MatrixXd::Zero(rows, cols);
    for (int i = 0; i < pings.size(); ++i) {
        for (int j = 0; j < pings[i].port.pings.size(); ++j) {
            //swath_img(i, pings[0].stbd.pings.size()+j) = (float(pings[i].port.pings[j]) + 32767.)/(2.*32767.);
            swath_img(i, pings[0].stbd.pings.size()+j) = double(pings[i].port.pings[j] + 4000.)/20000.;
        }
        for (int j = 0; j < pings[i].stbd.pings.size(); ++j) {
            //swath_img(i, pings[0].stbd.pings.size()-j-1) = (float(pings[i].stbd.pings[j]) + 32767.)/(2.*32767.);
            swath_img(i, pings[0].stbd.pings.size()-j-1) = double(pings[i].stbd.pings[j] + 4000.)/20000.;
        }
    }
    return swath_img;
}

void show_waterfall_image(const xtf_sss_ping::PingsT& pings)
{
    cv::Mat waterfall_image = make_waterfall_image(pings);
    cv::imshow("Waterfall image", waterfall_image);
    cv::waitKey();
}

xtf_sss_ping process_side_scan_ping(XTFPINGHEADER *PingHeader, XTFFILEHEADER *XTFFileHeader) {
/****************************************************************************/
// Put whatever processing here to be performed on SIDESCAN data.
// PingHeader points to the 256-byte ping header structure.  That structure
// identifies how many channels of sidescan follow.  The structure is followed
// by the sidescan data itself.
//
// For example: assume there are two channels of sidescan, stored 1024
// 8-bit samples per channel.  The data pointed to by PingHeader looks like:
//
// 256 bytes   - XTFPINGHEADER structure holds data about this ping
// 64 bytes    - XTFPINGCHANHEADER structure holds data about channel 1 (port)
// 1024 bytes  - channel 1 imagery
// 64 bytes    - XTFPINGCHANHEADER structure holds data about channel 2 (stbd)
// 1024 bytes  - channel 2 imagery
//
   const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000");
   WORD chan;
   int tmp;
   unsigned char *Ptr = (unsigned char *)PingHeader;

   // For backwards-compatibility.  The samples per channel used to
   // be stored in the file header.

   // skip past the ping header
   Ptr += sizeof(XTFPINGHEADER);

   xtf_sss_ping ping;
   ping.lat_ = PingHeader->SensorYcoordinate;
   ping.long_ = PingHeader->SensorXcoordinate;

   double easting, northing;
   string utm_zone;
   tie(northing, easting, utm_zone) = lat_long_utm::lat_long_to_UTM(ping.lat_, ping.long_);
   ping.pos_ = Eigen::Vector3d(easting, northing, -PingHeader->SensorDepth);
   ping.heading_ = M_PI/180.*PingHeader->SensorHeading;
   ping.heading_ = 0.5*M_PI-ping.heading_; // TODO: need to keep this for old data
   ping.roll_ = M_PI/180.*PingHeader->SensorRoll;
   ping.pitch_ =  M_PI/180.*PingHeader->SensorPitch;
   ping.sound_vel_ = PingHeader->SoundVelocity;

   boost::posix_time::ptime data_time(boost::gregorian::date(PingHeader->Year, PingHeader->Month, PingHeader->Day), boost::posix_time::hours(PingHeader->Hour)+boost::posix_time::minutes(PingHeader->Minute)+boost::posix_time::seconds(PingHeader->Second)+boost::posix_time::milliseconds(10*int(PingHeader->HSeconds)));
   stringstream time_ss;
   time_ss << data_time;
   ping.time_string_ = time_ss.str();
   boost::posix_time::time_duration const diff = data_time - epoch;
   ping.time_stamp_ = diff.total_milliseconds();

   ping.port.tilt_angle = M_PI/180.*XTFFileHeader->ChanInfo[0].TiltAngle;
   ping.port.beam_width = M_PI/180.*XTFFileHeader->ChanInfo[0].BeamWidth;
   ping.stbd.tilt_angle = M_PI/180.*XTFFileHeader->ChanInfo[1].TiltAngle;
   ping.stbd.beam_width = M_PI/180.*XTFFileHeader->ChanInfo[1].BeamWidth;

   for (chan=0; chan<PingHeader->NumChansToFollow; chan++) {

      XTFPINGCHANHEADER *ChanHeader;
      short *Imagery; // since BytesPerSample is 2, should be signed 16 bit
      WORD ChannelNumber;
      WORD BytesPerSample;
      DWORD SamplesPerChan;
      DWORD BytesThisChannel;

      // point to the channel header
      //
      ChanHeader = (XTFPINGCHANHEADER *) Ptr;

      // See which channel this is (0=port, 1=stbd, etc...)
      //
      ChannelNumber = ChanHeader->ChannelNumber;

      // Compute the number of bytes of imagery for this channel
      //
      BytesPerSample   = XTFFileHeader->ChanInfo[ChannelNumber].BytesPerSample;

      // If the NumSamples value in the channel header is zero, then this
      // must be an old-style XTF File where the samples per channel was
      // stored in this file header.
      SamplesPerChan = XTFFileHeader->ChanInfo[ChannelNumber].Reserved; // for backwards compatibility only!
      tmp = ChanHeader->NumSamples;
      if (tmp != 0) SamplesPerChan = tmp;

      BytesThisChannel = BytesPerSample * SamplesPerChan;

      // skip past the channel header
      //
      Ptr += sizeof(XTFPINGCHANHEADER);

      // Point to the imagery.  If BytesPerSample is 2, then
      // Imagery should be a pointer to a signed 16-bit value.
      // If BytesPerSample is 1, then Imagery should point to
      // a unsigned 8-bit value.
      Imagery = (short*)Ptr;
      xtf_sss_ping_side* ping_channel;
      if (ChannelNumber == 0) {
          ping_channel = &ping.port;
      }
      else if (ChannelNumber == 1) {
          ping_channel = &ping.stbd;
      }
      else {
          // skip past the imagery;
          Ptr += BytesThisChannel;
          continue;
      }
      ping_channel->pings.reserve(SamplesPerChan);
      for (int i = 0; i < SamplesPerChan; ++i) {
          // we should get port and starboard channel from header definition
          if (ChanHeader->Weight == 0) {
              ping_channel->pings.push_back(Imagery[i]);
          }
          else {
              ping_channel->pings.push_back((int)(Imagery[i]) << (12 - ChanHeader->Weight));
          }
      }
      ping_channel->time_duration = ChanHeader->TimeDuration;
      ping_channel->slant_range = ChanHeader->SlantRange;

      // Do whatever processing on the sidescan imagery here.
      //cout << "Processing a side scan ping!!" << endl;
      cout << "Size of short: " << sizeof(short) << endl;
      cout << "Channel number: " << int(ChannelNumber) << endl;
      //cout << "Channel name: " << ChannelName << endl;
      cout << "Bytes per sample: " << int(BytesPerSample) << endl;
      cout << "Samples per chan: " << int(SamplesPerChan) << endl;
      cout << "Ground range: " << float(ChanHeader->GroundRange) << endl; // seems to always be 0
      cout << "Slant range: " << float(ChanHeader->SlantRange) << endl;
      cout << "Time duration: " << ChanHeader->TimeDuration << endl;
      cout << "SecondsPerPing: " << ChanHeader->SecondsPerPing << endl; // seems to always be 0
      cout << "GAIN Code: " << ChanHeader->GainCode << endl;
      cout << "Initial GAIN Code: " << ChanHeader->InitialGainCode << endl;
      cout << "Weight: " << ChanHeader->Weight << endl;

      // skip past the imagery;
      Ptr += BytesThisChannel;
   }

   std::reverse(ping.port.pings.begin(), ping.port.pings.end());

   return ping;
}

xtf_sss_ping::PingsT read_xtf_file(int infl, XTFFILEHEADER* XTFFileHeader, unsigned char* buffer)
{
    /***************************************************************************************/
    // Given a handle to on open .XTF file, read through the file and
    // print out some data about it.

    //
    // Read the XTF file header
    //

    xtf_sss_ping::PingsT pings;
    if (ReadXTFHeader(infl, XTFFileHeader, buffer) == FALSE) {
        return pings;
    }

    ProcessXTFHeader(infl, XTFFileHeader, buffer);

    //
    // Read the XTF file one packet at a time
    //

    unsigned int amt;
    char* ptr;
    while ((amt = ReadXTFFormatFileData(infl, buffer)) != 0xFFFF) {
        //
        // Buffer now holds a single record which can be processed
        // here.  The return value from ReadXTFFormatFileData()
        // holds byte length of the data record (i.e., sidescan ping)
        //

        XTFPINGHEADER* PingHeader = (XTFPINGHEADER*)buffer;

        if (PingHeader->HeaderType != XTF_HEADER_SONAR) {
            continue;
        }

        xtf_sss_ping ping = process_side_scan_ping((XTFPINGHEADER*)PingHeader, XTFFileHeader);
        ping.first_in_file_ = false;
        pings.push_back(ping);
        cout << "SONAR "
             << int(PingHeader->Year) << " "
             << int(PingHeader->Month) << " "
             << int(PingHeader->Day) << " "
             << int(PingHeader->Hour) << " "
             << int(PingHeader->Minute) << " "
             << int(PingHeader->Second) << " "
             << int(PingHeader->HSeconds) << " "
             << "Sound vel=" << PingHeader->SoundVelocity << " "
             << "Computed sound vel=" << PingHeader->ComputedSoundVelocity << " "
             << "Y=" << PingHeader->SensorYcoordinate << " "
             << "X=" << PingHeader->SensorXcoordinate << " "
             << "altitude=" << PingHeader->SensorPrimaryAltitude << " "
             << "depth=" << PingHeader->SensorDepth << " "
             << "pitch=" << PingHeader->SensorPitch << " "
             << "roll=" << PingHeader->SensorRoll << " "
             << "heading=" << PingHeader->SensorHeading << " "  // [h] Fish heading in degrees
             << "heave=" << PingHeader->Heave << " "            // Sensor heave at start of ping.
                           // Positive value means sensor moved up.
             << "yaw=" << PingHeader->Yaw << endl;              // Sensor yaw.  Positive means turn to right.
        cout << "Tilt angle 0: " << XTFFileHeader->ChanInfo[0].TiltAngle << endl;        // Typically 30 degrees
        cout << "Beam width 0: " << XTFFileHeader->ChanInfo[0].BeamWidth << endl;        // 3dB beam width, Typically 50 degrees
        cout << "Tilt angle 1: " << XTFFileHeader->ChanInfo[1].TiltAngle << endl;        // Typically 30 degrees
        cout << "Beam width 1: " << XTFFileHeader->ChanInfo[1].BeamWidth << endl;        // 3dB beam width, Typically 50 degrees
        cout << ping.time_string_ << endl;
    }

    if (!pings.empty()) {
        pings[0].first_in_file_ = true;
    }

    if (amt == 0xFFFF) {
        cout << "Stopped - read -1 bytes" << endl;
    }

    cout << "Done!" << endl;

    return pings;
}

xtf_sss_ping::PingsT correct_sensor_offset(const xtf_sss_ping::PingsT& pings, const Eigen::Vector3d& sensor_offset)
{
    xtf_sss_ping::PingsT new_pings = pings;
    for (xtf_sss_ping& ping : new_pings) {
        Eigen::Matrix3d Rz = Eigen::AngleAxisd(ping.heading_, Eigen::Vector3d::UnitZ()).matrix();
        // these are my estimated values for the
        // sidescan offset from the center of motion
        //ping.pos_.array() += (2.*Rz.col(0) + -1.5*Rz.col(1)).array();
        ping.pos_.array() += (sensor_offset(0)*Rz.col(0) + sensor_offset(1)*Rz.col(1) + sensor_offset(2)*Rz.col(2)).array();
    }

    return new_pings;
}

xtf_sss_ping::PingsT match_attitudes(const xtf_sss_ping::PingsT& pings, const std_data::attitude_entry::EntriesT& entries)
{
    xtf_sss_ping::PingsT new_pings = pings;

    auto pos = entries.begin();
    for (xtf_sss_ping& ping : new_pings) {
        pos = std::find_if(pos, entries.end(), [&](const std_data::attitude_entry& entry) {
            return entry.time_stamp_ > ping.time_stamp_;
        });

        ping.pitch_ = 0.;
        ping.roll_ = 0.;
        double heave;
        if (pos == entries.end()) {
            ping.pitch_ = entries.back().pitch;
            ping.roll_ = entries.back().roll;
        }
        else if (pos == entries.begin()) {
                ping.pitch_ = pos->pitch;
                ping.roll_ = pos->roll;
        }
        else {
            const std_data::attitude_entry& previous = *(pos - 1);
            double ratio = double(ping.time_stamp_ - previous.time_stamp_)/double(pos->time_stamp_ - previous.time_stamp_);
            ping.pitch_ = previous.pitch + ratio*(pos->pitch - previous.pitch);
            ping.roll_ = previous.roll + ratio*(pos->roll - previous.roll);
        }
    }

    return new_pings;
}

} // namespace xtf_data

namespace std_data {

using namespace xtf_data;

template <>
xtf_sss_ping::PingsT parse_file(const boost::filesystem::path& file)
{
   xtf_sss_ping::PingsT pings;

   // quick sanity check to make sure the compiler didn't
   // screw up the data structure sizes by changing alignment.
   if (
      sizeof(XTFFILEHEADER)      != 1024 ||
      sizeof(CHANINFO)           != 128  ||
      sizeof(XTFPINGHEADER)      != 256  ||
      sizeof(XTFNOTESHEADER)     != 256  ||
      sizeof(XTFBATHHEADER)      != 256  ||
      sizeof(XTFRAWSERIALHEADER) != 64   ||
      sizeof(XTFPINGCHANHEADER)  != 64
      ) {
         cout << "Error: Bad structure size! " <<
            sizeof(XTFFILEHEADER)       << " " <<
            sizeof(CHANINFO)            << " " <<
            sizeof(XTFPINGHEADER)       << " " <<
            sizeof(XTFNOTESHEADER)      << " " <<
            sizeof(XTFBATHHEADER)       << " " <<
            sizeof(XTFRAWSERIALHEADER)  << " " <<
            sizeof(XTFPINGCHANHEADER)   << endl;
         return pings;
   }

#ifdef _MSC_VER
   int infl = open(file.string().c_str(), O_RDONLY | O_BINARY, 0000200);
#else
   int infl = open(file.string().c_str(), O_RDONLY, 0000200);
#endif
   if (infl <= 0) {
       cout << "Error: Can't open " << file.string() << " for reading!" << endl;
       return pings;
   }

   //
   // Allocate memory for reading file data
   //
   // NOTE: max file size is 268MB!
   unsigned char* buffer = (unsigned char*)malloc(268435456);
   if (buffer == NULL) {
       cout << "Can't allocate memory!" << endl;
       exit(-1);
       return pings;
   }

   //
   // Allocate memory for storing XTF header
   //
   XTFFILEHEADER XTFFileHeader; // = (XTFFILEHEADER*)malloc((WORD)sizeof(XTFFILEHEADER));
   pings = read_xtf_file(infl, &XTFFileHeader, buffer);

   if (infl > 0) {
       close(infl);
       infl = 0;
   }
   if (buffer != NULL) {
       free(buffer);
       buffer = NULL;
   }

   return pings;
}

} // namespace std_data


#ifndef ASLAM_BACKEND_TV_QUAT_HPP
#define ASLAM_BACKEND_TV_QUAT_HPP

#include "TransformationExpressionNode.hpp"
#include <Eigen/Core>
#include "RotationExpression.hpp"
#include "EuclideanExpression.hpp"

namespace aslam {
  namespace backend {

    class TransformationBasic : public TransformationExpressionNode
    {
    public:
      TransformationBasic(RotationExpression C_0_1, EuclideanExpression t_0_1_0);
      ~TransformationBasic() override;

      TransformationExpression toExpression();

      void accept(ExpressionNodeVisitor& visitor) override;
    private:
      Eigen::Matrix4d toTransformationMatrixImplementation() override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      RotationExpression toRotationExpression(const boost::shared_ptr<TransformationExpressionNode> & thisShared) const override;
      EuclideanExpression toEuclideanExpression(const boost::shared_ptr<TransformationExpressionNode> & thisShared) const override;
    private:
      boost::shared_ptr<RotationExpressionNode> _rotation;
      boost::shared_ptr<EuclideanExpressionNode>  _translation;
    };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_DV_QUAT_HPP */


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/Geometry>

using namespace std;

// NOTE the following workaround was needed on some 32 bits builds to kill extra precision of x87 registers.
// It seems that it is not needed anymore, but let's keep it here, just in case...

template<typename T> EIGEN_DONT_INLINE
void kill_extra_precision(T& /* x */) {
  // This one worked but triggered a warning:
  /* eigen_assert((void*)(&x) != (void*)0); */
  // An alternative could be:
  /* volatile T tmp = x; */
  /* x = tmp; */
}

template<typename BoxType> void alignedbox(const BoxType& box)
{
  /* this test covers the following files:
     AlignedBox.h
  */
  typedef typename BoxType::Scalar Scalar;
  typedef NumTraits<Scalar> ScalarTraits;
  typedef typename ScalarTraits::Real RealScalar;
  typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;

  const Index dim = box.dim();

  VectorType p0 = VectorType::Random(dim);
  VectorType p1 = VectorType::Random(dim);
  while( p1 == p0 ){
      p1 =  VectorType::Random(dim); }
  RealScalar s1 = internal::random<RealScalar>(0,1);

  BoxType b0(dim);
  BoxType b1(VectorType::Random(dim),VectorType::Random(dim));
  BoxType b2;

  kill_extra_precision(b1);
  kill_extra_precision(p0);
  kill_extra_precision(p1);

  b0.extend(p0);
  b0.extend(p1);
  VERIFY(b0.contains(p0*s1+(Scalar(1)-s1)*p1));
  VERIFY(b0.contains(b0.center()));
  VERIFY_IS_APPROX(b0.center(),(p0+p1)/Scalar(2));

  (b2 = b0).extend(b1);
  VERIFY(b2.contains(b0));
  VERIFY(b2.contains(b1));
  VERIFY_IS_APPROX(b2.clamp(b0), b0);

  // intersection
  BoxType box1(VectorType::Random(dim));
  box1.extend(VectorType::Random(dim));
  BoxType box2(VectorType::Random(dim));
  box2.extend(VectorType::Random(dim));

  VERIFY(box1.intersects(box2) == !box1.intersection(box2).isEmpty());

  // alignment -- make sure there is no memory alignment assertion
  BoxType *bp0 = new BoxType(dim);
  BoxType *bp1 = new BoxType(dim);
  bp0->extend(*bp1);
  delete bp0;
  delete bp1;

  // sampling
  for( int i=0; i<10; ++i )
  {
      VectorType r = b0.sample();
      VERIFY(b0.contains(r));
  }

}

template<typename BoxType> void alignedboxTranslatable(const BoxType& box)
{
  typedef typename BoxType::Scalar Scalar;
  typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;
  typedef Transform<Scalar, BoxType::AmbientDimAtCompileTime, Isometry> IsometryTransform;
  typedef Transform<Scalar, BoxType::AmbientDimAtCompileTime, Affine> AffineTransform;

  alignedbox(box);

  const VectorType Ones = VectorType::Ones();
  const VectorType UnitX = VectorType::UnitX();
  const Index dim = box.dim();

  // box((-1, -1, -1), (1, 1, 1))
  BoxType a(-Ones, Ones);

  VERIFY_IS_APPROX(a.sizes(), Ones * Scalar(2));

  BoxType b = a;
  VectorType translate = Ones;
  translate[0] = Scalar(2);
  b.translate(translate);
  // translate by (2, 1, 1) -> box((1, 0, 0), (3, 2, 2))

  VERIFY_IS_APPROX(b.sizes(), Ones * Scalar(2));
  VERIFY_IS_APPROX((b.min)(), UnitX);
  VERIFY_IS_APPROX((b.max)(), Ones * Scalar(2) + UnitX);

  // Test transform

  IsometryTransform tf = IsometryTransform::Identity();
  tf.translation() = -translate;

  BoxType c = b.transformed(tf);
  // translate by (-2, -1, -1) -> box((-1, -1, -1), (1, 1, 1))
  VERIFY_IS_APPROX(c.sizes(), a.sizes());
  VERIFY_IS_APPROX((c.min)(), (a.min)());
  VERIFY_IS_APPROX((c.max)(), (a.max)());

  c.transform(tf);
  // translate by (-2, -1, -1) -> box((-3, -2, -2), (-1, 0, 0))
  VERIFY_IS_APPROX(c.sizes(), a.sizes());
  VERIFY_IS_APPROX((c.min)(), Ones * Scalar(-2) - UnitX);
  VERIFY_IS_APPROX((c.max)(), -UnitX);

  // Scaling

  AffineTransform atf = AffineTransform::Identity();
  atf.scale(Scalar(3));
  c.transform(atf);
  // scale by 3 -> box((-9, -6, -6), (-3, 0, 0))
  VERIFY_IS_APPROX(c.sizes(), Scalar(3) * a.sizes());
  VERIFY_IS_APPROX((c.min)(), Ones * Scalar(-6) - UnitX * Scalar(3));
  VERIFY_IS_APPROX((c.max)(), UnitX * Scalar(-3));

  atf = AffineTransform::Identity();
  atf.scale(Scalar(-3));
  c.transform(atf);
  // scale by -3 -> box((27, 18, 18), (9, 0, 0))
  VERIFY_IS_APPROX(c.sizes(), Scalar(9) * a.sizes());
  VERIFY_IS_APPROX((c.min)(), UnitX * Scalar(9));
  VERIFY_IS_APPROX((c.max)(), Ones * Scalar(18) + UnitX * Scalar(9));

  // Check identity transform within numerical precision.
  BoxType transformedC = c.transformed(IsometryTransform::Identity());
  VERIFY_IS_APPROX(transformedC, c);

  for (size_t i = 0; i < 10; ++i)
  {
    VectorType minCorner;
    VectorType maxCorner;
    for (Index d = 0; d < dim; ++d)
    {
      minCorner[d] = internal::random<Scalar>(-10,10);
      maxCorner[d] = minCorner[d] + internal::random<Scalar>(0, 10);
    }

    c = BoxType(minCorner, maxCorner);

    translate = VectorType::Random();
    c.translate(translate);

    VERIFY_IS_APPROX((c.min)(), minCorner + translate);
    VERIFY_IS_APPROX((c.max)(), maxCorner + translate);
  }
}

template<typename Scalar, typename Rotation>
Rotation rotate2D(Scalar angle) {
  return Rotation2D<Scalar>(angle);
}

template<typename Scalar, typename Rotation>
Rotation rotate2DIntegral(typename NumTraits<Scalar>::NonInteger angle) {
  typedef typename NumTraits<Scalar>::NonInteger NonInteger;
  return Rotation2D<NonInteger>(angle).toRotationMatrix().
      template cast<Scalar>();
}

template<typename Scalar, typename Rotation>
Rotation rotate3DZAxis(Scalar angle) {
  return AngleAxis<Scalar>(angle, Matrix<Scalar, 3, 1>(0, 0, 1));
}

template<typename Scalar, typename Rotation>
Rotation rotate3DZAxisIntegral(typename NumTraits<Scalar>::NonInteger angle) {
  typedef typename NumTraits<Scalar>::NonInteger NonInteger;
  return AngleAxis<NonInteger>(angle, Matrix<NonInteger, 3, 1>(0, 0, 1)).
      toRotationMatrix().template cast<Scalar>();
}

template<typename Scalar, typename Rotation>
Rotation rotate4DZWAxis(Scalar angle) {
  Rotation result = Matrix<Scalar, 4, 4>::Identity();
  result.block(0, 0, 3, 3) = rotate3DZAxis<Scalar, AngleAxisd>(angle).toRotationMatrix();
  return result;
}

template <typename MatrixType>
MatrixType randomRotationMatrix()
{
  // algorithm from
  // https://www.isprs-ann-photogramm-remote-sens-spatial-inf-sci.net/III-7/103/2016/isprs-annals-III-7-103-2016.pdf
  const MatrixType rand = MatrixType::Random();
  const MatrixType q = rand.householderQr().householderQ();
  const JacobiSVD<MatrixType> svd = q.jacobiSvd(ComputeFullU | ComputeFullV);
  const typename MatrixType::Scalar det = (svd.matrixU() * svd.matrixV().transpose()).determinant();
  MatrixType diag = rand.Identity();
  diag(MatrixType::RowsAtCompileTime - 1, MatrixType::ColsAtCompileTime - 1) = det;
  const MatrixType rotation = svd.matrixU() * diag * svd.matrixV().transpose();
  return rotation;
}

template <typename Scalar, int Dim>
Matrix<Scalar, Dim, (1<<Dim)> boxGetCorners(const Matrix<Scalar, Dim, 1>& min_, const Matrix<Scalar, Dim, 1>& max_)
{
  Matrix<Scalar, Dim, (1<<Dim) > result;
  for(Index i=0; i<(1<<Dim); ++i)
  {
    for(Index j=0; j<Dim; ++j)
      result(j,i) = (i & (1<<j)) ? min_(j) : max_(j);
  }
  return result;
}

template<typename BoxType, typename Rotation> void alignedboxRotatable(
    const BoxType& box,
    Rotation (*rotate)(typename NumTraits<typename BoxType::Scalar>::NonInteger /*_angle*/))
{
  alignedboxTranslatable(box);

  typedef typename BoxType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::NonInteger NonInteger;
  typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;
  typedef Transform<Scalar, BoxType::AmbientDimAtCompileTime, Isometry> IsometryTransform;
  typedef Transform<Scalar, BoxType::AmbientDimAtCompileTime, Affine> AffineTransform;

  const VectorType Zero = VectorType::Zero();
  const VectorType Ones = VectorType::Ones();
  const VectorType UnitX = VectorType::UnitX();
  const VectorType UnitY = VectorType::UnitY();
  // this is vector (0, 0, -1, -1, -1, ...), i.e. with zeros at first and second dimensions
  const VectorType UnitZ = Ones - UnitX - UnitY;

  // in this kind of comments the 3D case values will be illustrated
  // box((-1, -1, -1), (1, 1, 1))
  BoxType a(-Ones, Ones);

  // to allow templating this test for both 2D and 3D cases, we always set all
  // but the first coordinate to the same value; so basically 3D case works as
  // if you were looking at the scene from top

  VectorType minPoint = -2 * Ones;
  minPoint[0] = -3;
  VectorType maxPoint = Zero;
  maxPoint[0] = -1;
  BoxType c(minPoint, maxPoint);
  // box((-3, -2, -2), (-1, 0, 0))

  IsometryTransform tf2 = IsometryTransform::Identity();
  // for some weird reason the following statement has to be put separate from
  // the following rotate call, otherwise precision problems arise...
  Rotation rot = rotate(NonInteger(EIGEN_PI));
  tf2.rotate(rot);

  c.transform(tf2);
  // rotate by 180 deg around origin -> box((1, 0, -2), (3, 2, 0))

  VERIFY_IS_APPROX(c.sizes(), a.sizes());
  VERIFY_IS_APPROX((c.min)(), UnitX - UnitZ * Scalar(2));
  VERIFY_IS_APPROX((c.max)(), UnitX * Scalar(3) + UnitY * Scalar(2));

  rot = rotate(NonInteger(EIGEN_PI / 2));
  tf2.setIdentity();
  tf2.rotate(rot);

  c.transform(tf2);
  // rotate by 90 deg around origin ->  box((-2, 1, -2), (0, 3, 0))

  VERIFY_IS_APPROX(c.sizes(), a.sizes());
  VERIFY_IS_APPROX((c.min)(), Ones * Scalar(-2) + UnitY * Scalar(3));
  VERIFY_IS_APPROX((c.max)(), UnitY * Scalar(3));

  // box((-1, -1, -1), (1, 1, 1))
  AffineTransform atf = AffineTransform::Identity();
  atf.linearExt()(0, 1) = Scalar(1);
  c = BoxType(-Ones, Ones);
  c.transform(atf);
  // 45 deg shear in x direction -> box((-2, -1, -1), (2, 1, 1))

  VERIFY_IS_APPROX(c.sizes(), Ones * Scalar(2) + UnitX * Scalar(2));
  VERIFY_IS_APPROX((c.min)(), -Ones - UnitX);
  VERIFY_IS_APPROX((c.max)(), Ones + UnitX);
}

template<typename BoxType, typename Rotation> void alignedboxNonIntegralRotatable(
    const BoxType& box,
    Rotation (*rotate)(typename NumTraits<typename BoxType::Scalar>::NonInteger /*_angle*/))
{
  alignedboxRotatable(box, rotate);

  typedef typename BoxType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::NonInteger NonInteger;
  enum { Dim = BoxType::AmbientDimAtCompileTime };
  typedef Matrix<Scalar, Dim, 1> VectorType;
  typedef Matrix<Scalar, Dim, (1 << Dim)> CornersType;
  typedef Transform<Scalar, Dim, Isometry> IsometryTransform;
  typedef Transform<Scalar, Dim, Affine> AffineTransform;

  const Index dim = box.dim();
  const VectorType Zero = VectorType::Zero();
  const VectorType Ones = VectorType::Ones();

  VectorType minPoint = -2 * Ones;
  minPoint[1] = 1;
  VectorType maxPoint = Zero;
  maxPoint[1] = 3;
  BoxType c(minPoint, maxPoint);
  // ((-2, 1, -2), (0, 3, 0))

  VectorType cornerBL = (c.min)();
  VectorType cornerTR = (c.max)();
  VectorType cornerBR = (c.min)(); cornerBR[0] = cornerTR[0];
  VectorType cornerTL = (c.max)(); cornerTL[0] = cornerBL[0];

  NonInteger angle = NonInteger(EIGEN_PI/3);
  Rotation rot = rotate(angle);
  IsometryTransform tf2;
  tf2.setIdentity();
  tf2.rotate(rot);

  c.transform(tf2);
  // rotate by 60 deg ->  box((-3.59, -1.23, -2), (-0.86, 1.5, 0))

  cornerBL = tf2 * cornerBL;
  cornerBR = tf2 * cornerBR;
  cornerTL = tf2 * cornerTL;
  cornerTR = tf2 * cornerTR;

  VectorType minCorner = Ones * Scalar(-2);
  VectorType maxCorner = Zero;
  minCorner[0] = (min)((min)(cornerBL[0], cornerBR[0]), (min)(cornerTL[0], cornerTR[0]));
  maxCorner[0] = (max)((max)(cornerBL[0], cornerBR[0]), (max)(cornerTL[0], cornerTR[0]));
  minCorner[1] = (min)((min)(cornerBL[1], cornerBR[1]), (min)(cornerTL[1], cornerTR[1]));
  maxCorner[1] = (max)((max)(cornerBL[1], cornerBR[1]), (max)(cornerTL[1], cornerTR[1]));

  for (Index d = 2; d < dim; ++d)
    VERIFY_IS_APPROX(c.sizes()[d], Scalar(2));

  VERIFY_IS_APPROX((c.min)(), minCorner);
  VERIFY_IS_APPROX((c.max)(), maxCorner);

  VectorType minCornerValue = Ones * Scalar(-2);
  VectorType maxCornerValue = Zero;
  minCornerValue[0] = Scalar(Scalar(-sqrt(2*2 + 3*3)) * Scalar(cos(Scalar(atan(2.0/3.0)) - angle/2)));
  minCornerValue[1] = Scalar(Scalar(-sqrt(1*1 + 2*2)) * Scalar(sin(Scalar(atan(2.0/1.0)) - angle/2)));
  maxCornerValue[0] = Scalar(-sin(angle));
  maxCornerValue[1] = Scalar(3 * cos(angle));
  VERIFY_IS_APPROX((c.min)(), minCornerValue);
  VERIFY_IS_APPROX((c.max)(), maxCornerValue);

  // randomized test - translate and rotate the box and compare to a box made of transformed vertices
  for (size_t i = 0; i < 10; ++i)
  {
    for (Index d = 0; d < dim; ++d)
    {
      minCorner[d] = internal::random<Scalar>(-10,10);
      maxCorner[d] = minCorner[d] + internal::random<Scalar>(0, 10);
    }

    c = BoxType(minCorner, maxCorner);

    CornersType corners = boxGetCorners(minCorner, maxCorner);

    typename AffineTransform::LinearMatrixType rotation =
        randomRotationMatrix<typename AffineTransform::LinearMatrixType>();

    tf2.setIdentity();
    tf2.rotate(rotation);
    tf2.translate(VectorType::Random());

    c.transform(tf2);
    corners = tf2 * corners;

    minCorner = corners.rowwise().minCoeff();
    maxCorner = corners.rowwise().maxCoeff();

    VERIFY_IS_APPROX((c.min)(), minCorner);
    VERIFY_IS_APPROX((c.max)(), maxCorner);
  }

  // randomized test - transform the box with a random affine matrix and compare to a box made of transformed vertices
  for (size_t i = 0; i < 10; ++i)
  {
    for (Index d = 0; d < dim; ++d)
    {
      minCorner[d] = internal::random<Scalar>(-10,10);
      maxCorner[d] = minCorner[d] + internal::random<Scalar>(0, 10);
    }

    c = BoxType(minCorner, maxCorner);

    CornersType corners = boxGetCorners(minCorner, maxCorner);

    AffineTransform atf = AffineTransform::Identity();
    atf.linearExt() = AffineTransform::LinearPart::Random();
    atf.translate(VectorType::Random());

    c.transform(atf);
    corners = atf * corners;

    minCorner = corners.rowwise().minCoeff();
    maxCorner = corners.rowwise().maxCoeff();

    VERIFY_IS_APPROX((c.min)(), minCorner);
    VERIFY_IS_APPROX((c.max)(), maxCorner);
  }
}

template<typename BoxType>
void alignedboxCastTests(const BoxType& box)
{
  // casting
  typedef typename BoxType::Scalar Scalar;
  typedef Matrix<Scalar, BoxType::AmbientDimAtCompileTime, 1> VectorType;

  const Index dim = box.dim();

  VectorType p0 = VectorType::Random(dim);
  VectorType p1 = VectorType::Random(dim);

  BoxType b0(dim);

  b0.extend(p0);
  b0.extend(p1);

  const int Dim = BoxType::AmbientDimAtCompileTime;
  typedef typename GetDifferentType<Scalar>::type OtherScalar;
  AlignedBox<OtherScalar,Dim> hp1f = b0.template cast<OtherScalar>();
  VERIFY_IS_APPROX(hp1f.template cast<Scalar>(),b0);
  AlignedBox<Scalar,Dim> hp1d = b0.template cast<Scalar>();
  VERIFY_IS_APPROX(hp1d.template cast<Scalar>(),b0);
}

void specificTest1()
{
    Vector2f m; m << -1.0f, -2.0f;
    Vector2f M; M <<  1.0f,  5.0f;

    typedef AlignedBox2f  BoxType;
    BoxType box( m, M );

    Vector2f sides = M-m;
    VERIFY_IS_APPROX(sides, box.sizes() );
    VERIFY_IS_APPROX(sides[1], box.sizes()[1] );
    VERIFY_IS_APPROX(sides[1], box.sizes().maxCoeff() );
    VERIFY_IS_APPROX(sides[0], box.sizes().minCoeff() );

    VERIFY_IS_APPROX( 14.0f, box.volume() );
    VERIFY_IS_APPROX( 53.0f, box.diagonal().squaredNorm() );
    VERIFY_IS_APPROX( std::sqrt( 53.0f ), box.diagonal().norm() );

    VERIFY_IS_APPROX( m, box.corner( BoxType::BottomLeft ) );
    VERIFY_IS_APPROX( M, box.corner( BoxType::TopRight ) );
    Vector2f bottomRight; bottomRight << M[0], m[1];
    Vector2f topLeft; topLeft << m[0], M[1];
    VERIFY_IS_APPROX( bottomRight, box.corner( BoxType::BottomRight ) );
    VERIFY_IS_APPROX( topLeft, box.corner( BoxType::TopLeft ) );
}

void specificTest2()
{
    Vector3i m; m << -1, -2, 0;
    Vector3i M; M <<  1,  5, 3;

    typedef AlignedBox3i  BoxType;
    BoxType box( m, M );

    Vector3i sides = M-m;
    VERIFY_IS_APPROX(sides, box.sizes() );
    VERIFY_IS_APPROX(sides[1], box.sizes()[1] );
    VERIFY_IS_APPROX(sides[1], box.sizes().maxCoeff() );
    VERIFY_IS_APPROX(sides[0], box.sizes().minCoeff() );

    VERIFY_IS_APPROX( 42, box.volume() );
    VERIFY_IS_APPROX( 62, box.diagonal().squaredNorm() );

    VERIFY_IS_APPROX( m, box.corner( BoxType::BottomLeftFloor ) );
    VERIFY_IS_APPROX( M, box.corner( BoxType::TopRightCeil ) );
    Vector3i bottomRightFloor; bottomRightFloor << M[0], m[1], m[2];
    Vector3i topLeftFloor; topLeftFloor << m[0], M[1], m[2];
    VERIFY_IS_APPROX( bottomRightFloor, box.corner( BoxType::BottomRightFloor ) );
    VERIFY_IS_APPROX( topLeftFloor, box.corner( BoxType::TopLeftFloor ) );
}

EIGEN_DECLARE_TEST(geo_alignedbox)
{
  for(int i = 0; i < g_repeat; i++)
  {
    CALL_SUBTEST_1( (alignedboxNonIntegralRotatable<AlignedBox2f, Rotation2Df>(AlignedBox2f(), &rotate2D)) );
    CALL_SUBTEST_2( alignedboxCastTests(AlignedBox2f()) );

    CALL_SUBTEST_3( (alignedboxNonIntegralRotatable<AlignedBox3f, AngleAxisf>(AlignedBox3f(), &rotate3DZAxis)) );
    CALL_SUBTEST_4( alignedboxCastTests(AlignedBox3f()) );

    CALL_SUBTEST_5( (alignedboxNonIntegralRotatable<AlignedBox4d, Matrix4d>(AlignedBox4d(), &rotate4DZWAxis)) );
    CALL_SUBTEST_6( alignedboxCastTests(AlignedBox4d()) );

    CALL_SUBTEST_7( alignedboxTranslatable(AlignedBox1d()) );
    CALL_SUBTEST_8( alignedboxCastTests(AlignedBox1d()) );

    CALL_SUBTEST_9( alignedboxTranslatable(AlignedBox1i()) );
    CALL_SUBTEST_10( (alignedboxRotatable<AlignedBox2i, Matrix2i>(AlignedBox2i(), &rotate2DIntegral<int, Matrix2i>)) );
    CALL_SUBTEST_11( (alignedboxRotatable<AlignedBox3i, Matrix3i>(AlignedBox3i(), &rotate3DZAxisIntegral<int, Matrix3i>)) );

    CALL_SUBTEST_14( alignedbox(AlignedBox<double,Dynamic>(4)) );
  }
  CALL_SUBTEST_12( specificTest1() );
  CALL_SUBTEST_13( specificTest2() );
}


#include <boost/numeric/conversion/cast.hpp>
#include "generalterm.h"

compile_time_expression_structure::generalTerm::generalTerm(serviceContainer & sc)
    :
      serviceContainer(sc)
{
    std::unique_ptr<generalPrimary> uPtr =
            generalPrimary::createPrimary(sc); // fabric method
    data.emplace_back(interPrimaryOperand::multiply, std::move(uPtr));
    bool primeEnds {false};
    while (true)
    {
        Token t {ts.pop()};
        interPrimaryOperand operand {interPrimaryOperand::undef};
        switch (t.kind) {
            case TokenKind::multiply:
            {
                operand = interPrimaryOperand::multiply;
                break;
            }
            case TokenKind::divide:
            {
                operand = interPrimaryOperand::divide;
                break;
            }
            case TokenKind::mod:
            {
                operand = interPrimaryOperand::mod;
                break;
            }
            default:
            {
                primeEnds = true;
                ts.push(t);
                break;
            }
        }
        if (!primeEnds)
        {
            std::unique_ptr<generalPrimary> prim =
                generalPrimary::createPrimary(sc); // fabric method
            data.emplace_back(operand, std::move(prim));
        }
        else
        {
            break;
        }
    }
}

double compile_time_expression_structure::generalTerm::value() const
{
    using boost::numeric_cast;
    double result {1.0};
    for (const auto& ref : data)
    {

        const auto rightSide {ref.second->value()};
        switch (ref.first)
        {
        case interPrimaryOperand::multiply:
            result *= rightSide;
            break;
        case interPrimaryOperand::divide:
            if (std::abs(rightSide) <= std::numeric_limits<double>::min())
                throw std::logic_error("division by zero");
            result /= ref.second->value();
            break;
        case interPrimaryOperand::mod:
        {
            int i1 = numeric_cast<int>(result);
            double di1 = numeric_cast<double>(i1);
            if (result - di1 > std::numeric_limits<double>::min())
                throw std::logic_error("left operand of %(mod) is not integer");
            int i2 = numeric_cast<int>(rightSide);
            double di2 = numeric_cast<double>(i2);
            if (rightSide - di2 > std::numeric_limits<double>::min())
                throw std::logic_error("right operand of %(mod) is not integer");
            result = i1 % i2;
            break;
        }
        default:
            throw std::logic_error("undefined interPrimaryOperand in generalTerm::value");
        }
    }
    return result;

}


// This is a personal academic project. Dear PVS-Studio, please check it.
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: http://www.viva64.com

#include <fstream>
#include <iomanip>
#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string.hpp>
#include "kyiv.h"
#include "asm_disasm.h"

#define COMMAND_SIZE 4

constexpr char outputf[] = "../punched_tape.txt";

std::map <std::string, std::string> en_instructions = {
        {"add",  "01"},
        {"sub",  "02"},
        {"addcmd",  "03"},
        {"suba", "06"},
        {"addcyc",  "07"},
        {"mul",  "10"},
        {"rmul", "11"},
        {"div",  "12"},
        {"sh",  "13"},
        {"and",  "15"},
        {"or",  "14"},
        {"xor",  "17"},
        {"norm", "35"},
        {"jle",  "04"},
        {"jlea",  "05"},
        {"je", "16"},
        {"nfork",  "30"},
        {"ncall",  "31"},
        {"ret", "32"},
        {"gob",  "26"},
        {"goe",  "27"},
        {"fop", "34"},
        {"rdt",  "20"},
        {"rbn",  "21"},
        {"wbn", "22"},
        {"wmd",  "23"},
        {"rmd",  "24"},
        {"imd", "25"},
        {"ostanov",  "33"}
};

std::map <std::string, std::string> ua_instructions = {
        {"дод",  "01"},
        {"від",  "02"},
        {"обк",  "03"},
        {"авід", "06"},
        {"цдод",  "07"},
        {"множ",  "10"},
        {"замнож", "11"},
        {"діл",  "12"},
        {"зсв",  "13"},
        {"і",  "15"},
        {"або",  "14"},
        {"вабо",  "17"},
        {"норм", "35"},
        {"кмр",  "04"},
        {"кмра",  "05"},
        {"кр", "16"},
        {"упп",  "30"},
        {"упч",  "31"},
        {"прп", "32"},
        {"пго",  "26"},
        {"кго",  "27"},
        {"фікс", "34"},
        {"чдн",  "20"},
        {"чбн",  "21"},
        {"дру", "22"},
        {"мбз",  "23"},
        {"мбч",  "24"},
        {"мбп", "25"},
        {"останов",  "33"}
};

//int disassembly(const uint64_t & command_oct, Kyiv_memory & kmem) {
//
////    std::cout << command_oct << std::endl;
//    std::ostringstream str;
//    str << std::oct << command_oct;
//    std::string command = str.str();
//
//    if (command.size() != 13 && command.size() != 14) {
//        return -1;
//    }
//    if (command.size() != 14) {
//        command.insert(0, "0");
//    }
//    std::string result;
//    for (const auto & it : en_instructions) {
//        if ( it.second == command.substr(0, 2) )
//            result.append(it.first);
//    }
//    if (result.empty()) {
//        for (const auto & it : ua_instructions) {
//            if ( it.second == command.substr(0, 2) )
//                result.append(it.first);
//        }
//    }
//    if (result.empty()) {
//        assert("Shouldn't be here!");
//    }
//    result.append(" " + command.substr(2, 4) + " " + command.substr(6, 4) + " " + command.substr(10, 4));
//    word_t Addr_1_mask_shift = (40-6-11)+1;
//    word_t Addr_1_mask = 0b11'111'111'111ULL << (Addr_1_mask_shift-1);
//    word_t Addr_2_mask_shift = (40-6-12-11)+1;
//    word_t Addr_2_mask = 0b11'111'111'111ULL << (Addr_2_mask_shift-1);
//    std::string val1 = std::to_string((word_to_number(kmem[(command_oct & Addr_1_mask) >> Addr_1_mask_shift]) ));  //* std::pow(2, -40)
//    std::string val2 = std::to_string(word_to_number(kmem[(command_oct & Addr_2_mask) >> Addr_2_mask_shift]));
//    result.append("\t;; " + val1 + " " + val2);
//    std::cout << result << std::endl;
//    return 0;
//}

int disassembly(const uint64_t & command_oct, Kyiv_memory_t & kmem, const addr3_t &addr3) {

//    std::cout << command_oct << std::endl;
    std::ostringstream str;
    str << std::oct << command_oct;
    std::string command = str.str();

    if (command.size() != 13 && command.size() != 14) {
        return -1;
    }
    if (command.size() != 14) {
        command.insert(0, "0");
    }
    std::string result;
    for (const auto & it : en_instructions) {
        if ( it.second == command.substr(0, 2) )
            result.append(it.first);
    }
    if (result.empty()) {
        for (const auto & it : ua_instructions) {
            if ( it.second == command.substr(0, 2) )
                result.append(it.first);
        }
    }
    if (result.empty()) {
        assert("Shouldn't be here!");
    }
    result.append(" " + command.substr(2, 4) + " " + command.substr(6, 4) + " " + command.substr(10, 4));
    word_t Addr_1_mask_shift = (40-6-11)+1;
    word_t Addr_1_mask = 0b11'111'111'111ULL << (Addr_1_mask_shift);
    word_t Addr_2_mask_shift = (40-6-12-11)+1;
    word_t Addr_2_mask = 0b11'111'111'111ULL << (Addr_2_mask_shift);
//    std::cout << "NUM : " << std::bitset<41>(command_oct) << std::endl;
//    std::cout << "TRUE : " << std::bitset<41>(0'11'0002'3067'0002ULL) << std::endl;
    std::string val1 = std::to_string((word_to_number(kmem.read_memory(addr3.source_1)) )) + "  " +
                       std::to_string(word_to_number(kmem.read_memory(addr3.source_1)) * std::pow(2, -40));  //* std::pow(2, -40)
    std::string val2 = std::to_string(word_to_number(kmem.read_memory(addr3.source_2))) + "  " +
                       std::to_string(word_to_number(kmem.read_memory(addr3.source_2)) * std::pow(2, -40) );
    result.append("\t;; " + val1 + "\t" + val2);
    std::cout << result << std::endl;
    addr_t a = (command_oct & Addr_2_mask) >> Addr_2_mask_shift;
//    std::cout << "TRUE : " << std::bitset<41>(a) << std::endl;
//    std::cout << "val2 : " << kmem.read_memory((command_oct & Addr_2_mask) >> Addr_2_mask_shift) << std::endl;
    return 0;
}

int disassembly_text(const std::string file_from, const std::string file_to) {
    word_t Addr_1_mask_shift = (40-6-11)+1;
    word_t Addr_1_mask = 0b11'111'111'111ULL << (Addr_1_mask_shift-1);
    word_t Addr_2_mask_shift = (40-6-12-11)+1;
    word_t Addr_2_mask = 0b11'111'111'111ULL << (Addr_2_mask_shift-1);
    word_t oct_command;
    std::map<std::string, std::string> program;
    std::map<std::string, std::string> jumps;
    size_t jump_counter = 0;

    std::ifstream infile(file_from);

    std::string line;

    while (std::getline(infile, line)) {
        line.erase(remove_if(line.begin(), line.end(), isspace), line.end());
        std::ostringstream str;
        str << std::oct << line;
        std::string command = str.str();
        std::string address = command.substr(0, 4);
        command = command.substr(4);

        if (command.size() != 13 && command.size() != 14) {
            return -1;
        }
        if (command.size() != 14) {
            command.insert(0, "0");
        }
        std::string result;

        if (command.substr(0, 2) == "00") {
            program[address] = command;
            continue;
        }

        for (const auto &it: en_instructions) {
            if (it.second == command.substr(0, 2)) {
                result.append(it.first + " ");
                std::string addr_1 = command.substr(2, 4) + " ";
                std::string addr_2 = command.substr(6, 4) + " ";
                std::string addr_3 = command.substr(10, 4) + " ";
                std::string mod = mod_comment(addr_1, addr_2, addr_3);

                if (it.first == "jle" || it.first == "jlea" || it.first == "je" || it.first == "ncall" ||
                    it.first == "gob") {
                    std::string label = "jump" + std::to_string(jump_counter++);
                    jumps[addr_3.substr(0, 4)] = label + ":";

                    result.append(addr_1);
                    result.append(addr_2);
                    result.append(label + " ");
                } else if (it.first == "nfork" || it.first == "goe") {
                    std::string label_1 = "jump" + std::to_string(jump_counter++);
                    std::string label_2 = "jump" + std::to_string(jump_counter++);
                    jumps[addr_2.substr(0, 4)] = label_1 + ":";
                    jumps[addr_3.substr(0, 4)] = label_2 + ":";

                    result.append(addr_1);
                    result.append(label_1 + " ");
                    result.append(label_2 + " ");
                } else {
                    result.append(addr_1);
                    result.append(addr_2);
                    result.append(addr_3);
                }

                if (mod != ";") {
                    result.append("\t" + mod);
                }
            }
//                if (result.empty()) {
//                    for (const auto &it: ua_instructions) {
//                        if (it.second == command.substr(0, 2))
//                            result.append(it.first);
//                    }
//                }
            if (result.empty()) {
                assert("Shouldn't be here!");
            }
            program[address] = result;
        }
    }
    disassembler_second_pass(program, jumps, file_to);
    return 0;
}

int disassembler_second_pass(std::map<std::string, std::string> program, std::map<std::string, std::string> jumps, const std::string& file_to) {
    std::string prog_to_file = "";
    int last_address = std::stoi(program.begin()->first, 0, 8);
    int origin_counter = 1;
    prog_to_file.append("org0 " + program.begin()->first + "\n");

    for(const auto & it : program) {
        if(jumps.find(it.first) != jumps.end()) {
            prog_to_file.append(jumps.find(it.first)->second + "\n");
        }
        if (std::stoi(it.first, 0, 8) - last_address > 1) {
            prog_to_file.append("org" + std::to_string(origin_counter++) + " " + it.first + "\n");
        }
        last_address = std::stoi(it.first, 0, 8);   // not sure if I should initialize a new variable for this or leave it like that

        prog_to_file.append("\t" + it.second + "\n");
    }

    std::ofstream infile(file_to);
    if (!infile.is_open())
        return -1;
    infile << prog_to_file;
    infile.close();

    return 0;
}

int check_modification(std::string addr) {
    word_t mod_bit = 0b100'000'000'000ULL;
    int num = std::stoi(addr, 0, 8);

    if (num & mod_bit) {
        return num^mod_bit;
    }
    return -1;
}

std::string mod_comment(std::string addr_1, std::string addr_2, std::string addr_3) {
    std::string comment = ";";
    int check = check_modification(addr_1);

    if (check != -1) {
        std::ostringstream str;
        str << std::oct << check;
        std::string to_print_1 = str.str();
        comment.append(" Addr 1: " + to_print_1 + " + A; ");
    }

    check = check_modification(addr_2);

    if (check != -1) {
        std::ostringstream str;
        str << std::oct << check;
        std::string to_print_1 = str.str();
        comment.append(" Addr 2: " + to_print_1 + " + A; ");
    }

    check = check_modification(addr_3);

    if (check != -1) {
        std::ostringstream str;
        str << std::oct << check;
        std::string to_print_1 = str.str();
        comment.append(" Addr 3: " + to_print_1 + " + A; ");
    }
    return comment;
}

class Assembly {
    /*
     * Class for reading and executing assembly code on Kyiv.
     */
private:
    std::map<std::string, std::string> references;          // contains origins, labels
    std::vector<std::string> readers;                       // helper for executing final code on Kyiv
    std::vector<std::string> lines_cout;                    // Optional - saves command address
    size_t command_count;                                   // helper to save current command address
    size_t org_counter;                                     // helper to numerate origin
    size_t end;                                             // helper to find address of last command
    bool text;                                              // check if we input command or value
    bool numer;                                             // Optional - if we want to address commands

public:
    /*
     * Read file in assembly code, convert it into Kyiv code and write into output file.
     * If numerate set in true -- output file will contain commands numeration.
     * @param input filename as string, flag to indicate if we address commands as bool
     * @return 0 if success
     */
    int read_file(const std::string& filename, bool numerate=false) {
        references = {};
        readers = {};
        command_count = 0;
        org_counter = 0;
        numer = numerate;

        std::ifstream infile(filename);
        std::string line;
        std::vector<std::string> commands;

        while (std::getline(infile, line)) {
            if (line.find(';') != std::string::npos)
                line.erase(line.find_first_of(';')); // remove comment
            if (!line.empty() && find_special_bts(line) == 0) {
                commands.push_back(line);
                if (numer) {
                    std::ostringstream oct;
                    oct << std::setw(4) << std::setfill('0') << std::oct << command_count;
                    lines_cout.insert(lines_cout.begin(), oct.str());
                    command_count++;
                }
                end++;
            }
        }
        std::ostringstream oct;
        oct << std::setw(4) << std::setfill('0') << std::oct << end;
        references["org" + std::to_string(org_counter-1)] += oct.str() + "0000";
        infile.close();
        std::string res;
        for (auto & command : commands) {
            std::vector<std::string> argv;
            boost::split(argv,command,boost::is_any_of(" "), boost::algorithm::token_compress_off);
            if (references.find(argv[0]) != references.end()) {
                readers.push_back(references[argv[0]]);
            } else {
                if (assembly_command(command, res) == -2) {
                    res += (numer) ? lines_cout.back()+ "\t" : "";
                    res += command + "\n";
                }
                if (numer)
                    lines_cout.pop_back();
            }
        }
        write_file(outputf, res);
        return 0;
    }

    /*
     * Check if we have any helper in line, such as origin, label, text or data section
     * @param input line as string
     * @return 0 if success
     */
    int find_special_bts(std::string &line) {
        std::vector<std::string> argv;
        boost::algorithm::trim(line);
        boost::split(argv,line,boost::is_any_of(" "), boost::algorithm::token_compress_off);
        if (argv[0] == ".text"){
            text = true;
            return 1;
        }
        else if (argv[0] == ".data") {
            text = false;
            return 1;
        } else if (argv[0] == "org") { // 20/21 start end 0000
            if (org_counter != 0) {
                std::ostringstream oct;
                oct << std::setw(4) << std::setfill('0') << std::oct << end;
                references["org" + std::to_string(org_counter-1)] += oct.str() + "0000";
            }
            end = std::stoi(argv[1], 0, 8) - 1;
            std::string com = text ? "21" : "20";
            com += argv[1];
            if (numer)
                command_count =  std::stoi(argv[1]);
            references["org" + std::to_string(org_counter)] = com;
            line = "org" + std::to_string(org_counter++);
        } else if (!text && argv.size() == 2) {
            std::ostringstream oct;
            oct << std::setw(4) << std::setfill('0') << std::oct << end;
            references[argv[0]] = oct.str();
            line = argv[1];
        } else if (!text) {
            if (stoi(argv[0]) == 1)
                line = "1099511627775";
            else
                line = std::to_string( (int64_t) (stof(argv[0]) * std::pow(2, 40)));

        } else if (text && argv.size() == 1) {
            std::ostringstream oct;
            oct << std::setw(4) << std::setfill('0') << std::oct << end;
            argv[0].pop_back();
            references[argv[0]] = oct.str();
            return 1;
        }
        return 0;
    }

    /*
     * Check if given line contains command, if yes - convert it into Kyiv code and write into result line
     * @param input line as string, result with Kyiv codes as string
     * @return 0 if success, -2 if contains data
     */
    int assembly_command(std::string& command, std::string &result) {
        std::vector<std::string> argv;
        boost::split(argv,command,boost::is_any_of(" "), boost::algorithm::token_compress_off);
        if (argv.size() != COMMAND_SIZE)
            return -2;
        for (uint8_t i = 1; i < COMMAND_SIZE; i++) {
            if (argv[i].find_first_not_of("01234567") != std::string::npos) {
                if (references.find(argv[i]) != references.end()) {
                    std::cout << command << "\t" << argv[i] << std::endl;
                    argv[i] = references.find(argv[i])->second;
                } else {
                    return -1;
                }
            }
            if (argv[i].size() != COMMAND_SIZE)
                return -1;
        }
        if (en_instructions.find(argv[0]) != en_instructions.end())
            argv[0] = en_instructions[argv[0]];
        else if (ua_instructions.find(argv[0]) != ua_instructions.end())
            argv[0] = ua_instructions[argv[0]];
        else
            return -1;
        result += (numer && !lines_cout.empty()) ? lines_cout.back() + "\t": "";
        result += boost::algorithm::join(argv, "") + "\n";
        return 0;
    }

    /*
     * Write result to given file (punched tape, or perfocard)
     * @param file name as pointer to char array, result as string
     * @return 0 if success
     */
    int write_file(const char* filename, std::string &res) {
        std::ofstream infile(filename);
        if (!infile.is_open())
            return -1;
        infile << res;
        infile.close();
        return 0;
    }

    /*
     * Execute all commands that we convert from assembly into Kyiv codes
     * @param struct Kyiv_t, address from which we start execution
     * @return 0 if success
     */
    int execute(Kyiv_t & machine, const size_t start) {
        for (auto & i : readers) {
            machine.kmem.write_memory(0001, stol(i, 0, 8));
            machine.C_reg = 1;
            machine.execute_opcode();
        }
        machine.C_reg = start;
        while (machine.execute_opcode()) {
            std::cout << "\tRES: " << machine.kmem.read_memory(0015) << " - " << word_to_number(machine.kmem.read_memory(0015)) * std::pow(2, -40) <<  std::endl;
        }
        std::cout << "RES: " << machine.kmem.read_memory(0015) << " - " << machine.kmem.read_memory(0015) * std::pow(2, -40) <<  std::endl;
        return 0;
    }
};

int main(int argc, char *argv[]) {
    if (argc != 1) {
        std::cerr << "Wrong arg" << std::endl;
        return -1;
    }
    // disassembly_text("../ROM.txt", "../rom_k.txt");
//    std::cout << std::setprecision(10);
//    disassembly_text(outputf, "../h.txt");
//    Assembly as;
//    as.read_file("../commans.txt", false);
    Kyiv_t machine;
    machine.kmem.write_memory(00002, 659706976665);
//    machine.kmem.write_memory(00010, 659706976665);
//    machine.kmem.write_memory(00004, 762123384786);
//    machine.kmem.write_memory(00001, 1);
    std::cout <<  machine.kmem.read_memory(03067) << std::endl;
    machine.C_reg = 03116;
    while (machine.execute_opcode()) {
        std::cout << "\tRES 0003: " << word_to_number(machine.kmem.read_memory(00003)) << " - " << word_to_number(machine.kmem.read_memory(00003))  * std::pow(2, -40) <<  std::endl;

    }

    std::cout << "\tRES 0004: " << word_to_number(machine.kmem.read_memory(00004)) << " - " << word_to_number(machine.kmem.read_memory(00004))  * std::pow(2, -40) <<  std::endl;

//    as.execute(machine, 2);
//    std::string result;
//
//    write_file(outputf, result);
//
//
//    // machine.kmem[0001] = 0'12'0016'0003'0016ULL;
//    machine.kmem[0001] = 0'21'0002'0010'0000ULL;
//    machine.kmem[0012] = 549755813888;
//    machine.kmem[0013] = 16;
//    // machine.kmem[0016] = 9;
//    machine.C_reg = 2;
//
//
//    while (machine.execute_opcode()) {
//        std::cout << "\tRES: " << machine.kmem[0014] << " - " << machine.kmem[0014] * std::pow(2, -40) <<  std::endl;
//    }
//    std::cout << "RES: " << machine.kmem[0014] << " - " << machine.kmem[0014] * std::pow(2, -40) <<  std::endl;
    return 0;
}


#include <boost/units/base_units/angle/steradian.hpp>