text:
/*            _       _
 *  _ __ ___ (_)_ __ | | __
 * | '_ ` _ \| | '_ \| |/ /
 * | | | | | | | | | |   <
 * |_| |_| |_|_|_| |_|_|\_\
 *
 * SPDX-License-Identifier: MIT
 *
 */
#include <getopt.h>
#include <regex>
#include <iostream>
#include "ws_server.h"
#include "jrpc.h"
#include <json_rpc.h>
#include <boost/asio/signal_set.hpp>
#include <gdt.pb.enums_only.h>
#include <config.h>

/**********************/
/* JsonRpcdDescriptor */
/**********************/
JsonRpcdDescriptor::JsonRpcdDescriptor(const char *_type,
                                       const char *_desc)
    : mink::DaemonDescriptor(_type, nullptr, _desc) {

#ifdef ENABLE_CONFIGD
    config = new config::Config();
    // set daemon params
    set_param(0, config);
#endif
    // default extra param values
    // --gdt-streams
    dparams.set_int(0, 1000);
    // --gdt-stimeout
    dparams.set_int(1, 5);
    // --gdt-smsg-pool
    dparams.set_int(2, 1000);
    // --gdt-sparam-pool
    dparams.set_int(3, 5000);
    // -S
    dparams.set_bool(4, false);
    // -u
    dparams.set_bool(5, false);
}

JsonRpcdDescriptor::~JsonRpcdDescriptor(){
    delete gdtsmm;
}

void JsonRpcdDescriptor::process_args(int argc, char **argv){
    std::regex addr_regex("(\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}):(\\d+)");
    std::regex ipv4_regex("\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}");
    int opt;
    int option_index = 0;
    struct option long_options[] = {{"gdt-streams", required_argument, 0, 0},
                                    {"gdt-stimeout", required_argument, 0, 0},
                                    {"gdt-smsg-pool", required_argument, 0, 0},
                                    {"gdt-sparam-pool", required_argument, 0, 0},
                                    {0, 0, 0, 0}};

    if (argc < 5) {
        print_help();
        exit(EXIT_FAILURE);
    }

    while ((opt = getopt_long(argc, argv, "?c:h:i:w:s:C:DSu", long_options,
                              &option_index)) != -1) {
        switch (opt) {
        // long options
        case 0:
            if (long_options[option_index].flag != 0)
                break;
            switch (option_index) {
            // gdt-streams
            case 0:
                dparams.set_int(0, atoi(optarg));
                break;

            // gdt-stimeout
            case 1:
                dparams.set_int(1, atoi(optarg));
                break;

            // gdt-smsg-pool
            case 2:
                dparams.set_int(2, atoi(optarg));
                break;

            // gdt-sparam-pool
            case 3:
                dparams.set_int(3, atoi(optarg));
                break;

            default:
                break;
            }
            break;

        // help
        case '?':
            print_help();
            exit(EXIT_FAILURE);

        // daemon id
        case 'i':
            if (set_daemon_id(optarg) > 0) {
                std::cout << "ERROR: Maximum size of daemon id string is "
                             "15 characters!"
                          << std::endl;
                exit(EXIT_FAILURE);
            }
            break;

        // router daemon address
        case 'c':
            // check pattern (ipv4:port)
            // check if valid
            if (!std::regex_match(optarg, addr_regex)) {
                std::cout << "ERROR: Invalid daemon address format '"
                          << optarg << "'!" << std::endl;
                exit(EXIT_FAILURE);

            } else {
                rtrd_lst.push_back(std::string(optarg));
            }
            break;

        // local ip
        case 'h':
            if (!std::regex_match(optarg, ipv4_regex)) {
                std::cout << "ERROR: Invalid local IPv4 address format '"
                          << optarg << "'!" << std::endl;
                exit(EXIT_FAILURE);

            } else {
                local_ip.assign(optarg);
            }

            break;

        // ws addr
        case 'w':
            // check pattern (ipv4:port)
            // check if valid
            if (!std::regex_match(optarg, addr_regex)) {
                std::cout << "ERROR: Invalid web socket address format '"
                          << optarg << "'!" << std::endl;
                exit(EXIT_FAILURE);

            } else {
                ws_addr.assign(optarg);
            }
            break;

        // sqlite database
        case 's':
            try {
                dbm.connect(optarg);
            } catch (std::invalid_argument &e) {
                std::cout << "ERROR: Invalid db filename!" << std::endl;
                exit(EXIT_FAILURE);
            }
            break;

        // certificates
        case 'C':
            try {
                // path and certificates
                int fsz = 0;
                std::string cpath(optarg);
                std::string f_cert(cpath + "/cert.pem");
                std::string f_key(cpath + "/key.pem");
                std::string f_dh(cpath + "/dh.pem");
                // check sizes
                int sz_cert = mink_utils::get_file_size(f_cert.c_str());
                int sz_key = mink_utils::get_file_size(f_key.c_str());
                int sz_dh = mink_utils::get_file_size(f_dh.c_str());

                if (!(sz_cert && sz_key && sz_dh))
                    throw std::invalid_argument("invalid file size");

                // setup buffers
                char b_cert[sz_cert];
                char b_key[sz_key];
                char b_dh[sz_dh];

                // load data
                if (mink_utils::load_file(f_cert.c_str(), b_cert, &sz_cert))
                    throw std::invalid_argument("invalid file size");

                if (mink_utils::load_file(f_key.c_str(), b_key, &sz_key))
                    throw std::invalid_argument("invalid file size");

                if (mink_utils::load_file(f_dh.c_str(), b_dh, &sz_dh))
                    throw std::invalid_argument("invalid file size");

                // save certs
                wss_cert.assign(b_cert, sz_cert);
                wss_key.assign(b_key, sz_key);
                wss_dh.assign(b_dh, sz_dh);

            } catch (std::exception &e) {
                std::cout << "ERROR: Cannot load certificates!" << std::endl;
                exit(EXIT_FAILURE);
            }

            break;

        // debug mode
        case 'D':
            set_log_level(mink::LLT_DEBUG);
            break;

        // single session mode
        case 'S':
            dparams.set_bool(4, true);
            break;

        // enable unencrypted ws
        case 'u':
            dparams.set_bool(5, true);
            break;

        default:
            break;
        }
    }

    // check mandatory id
    if (strlen(get_daemon_id()) == 0) {
        std::cout << "ERROR: Daemon id not defined!" << std::endl;
        exit(EXIT_FAILURE);
    }

    // check certificates
    if (wss_cert.empty() || wss_key.empty() || wss_dh.empty()) {
        std::cout << "ERROR: Missing certificates!" << std::endl;
        exit(EXIT_FAILURE);
    }
}

void JsonRpcdDescriptor::print_help(){
    std::cout << daemon_type << " - " << daemon_description << std::endl;
    std::cout << std::endl;
    std::cout << "Options:" << std::endl;
    std::cout << " -?\thelp" << std::endl;
    std::cout << " -i\tunique daemon id" << std::endl;
    std::cout << " -c\trouter daemon address (ipv4:port)" << std::endl;
    std::cout << " -h\tlocal IPv4 address" << std::endl;
    std::cout << " -w\tWebSocket server IPv4 address (ipv4:port)" << std::endl;
    std::cout << " -s\tpath to sqlite database file" << std::endl;
    std::cout << " -C\tcertificates path (key.pem, cert.pem, dh.pem)" << std::endl;
    std::cout << " -D\tstart in debug mode" << std::endl;
    std::cout << " -S\tsingle session mode" << std::endl;
    std::cout << " -u\tenable unencrypted WebSocket (ws://)" << std::endl;
    std::cout << std::endl;
    std::cout << "GDT Options:" << std::endl;
    std::cout << "=============" << std::endl;
    std::cout << " --gdt-streams     GDT Session stream pool            (default = 1000)"
              << std::endl;
    std::cout << " --gdt-stimeout    GDT Stream timeout in seconds      (default = 5)"
              << std::endl;
    std::cout << " --gdt-smsg-pool   GDT Service message pool           (default = 1000)"
              << std::endl;
    std::cout << " --gdt-sparam-pool GDT Service message parameter pool (default = 5000)"
              << std::endl;
}

static void rtrds_connect(JsonRpcdDescriptor *d){
    // connect to routing daemons
    std::smatch regex_groups;
    std::regex addr_regex("(\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}\\.\\d{1,3}):(\\d+)");

    // loop routing daemons
    for (size_t i = 0; i < d->rtrd_lst.size(); i++) {
        // separate IP and PORT
        if (!std::regex_match(d->rtrd_lst[i], regex_groups, addr_regex))
            continue;
        // connect to routing daemon
        gdt::GDTClient *gdtc = d->gdts->connect(regex_groups[1].str().c_str(),
                                                atoi(regex_groups[2].str().c_str()),
                                                16,
                                                (d->local_ip.empty() ? nullptr : d->local_ip.c_str()),
                                                0);

        // setup client for service messages
        if (gdtc!= nullptr) {
            d->rtrd_gdtc = gdtc;
            // setup service message event handlers
            d->gdtsmm->setup_client(gdtc);
        }
    }
}

void JsonRpcdDescriptor::init_gdt(){
    // service message manager
    gdtsmm = new gdt::ServiceMsgManager(&idt_map,
                                        nullptr,
                                        nullptr,
                                        dparams.get_pval<int>(2),
                                        dparams.get_pval<int>(3));

    // set daemon params
#ifdef ENABLE_CONFIGD
    set_param(0, config);
#endif
    set_param(1, gdtsmm);

    // set service message handlers
    gdtsmm->set_new_msg_handler(&ev_srvcm_rx);
    gdtsmm->set_msg_err_handler(&ev_srvcm_rx.msg_err);

    // start GDT session
    gdts = gdt::init_session(get_daemon_type(),
                             get_daemon_id(),
                             dparams.get_pval<int>(0),
                             dparams.get_pval<int>(1),
                             false,
                             dparams.get_pval<int>(1));

    // connect to routing daemons
    rtrds_connect(this);

    // try to connect if unsuccessful
    while (gdts->get_client_count() == 0 &&
           !mink::CURRENT_DAEMON->DAEMON_TERMINATED) {

        mink::CURRENT_DAEMON->log(mink::LLT_INFO,
                                 "Cannot connect to routingd, trying again...");
        rtrds_connect(this);
        sleep(2);
    }
}

static void load_wss_certs(boost::asio::ssl::context &ctx,
                           const std::string &cert,
                           const std::string &key,
                           const std::string &dh) {
    /*
        openssl dhparam -out dh.pem 2048
        openssl req -newkey rsa:2048 \
                    -nodes \
                    -keyout key.pem -x509 -days 10000 \
                    -out cert.pem \
                    -subj "/CN=www.example.com"
    */

    ctx.set_password_callback(
        [](std::size_t, boost::asio::ssl::context_base::password_purpose) {
            return "n/a";
        });

    ctx.set_options(boost::asio::ssl::context::default_workarounds |
                    boost::asio::ssl::context::no_sslv2 |
                    boost::asio::ssl::context::no_tlsv1 |
                    boost::asio::ssl::context::no_tlsv1_1 |
#ifndef ENABLE_TLSV12
                    boost::asio::ssl::context::no_tlsv1_2 |
#endif
                    boost::asio::ssl::context::single_dh_use);

    ctx.use_certificate_chain(boost::asio::buffer(cert.data(), cert.size()));

    ctx.use_private_key(boost::asio::buffer(key.data(),
                        key.size()),
                        boost::asio::ssl::context::file_format::pem);

    ctx.use_tmp_dh(boost::asio::buffer(dh.data(), dh.size()));
}

void JsonRpcdDescriptor::init_wss(const std::string &ws_addr){
    try{
        auto const addr = net::ip::make_address(ws_addr.substr(0, ws_addr.find(":")));
        auto const th_nr = 1;
        auto const droot = std::make_shared<std::string>("/");
        // get port
        uint16_t port = std::stoi(ws_addr.substr(ws_addr.find(":") + 1));

        // The io_context is required for all I/O
        net::io_context ioc{th_nr};

        //ssl context
        ssl::context ctx{ssl::context::tls_server};

        // This holds the self-signed certificate used by the server
        load_wss_certs(ctx, wss_cert, wss_key, wss_dh);

        // Create and launch a listening port
        std::make_shared<Listener>(ioc,
                                   ctx,
                                   tcp::endpoint{addr, port},
                                   droot)->run();
        // Run the I/O service on the requested number of threads
        //std::vector<std::thread> v;
        //v.reserve(th_nr);
        //for (auto i = th_nr- 1; i > 0; --i)
        //    v.emplace_back([&ioc] { ioc.run(); });

        // Construct a signal set registered for process termination.
        boost::asio::signal_set signals(ioc, SIGINT, SIGTERM);

        // Start an asynchronous wait for one of the signals to occur.
        signals.async_wait([&ioc](const boost::system::error_code error, int signum) {
            ioc.stop();
        });

        ioc.run();
    } catch (std::exception &e) {
        mink::CURRENT_DAEMON->log(mink::LLT_ERROR,
                                  "Cannot create web socket listener: %s",
                                  e.what());
        exit(EXIT_FAILURE);
    }

    DaemonDescriptor::DAEMON_TERMINATED = true;
}

#ifdef ENABLE_CONFIGD
int JsonRpcdDescriptor::init_cfg(bool _proc_cfg) const {
    // reserved
    return 0;
}
#endif

void JsonRpcdDescriptor::init(){
#ifdef ENABLE_CONFIGD
    init_cfg(true);
#endif
    init_gdt();
    init_wss(ws_addr);
}

#ifdef ENABLE_CONFIGD
void JsonRpcdDescriptor::process_cfg(){
    // reserved
}
#endif

void JsonRpcdDescriptor::terminate(){
    gdt::destroy_session(gdts);
#ifdef ENABLE_CONFIGD
    // deallocate config memory
    if (config->get_definition_root() != nullptr)
        delete config->get_definition_root();
    // free config
    delete config;
#endif
}

max_stars_repo_name: dfranusic/mink-core

max_issues_repo_name: dfranusic/mink-core

max_forks_repo_name: dfranusic/mink-core


text:
/*! \file
* \brief Vector.
*
* This file contains the class vector, an self-healing STL vector with
* checksumming. This allows to monitor the validity of the vector's content.
*
* (C) Copyright Sascha Peilicke 2010.
*
* Distributed under the Boost Software License, Version 1.0. (See
* accompanying file LICENSE_1_0.txt or copy at
* http://www.boost.org/LICENSE_1_0.txt)
*
* 23 April 2010 - Initial Revision (Sascha Peilicke)
*/

#ifndef BOOST_SELF_HEALING_VECTOR_HPP
#define BOOST_SELF_HEALING_VECTOR_HPP

#include "array.hpp"
#include "detail/child.hpp"
#include "detail/safe_ref.hpp"
#include "detail/sized.hpp"
#include "detail/utility.hpp"

#include <boost/config.hpp>
#include <boost/detail/iterator.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/swap.hpp>
#include <boost/throw_exception.hpp>

#include <algorithm>
#include <cmath>
#ifdef BOOST_SELF_HEALING_DEBUG
#   include <iostream>
#endif
#include <stdexcept>
#include <vector>

/// The namespace self_healing contains fault-tolerant data structures and utility classes.
namespace boost { namespace self_healing {

    /*! \brief Self-healing vector.
    *
    * TODO.
    *
    * \param T The data type of the stored values.
    * \param ChunkSize Optional storage capacity of the internal chunks.
    * \remarks The chunk size should be chosen based on CPU cache size.
    * \see std:vector, chunk<T, ChunkSize>
    */
    template <class T, std::size_t ChunkSize = 64>
    class vector : private sized
    {
        // private type definitions
        typedef vector<T, ChunkSize>   vector_type;
        typedef vector<T, ChunkSize> * vector_pointer;
        typedef vector<T, ChunkSize> & vector_reference;

        /*! \brief Vector element storage chunk.
        *
        * A chunk is a checksummed array and a child and part of the self-healing
        * vector storage mechanism
        * \see child, array
        */
        struct chunk : public child<vector_type>, public array<T, ChunkSize>
        {
            explicit chunk(vector_pointer const parent = 0, const T &value = 0)
                : child<vector_type>(parent), array<T, ChunkSize>(value) {}

            bool is_valid(vector_pointer const parent = 0) const {
                return child<vector_type>::is_valid(parent) &&
                       array<T, ChunkSize>::is_valid();
            }
        };

        typedef chunk   chunk_type;    //!< A vector chunk.
        typedef chunk * chunk_pointer; //!< A pointer to vector chunk.

    public:
        // type definitions
        typedef T               value_type;         //!< The type of elements stored in the vector.
        class                   iterator;           //!< Forward declaration of class iterator.
        class                   const_iterator;     //!< Forward declaration of class const_iterator.
        typedef T *             pointer;            //!< A pointer to an element.
        typedef const T *       const_pointer;      //!< A const pointer to an element.
        typedef safe_ref<T>     reference;          //!< A reference to an element.
        typedef const T &       const_reference;    //!< A const reference to an element.
        typedef typename sized::size_type size_type;//!< An unsigned integral type that can represent any non-negative value of the container's distance type.
        typedef std::ptrdiff_t  difference_type;    //!< A signed integral type used to represent the distance between two iterators.

        /*! \brief A (random access) iterator used to iterate through the vector.
        */
        class iterator : public child<vector_type>, public std::iterator<std::random_access_iterator_tag, value_type>
        {
            friend class vector;

            /*! Private constructor.
            * \param parent The vector parent.
            * \param index The index to start with.
            */
            explicit iterator(vector_pointer const parent, size_type index)
                : child<vector_type>(parent), i(index) {}

        public:
            /*! Default constructor.
            * \remarks This constructor does not build a valid iterator instance, it is just
            *          there to satisfy some STL algorithms.
            */
            iterator()
                : child<vector_type>(0), i(-1) {}

            iterator operator+(difference_type n) const { return iterator(child<vector_type>::parent(), i + n); }
            iterator operator-(difference_type n) const { return iterator(child<vector_type>::parent(), i - n); }
            difference_type operator+(const iterator &rhs) const { return i + rhs.i; }
            difference_type operator-(const iterator &rhs) const { return i - rhs.i; }

            iterator& operator+=(difference_type n) { i += n; return *this; }
            iterator& operator-=(difference_type n) { i -= n; return *this; }
            iterator& operator++() { ++i; return *this; }
            iterator& operator++(int) { i++; return *this; }
            iterator& operator--() { --i; return *this; }
            iterator& operator--(int) { i--; return *this; }

            // comparison
            bool operator==(const iterator &other) const { return i == other.i; }
            bool operator!=(const iterator &other) const { return i != other.i; }
            bool operator>(const iterator &other) const { return i > other.i; }
            bool operator>=(const iterator &other) const { return i >= other.i; }
            bool operator<(const iterator &other) const { return i < other.i; }
            bool operator<=(const iterator &other) const { return i <= other.i; }

            reference operator*() const { return child<vector_type>::parent()->at(i); }
            operator const_iterator() const { return const_iterator(child<vector_type>::parent(), i); }

            /*! Overload for operator<<() of std::ostream to print an iterator.
            */
            friend std::ostream &operator<<(std::ostream &os, const iterator &it) { return os << it.i; }

        private:
            size_type i;    //!< Internal index to the current element.
        };

        /*! A const (random access) iterator used to iterate through the vector.
        */
        class const_iterator : public child<vector_type>, public std::iterator<std::random_access_iterator_tag, value_type>
        {
            friend class vector;

            /*! Private constructor.
            * \param parent The vector parent.
            * \param index The index to start with.
            */
            explicit const_iterator(vector_pointer const parent, size_type index)
                : child<vector_type>(parent), i(index) {}

        public:
            const_iterator operator+(difference_type n) const { return const_iterator(child<vector_type>::parent(), i + n); }
            const_iterator operator-(difference_type n) const { return const_iterator(child<vector_type>::parent(), i - n); }
            difference_type operator+(const const_iterator &rhs) const { return i + rhs.i; }
            difference_type operator-(const const_iterator &rhs) const { return i - rhs.i; }

            const_iterator& operator+=(difference_type n) { i += n; return *this; }
            const_iterator& operator-=(difference_type n) { i -= n; return *this; }
            const_iterator& operator++() { ++i; return *this; }
            const_iterator& operator++(int) { i++; return *this; }
            const_iterator& operator--() { --i; return *this; }
            const_iterator& operator--(int) { i--; return *this; }

            // comparison
            bool operator==(const const_iterator &other) const { return i == other.i; }
            bool operator!=(const const_iterator &other) const { return i != other.i; }
            bool operator>(const const_iterator &other) const { return i > other.i; }
            bool operator>=(const const_iterator &other) const { return i >= other.i; }
            bool operator<(const const_iterator &other) const { return i < other.i; }
            bool operator<=(const const_iterator &other) const { return i <= other.i; }

            const_reference operator*() const { return child<vector_type>::parent()->at(i); }

            /*! Overload for operator<<() of std::ostream to print a const_iterator.
            */
            friend std::ostream &operator<<(std::ostream &os, const const_iterator &it) { return os << it.i; }

        private:
            size_type i;    //!< Internal index to the current element.
        };

#if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
        typedef std::reverse_iterator<iterator> reverse_iterator;
        typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
#elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
        // workaround for broken reverse_iterator in VC7
        typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, iterator,
                                                  reference, iterator, reference> > reverse_iterator;
        typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, const_iterator,
                                                  const_reference, iterator, reference> > const_reverse_iterator;
#elif defined(_RWSTD_NO_CLASS_PARTIAL_SPEC)
        typedef std::reverse_iterator<iterator, std::random_access_iterator_tag,
                                      value_type, reference, iterator, difference_type> reverse_iterator;
        typedef std::reverse_iterator<const_iterator, std::random_access_iterator_tag,
                                      value_type, const_reference, const_iterator, difference_type> const_reverse_iterator;
#else
        // workaround for broken reverse_iterator implementations
        typedef std::reverse_iterator<iterator, value_type> reverse_iterator;
        typedef std::reverse_iterator<const_iterator, value_type> const_reverse_iterator;
#endif

        /*! Default constructor.
        */
        explicit vector()
            : head(0), chunks(0), tail(0) {}

        /*! Constructor to initialize vector with a custom size and an optional fill value.
        * \param n Custom initial vector size.
        * \param x Optional value to fill the vector with.
        */
        vector(size_type n, const_reference x = value_type())
            : head(0), chunks(0), tail(0) {
            assign(n, x);
        }

        /*! Constructor to initialize vector with a custom value range supplied by an iterator.
        * \param first Begin of value range.
        * \param last End of value range.
        */
        template <class InputIterator>
        vector(InputIterator first, InputIterator last)
            : head(0), chunks(0), tail(0) {
            assign(first, last);
        }

        /*! Copy constructor to copy from a boost::self_healing::vector.
        * \param rhs The other boost::self_healing::vector to copy from.
        */
        vector(const vector_reference rhs)
            : head(0), chunks(0), tail(0) {
            assign(rhs.begin(), rhs.end());
        }

        /*! Copy constructor to copy from a std::vector.
        * \param rhs The other std::vector to copy from.
        */
        vector(const std::vector<value_type> &rhs)
            : head(0), chunks(0), tail(0) {
            assign(rhs.begin(), rhs.end());
        }

        /*! Destructor.
        */
        ~vector() {
            check_header();
            delete[] head;
        }

        vector_reference operator=(const vector_reference rhs) {
            assign(rhs.begin(), rhs.end());
        };

        template <class InputIterator>
        void assign(InputIterator first, InputIterator last) {
            resize(last - first);
            size_type i = 0;
            check_header();
            for (InputIterator it = first; it != last; it++) {
                head[i / ChunkSize][i % ChunkSize] = *it;
                i++;
            }
            sized::set_size(i);
        }
        template <class Size, class TT>
        void assign(Size n, const TT &x = TT()) {
            resize(n, x);
            /*size_type i = 0;
            check_header();
            for (; i < n; i++) {
                head[i / ChunkSize][i % ChunkSize] = x;
            }
            sized::set_size(i);*/
        }

        // iterator support
        iterator begin() { check_header(); return iterator(this, 0); }
        const_iterator begin() const { check_header(); return const_iterator(this, 0); }
        iterator end() { check_header(); return iterator(this, sized::size()); }
        const_iterator end() const { check_header(); return const_iterator(this, sized::size()); }

        // reverse iterator support
        reverse_iterator rbegin() { return reverse_iterator(end()); }
        const_reverse_iterator rbegin() const { return const_reverse_iterator(end()); }
        reverse_iterator rend() { return reverse_iterator(begin()); }
        const_reverse_iterator rend() const { return const_reverse_iterator(begin()); }

        // capacity
        size_type size() const { return sized::size(); }
        bool empty() const { return sized::size() == 0; }
        size_type max_size() const {
            // determin how much chunks fit into memory and thus how much elements we can have
            const long int max_elems = std::numeric_limits<size_type>::max() / sizeof(chunk_type) * ChunkSize;
            return std::min(max_elems, std::numeric_limits<difference_type>::max());
        }
        size_type capacity() const { check_header(); return chunks * chunk_type::size(); }

        void resize(size_type new_size, value_type item = T()) {
            if (new_size > size()) {
                if (new_size > capacity()) {
                    reserve(new_size);
                }
                insert(end(), new_size - size(), item);
            } else {
                // Invalidate values after 'new_size', capacity remains the same.
                iterator e = end();
                erase(e - (size() - new_size), e);
            }
        }

        /*! Reserve storage capacity.
        * \param new_capacity The (minimum) storage capacity to reserve.
        * \throws std::length_error
        */
        void reserve(size_type new_capacity) {
            const size_type new_chunk_count = std::ceil(static_cast<float>(new_capacity) / ChunkSize);
            new_capacity = new_chunk_count * ChunkSize;

            if (new_capacity > max_size()) {
                std::length_error e("unable to reserve capacity: " + to_string(new_capacity));
                boost::throw_exception(e);
            }

            const chunk_pointer new_head = new chunk_type[new_chunk_count];
#ifdef BOOST_SELF_HEALING_DEBUG
            std::cout << "boost::self_healing::vector<T, ChunkSize>::reserve() new head: " << new_head << std::endl;
#endif

            // set this vector as the parent of all vector chunks
            for (size_type c = 0; c < new_chunk_count; c++) {
                new_head[c].set_parent(reinterpret_cast<vector_pointer>(new_head));
            }

            check_header();
            if (head) {
                // copy values from old location to new location
                for (size_type i = 0; i < std::min(size(), new_capacity); i++) {
                    new_head[i / ChunkSize][i % ChunkSize] = head[i / ChunkSize][i % ChunkSize];
                };
                delete[] head;
            }

            head = new_head;
            tail = &new_head[new_chunk_count - 1]; // set to address of last chunk
            chunks = new_chunk_count;
        }

        // operator[]
        reference operator[](size_type i) { check_header(); return head[i / ChunkSize][i % ChunkSize]; }
        const_reference operator[](size_type i) const { check_header(); return head[i / ChunkSize][(i % ChunkSize)]; }

        // at() with range check
        reference at(size_type i) { rangecheck(i); return operator[](i); }
        const_reference at(size_type i) const { rangecheck(i); return operator[](i); }

        // front() and back()
        reference front() { check_header(); return head->front(); }
        const_reference front() const { check_header(); return head->front(); }
        reference back() { check_header(); return tail->back(); }
        const_reference back() const { check_header(); return tail->back(); }

        // modifiers
        iterator insert(iterator position, const_reference value) {
            // reserve space if needed
            if (size() == capacity()) {
                reserve(capacity() + 1);
            }

            // move all elements one field backwards to make room for the new
            // element. iterate from back to front.
            iterator it = end() - 1;
            while (it > position) {
                *it = *(it - 1);
                it--;
            }
            *position = value;

            // adjust size accordingly
            set_size(sized::size() + 1);
            return position;
        }
        void insert(iterator position, size_type n, const_reference value) {
            if (n == 0) {
                return;
            }
            // reserve slightly more space if needed, simpler computation
            if (size() + n >= capacity()) {
                reserve(capacity() + n);
            }

            // if we already have elements, we may have to move them backwards
            if (size() != 0) {
                // move all elements n fields backwards to make room for the new
                // elements. iterate from back to front.
                iterator source = begin() + (size() - 1); // source is last element
                iterator target = source + n; // target is n steps to the right
                while (source >= position) {
                    *target = *source;
                    source--;
                    target--;
                }
            }

            // adjust size accordingly
            set_size(sized::size() + n);

            // insert 'n' new elements of 'value' at 'position'
            for (iterator it = position; it != position + n; it++) {
                *it = value;
            }
        }
        template <class InputIterator>
        void insert(iterator position, InputIterator first, InputIterator last) {
            const difference_type n = last - first;
            if (n <= 0) {
                return;
            }
            // reserve slightly more space if needed, simpler computation
            if (size() + n >= capacity()) {
                reserve(capacity() + n);
            }

            // move all elements n fields backwards to make room for the new
            // elements. iterate from back to front.
            iterator source = begin() + (size() - 1); // source is last element
            iterator target = source + n; // target is n steps to the right
            while (source >= position) {
                *target = *source;
                source--;
                target--;
            }

            // adjust size accordingly
            set_size(sized::size() + n);

            // insert range [first, last) of new elements at 'position'
            iterator to = position, it = first;
            while (it != last) {
                *to = *it;
                to++;
                it++;
            }
        }

        iterator erase(iterator position)  {
            if (position + 1 != end()) {
                iterator it = position + 1;
                // move all elements one position backwards
                while (it != end()) {
                    *(it - 1) = *it;
                    it++;
                }
                // set new vector size and check if we can shrink the vector
                const size_type new_size = sized::size() - 1;
                const size_type new_chunk_count = std::ceil(static_cast<float>(new_size) / ChunkSize);
                set_size(new_size);
                if (new_chunk_count < chunks) {
                    reserve(new_chunk_count * ChunkSize);
                }
            }

            return position;
        }
        iterator erase(iterator first, iterator last) {
            iterator res = first, it = last;
            // move elements after the erase-range forward
            while (it != end()) {
                *res++ = *it++;
            }
            // reduce the size accordingly and check if we can shrink the vector
            const size_type new_size = sized::size() - (last - first);
            const size_type new_chunk_count = std::ceil(static_cast<float>(new_size) / ChunkSize);
            set_size(new_size);
            if (new_chunk_count < chunks) {
                reserve(new_chunk_count * ChunkSize);
            }
            return first;
        }

        void push_back(const_reference value) {
            if (size() < capacity()) {
                // vector is not full yet
                operator[](sized::size()) = value;
                set_size(sized::size() + 1);
            } else {
                insert(end(), value);
            }
        }
        void pop_back() {
            // set new vector size and check if we can shrink the vector
            const size_type new_size = sized::size() - 1;
            const size_type new_chunk_count = std::ceil(static_cast<float>(new_size) / ChunkSize);
            set_size(new_size);
            if (new_chunk_count < chunks) {
                reserve(new_chunk_count * ChunkSize);
            }
        }

        void clear() {
            if (!empty()) {
                erase(begin(), end());
            }
        }

        void swap(vector_reference rhs) {
            boost::swap(head, rhs.head);
            boost::swap(tail, rhs.tail);
            boost::swap(chunks, rhs.chunks);

            size_type tmp_size = size();
            set_size(rhs.size());
            rhs.set_size(tmp_size);

            check_header();
        }

        /*! Check index validity against size.
        * \param index The index to check.
        * \throws std::out_of_range Thrown if index is out of range.
        */
        void rangecheck(size_type index) const {
/*#ifdef BOOST_SELF_HEALING_DEBUG
            std::cout << "boost::self_healing::vector<T, ChunkSize>::rangecheck(" << index << ")" << std::endl;
#endif*/
            if (index != 0 && index >= size()) { // size() is 0 initially
                std::out_of_range e("index out of range");
                boost::throw_exception(e);
            }
        }

        /*! Validity check that tries to correct minor faults silently.
        * \return true, if the internal structure and data is valid
        * \see check_storage
        */
        bool is_valid() const {
#ifdef BOOST_SELF_HEALING_DEBUG
            std::cout << "boost::self_healing::vector<T, ChunkSize>::is_valid()" << std::endl;
#endif
            try {
                // stage 1: check header
                check_header();
                // stage 2: check nodes
                for (size_type i = 0; i < chunks; i++) {
                    // compute address of next chunk
                    chunk_pointer chunk = head + i * sizeof(chunk_type);
                    chunk->is_valid(reinterpret_cast<vector_pointer>(head));
                }
                return sized::is_valid();
            } catch (const std::runtime_error &e) {
#ifdef BOOST_SELF_HEALING_DEBUG
                std::cout << "boost::self_healing::vector<T, ChunkSize>::is_valid() caught runtime error while checking nodes: " << e.what() << std::endl;
#endif
                return false;
            };
        }

    private:
        void check_header() const {
            if ((head == 0 && tail == 0 && chunks == 0) || size() == 0) {
                return; // initial case where nothing is allocated yet
            }

#ifdef BOOST_SELF_HEALING_DEBUG
            std::cout << "boost::self_healing::vector<T, ChunkSize>::check_header()" << std::endl;
#endif

            // check if chunk counter is valid and fix accordingly
            const size_type estimated_chunks = size() == 0 ? 1 : std::ceil(static_cast<float>(size()) / ChunkSize);
            if (chunks != estimated_chunks) {
                const_cast<size_type &>(chunks) = estimated_chunks;
            }

            if (head + (chunks - 1) != tail) {
                // either one or both pointers are damaged
                const bool head_valid = is_valid_heap_address(head);
                const bool tail_valid = is_valid_heap_address(tail);

                if (head_valid && tail_valid) {
                    // the currently imperfect heap address check passed out
                    // indicating both are correct, which is wrongs
                    std::runtime_error e("head and tail pointer error");
                    boost::throw_exception(e);
                } else if (head_valid && head->parent() == reinterpret_cast<vector_pointer>(head)) {
                    const_cast<chunk_pointer &>(tail) = head + (chunks -1);
                } else if (tail_valid && tail->parent() == reinterpret_cast<vector_pointer>(tail - (chunks - 1))) {
                    const_cast<chunk_pointer &>(head) = tail - (chunks -1);
                } else {
                    std::runtime_error e("head and tail pointer error");
                    boost::throw_exception(e);
                }
            }
        }

        chunk_pointer head;  //!< Pointer to the first chunk of an array of chunk instances.
        size_type chunks;    //!< Chunk counter.
        chunk_pointer tail;  //!< Pointer to the last chunk in the array of chunk instances.
    };

    // comparisons
    template <class T, std::size_t ChunkSize>
    inline bool operator==(const vector<T, ChunkSize> &x, const vector<T, ChunkSize> &y) {
        return std::equal(x.begin(), x.end(), y.begin());
    }
    template <class T, std::size_t ChunkSize>
    inline bool operator<(const vector<T, ChunkSize> &x, const vector<T, ChunkSize> &y) {
        return std::lexicographical_compare(x.begin(), x.end(), y.begin(), y.end());
    }
    template <class T, std::size_t ChunkSize>
    inline bool operator!=(const vector<T, ChunkSize> &x, const vector<T, ChunkSize> &y) {
        return !(x == y);
    }
    template <class T, std::size_t ChunkSize>
    inline bool operator>(const vector<T, ChunkSize> &x, const vector<T, ChunkSize> &y) {
        return y < x;
    }
    template <class T, std::size_t ChunkSize>
    inline bool operator<=(const vector<T, ChunkSize> &x, const vector<T, ChunkSize> &y) {
        return !(y < x);
    }
    template <class T, std::size_t ChunkSize>
    inline bool operator>=(const vector<T, ChunkSize> &x, const vector<T, ChunkSize> &y) {
        return !(x < y);
    }

    /*! Global swap function.
    */
    template<class T, std::size_t ChunkSize>
    inline void swap(vector<T, ChunkSize> &x, vector<T, ChunkSize> &y) {
        x.swap(y);
    }

} } // namespace boost::self_healing

/*! Overload for operator << of std::ostream to print a vector.
*/
template <class T, std::size_t ChunkSize>
std::ostream &operator<<(std::ostream &os, const boost::self_healing::vector<T, ChunkSize> &vector)
{
    os << "[";
    for (std::size_t i = 0; i < vector.size(); i++) {
        os << (i == 0 ? "" : ",") << vector[i];
    }
    return os << "]";
}

#endif // BOOST_SELF_HEALING_VECTOR_HPP

ext: hpp


text:
#include <df/optimization/nonrigidIcp.h>

#include <df/optimization/deformationGraphRegularization.h>
#include <df/util/dualQuaternion.h>
#include <df/util/typeList.h>

#include <sophus/se3.hpp>

#include <Eigen/CholmodSupport>

#include <fstream> // TODO

namespace df {

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT>
Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> solveSparseLinearSystem(const NonrigidTransformer<Scalar,TransformT> & transformer,
                                                                  const Eigen::SparseMatrix<ScalarOpt> & JTJ,
                                                                  const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & JTr,
                                                                  const Scalar diagonalRegularization) {

    typedef Eigen::SparseMatrix<ScalarOpt> SparseMat;

    static Eigen::CholmodSimplicialLDLT<SparseMat, Eigen::Upper> solver;

    static uint lastTransformerSerialNumber = std::numeric_limits<uint>::max();

    const uint currentTransformerSerialNumber = transformer.serialNumber();

//    std::cout << "summed: " << std::endl;
//    std::cout << "max: " << JTJ.toDense().maxCoeff() << std::endl;
//    std::cout << "min: " << JTJ.toDense().minCoeff() << std::endl;

//    std::cout << JTJ.rows() << " x " << JTJ.cols() << std::endl;
//    std::cout << JTr.rows() << " x " << JTr.cols() << std::endl;

//    std::cout << "max: " << JTr.maxCoeff() << std::endl;
//    std::cout << "min: " << JTr.minCoeff() << std::endl;

    for (int k=0; k<JTJ.outerSize(); ++k) {
        for (typename SparseMat::InnerIterator it(JTJ,k); it; ++it) {
//            std::cout << it.row() << ", " << it.col() << std::endl;
            if (!std::isfinite(it.value())) {
                throw std::runtime_error(std::to_string(it.row()) + "," + std::to_string(it.col()) + " not finite (" + std::to_string(it.value()) + ")");
            }
        }
    }

    for (int r = 0; r < JTr.rows(); ++r) {
        if (!std::isfinite(JTr(r))) {
            throw std::runtime_error("row " + std::to_string(r) + " not finite (" + std::to_string(JTr(r)) + ")");
        }
    }

    // check if the transformer's serial number has changed.
    // if so, the deformation graph has been updated and we should
    // redo the pattern analysis.
    if (currentTransformerSerialNumber != lastTransformerSerialNumber) {

        solver.analyzePattern(JTJ);

        lastTransformerSerialNumber = currentTransformerSerialNumber;

    }

    solver.setShift(diagonalRegularization);

    solver.factorize(JTJ);

    return -solver.solve(JTr);

}

//template Eigen::VectorXd solveSparseLinearSystem(const NonrigidTransformer<double,DualQuaternion> &,
//                                                 const Eigen::SparseMatrix<double> &,
//                                                 const Eigen::VectorXd &,
//                                                 const double);

//template Eigen::VectorXd solveSparseLinearSystem(const NonrigidTransformer<double,Sophus::SE3Group> &,
//                                                 const Eigen::SparseMatrix<double> &,
//                                                 const Eigen::VectorXd &,
//                                                 const double);

template <typename Scalar, int Options, template <typename,int...> class TransformT>
inline TransformT<Scalar,Options> applyUpdate(TransformT<Scalar,Options> & transform,
                                      const TransformT<Scalar,Options> & update,
                                      const IntToType<internal::TransformUpdateLeftMultiply>) {

//    std::cout << "left update" << std::endl;
    return update*transform;

}

template <typename Scalar, int Options, template <typename,int...> class TransformT>
inline TransformT<Scalar,Options> applyUpdate(TransformT<Scalar,Options> & transform,
                                      const TransformT<Scalar,Options> & update,
                                      const IntToType<internal::TransformUpdateRightMultiply>) {

//    std::cout << "right update" << std::endl;
    return transform*update;

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U = internal::TransformUpdateLeftMultiply>
void updateDeformationGraphTransforms(NonrigidTransformer<Scalar,TransformT> & transformer,
                                      const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & vectorizedUpdate) {

    typedef TransformT<Scalar> Transform;

    static constexpr uint BlockDim = 6;

    assert(vectorizedUpdate.rows() == BlockDim * transformer.numVerticesTotal());

    uint runningVertexCount = 0;

    for (uint level = 0; level < transformer.numRegularizationTreeLevels(); ++level) {

//        std::cout << "updating level " << level << std::endl;

        for (uint index = 0; index < transformer.numVerticesAtLevel(level); ++index) {

//            std::cout << "updating vertex " << index << std::endl;

//            std::cout << vectorizedUpdate.template block<6,1>((runningVertexCount + index)*BlockDim,0).transpose() << std::endl;

            // TODO: investigate an exponential mappint directly into the DualQuaternion space
            Transform update = Sophus::SE3Group<Scalar>::exp(vectorizedUpdate.template block<6,1>((runningVertexCount + index)*BlockDim,0).template cast<Scalar>());

            Transform & transform = transformer.transforms(level)[index];

            transform = applyUpdate(transform,update,IntToType<U>());

        }

        runningVertexCount += transformer.numVerticesAtLevel(level);

    }

}

//template void updateDeformationGraphTransforms(NonrigidTransformer<float,Sophus::SE3Group> &,
//                                               const Eigen::VectorXf &);

//template void updateDeformationGraphTransforms(NonrigidTransformer<float,DualQuaternion> &,
//                                               const Eigen::VectorXf &);

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U = internal::TransformUpdateLeftMultiply>
void icpNonrigid(NonrigidTransformer<Scalar,TransformT> & transformer,
                 Eigen::SparseMatrix<ScalarOpt> & dataJTJ,
                 Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & dataJTr,
                 const Scalar diagonalRegularization,
                 const Scalar regularizationWeight) {

    Eigen::SparseMatrix<ScalarOpt> regularizerJTJ(6*transformer.numVerticesTotal(),6*transformer.numVerticesTotal());
    Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> regularizerJTr(6*transformer.numVerticesTotal());

    std::cout << "compute regularizer normal equations:" << std::endl;
    computeRegularizerNormalEquations<Scalar,ScalarOpt,TransformT,U>(transformer,regularizerJTJ,regularizerJTr);

//    std::cout << regularizerJTJ.toDense() << std::endl;

    std::cout << "solve sparse linear system" << std::endl;
    const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> solution = solveSparseLinearSystem<Scalar,ScalarOpt,TransformT>(transformer,
//                                                                                dataJTJ,
//                                                                                dataJTr,
                                                                                regularizationWeight*regularizerJTJ + dataJTJ,
                                                                                regularizationWeight*regularizerJTr + dataJTr,
//                                                                                regularizerJTJ,
//                                                                                regularizerJTr,
                                                                                diagonalRegularization);
    std::cout << "update deformation graph transforms" << std::endl;
    updateDeformationGraphTransforms<Scalar,ScalarOpt,TransformT,U>(transformer,solution);

}

template void icpNonrigid<float,double,Sophus::SE3Group,internal::TransformUpdateLeftMultiply>
    (NonrigidTransformer<float,Sophus::SE3Group> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template void icpNonrigid<float,double,Sophus::SE3Group,internal::TransformUpdateRightMultiply>
    (NonrigidTransformer<float,Sophus::SE3Group> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template void icpNonrigid<float,double,DualQuaternion,internal::TransformUpdateLeftMultiply>
    (NonrigidTransformer<float,DualQuaternion> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template void icpNonrigid<float,double,DualQuaternion,internal::TransformUpdateRightMultiply>
    (NonrigidTransformer<float,DualQuaternion> &,
     Eigen::SparseMatrix<double> &,
     Eigen::VectorXd &, float, float);

template <typename Scalar, typename ScalarOpt, typename CameraModelT,
          template <typename,int...> class TransformT, int K,
          internal::TransformUpdateMethod U = internal::TransformUpdateLeftMultiply,
          typename ... DebugArgTs>
void nonrigidICP(const DeviceTensor2<Eigen::UnalignedVec3<Scalar> > & liveVertices,
                 DeviceTensor2<Eigen::UnalignedVec3<Scalar> > & predictedWarpedVertices,
                 DeviceTensor2<Eigen::UnalignedVec3<Scalar> > & predictedWarpedNormals,
                 const DeviceTensor2<Eigen::UnalignedVec4<Scalar> > & predictedCanonicalVertices,
                 const DeviceTensor2<Eigen::UnalignedVec4<Scalar> > & predictedCanonicalNormals,
                 const CameraModelT & cameraModel,
                 NonrigidTransformer<Scalar,TransformT> & transformer,
                 const Sophus::SE3Group<Scalar> & updatePredictionToLive,
                 const Eigen::Matrix<Scalar,2,1> & depthRange,
                 const uint numIterations,
                 const Scalar diagonalRegularization,
                 const Scalar regularizationWeight,
                 DebugArgTs ... debugArgs) {

    typedef Eigen::Triplet<ScalarOpt> Triplet;
    typedef Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> VecX;

    static constexpr int ModelDim = 6;

    const int numBaseLevelVertices = transformer.numVerticesAtLevel(0);

    const int numVerticesTotal = transformer.numVerticesTotal();

    for (uint iteration = 0; iteration < numIterations; ++iteration) {

        // -=-=-=- update prediction -=-=-=-
        transformer.template warpMesh<K,2,4>(predictedWarpedVertices, predictedWarpedNormals,
                                             predictedCanonicalVertices, predictedCanonicalNormals);

        // -=-=-=- data term -=-=-=-
        std::vector<Triplet> JTJTriplets;
        VecX dataJTr;

        internal::computeDataNormalEquations<Scalar,ScalarOpt,CameraModelT,TransformT,K,U,DebugArgTs...>(
                    liveVertices,
                    predictedWarpedVertices, predictedWarpedNormals,
                    predictedCanonicalVertices, predictedCanonicalNormals,
                    cameraModel, transformer, updatePredictionToLive, depthRange,
                    JTJTriplets, dataJTr, debugArgs...);

        Eigen::SparseMatrix<ScalarOpt> dataJTJ(numVerticesTotal * ModelDim, numVerticesTotal * ModelDim);

        dataJTJ.setFromTriplets(JTJTriplets.begin(), JTJTriplets.end());

        dataJTJ.finalize();

        //    std::cout << "data: " << std::endl;

        //    std::ofstream dataJTJStream("/tmp/dataJTJ.txt");
        //    dataJTJStream << dataJTJ.toDense() << std::endl;
        //    std::cout << "max: " << dataJTJ.toDense().maxCoeff() << std::endl;
        //    std::cout << "min: " << dataJTJ.toDense().minCoeff() << std::endl;

        //    std::cout << dataJTJ.coeffRef(588,588) << std::endl;

        // -=-=-=- regularization term -=-=-=-

        Eigen::SparseMatrix<ScalarOpt> regularizerJTJ(ModelDim * numVerticesTotal,ModelDim * numVerticesTotal);
        Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> regularizerJTr(ModelDim * numVerticesTotal);

        computeRegularizerNormalEquations<Scalar,ScalarOpt,TransformT,U>(transformer,regularizerJTJ,regularizerJTr);

        //    std::cout << "reg: " << std::endl;

        //    std::ofstream regJTJStream("/tmp/regJTJ.txt");
        //    regJTJStream << regularizerJTJ.toDense() << std::endl;

        //    std::cout << "max: " << regularizerJTJ.toDense().maxCoeff() << std::endl;
        //    std::cout << "min: " << regularizerJTJ.toDense().minCoeff() << std::endl;

        //    std::cout << regularizerJTJ.coeffRef(588,588) << std::endl;

        // -=-=-=- solution -=-=-=-
        Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> fullDataJTr(ModelDim * numVerticesTotal);

        fullDataJTr.head(ModelDim*numBaseLevelVertices) = dataJTr;

        fullDataJTr.tail(ModelDim*(numVerticesTotal - numBaseLevelVertices)) = Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1>::Zero( ModelDim*(numVerticesTotal - numBaseLevelVertices),1);

        const Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> solution = solveSparseLinearSystem<Scalar,ScalarOpt,TransformT>(transformer,
                                                                                regularizationWeight*regularizerJTJ + dataJTJ,
                                                                                regularizationWeight*regularizerJTr + fullDataJTr,
                                                                                diagonalRegularization);

        // -=-=-=- solution -=-=-=-
        updateDeformationGraphTransforms<Scalar,ScalarOpt,TransformT,U>(transformer,solution);

    }

}

#define NONRIGID_ICP_EXPLICIT_INSTANTIATION(type, opt_type, camera, transform, K, update)                                       \
template void nonrigidICP<type,opt_type,camera##CameraModel<type>,transform, K, internal::TransformUpdate##update##Multiply>(   \
    const DeviceTensor2<Eigen::UnalignedVec3<type>  > &,                                                                        \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const camera##CameraModel<type> &,                                                                                \
    NonrigidTransformer<type,transform> &,                                                                                \
    const Sophus::SE3Group<type> &,                                                                                \
    const Eigen::Matrix<type,2,1> &,                                                                                \
    const uint, const type, const type);                                                                                \
                                                                                \
template void nonrigidICP<type,opt_type,camera##CameraModel<type>,transform, K, internal::TransformUpdate##update##Multiply, DeviceTensor2<Eigen::UnalignedVec4<uchar> > >(   \
    const DeviceTensor2<Eigen::UnalignedVec3<type>  > &,                                                                        \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    DeviceTensor2<Eigen::UnalignedVec3<type> > &,                                                                               \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const DeviceTensor2<Eigen::UnalignedVec4<type>  > &,                                                                        \
    const camera##CameraModel<type> &,                                                                                \
    NonrigidTransformer<type,transform> &,                                                                                \
    const Sophus::SE3Group<type> &,                                                                                \
    const Eigen::Matrix<type,2,1> &,                                                                                \
    const uint, const type, const type,                                                                                \
    DeviceTensor2<Eigen::UnalignedVec4<uchar> > )

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, DualQuaternion, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, DualQuaternion, 4, Right);

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, Sophus::SE3Group, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Poly3, Sophus::SE3Group, 4, Right);

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, DualQuaternion, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, DualQuaternion, 4, Right);

NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, Sophus::SE3Group, 4, Left);
NONRIGID_ICP_EXPLICIT_INSTANTIATION(float, double, Linear, Sophus::SE3Group, 4, Right);

} // namespace df

ext: cpp


text:
/*
*** Copyright Notice ***
If you have questions about your rights to use or distribute this software, please contact Berkeley Lab's Intellectual Property Office at  IPO@lbl.gov.
NOTICE.  This Software was developed under funding from the U.S. Department of Energy and the U.S. Government consequently retains certain rights. As such, the U.S. Government has been granted for itself and others acting on its behalf a paid-up, nonexclusive, irrevocable, worldwide license in the Software to reproduce, distribute copies to the public, prepare derivative works, and perform publicly and display publicly, and to permit other to do so.
****************************
*** License Agreement ***
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 University of California, Lawrence Berkeley National Laboratory, U.S. Dept. of Energy, 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 OWNER 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.
You are under no obligation whatsoever to provide any bug fixes, patches, or upgrades to the features, functionality or performance of the source code ("Enhancements") to anyone; however, if you choose to make your Enhancements available either publicly, or directly to Lawrence Berkeley National Laboratory, without imposing a separate written license agreement for such Enhancements, then you hereby grant the following license: a  non-exclusive, royalty-free perpetual license to install, use, modify, prepare derivative works, incorporate into other computer software, distribute, and sublicense such enhancements or derivative works thereof, in binary and source code form.
---------------------------------------------------------------

 */

/*
 * File:   serviceInstanceTemplateApp2.cpp
 * Author: mcp
 *
 * Created on January 24, 2016, 5:01 PM
 */
#include <stdint.h>
#include <iostream>
#include <fstream>
#include <chrono>
#include <thread>
#include <atomic>
#include <set>
#include <mutex>
#include <pqxx/pqxx>
#include "cassandra.h"
#include <curl/curl.h>
#include <boost/make_shared.hpp>
#include <boost/thread.hpp>
#include <boost/ref.hpp>
#include <boost/any.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/dll/alias.hpp>
#include <boost/thread/condition.hpp>
#include <log4cpp/Appender.hh>
#include <log4cpp/Category.hh>
#include <log4cpp/FileAppender.hh>
#include <log4cpp/OstreamAppender.hh>
#include <log4cpp/Layout.hh>
#include <log4cpp/BasicLayout.hh>
#include <log4cpp/Priority.hh>

#include "dbConnection.h"
#include "domainAndMessageTypes.h"
#include "serviceCommand.h"
#include "serviceCommandQueueTemplate.h"
#include "serviceInstanceApi.h"
#include "serviceInstance.h"
#include "serviceInstanceList.h"
#include "serviceStatus.h"
#include "serviceDataBuffer.h"
#include "serviceOperationsLoggerPostgres.h"
#include "serviceOperationsLoggerCassandra.h"
#include "upmuFtpAccess.h"
#include "upmuStructKeyValueListProtobuf.pb.h"

using namespace serviceCommon;

#include "serviceCommonStats.h"
#include "serviceSpecificStats.h"
#include "uPMUgmtime.h"

#include "serviceDataDiscovery.h"

#define IS_BIG_ENDIAN (*(uint16_t *)"\0\xff" < 0x100)

namespace serviceDataDiscovery {
using namespace pqxx;
using namespace serviceCommon;
using namespace boost;

serviceDataDiscovery::serviceDataDiscovery() {
    std::cout<< "Creating serviceDataDiscovery" << std::endl;
    cmdQueue = new SynchronisedCommandQueue<SERVICE_COMMAND *>(CMD_QUEUE_LEN);
    m_thread=NULL;
    m_mustStop=false;
    m_msg_num=0;
    serviceStatus = NOT_STARTED;
}

serviceDataDiscovery::~serviceDataDiscovery() {
    std::cout<< "Destroying serviceDataDiscovery" << std::endl;
    if (m_thread!=NULL) delete m_thread;
}

// Start the thread
void serviceDataDiscovery::start() {
    // Create thread and start it with myself as argument.
    // Pass myself as reference since I don't want a copy
    m_mustStop = false;
    /* start thread.  operator() () is the function executed */
    m_thread=new boost::thread(boost::ref(*this));
}

// Stop the thread
void serviceDataDiscovery::stop()
{
    // Signal the thread to stop (thread-safe)
    serviceStatus = STOPPING;
    m_mustStopMutex.lock();
    m_mustStop=true;
    m_mustStopMutex.unlock();
    // notify running thread
    wake_up.notify_one();

    // Wait for the thread to finish.
    if (m_thread!=NULL) m_thread->join();
}

dataBufferOffer_t serviceDataDiscovery::dataBufferOffer(
    const std::shared_ptr<serviceDataBuffer>& offeredBuf) {
    //std::cout<< "serviceDataDiscovery::dataBufferOffer" << std::endl;
    /* this service does not accept buffers */
    return BUFFER_REFUSED;
}

// Thread function
void serviceDataDiscovery::operator () () {
    /* init some local flags */
    bool localMustStop = false;
    SERVICE_COMMAND * cmd;
    serviceStatus = RUNNING;
    /* create log4cpp related appender objects for this service. */
    logger_serviceDataDiscovery.addAppender(logger_appender);
    /* log startup message */
    logger_serviceDataDiscovery.info("serviceDataDiscovery starting.");

    if(!initService()) {
        /* log error and exit*/
        logger_serviceDataDiscovery.error("Could not complete initialization, exiting.");
        return;
    }

    try {
        while(!localMustStop) {
            /* always check for possible command */
            cmd = cmdQueue->Dequeue();
            if(cmd != 0) {
                processCmd(cmd);
            }
            processUPMUdata();
            /* sleep for a bit. Interval changes based on operating mode. */
            if(scanMode == normal) {
                std::this_thread::sleep_for(std::chrono::seconds(SCAN_MODE_NORMAL_DELAY_SEC));
            }
            else if(scanMode == catch_up) {
                std::this_thread::sleep_for(std::chrono::seconds(SCAN_MODE_CATCH_UP_DELAY_SEC));
            }
            else {
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
            // Get the "must stop" state (thread-safe)
            m_mustStopMutex.lock();
            localMustStop=m_mustStop;
            m_mustStopMutex.unlock();
        }
        // exiting function kills thread
        serviceStatus = NORMAL_STOP;
        logger_serviceDataDiscovery.info("Normal (requested) service stop.");
        return;
    }
    catch (const std::exception &e) {
        serviceStatus = ERROR_STOP;
        logger_serviceDataDiscovery.error("Abnormal (error) service stop.");
        return;
    }
};

void serviceDataDiscovery::processCmd(SERVICE_COMMAND * cmd) {
    std::cout<< "         Process a command." << std::endl;

}

bool serviceDataDiscovery::initService() {
    /* init service data structures */
    createServiceCommonStats(this);
    createServiceSpecificStats(this);

    /* init file length tracker and initial serviceState. */
    mostRecentDataFileLength = 0;
    scanMode = catch_up;
    serviceState = STATE_CATCH_UP;

    /* get a db connection for local database */
    dbConn = dbConnection::getInstance();
    if(dbConn->conn == NULL) {
        /* log an error and exit. */
        return false;
    }

    /* prepare any postgres statements used regularly */
    dbConn->acquireDBaccess();
    (dbConn->conn)->prepare("add_dir_record",
            "INSERT INTO upmu_data_directory (year, month, day, hour, "
                "dir_name, state, discovery_time, release_time) VALUES "
                "($1, $2, $3, $4, $5, $6, "
                "NOW(), NULL)");
    (dbConn->conn)->prepare("get_dir_record_key",
            "SELECT currval('upmu_data_directory_id_seq')");
    (dbConn->conn)->prepare("release_dir_record",
            "UPDATE upmu_data_directory SET (state, release_time) = ('processed', NOW()) WHERE id = $1");
    (dbConn->conn)->prepare("abandon_dir_record",
            "UPDATE upmu_data_directory SET (state, release_time) = ('abandoned', NOW()) WHERE id = $1");
    (dbConn->conn)->prepare("update_dir_record_state",
            "UPDATE upmu_data_directory SET state = $1 WHERE id = $2");
    (dbConn->conn)->prepare("add_data_record",
            "INSERT INTO upmu_data_file (file_name, directory, state, "
            "access_start_time, release_time) VALUES "
            "($1, $2, $3, NOW(), NULL)");
    (dbConn->conn)->prepare("release_data_record",
            "UPDATE upmu_data_file SET (state, release_time) = ('released', NOW()) WHERE id = $1");
    (dbConn->conn)->prepare("abandon_data_record",
            "UPDATE upmu_data_file SET (state, release_time) = ('abandoned', NOW()) WHERE id = $1");
    (dbConn->conn)->prepare("get_data_record_key",
            "SELECT currval('upmu_data_file_id_seq')");
    (dbConn->conn)->prepare("search_data_record_by_dir_index",
            "SELECT * FROM upmu_data_file WHERE directory = $1");
    dbConn->releaseDBaccess();
    logger_serviceDataDiscovery.info("Postgres statements prepared.");

    /* declare uPMUgateway started at this point. Note: startup message always
     contains sessionID and major/minor version info.  Also, these params are ad hoc
     in the gateway; but are part of standard plugin statistices package in plugins. */
    postgresOperationsLogger->createStringOperationsRecord(dbConn, serviceState, upmuSerialNumber,
        sessionID, serviceName, "Service_Startup_Msg", "Initialized, looking for current directory on uPMU.");
    postgresOperationsLogger->createStringOperationsRecord(dbConn, serviceState, upmuSerialNumber,
        sessionID, serviceName,
        (*serviceSpecStatsLabels)[INDX_STR_DATA_DISCOVERY_MAJ_VER]->c_str(),
        any_cast<std::string *>((*serviceSpecStatsValues)[INDX_STR_DATA_DISCOVERY_MAJ_VER])->c_str());
    postgresOperationsLogger->createStringOperationsRecord(dbConn, serviceState, upmuSerialNumber,
        sessionID, serviceName,
        (*serviceSpecStatsLabels)[INDX_STR_DATA_DISCOVERY_MIN_VER]->c_str(),
        any_cast<std::string *>((*serviceSpecStatsValues)[INDX_STR_DATA_DISCOVERY_MIN_VER])->c_str());

    if(cassArchivingEnabled && cassConnected) {
        /* make a KeyValueList to log similar message to cassandra, if available.*/
        upmuStructKeyValueList * kvList = new upmuStructKeyValueList();
        createServiceStartupKeyValueList(kvList);
        uint32_t byteLen = kvList->ByteSize();
        void * pbBuffer = std::malloc(byteLen);
        if(pbBuffer != nullptr) {
            kvList->SerializeToArray(pbBuffer, byteLen);
            cassandraOperationsLogger->cassandraArchive(cassPreparedOperations, cassSession,
                (uint64_t)kvList->timestamp(), sessionID, serviceName,
                upmuSerialNumber, DOMAIN_OPERATIONS, MSG_KEY_VALUE_LIST, pbBuffer, byteLen);
            try{
                std::free(pbBuffer);
            }
            catch(...){

            }
        }
        else {
            logger_serviceDataDiscovery.error("Cannot form serialized protocol buffer, "
                "insufficient memory.");
        }
    }

    /* create several structures used throughout thread */
    dataDir = new DATA_DIR();

    int sts =getCurrentDirectory();
    if(sts != DATA_DIR_OK) {
        return false;
    }
    sts = getCurrentDataFileSet();
    if(sts != DATA_DIR_OK) {
        return false;
    }
    return true;
}

void serviceDataDiscovery::createServiceStartupKeyValueList(upmuStructKeyValueList * kvList) {
    kvList->set_name("Service_Startup_Info");
    uint32_t startTime = (uint32_t)time(NULL);
    kvList->set_timestamp(startTime);
    KeyValueList * list = kvList->add_list();
    list->set_category("ServiceStatus");

    /* add a few key values */
    serviceSpecStatsMutex.lock();
    KeyValue * pair = list->add_element();
    pair->set_key((*serviceSpecStatsLabels)[INDX_STR_DATA_DISCOVERY_MAJ_VER]->c_str());
    pair->set_stringval(any_cast<std::string *>
                ((*serviceSpecStatsValues)[INDX_STR_DATA_DISCOVERY_MAJ_VER])->c_str());

    pair = list->add_element();
    pair->set_key((*serviceSpecStatsLabels)[INDX_STR_DATA_DISCOVERY_MIN_VER]->c_str());
    pair->set_stringval(any_cast<std::string *>
                ((*serviceSpecStatsValues)[INDX_STR_DATA_DISCOVERY_MIN_VER])->c_str());
    serviceSpecStatsMutex.unlock();
}

int serviceDataDiscovery::getCurrentDirectory() {
    try {
        /* get list of current data directory - if we have one */
        bool startingDirInDB = false;

        dbConn->acquireDBaccess();
        nontransaction n(*(dbConn->conn));
        result r = n.exec("SELECT * FROM upmu_data_directory WHERE state = 'current'");
        if(r.size() == 1) {
            dataDir->year = r[0]["\"year\""].as<int>();
            dataDir->month = r[0]["\"month\""].as<int>();
            if((dataDir->month < 1) || (dataDir->month >12)) {
                /* month not in correct range */
                n.commit();
                dbConn->releaseDBaccess();
                return DATA_DIR_ERROR;
            }
            dataDir->day = r[0]["\"day\""].as<int>();
            if(dataDir->day <= 0) {
                /* days start at 1, not in correct range */
                n.commit();
                dbConn->releaseDBaccess();
                return DATA_DIR_ERROR;
            }
            dataDir->hour = r[0]["\"hour\""].as<int>();
            dataDir->dbID = r[0]["\"id\""].as<int>();
            dataDir->dirName = upmuFtpAccess::createDirName_YrMonDayHr(dataDir);
            dataDir->state = current;
            startingDirInDB = true;
        }
        else {
            result r = n.exec("SELECT * FROM upmu_data_directory WHERE state = 'processed'"
                    " ORDER BY id DESC LIMIT 1");
            if(r.size() == 1) {
                dataDir->year = r[0]["\"year\""].as<int>();
                dataDir->month = r[0]["\"month\""].as<int>();
                if((dataDir->month < 1) || (dataDir->month >12)) {
                    /* month not in correct range */
                    n.commit();
                    dbConn->releaseDBaccess();
                   return DATA_DIR_ERROR;
                }
                dataDir->day = r[0]["\"day\""].as<int>();
                if(dataDir->day <= 0) {
                    /* days start at 1, not in correct range */
                    n.commit();
                    dbConn->releaseDBaccess();
                    return DATA_DIR_ERROR;
                }
                dataDir->hour = r[0]["\"hour\""].as<int>();
                dataDir->dbID = r[0]["\"id\""].as<int>();
                dataDir->dirName = upmuFtpAccess::createDirName_YrMonDayHr(dataDir);
                dataDir->state = processed;
                startingDirInDB = true;
            }
        }
        if(!startingDirInDB) {
            /* there is no current uPMU data directory, s propose one */
            dataDir->year = DEFAULT_STARTING_YEAR;
            dataDir->month = DEFAULT_STARTING_MONTH;
            dataDir->day = DEFAULT_STARTING_DAY;
            dataDir->hour = DEFAULT_STARTING_HOUR;
            dataDir->dirName = upmuFtpAccess::createDirName_YrMonDayHr(dataDir);
            dataDir->state = proposed;
        }
        /* close transaction */
        n.commit();
        dbConn->releaseDBaccess();

        unsigned int et = upmuFtpAccess::createTimeFromDataDir(dataDir);
        dataDir->epochDay = et / 86400;

        /* loop til most recent directory is found */
        bool termLoop = false;
        bool ftpError = false;
        dataDirValid = false;
        mostRecentDataFileNameValid = false;
        int sts;
        while (!termLoop) {
            /* deal with special case that we have no current dir,
             but have latest processed account.  In that case, we need
             to inc directory before checking for availability. */
            if(dataDir->state == processed) {
                sts = upmuFtpAccess::incDataDir(dataDir);
                    if(sts == DATA_DIR_AT_PRESENT) {
                        dataDirValid = false;
                        termLoop = true;
                        return NO_DATA_DIR;
                   }
            }
            /* is directory valid? */
            sts = upmuFtpAccess::verifyDataDirPresent(dataDir->dirName);
            if(sts == FTP_NOT_RESP) {
                /* ftp error */
                ftpError = true;
                dataDirValid = false;
                termLoop = true;
            }
            else if(sts == DIR_PRESENT) {
                /* dirctory present - are we finished processing it? */
                std::cout << "Directory Present: " << dataDir->dirName << std::endl;
                if(dataDir->state == proposed) {
                    /* save proposed directory in db as current dir */
                    dbConn->acquireDBaccess();
                    work w(*(dbConn->conn));
                    w.prepared("add_dir_record")(dataDir->year)(dataDir->month)\
                        (dataDir->day)(dataDir->hour)((dataDir->dirName).c_str())\
                        ("current").exec();
                        w.commit();
                    /* select it so we can use common code at end of this section.  */
                    nontransaction n(*(dbConn->conn));
                    r = n.exec("SELECT * FROM upmu_data_directory WHERE state = 'current'");
                    n.commit();
                    dataDir->dbID = r[0]["\"id\""].as<int>();
                    dbConn->releaseDBaccess();
                    dataDir->state = current;
                }

                dataDirValid = true;
                termLoop = true;
            }
            else {
                std::cout << "Directory Not Present: " << dataDir->dirName.c_str() << std::endl;
                if(dataDir->state == current) {
                    /* we need to move to new directory, mark this one as abandoned */
                    dbConn->acquireDBaccess();
                    work w(*(dbConn->conn));
                    w.prepared("abandon_dir_record")(dataDir->dbID).exec();
                    w.commit();
                    dbConn->releaseDBaccess();
                }
                sts = upmuFtpAccess::incDataDir(dataDir);
                if(sts == DATA_DIR_AT_PRESENT) {
                    dataDirValid = false;
                    termLoop = true;
                }

            }
        }
        return DATA_DIR_OK;
    }
    catch (const std::exception &e) {
        /* log error */
        std::cerr << e.what() << std::endl;
        return NO_DATA_DIR;
    }
}

int serviceDataDiscovery::getCurrentDataFileSet() {

    try {
        dbConn->acquireDBaccess();
        nontransaction n(*(dbConn->conn));
        result r = n.prepared("search_data_record_by_dir_index")(dataDir->dbID).exec();
        n.commit();

        if(r.size() == 0) {
            /* no data files from this directory have been processed.
            Set scan context variables as needed. Note, data processing sets will
            properly initialized in upmuProcessData(). */
            dataDirAccessInProgress = false;
        }
        else {
            dataFilesInDir.clear();
            dataFilesProcessed.clear();
            dataDirAccessInProgress = true;

            for (pqxx::result::const_iterator row = r.begin(); row != r.end(); row++) {
                /* get important fields into local variables */
                pqxx::result::field field = row->at("state");
                std::string s = field.as<std::string>();
                if((s == "abandoned") || (s == "released")) {
                    /* we have dealt with this file. */
                    field = row->at("file_name");
                    std::string t = field.as<std::string>();
                    dataFilesProcessed.insert(t);
                }
            }
        }
        dbConn->releaseDBaccess();
        return DATA_DIR_OK;
    }
    catch (const std::exception &e) {
        /* log error */
        dbConn->releaseDBaccess();
    }
    return DATA_DIR_ERROR;
}

int serviceDataDiscovery::addDataFile2DB(upmuFtpAccess * upmuDaq,
    std::string fileName, const std::string& status) {

    return 0;
}

int serviceDataDiscovery::processUPMUdata() {
    if(dataDirValid) {
        if(!dataDirAccessInProgress) {
            dataDirAccessInProgress = true;
            time(&dataDirAccessStartTime);
            dataFilesInDir.clear();
            dataFilesProcessed.clear();
        }
        if(dataFilesInDir.empty()) {
            upmuFtpAccess::getDataFileList(&dataFilesInDir, &dataFilesProcessed,
                &(dataDir->dirName));
            /*******/
        }
        if(!dataFilesInDir.empty()) {
            /* is DataDiscovery service flow controlled? */
            if(sharedInMemDataBufferCnt <= MAX_OUTSTANDING_MEM_BUF) {
                /* process first data file in list */
                std::string dataFile = * dataFilesInDir.begin();
                /* determine if data file is complete or still being filled */
                if(dataFileComplete(&dataFile)) {
                    /* keep a copy for abandon calculation */
                    mostRecentDataFileNameValid =true;
                    mostRecentDataFileName = dataFile;
                    dataFilesProcessed.insert(dataFile);
                    dataFilesInDir.erase(dataFile);
                    try {
                        unsigned char *dataBufAddr;
                        unsigned int fileLen;
                        /* insert record into upmu_data_file table that documents we
                         are processing this data file. */
                        work w(*(dbConn->conn));
                        w.prepared("add_data_record")(dataFile.c_str())(dataDir->dbID)
                            ("processing").exec();
                        w.commit();
                        nontransaction n(*(dbConn->conn));
                        /* get the id for the record we just created. */
                        result r = n.prepared("get_data_record_key").exec();
                        n.commit();
                        int dataRecordID = r[0][0].as<int>();
                        std::cout << "Data File Processed: " << dataFile.c_str() << std::endl;
                        fileLen = upmuFtpAccess::getDataFile(&(dataDir->dirName),
                            &dataFile, &dataBufAddr);
                        postProcessUPMUdata(dataBufAddr, (unsigned int)fileLen);
                        /* save recent file lengths for future use */
                        mostRecentDataFileLength = (unsigned int)fileLen;
                        /* Mark the data file record just created as released. */
                        work w1(*(dbConn->conn));
                        w1.prepared("release_data_record")(dataRecordID).exec();
                        w1.commit();
                    }
                    catch (const std::exception &e) {
                        /* log error */
                        std::cerr << e.what() << std::endl;
                        return DB_ACCESS_ERROR;
                    }
                }
                else {
                    /* data file incomplete or ftp error.  retry and hope upmu
                     will become available. */
                    //DEBUG: std::cout << "Data File Incomplete. " << std::endl;
                }
            }
        }
        /* are we done with - or need to abandon - this directory? */
        if(dataFilesInDir.empty()) {
            time_t rawt;
            time(&rawt);
            if(!dataDirValid) {
                std::cout << " datDirValid in wrong state." << std::endl;
            }
            else {
                unsigned int dirt = upmuFtpAccess::createTimeFromDataDir(dataDir);
                if(((unsigned int)rawt - dirt) > 60 * 60) {
                    /* directory is over 1 hr. old, abandon it. */
                    dataDirValid = false;
                    dataDirAccessInProgress = false;
                    mostRecentDataFileNameValid = false;
                }
            }
        }
    }
    if(!dataDirValid) {
        /* just exhausted data files in this directory */
        /*  mark this one as done */
        if(dataDir->state == current) {
            /* just finished a current directory, so let's mark it as closed */
            try {
                work w(*(dbConn->conn));
                w.prepared("release_dir_record")(dataDir->dbID).exec();
                w.commit();
            }
            catch (const std::exception &e) {
                /* log error */
                std::cerr << e.what() << std::endl;
                return DB_ACCESS_ERROR;
            }
            dataDir->state = unknown;
            dataDirAccessInProgress = false;
        }

        int sts = upmuFtpAccess::isDataDirAtPresent(dataDir);
        if(sts != DATA_DIR_AT_PRESENT) {
            int sts = upmuFtpAccess::incDataDir(dataDir);
        }
        /* we're just beyond the most recent directory */
        /* wait here until something shows up in uPMU */
        sts = upmuFtpAccess::verifyDataDirPresent(dataDir->dirName);
        if(sts == DIR_PRESENT) {
            addCurrentDirectoryRecord();
            dataDir->state = current;
            dataDirValid = true;
            std::cout << "Directory Present: " << dataDir->dirName << std::endl;
        }
        else {
            if(sts == FTP_NOT_RESP){
                std::cout << "FTP connection BROKE, waiting 45minutes to continue: " << std::endl;
                std::this_thread::sleep_for(std::chrono::minutes(45));
            }
            else{
            /*********/
            std::cout << "Directory Not Present: " << dataDir->dirName << std::endl;
            }
        }
    }
}

bool serviceDataDiscovery::dataFileComplete(std::string * dataFile) {
    time_t rawt;
    time(&rawt);
    unsigned int dft = upmuFtpAccess::createTimeFromDataFileName(*dataFile);
    if(((unsigned int)rawt - dft) > CATCH_UP_VS_NORMAL_THRESH_SEC) {
        /* data file is sufficiently old, it must be complete */
        scanMode = catch_up;
        serviceState = STATE_CATCH_UP;
        return true;
    }
    else {
        double fileLength;
        scanMode = normal;
        serviceState = STATE_NORMAL;
        int sts = upmuFtpAccess::getDataFileLength(
            &(dataDir->dirName), dataFile, &fileLength);
        if(sts != DATA_ACCESS_OK) {
            return false;
        }
        else {
            unsigned int fileSize = (unsigned int)fileLength;
            if(fileSize < mostRecentDataFileLength) {
                /* not the same size as previous data files, so we wait.
                 Note that if file lengths change in uPMU configure, we will
                 take an earlier branch once its old enough.  */
                return false;
            }
            else {
                return true;
            }
        }

    }
}

void serviceDataDiscovery::postProcessUPMUdata(unsigned char *dataBuf,
    unsigned int dataLen) {
    int sts;
    unsigned char * dataPtr = dataBuf;

    /* inspect first top bytes of year  */
    bool dataBigEndian = false;
    bool platformBigEndian = IS_BIG_ENDIAN;
    if((*(dataPtr + 4) == 0) && (*(dataPtr + 5) == 0)) {
        dataBigEndian = true;
    }
    if(platformBigEndian != dataBigEndian) {
        /* we need to swap bytes in the buffer */
        while (dataPtr < dataBuf + dataLen) {
            unsigned char * tempDataPtr = dataPtr;
            /* loop over 1 sec. of data */
            while (tempDataPtr < (dataPtr + 6312)) {
                /* treat everything as if it were 32 bit value */
                swapBytesUint((unsigned int *)tempDataPtr);
                tempDataPtr += 4;
            }
            dataPtr += 6312;
        }
    }

    std::shared_ptr<serviceDataBuffer>
            ptr(new serviceDataBuffer(&sharedInMemDataBufferCnt,
            &sharedTotalDataBufferCnt, dataLen, dataBuf));
    /* offer data to other services */
    for (auto si: services->services) {
        if(si.second->serviceAcceptsData== true) { //this is better than the old solution
    //    if(si.first.compare("serviceDataDiscovery") != 0) {
    //       if(si.first.compare("serviceDataArchiver") == 0) {
                dataBufferOffer_t sts =
                    si.second->serviceThread->dataBufferOffer(ptr);
                std::cout << "Discovery Buffer offered, internal counter: " << sharedInMemDataBufferCnt << ", to" << si.first << std::endl;
      //      }
        }
    }
    /* release our reference to the shared pointer.  If any other service is
     using it, they have already added their own reference.  It will be destroyed when the
     last reference is released. */
    ptr.reset();
}

void serviceDataDiscovery::swapBytesUint(unsigned int * val) {
    unsigned int temp = *val;
    *val =  ((((temp) & 0xff000000) >> 24) |
                (((temp) & 0x00ff0000) >>  8) |
                (((temp) & 0x0000ff00) <<  8) |
                (((temp) & 0x000000ff) << 24));
}

int serviceDataDiscovery::cassandraCommonArchive(const CassPrepared * prepared,
        uint32_t statsIndex,
        uint64_t timestampMsec, uint64_t day,
        std::string device, uint32_t domainType, uint32_t msgType,
        void * buffer, uint32_t byteLen) {

    CassStatement * statement = cass_prepared_bind(prepared);
    /* Bind the values using the indices of the bind variables */
    cass_statement_bind_int64_by_name(statement, "TIMESTAMP_MSEC", timestampMsec);
    cass_statement_bind_int64_by_name(statement, "DAY", day);
    cass_statement_bind_string_by_name(statement, "DEVICE", device.c_str());
    cass_statement_bind_int32_by_name(statement, "DOMAIN_TYPE", domainType);
    cass_statement_bind_int32_by_name(statement, "MSG_TYPE", msgType);
    cass_statement_bind_bytes_by_name(statement, "DATA", (const cass_byte_t *)buffer, byteLen);

    CassFuture* cassQuery_future = cass_session_execute(cassSession, statement);

    /* This will block until the query has finished */
    CassError rc = cass_future_error_code(cassQuery_future);
    if(rc != CASS_OK) {
        /* count errors for each cassandra table */
        serviceSpecStatsMutex.lock();
        unsigned int * cnt = any_cast<unsigned int *>
            ((*serviceSpecStatsValues)[statsIndex]);
        *cnt++;
        serviceSpecStatsMutex.unlock();
    }
    /* Statement objects can be freed immediately after being executed */
    cass_statement_free(statement);
    cass_future_free(cassQuery_future);
}

void serviceDataDiscovery::addCurrentDirectoryRecord() {

    try {
        work w(*(dbConn->conn));
        w.prepared("add_dir_record")(dataDir->year)(dataDir->month)\
            (dataDir->day)(dataDir->hour)((dataDir->dirName).c_str())\
            ("current").exec();
            w.commit();
        /* select it so we can use common code at end of this section.  */
        nontransaction n(*(dbConn->conn));
        result r = n.prepared("get_dir_record_key").exec();
        n.commit();
        dataDir->dbID = r[0][0].as<int>();
    }
    catch (const std::exception &e) {
        /* log error */
        std::cerr << e.what() << std::endl;
    }
}

serviceDataDiscovery plugin;
BOOST_DLL_AUTO_ALIAS(plugin)

}

max_stars_repo_name: lbnl-cybersecurity/sparcs

max_issues_repo_name: lbnl-cybersecurity/sparcs

max_forks_repo_name: lbnl-cybersecurity/sparcs