text:
#pragma once

// `krbn::human_interface_device` can be used safely in a multi-threaded environment.

#include "boost_defs.hpp"

#include "apple_hid_usage_tables.hpp"
#include "cf_utility.hpp"
#include "connected_devices.hpp"
#include "core_configuration.hpp"
#include "device_detail.hpp"
#include "event_queue.hpp"
#include "iokit_utility.hpp"
#include "keyboard_repeat_detector.hpp"
#include "logger.hpp"
#include "spdlog_utility.hpp"
#include "types.hpp"
#include <IOKit/hid/IOHIDDevice.h>
#include <IOKit/hid/IOHIDElement.h>
#include <IOKit/hid/IOHIDQueue.h>
#include <IOKit/hid/IOHIDUsageTables.h>
#include <IOKit/hid/IOHIDValue.h>
#include <boost/algorithm/string.hpp>
#include <boost/optional.hpp>
#include <boost/signals2.hpp>
#include <cstdint>
#include <list>
#include <mach/mach_time.h>
#include <unordered_map>
#include <vector>

namespace krbn {
class human_interface_device final : pqrs::dispatcher::extra::dispatcher_client {
public:
  // Signals (invoked from the shared dispatcher thread)

  boost::signals2::signal<void(void)> opened;
  boost::signals2::signal<void(IOReturn error)> open_failed;

  boost::signals2::signal<void(void)> closed;
  boost::signals2::signal<void(IOReturn error)> close_failed;

  // `event_queue` is not owned by `human_interface_device`.
  boost::signals2::signal<void(std::shared_ptr<event_queue::queue>)> values_arrived;

  boost::signals2::signal<void(IOHIDReportType type, uint32_t report_id, std::shared_ptr<std::vector<uint8_t>>)> report_arrived;

  // Methods

  human_interface_device(const human_interface_device&) = delete;

  human_interface_device(IOHIDDeviceRef _Nonnull device,
                         registry_entry_id registry_entry_id) : dispatcher_client(),
                                                                device_(device),
                                                                registry_entry_id_(registry_entry_id),
                                                                device_id_(types::make_new_device_id(std::make_shared<device_detail>(device))),
                                                                removed_(false),
                                                                opened_(false),
                                                                queue_(nullptr) {
    // ----------------------------------------

    run_loop_thread_ = std::make_unique<cf_utility::run_loop_thread>();

    // ----------------------------------------
    // Retain device_

    CFRetain(device_);

    // Set name_for_log_
    {
      std::stringstream stream;

      if (auto product_name = find_product()) {
        stream << boost::trim_copy(*product_name);
      } else {
        if (auto vendor_id = find_vendor_id()) {
          if (auto product_id = find_product_id()) {
            stream << std::hex
                   << "(vendor_id:0x" << static_cast<uint32_t>(*vendor_id)
                   << ", product_id:0x" << static_cast<uint32_t>(*product_id)
                   << ")"
                   << std::dec;
          }
        }
      }

      stream << " (device_id:" << static_cast<uint32_t>(device_id_) << ")";
      name_for_log_ = stream.str();
    }

    // Create connected_device_.
    {
      std::string manufacturer;
      std::string product;
      if (auto m = iokit_utility::find_manufacturer(device_)) {
        manufacturer = *m;
      }
      if (auto p = iokit_utility::find_product(device_)) {
        product = *p;
      }
      connected_devices::device::descriptions descriptions(manufacturer, product);

      auto vendor_id = iokit_utility::find_vendor_id(device_);
      auto product_id = iokit_utility::find_product_id(device_);
      bool is_keyboard = iokit_utility::is_keyboard(device_);
      bool is_pointing_device = iokit_utility::is_pointing_device(device_);
      device_identifiers identifiers(vendor_id,
                                     product_id,
                                     is_keyboard,
                                     is_pointing_device);

      bool is_built_in_keyboard = false;
      if (is_keyboard &&
          !is_pointing_device &&
          descriptions.get_product().find("Apple Internal ") != std::string::npos) {
        is_built_in_keyboard = true;
      }

      bool is_built_in_trackpad = false;
      if (!is_keyboard &&
          is_pointing_device &&
          descriptions.get_product().find("Apple Internal ") != std::string::npos) {
        is_built_in_trackpad = true;
      }

      connected_device_ = std::make_shared<connected_devices::device>(descriptions,
                                                                      identifiers,
                                                                      is_built_in_keyboard,
                                                                      is_built_in_trackpad);
    }

    // ----------------------------------------
    // Setup elements_

    // Note:
    // Some devices has duplicated entries for same usage_page and usage.
    //
    // For example, there are entries of Microsoft Designer Mouse:
    //
    //   * Microsoft Designer Mouse usage_page 1 usage 2
    //   * Microsoft Designer Mouse usage_page 1 usage 2
    //   * Microsoft Designer Mouse usage_page 1 usage 1
    //   * Microsoft Designer Mouse usage_page 1 usage 56
    //   * Microsoft Designer Mouse usage_page 1 usage 56
    //   * Microsoft Designer Mouse usage_page 1 usage 568
    //   * Microsoft Designer Mouse usage_page 12 usage 568
    //   * Microsoft Designer Mouse usage_page 9 usage 1
    //   * Microsoft Designer Mouse usage_page 9 usage 2
    //   * Microsoft Designer Mouse usage_page 9 usage 3
    //   * Microsoft Designer Mouse usage_page 9 usage 4
    //   * Microsoft Designer Mouse usage_page 9 usage 5
    //   * Microsoft Designer Mouse usage_page 1 usage 48
    //   * Microsoft Designer Mouse usage_page 1 usage 49

    if (auto elements = IOHIDDeviceCopyMatchingElements(device_, nullptr, kIOHIDOptionsTypeNone)) {
      for (CFIndex i = 0; i < CFArrayGetCount(elements); ++i) {
        // Add to elements_.
        if (auto e = cf_utility::get_value<IOHIDElementRef>(elements, i)) {
          CFRetain(e);

#if 0
          logger::get_logger().info("{0} usage_page:{1} usage:{2} min:{3} max:{4}",
                                    name_for_log_,
                                    IOHIDElementGetUsagePage(e),
                                    IOHIDElementGetUsage(e),
                                    IOHIDElementGetLogicalMin(e),
                                    IOHIDElementGetLogicalMax(e));
#endif

          elements_.push_back(e);
        }
      }
      CFRelease(elements);
    }

    // ----------------------------------------
    // setup queue_

    const CFIndex depth = 1024;
    queue_ = IOHIDQueueCreate(kCFAllocatorDefault, device_, depth, kIOHIDOptionsTypeNone);
    if (!queue_) {
      logger::get_logger().error("IOHIDQueueCreate error @ {0}", __PRETTY_FUNCTION__);
    } else {
      // Add elements into queue_.
      for (const auto& e : elements_) {
        IOHIDQueueAddElement(queue_, e);
      }
      IOHIDQueueRegisterValueAvailableCallback(queue_, static_queue_value_available_callback, this);
    }
  }

  virtual ~human_interface_device(void) {
    detach_from_dispatcher([this] {
      unschedule();
      disable_report_callback();
      queue_stop();
      close();

      // ----------------------------------------

      types::detach_device_id(device_id_);

      // ----------------------------------------
      // Release queue_

      if (queue_) {
        CFRelease(queue_);
        queue_ = nullptr;
      }

      // ----------------------------------------
      // Release elements_

      for (const auto& e : elements_) {
        CFRelease(e);
      }
      elements_.clear();

      // ----------------------------------------
      // Release device_

      CFRelease(device_);
    });

    run_loop_thread_->terminate();
    run_loop_thread_ = nullptr;

    logger::get_logger().info("human_interface_device:{0} is destroyed.", name_for_log_);
  }

  registry_entry_id get_registry_entry_id(void) const {
    return registry_entry_id_;
  }

  std::string get_name_for_log(void) const {
    return name_for_log_;
  }

  device_id get_device_id(void) const {
    return device_id_;
  }

  bool get_removed(void) const {
    return removed_;
  }

  void set_removed(void) {
    removed_ = true;
  }

  std::shared_ptr<connected_devices::device> get_connected_device(void) const {
    return connected_device_;
  }

  bool is_keyboard(void) const {
    return connected_device_->get_identifiers().get_is_keyboard();
  }

  bool is_pointing_device(void) const {
    return connected_device_->get_identifiers().get_is_pointing_device();
  }

  bool is_built_in_keyboard(void) const {
    return connected_device_->get_is_built_in_keyboard();
  }

  bool is_built_in_trackpad(void) const {
    return connected_device_->get_is_built_in_trackpad();
  }

  device_detail make_device_detail(void) const {
    return device_detail(device_);
  }

  bool validate(void) const {
    // `iokit_utility::find_registry_entry_id` is failed after `device_` is removed.

    if (!iokit_utility::find_registry_entry_id(device_)) {
      return false;
    }

    return true;
  }

  void async_open(IOOptionBits options = kIOHIDOptionsTypeNone) {
    enqueue_to_dispatcher([this, options] {
      open(options);
    });
  }

  void async_close(void) {
    enqueue_to_dispatcher([this] {
      close();
    });
  }

  void async_schedule(void) {
    enqueue_to_dispatcher([this] {
      schedule();
    });
  }

  void async_unschedule(void) {
    enqueue_to_dispatcher([this] {
      unschedule();
    });
  }

  void async_enable_report_callback(void) {
    enqueue_to_dispatcher([this] {
      enable_report_callback();
    });
  }

  void async_disable_report_callback(void) {
    enqueue_to_dispatcher([this] {
      disable_report_callback();
    });
  }

  void async_queue_start(void) {
    enqueue_to_dispatcher([this] {
      queue_start();
    });
  }

  void async_queue_stop(void) {
    enqueue_to_dispatcher([this] {
      queue_stop();
    });
  }

  void async_set_report(IOHIDReportType report_type,
                        CFIndex report_id,
                        std::shared_ptr<std::vector<uint8_t>> report) {
    enqueue_to_dispatcher([this, report_type, report_id, report] {
      set_report(report_type,
                 report_id,
                 report);
    });
  }

  boost::optional<long> find_max_input_report_size(void) const {
    return iokit_utility::find_max_input_report_size(device_);
  }

  boost::optional<vendor_id> find_vendor_id(void) const {
    return iokit_utility::find_vendor_id(device_);
  }

  boost::optional<product_id> find_product_id(void) const {
    return iokit_utility::find_product_id(device_);
  }

  boost::optional<location_id> find_location_id(void) const {
    return iokit_utility::find_location_id(device_);
  }

  boost::optional<std::string> find_manufacturer(void) const {
    return iokit_utility::find_manufacturer(device_);
  }

  boost::optional<std::string> find_product(void) const {
    return iokit_utility::find_product(device_);
  }

  boost::optional<std::string> find_serial_number(void) const {
    return iokit_utility::find_serial_number(device_);
  }

  boost::optional<std::string> find_transport(void) const {
    return iokit_utility::find_transport(device_);
  }

  bool is_karabiner_virtual_hid_device(void) const {
    return iokit_utility::is_karabiner_virtual_hid_device(device_);
  }

#pragma mark - usage specific utilities

  // This method requires root privilege to use IOHIDDeviceSetValue for kHIDPage_LEDs usage.
  void async_set_caps_lock_led_state(led_state state) {
    enqueue_to_dispatcher([this, state] {
      for (const auto& e : elements_) {
        auto usage_page = hid_usage_page(IOHIDElementGetUsagePage(e));
        auto usage = hid_usage(IOHIDElementGetUsage(e));

        if (usage_page == hid_usage_page::leds &&
            usage == hid_usage::led_caps_lock) {
          CFIndex integer_value = 0;
          if (state == led_state::on) {
            integer_value = IOHIDElementGetLogicalMax(e);
          } else {
            integer_value = IOHIDElementGetLogicalMin(e);
          }

          if (auto value = IOHIDValueCreateWithIntegerValue(kCFAllocatorDefault,
                                                            e,
                                                            mach_absolute_time(),
                                                            integer_value)) {
            IOHIDDeviceSetValue(device_, e, value);

            CFRelease(value);
          }
        }
      }
    });
  }

private:
  // This method is executed in the dispatcher thread.
  void open(IOOptionBits options) {
    if (removed_) {
      return;
    }

    if (!opened_) {
      auto r = IOHIDDeviceOpen(device_, options);
      if (r == kIOReturnSuccess) {
        opened_ = true;

        enqueue_to_dispatcher([this] {
          opened();
        });
      } else {
        enqueue_to_dispatcher([this, r] {
          open_failed(r);
        });
      }
    }
  }

  // This method is executed in the dispatcher thread.
  void close(void) {
    if (removed_) {
      return;
    }

    if (opened_) {
      auto r = IOHIDDeviceClose(device_, kIOHIDOptionsTypeNone);
      if (r == kIOReturnSuccess) {
        enqueue_to_dispatcher([this] {
          closed();
        });
      } else {
        enqueue_to_dispatcher([this, r] {
          close_failed(r);
        });
      }
      opened_ = false;
    }
  }

  // This method is executed in the dispatcher thread.
  void schedule(void) {
    if (removed_) {
      return;
    }

    IOHIDDeviceScheduleWithRunLoop(device_,
                                   run_loop_thread_->get_run_loop(),
                                   kCFRunLoopCommonModes);
    if (queue_) {
      IOHIDQueueScheduleWithRunLoop(queue_,
                                    run_loop_thread_->get_run_loop(),
                                    kCFRunLoopCommonModes);
    }

    run_loop_thread_->wake();
  }

  // This method is executed in the dispatcher thread.
  void unschedule(void) {
    if (removed_) {
      return;
    }

    if (queue_) {
      IOHIDQueueUnscheduleFromRunLoop(queue_,
                                      run_loop_thread_->get_run_loop(),
                                      kCFRunLoopCommonModes);
    }
    IOHIDDeviceUnscheduleFromRunLoop(device_,
                                     run_loop_thread_->get_run_loop(),
                                     kCFRunLoopCommonModes);
  }

  // This method is executed in the dispatcher thread.
  void enable_report_callback(void) {
    if (removed_) {
      return;
    }

    resize_report_buffer();
    IOHIDDeviceRegisterInputReportCallback(device_, &(report_buffer_[0]), report_buffer_.size(), static_input_report_callback, this);
  }

  // This method is executed in the dispatcher thread.
  void disable_report_callback(void) {
    if (removed_) {
      return;
    }

    resize_report_buffer();
    IOHIDDeviceRegisterInputReportCallback(device_, &(report_buffer_[0]), report_buffer_.size(), nullptr, this);
  }

  // This method is executed in the dispatcher thread.
  void queue_start(void) {
    if (removed_) {
      return;
    }

    if (queue_) {
      IOHIDQueueStart(queue_);
    }
  }

  // This method is executed in the dispatcher thread.
  void queue_stop(void) {
    if (removed_) {
      return;
    }

    if (queue_) {
      IOHIDQueueStop(queue_);
    }
  }

  // This method is executed in the dispatcher thread.
  void set_report(IOHIDReportType report_type,
                  CFIndex report_id,
                  std::shared_ptr<std::vector<uint8_t>> report) {
    if (removed_) {
      return;
    }

    if (report) {
      IOHIDDeviceSetReport(device_,
                           report_type,
                           report_id,
                           &((*report)[0]),
                           report->size());
    }
  }

  static void static_queue_value_available_callback(void* _Nullable context, IOReturn result, void* _Nullable sender) {
    if (result != kIOReturnSuccess) {
      return;
    }

    auto self = static_cast<human_interface_device*>(context);
    if (!self) {
      return;
    }

    self->queue_value_available_callback();
  }

  void queue_value_available_callback(void) {
    enqueue_to_dispatcher([this] {
      auto input_event_queue = std::make_shared<event_queue::queue>();
      std::vector<hid_value> hid_values;

      while (auto value = IOHIDQueueCopyNextValueWithTimeout(queue_, 0.)) {
        hid_values.emplace_back(value);

        CFRelease(value);
      }

      for (const auto& pair : event_queue::queue::make_entries(hid_values, device_id_)) {
        auto& hid_value = pair.first;
        auto& entry = pair.second;

        input_event_queue->push_back_event(entry);

        if (hid_value) {
          if (auto hid_usage_page = hid_value->get_hid_usage_page()) {
            if (auto hid_usage = hid_value->get_hid_usage()) {
              if (entry.get_event().get_type() == event_queue::event::type::key_code ||
                  entry.get_event().get_type() == event_queue::event::type::consumer_key_code ||
                  entry.get_event().get_type() == event_queue::event::type::pointing_button) {
                // Send `device_keys_and_pointing_buttons_are_released` event if needed.

                if (entry.get_event_type() == event_type::key_down) {
                  pressed_keys_.insert(elements_key(*hid_usage_page, *hid_usage));
                } else {
                  size_t size = pressed_keys_.size();
                  pressed_keys_.erase(elements_key(*hid_usage_page, *hid_usage));
                  if (size > 0) {
                    post_device_keys_and_pointing_buttons_are_released_event_if_needed(input_event_queue,
                                                                                hid_value->get_time_stamp());
                  }
                }
              }
            }
          }
        }
      }

      enqueue_to_dispatcher([this, input_event_queue] {
        values_arrived(input_event_queue);
      });
    });
  }

  void post_device_keys_and_pointing_buttons_are_released_event_if_needed(std::shared_ptr<event_queue::queue> input_event_queue,
                                                                          absolute_time time_stamp) {
    if (pressed_keys_.empty()) {
      auto event = event_queue::event::make_device_keys_and_pointing_buttons_are_released_event();
      input_event_queue->emplace_back_event(device_id_,
                                            event_queue::event_time_stamp(time_stamp),
                                            event,
                                            event_type::single,
                                            event);
    }
  }

  static void static_input_report_callback(void* _Nullable context,
                                           IOReturn result,
                                           void* _Nullable sender,
                                           IOHIDReportType type,
                                           uint32_t report_id,
                                           uint8_t* _Nullable report,
                                           CFIndex report_length) {
    if (result != kIOReturnSuccess) {
      return;
    }

    auto self = static_cast<human_interface_device*>(context);
    if (!self) {
      return;
    }

    self->input_report_callback(type, report_id, report, report_length);
  }

  void input_report_callback(IOHIDReportType type,
                             uint32_t report_id,
                             uint8_t* _Nullable report,
                             CFIndex report_length) {
    auto r = std::make_shared<std::vector<uint8_t>>(report_length);
    memcpy(&((*r)[0]), report, report_length);

    enqueue_to_dispatcher([this, type, report_id, r] {
      report_arrived(type, report_id, r);
    });
  }

  uint64_t elements_key(hid_usage_page usage_page, hid_usage usage) const {
    return ((static_cast<uint64_t>(usage_page) << 32) | static_cast<uint32_t>(usage));
  }

  void resize_report_buffer(void) {
    size_t buffer_size = 32; // use this provisional value if we cannot get max input report size from device.
    if (auto size = find_max_input_report_size()) {
      buffer_size = *size;
    }

    report_buffer_.resize(buffer_size);
  }

  IOHIDDeviceRef _Nonnull device_;
  registry_entry_id registry_entry_id_;

  std::unique_ptr<cf_utility::run_loop_thread> run_loop_thread_;
  device_id device_id_;
  std::string name_for_log_;
  std::atomic<bool> removed_;
  std::atomic<bool> opened_;
  std::shared_ptr<connected_devices::device> connected_device_;

  IOHIDQueueRef _Nullable queue_;
  std::vector<IOHIDElementRef> elements_;
  std::vector<uint8_t> report_buffer_;
  std::unordered_set<uint64_t> pressed_keys_;
};
} // namespace krbn

max_stars_repo_name: cyrusccy/Karabiner-Elements

max_issues_repo_name: cyrusccy/Karabiner-Elements

max_forks_repo_name: cyrusccy/Karabiner-Elements


text:
/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#include "application/impl/configuration_storage_impl.hpp"

#include <boost/property_tree/json_parser.hpp>
#include <libp2p/multi/multiaddress.hpp>

#include "application/impl/config_reader/error.hpp"
#include "application/impl/config_reader/pt_util.hpp"
#include "common/hexutil.hpp"

namespace kagome::application {

  GenesisRawConfig ConfigurationStorageImpl::getGenesis() const {
    return genesis_;
  }

  network::PeerList ConfigurationStorageImpl::getBootNodes() const {
    return boot_nodes_;
  }

  outcome::result<std::shared_ptr<ConfigurationStorageImpl>>
  ConfigurationStorageImpl::create(const std::string &path) {
    auto config_storage =
        std::make_shared<ConfigurationStorageImpl>(ConfigurationStorageImpl());
    OUTCOME_TRY(config_storage->loadFromJson(path));

    return config_storage;
  }

  namespace pt = boost::property_tree;

  outcome::result<void> ConfigurationStorageImpl::loadFromJson(
      const std::string &file_path) {
    pt::ptree tree;
    try {
      pt::read_json(file_path, tree);
    } catch (pt::json_parser_error &e) {
      spdlog::error("Parser error: {}, line {}: {}", e.filename(), e.line(), e.message());
      return ConfigReaderError::PARSER_ERROR;
    }

    OUTCOME_TRY(loadGenesis(tree));
    OUTCOME_TRY(loadBootNodes(tree));
    return outcome::success();
  }

  outcome::result<void> ConfigurationStorageImpl::loadGenesis(
      const boost::property_tree::ptree &tree) {
    OUTCOME_TRY(genesis_tree, ensure(tree.get_child_optional("genesis")));
    OUTCOME_TRY(genesis_raw_tree,
                ensure(genesis_tree.get_child_optional("raw")));
    boost::property_tree::ptree top_tree;
    // v0.7 format
    if(auto top_tree_opt = genesis_raw_tree.get_child_optional("top"); top_tree_opt.has_value()) {
      top_tree = top_tree_opt.value();
    } else {
      // Try to fall back to v0.6
      top_tree = genesis_raw_tree.begin()->second;
    }

    for (const auto &[key, value] : top_tree) {
      // get rid of leading 0x for key and value and unhex
      OUTCOME_TRY(key_processed, common::unhexWith0x(key));
      OUTCOME_TRY(value_processed, common::unhexWith0x(value.data()));
      genesis_.emplace_back(key_processed, value_processed);
    }
    // ignore child storages as they are not yet implemented

    return outcome::success();
  }

  outcome::result<void> ConfigurationStorageImpl::loadBootNodes(
      const boost::property_tree::ptree &tree) {
    OUTCOME_TRY(boot_nodes, ensure(tree.get_child_optional("bootNodes")));
    for (auto &v : boot_nodes) {
      OUTCOME_TRY(multiaddr,
                  libp2p::multi::Multiaddress::create(v.second.data()));
      OUTCOME_TRY(peer_id_base58, ensure(multiaddr.getPeerId()));

      OUTCOME_TRY(peer_id, libp2p::peer::PeerId::fromBase58(peer_id_base58));
      libp2p::peer::PeerInfo info{.id = std::move(peer_id),
                                  .addresses = {std::move(multiaddr)}};
      boot_nodes_.peers.push_back(info);
    }
    return outcome::success();
  }

}  // namespace kagome::application

ext: cpp

max_stars_repo_name: iceseer/kagome

max_issues_repo_name: iceseer/kagome

max_forks_repo_name: iceseer/kagome


text:
/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2018 Sylko Olzscher
 *
 */

#include <smf/sml/exporter/db_sml_exporter.h>
#include <smf/sml/obis_db.h>
#include <smf/sml/obis_io.h>
#include <smf/sml/srv_id_io.h>
#include <smf/sml/units.h>
#include <smf/sml/scaler.h>

#include <cyng/io/io_buffer.h>
#include <cyng/io/io_chrono.hpp>
#include <cyng/io/serializer.h>
#include <cyng/numeric_cast.hpp>
#include <cyng/intrinsics/traits/tag_names.hpp>
#include <cyng/sql.h>
#include <cyng/db/interface_statement.h>
#include <cyng/factory.h>

#include <boost/uuid/nil_generator.hpp>
#include <boost/core/ignore_unused.hpp>

namespace node
{
	namespace sml
	{

		db_exporter::db_exporter(cyng::table::mt_table const& mt, std::string const& schema)
			: mt_(mt)
			, schema_(schema)
			, source_(0)
			, channel_(0)
			, target_()
			, rgn_()
			, ro_(rgn_())
		{
			reset();
		}

		db_exporter::db_exporter(cyng::table::mt_table const& mt
			, std::string const& schema
			, std::uint32_t source
			, std::uint32_t channel
			, std::string const& target)
		: mt_(mt)
			, schema_(schema)
			, source_(source)
			, channel_(channel)
			, target_(target)
			, rgn_()
			, ro_(rgn_())
		{
			reset();
		}

		void db_exporter::reset()
		{
			ro_.reset(rgn_(), 0);
		}

		void db_exporter::write(cyng::db::session sp, cyng::tuple_t const& msg, std::size_t idx)
		{
			read_msg(sp, msg.begin(), msg.end(), idx);
		}

		void db_exporter::read_msg(cyng::db::session sp, cyng::tuple_t::const_iterator pos, cyng::tuple_t::const_iterator end, std::size_t idx)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 5, "SML message");
			if (count != 5)	return;

			//
			//	reset readout context
			//
			ro_.set_index(idx);

			//
			//	(1) - transaction id
			//	This is typically a printable sequence of 6 up to 9 ASCII values
			//
			cyng::buffer_t buffer;
			ro_.set_trx(cyng::value_cast(*pos++, buffer));

			//
			//	(2) groupNo
			//
			ro_.set_value("groupNo", *pos++);

			//
			//	(3) abortOnError
			//
			ro_.set_value("abortOnError", *pos++);

			//
			//	(4/5) CHOICE - msg type
			//
			cyng::tuple_t choice;
			choice = cyng::value_cast(*pos++, choice);
			BOOST_ASSERT_MSG(choice.size() == 2, "CHOICE");
			if (choice.size() == 2)
			{
				ro_.set_value("code", choice.front());
				read_body(sp, choice.front(), choice.back());
			}

			//
			//	(6) CRC16
			//
			ro_.set_value("crc16", *pos);
		}

		void db_exporter::read_body(cyng::db::session sp, cyng::object type, cyng::object body)
		{
			auto code = cyng::value_cast<std::uint16_t>(type, 0);
			//node.append_attribute("type").set_value(messages::name(code));

			cyng::tuple_t tpl;
			tpl = cyng::value_cast(body, tpl);

			switch (code)
			{
			case BODY_OPEN_REQUEST:
				read_public_open_request(tpl.begin(), tpl.end());
				break;
			case BODY_OPEN_RESPONSE:

				//
				//	set default readout time
				//
				ro_.set_value("roTime", cyng::make_now());
				read_public_open_response(tpl.begin(), tpl.end());
				break;
			case BODY_CLOSE_REQUEST:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_CLOSE_RESPONSE:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_GET_PROFILE_PACK_REQUEST:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_GET_PROFILE_PACK_RESPONSE:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_GET_PROFILE_LIST_REQUEST:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_GET_PROFILE_LIST_RESPONSE:
				read_get_profile_list_response(sp, tpl.begin(), tpl.end());
				break;
			case BODY_GET_PROC_PARAMETER_REQUEST:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_GET_PROC_PARAMETER_RESPONSE:
				read_get_proc_parameter_response(sp, tpl.begin(), tpl.end());
				break;
			case BODY_SET_PROC_PARAMETER_REQUEST:
				//cyng::xml::write(node, body);
				break;
			case BODY_SET_PROC_PARAMETER_RESPONSE:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_GET_LIST_REQUEST:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_GET_LIST_RESPONSE:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			case BODY_ATTENTION_RESPONSE:
				read_attention_response(tpl.begin(), tpl.end());
				break;
			default:
				//cyng::xml::write(node.append_child("data"), body);
				break;
			}
		}

		void db_exporter::read_public_open_request(cyng::tuple_t::const_iterator pos, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 7, "Public Open Request");
			if (count != 7)	return;

			//
			//	reset context
			//
			reset();

			//	codepage "ISO 8859-15"
			ro_.set_value("codepage", *pos++);

			//
			//	clientId (MAC)
			//	Typically 7 bytes to identify gateway/MUC
			read_client_id(*pos++);

			//
			//	reqFileId
			//
			ro_.set_value("reqFileId", *pos++);

			//
			//	serverId
			//
			read_server_id(*pos++);

			//
			//	username
			//
			read_string("userName", *pos++);

			//
			//	password
			//
			read_string("password", *pos++);

			//
			//	sml-Version: default = 1
			//
			ro_.set_value("SMLVersion", *pos++);

		}

		void db_exporter::read_public_open_response(cyng::tuple_t::const_iterator pos, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 6, "Public Open Response");
			if (count != 6)	return;

			//
			//	reset context
			//
			reset();

			//	codepage "ISO 8859-15"
			ro_.set_value("codepage", *pos++);

			//
			//	clientId (MAC)
			//	Typically 7 bytes to identify gateway/MUC
			read_client_id(*pos++);

			//
			//	reqFileId
			//
			read_string("reqFileId", *pos++);

			//
			//	serverId
			//
			read_server_id(*pos++);

			//
			//	refTime
			//
			ro_.set_value("refTime", *pos++);

			//	sml-Version: default = 1
			ro_.set_value("SMLVersion", *pos++);

		}

		bool db_exporter::read_get_profile_list_response(cyng::db::session sp, cyng::tuple_t::const_iterator pos, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 9, "Get Profile List Response");
			if (count != 9)	return false;

			//
			//	serverId
			//
			read_server_id(*pos++);

			//
			//	actTime
			//
			read_time("actTime", *pos++);

			//
			//	regPeriod
			//
			ro_.set_value("regPeriod", *pos++);

			//
			//	parameterTreePath (OBIS)
			//
			std::vector<obis> path = read_param_tree_path(*pos++);

			//
			//	valTime
			//
			read_time("valTime", *pos++);

			//
			//	M-bus status
			//
			ro_.set_value("status", *pos++);

			//	period-List (1)
			//	first we make the TSMLGetProfileListResponse complete, then
			//	the we store the entries in TSMLPeriodEntry.
			cyng::tuple_t tpl;
			tpl = cyng::value_cast(*pos++, tpl);
			read_period_list(sp, path, tpl.begin(), tpl.end());

			//	rawdata
			ro_.set_value("rawData", *pos++);

			//	periodSignature
			ro_.set_value("signature", *pos++);

			return store_meta(sp
				, ro_.pk_
				, ro_.trx_
				, ro_.idx_
				, ro_.get_value("roTime")
				, ro_.get_value("actTime")
				, ro_.get_value("valTime")
				//, ro_.get_value("client")	//	gateway
				, ro_.client_id_	//	gateway - formatted (00:15:3b:02:29:7e)
				//, ro_.get_value("server")	//	server
				, ro_.server_id_	//	server - formatted
				, ro_.get_value("status")
				, path.empty() ? obis() : path.front());
		}

		void db_exporter::read_get_proc_parameter_response(cyng::db::session sp, cyng::tuple_t::const_iterator pos, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 3, "Get Proc Parameter Response");
			if (count != 3)	return;

			//
			//	serverId
			//
			read_server_id(*pos++);

			//
			//	parameterTreePath (OBIS)
			//
			std::vector<obis> path = read_param_tree_path(*pos++);

			//
			//	parameterTree
			//
			cyng::tuple_t tpl;
			tpl = cyng::value_cast(*pos++, tpl);
			read_param_tree(0, tpl.begin(), tpl.end());

		}

		void db_exporter::read_attention_response(cyng::tuple_t::const_iterator pos, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 4, "Attention Response");
			if (count != 4)	return;

			//
			//	serverId
			//
			read_server_id(*pos++);

			//
			//	attentionNo (OBIS)
			//
			obis code = read_obis(*pos++);
			boost::ignore_unused(code);

			//
			//	attentionMsg
			//
			ro_.set_value("attentionMsg", *pos++);

			//
			//	attentionDetails (SML_Tree)
			//
			cyng::tuple_t tpl;
			tpl = cyng::value_cast(*pos++, tpl);
			read_param_tree(0, tpl.begin(), tpl.end());

		}

		void db_exporter::read_param_tree(std::size_t depth
			, cyng::tuple_t::const_iterator pos
			, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 3, "SML Tree");
			if (count != 3)	return;

			//
			//	1. parameterName Octet String,
			//
			obis code = read_obis(*pos++);

			//
			//	2. parameterValue SML_ProcParValue OPTIONAL,
			//
			read_parameter(code, *pos++);

			//
			//	3. child_List List_of_SML_Tree OPTIONAL
			//
			//cyng::xml::write(node.append_child("child_List"), *pos++);
			cyng::tuple_t tpl;
			tpl = cyng::value_cast(*pos++, tpl);
			for (auto const child : tpl)
			{
				cyng::tuple_t tmp;
				tmp = cyng::value_cast(child, tmp);
				read_param_tree(++depth, tmp.begin(), tmp.end());
			}
		}

		void db_exporter::read_period_list(cyng::db::session sp
			, std::vector<obis> const& path
			, cyng::tuple_t::const_iterator pos
			, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			boost::ignore_unused(count);

			//
			//	list of tuples (period entry)
			//
			std::size_t counter{ 0 };
			while (pos != end)
			{
				cyng::tuple_t tpl;
				tpl = cyng::value_cast(*pos++, tpl);
				read_period_entry(sp, path, counter, tpl.begin(), tpl.end());
				++counter;

			}
		}

		bool db_exporter::read_period_entry(cyng::db::session sp
			, std::vector<obis> const& path
			, std::size_t index
			, cyng::tuple_t::const_iterator pos
			, cyng::tuple_t::const_iterator end)
		{
			std::size_t count = std::distance(pos, end);
			BOOST_ASSERT_MSG(count == 5, "Period Entry");
			if (count != 5)	return false;

			//
			//	object name
			//
			obis code = read_obis(*pos++);

			//
			//	unit (see sml_unit_enum)
			//
			const auto unit = read_unit("SMLUnit", *pos++);

			//
			//	scaler
			//
			const auto scaler = read_scaler(*pos++);

			//
			//	value
			//
			const auto type_tag = read_value(code, scaler, unit, *pos++);

			//
			//	valueSignature
			//
			ro_.set_value("signature", *pos++);

			return store_data(sp
				, ro_.pk_
				, ro_.trx_
				, ro_.idx_
				, code	//	.to_buffer()
				, unit
				, get_unit_name(unit)
				, cyng::traits::get_type_name(type_tag)	//	CYNG data type name
				, scaler	//	scaler
				, ro_.get_value("raw")	//	raw value
				, ro_.get_value("value"));	//	formatted value
		}

		void db_exporter::read_time(std::string const& name, cyng::object obj)
		{
			cyng::tuple_t choice;
			choice = cyng::value_cast(obj, choice);
			BOOST_ASSERT_MSG(choice.size() == 2, "TIME");
			if (choice.size() == 2)
			{
				auto code = cyng::value_cast<std::uint8_t>(choice.front(), 0);
				switch (code)
				{
				case TIME_TIMESTAMP:
				{
					const std::uint32_t sec = cyng::value_cast<std::uint32_t>(choice.back(), 0);
					ro_.set_value(name, cyng::make_time_point(sec));
				}
				break;
				case TIME_SECINDEX:
					ro_.set_value(name, choice.back());
					break;
				default:
					break;
				}
			}

		}

		std::vector<obis> db_exporter::read_param_tree_path(cyng::object obj)
		{
			std::vector<obis> result;

			cyng::tuple_t tpl;
			tpl = cyng::value_cast(obj, tpl);
			for (auto obj : tpl)
			{

				const obis object_type = read_obis(obj);
				result.push_back(object_type);

			}
			return result;
		}

		obis db_exporter::read_obis(cyng::object obj)
		{
			cyng::buffer_t tmp;
			tmp = cyng::value_cast(obj, tmp);

			return obis(tmp);
		}

		std::uint8_t db_exporter::read_unit(std::string const& name, cyng::object obj)
		{
			std::uint8_t unit = cyng::value_cast<std::uint8_t>(obj, 0);
			//node.append_attribute("type").set_value(get_unit_name(unit));
			//node.append_child(pugi::node_pcdata).set_value(std::to_string(+unit).c_str());
			ro_.set_value(name, obj);
			return unit;
		}

		std::string db_exporter::read_string(std::string const& name, cyng::object obj)
		{
			cyng::buffer_t buffer;
			buffer = cyng::value_cast(obj, buffer);
			const auto str = cyng::io::to_ascii(buffer);
			ro_.set_value(name, cyng::make_object(str));
			return str;
		}

		std::string db_exporter::read_server_id(cyng::object obj)
		{
			ro_.server_id_ = cyng::value_cast(obj, ro_.server_id_);
			return from_server_id(ro_.server_id_);
		}

		std::string db_exporter::read_client_id(cyng::object obj)
		{
			ro_.client_id_ = cyng::value_cast(obj, ro_.client_id_);
			return from_server_id(ro_.client_id_);
		}

		std::int8_t db_exporter::read_scaler(cyng::object obj)
		{
// 			return cyng::value_cast<std::int8_t>(obj, 0);
 			return cyng::numeric_cast<std::int8_t>(obj, 0);
		}

		std::size_t db_exporter::read_value(obis code, std::int8_t scaler, std::uint8_t unit, cyng::object obj)
		{
			//
			//	write value
			//
			const auto type_tag = obj.get_class().tag();
			if (type_tag == cyng::TC_BUFFER) {
				//
				//	write binary data (octets) as hex string
				//
				cyng::buffer_t buffer;
				buffer = cyng::value_cast(obj, buffer);
				const auto raw = cyng::io::to_hex(buffer);
				ro_.set_value("raw", cyng::make_object(raw));
			}
			else {
				ro_.set_value("raw", obj);
			}

			if (code == OBIS_DATA_MANUFACTURER)
			{
				cyng::buffer_t buffer;
				buffer = cyng::value_cast(obj, buffer);
				const auto manufacturer = cyng::io::to_ascii(buffer);
				ro_.set_value("value", cyng::make_object(manufacturer));
				ro_.set_value("type", cyng::make_object("manufacturer"));
			}
			else if (code == OBIS_CURRENT_UTC)
			{
				if (type_tag == cyng::TC_TUPLE)
				{
					read_time("value", obj);
					read_time("roTime", obj);
				}
				else
				{
					const auto tm = cyng::value_cast<std::uint32_t>(obj, 0);
					const auto tp = std::chrono::system_clock::from_time_t(tm);
					ro_.set_value("value", cyng::make_object(tp));
					ro_.set_value("roTime", cyng::make_object(tp));
				}
				ro_.set_value("type", cyng::make_object("roTime"));
			}
			else if (code == OBIS_ACT_SENSOR_TIME)
			{
				const auto tm = cyng::value_cast<std::uint32_t>(obj, 0);
				const auto tp = std::chrono::system_clock::from_time_t(tm);
				ro_.set_value("value", cyng::make_object(tp));
				ro_.set_value("type", cyng::make_object("actTime"));
			}
			else if (code == OBIS_SERIAL_NR)
			{
				cyng::buffer_t buffer;
				buffer = cyng::value_cast(obj, buffer);
				std::reverse(buffer.begin(), buffer.end());
				const auto serial_nr = cyng::io::to_hex(buffer);
				ro_.set_value("value", cyng::make_object(serial_nr));
				ro_.set_value("type", cyng::make_object("serialNr"));
			}
			else if (code == OBIS_SERIAL_NR_SECOND)
			{
				cyng::buffer_t buffer;
				buffer = cyng::value_cast(obj, buffer);
				std::reverse(buffer.begin(), buffer.end());
				const auto serial_nr = cyng::io::to_hex(buffer);
				ro_.set_value("value", cyng::make_object(serial_nr));
				ro_.set_value("type", cyng::make_object("serialNr2"));
			}
			else if (code == OBIS_MBUS_STATE)
			{
				//	see EN13757-3
				//std::uint8_t status = cyng::value_cast<std::uint8_t>(obj, 0u);
				//ro_.set_value("MBusPermanentError", cyng::make_object(((status & 0x08) == 0x08)));
				//ro_.set_value("MBusTemporaryError", cyng::make_object(((status & 0x10) == 0x10)));
				ro_.set_value("value", obj);
				ro_.set_value("type", cyng::make_object("MBus"));
			}
			else
			{
				if (scaler != 0)
				{
					switch (type_tag)
					{
					case cyng::TC_INT64:
					{
						const std::int64_t value = cyng::value_cast<std::int64_t>(obj, 0);
						const auto str = scale_value(value, scaler);
						ro_.set_value("value", cyng::make_object(str));
						ro_.set_value("type", cyng::make_object("valueScaled64"));
					}
					break;
					case cyng::TC_INT32:
					{
						const std::int32_t value = cyng::value_cast<std::int32_t>(obj, 0);
						const auto str = scale_value(value, scaler);
						ro_.set_value("value", cyng::make_object(str));
						ro_.set_value("type", cyng::make_object("valueScaled32"));
					}
					break;
					default:
						ro_.set_value("value", obj);
						ro_.set_value("type", cyng::make_object("value"));
						break;
					}
				}
				else
				{
					ro_.set_value("value", obj);
					ro_.set_value("type", cyng::make_object("value"));
				}
			}

			return type_tag;
		}

		void db_exporter::read_parameter(obis code, cyng::object obj)
		{
			cyng::tuple_t tpl;
			tpl = cyng::value_cast(obj, tpl);
			if (tpl.size() == 2)
			{
				const auto type = cyng::value_cast<std::uint8_t>(tpl.front(), 0);
				switch (type)
				{
				case PROC_PAR_VALUE:
					//node.append_attribute("parameter").set_value("value");
					//cyng::xml::write(node, tpl.back());
					if ((code == OBIS_CLASS_OP_LSM_ACTIVE_RULESET) || (code == OBIS_CLASS_OP_LSM_PASSIVE_RULESET))
					{
						cyng::buffer_t buffer;
						buffer = cyng::value_cast(tpl.back(), buffer);
						const auto name = cyng::io::to_ascii(buffer);
						//node.append_attribute("name").set_value(name.c_str());
					}
					break;
				case PROC_PAR_PERIODENTRY:
					//node.append_attribute("parameter").set_value("period");
					//cyng::xml::write(node, tpl.back());
					break;
				case PROC_PAR_TUPELENTRY:
					//node.append_attribute("parameter").set_value("tuple");
					//cyng::xml::write(node, tpl.back());
					break;
				case PROC_PAR_TIME:
					//node.append_attribute("parameter").set_value("time");
					read_time("parTime", tpl.back());
					break;
				default:
					//node.append_attribute("parameter").set_value("undefined");
					//cyng::xml::write(node, obj);
					break;
				}
			}
			else
			{
				//cyng::xml::write(node, obj);
			}
		}

		bool db_exporter::store_meta(cyng::db::session sp
			, boost::uuids::uuid pk
			, std::string const& trx
			, std::size_t idx
			, cyng::object obj_ro_time
			, cyng::object obj_act_time
			, cyng::object obj_val_time
			, cyng::buffer_t const& client_id	//	gateway - formatted
			, cyng::buffer_t const& server_id	//	server - formatted
			, cyng::object obj_status
			, obis profile)
		{

			cyng::sql::command cmd(mt_.find("TSMLMeta")->second, sp.get_dialect());
			cmd.insert();
			auto sql = cmd.to_str();
			auto stmt = sp.create_statement();
			std::pair<int, bool> r = stmt->prepare(sql);
			BOOST_ASSERT(r.second);
			boost::ignore_unused(r);	//	release version

			if (boost::algorithm::equals(schema_, "v4.0"))
			{
				//
				//	examples:
				//
				//	sqlite> select * from TSMLMeta where roTime > julianday('2018-07-01');
				//	ident|trxID|midx|roTime|actTime|valTime|gateway|server|meter|status|source|channel
				//	91e54897-68aa-477b-97ae-efe459c192be|618732131|1|2458300.51030093|2458300.51070602|63122537|0500153b025459|01-2d2c-40800078-1c-04|78008040|0|949333985|581869302
				//	fa6f80d6-2a18-4873-ae15-407629529757|535249935|1|2458300.51025463|2458300.51070602|81491646|0500153b02499e|01-a815-76007904-01-02|04790076|131712|545404204|581869302
				//	5e45f89d-9a36-4f9b-8e79-8fdd617ace8f|65331623|1|2458300.50975694|2458300.51070602|41419967|0500153b023b36|02-d81c-05252359-ff-02|59232505|0|-1579004998|581869302
				//	a21ee414-8442-4b5b-bab0-2a815a034e73|510800075|1|2458300.50994213|2458300.51070602|55253893|0500153b025182|01-a815-77007904-01-02|04790077|131712|-372047867|581869302
				//	1df825d4-93f7-4518-b493-030b150ea3fa|439163211|1|2458300.51001157|2458300.51070602|92472720|0500153b024974|01-a815-46007904-01-02|04790046|131712|-1946129057|581869302
				//	c664ca3b-4dbf-4473-a847-f404118ce3fc|612380399|1|2458300.51028935|2458300.51070602|89139036|0500153b024996|01-a815-73007904-01-02|04790073|131714|-133711905|581869302
				//	5d2f7607-21b4-4a0b-835e-055118d8feb9|523186555|3|2458300.51025463|2458300.51070602|81442784|0500153b024998|01-a815-75007904-01-02|04790075|131712|-30574576|581869302
				//	90202d0f-46df-4f4e-9327-cd77ef96d0bb|51786655|4|2458300.51037037|2458300.51070602|44248573|0500153b024952|01-a815-65957202-01-02|02729565|640|418932835|581869302
				//	f29ff3f0-f570-43a5-93f2-933838e42d1f|514196843|4|2458300.51011574|2458300.51070602|79963528|0500153b024972|01-a815-44007904-01-02|04790044|131778|537655879|581869302
				//	601a7c18-ee9a-4957-8d06-d255b6b42ca2|533395235|1|2458300.51017361|2458300.51070602|48450161|0500153b026d31|02-d81c-05252350-ff-02|50232505|0|-150802599|581869302
				//	b70815c1-74e9-4881-b9d8-00db2f9a7fe6|49606703|4|2458300.51018519|2458300.51070602|44489752|0500153b026d35|01-a815-78957202-01-02|02729578|640|1323567403|581869302

				//cyng::buffer_t server, client;
				//server = cyng::value_cast(obj_server, server);
				//client = cyng::value_cast(obj_client, client);

				stmt->push(cyng::make_object(pk), 0)	//	ident
					.push(cyng::make_object(trx), 16)	//	trxID
					.push(cyng::make_object(idx), 0)	//	midx
					.push(obj_ro_time, 0)	//	roTime
					.push(obj_act_time, 0)	//	actTime
					.push(obj_val_time, 0)	//	valTime
					.push(cyng::make_object(cyng::io::to_hex(client_id)), 0)	//	gateway/client
																			//	clientId from_server_id
					.push(cyng::make_object(sml::from_server_id(server_id)), 0)	//	server
																				//	serverId
					.push(cyng::make_object(sml::get_serial(server_id)), 0)	//	meter
					.push(obj_status, 0)	//	status
					.push(cyng::make_object(source_), 0)	//	source
					.push(cyng::make_object(channel_), 0)	//	channel
					;
			}
			else
			{
				//
				//	examples:
				//
				//	pk|trxID|msgIdx|roTime|actTime|valTime|gateway|server|status|source|channel|target
				//	0bf4bfff-6a83-411d-ab60-1e98fc232fd2|51207|1|2458299.33280093|2458301.43333333|98573619|00:15:3b:02:29:7e|01-e61e-13090016-3c-07|0|-1324790809|-1649078317|water@solostec

				auto obj_client = cyng::make_object(from_server_id(client_id));
				auto obj_server = cyng::make_object(from_server_id(server_id));

				//	bind parameters
				stmt->push(cyng::make_object(pk), 0)	//	pk
					.push(cyng::make_object(trx), 16)	//	trxID
					.push(cyng::make_object(idx), 0)	//	msgIdx
					.push(obj_ro_time, 0)	//	roTime
					.push(obj_act_time, 0)	//	actTime
					.push(obj_val_time, 0)	//	valTime
											//	client
					.push(obj_client, 0)	//	gateway
											//	server
					.push(obj_server, 0)	//	server
					.push(obj_status, 0)	//	status
					.push(cyng::make_object(source_), 0)	//	source
					.push(cyng::make_object(channel_), 0)	//	channel
					.push(cyng::make_object(target_), 32)	//	target
					.push(cyng::make_object(cyng::io::to_hex(profile.to_buffer())), 24)	//	OBIS
					;
			}

			const bool b = stmt->execute();
			stmt->clear();
			return b;
		}

		bool db_exporter::store_data(cyng::db::session sp
				, boost::uuids::uuid pk
				, std::string const& trx
				, std::size_t idx
				, obis const& code
				, std::uint8_t unit
				, std::string unit_name
				, std::string type_name	//	CYNG data type name
				, std::int8_t scaler	//	scaler
				, cyng::object raw		//	raw value
				, cyng::object value)
		{
			cyng::sql::command cmd(mt_.find("TSMLData")->second, sp.get_dialect());
			cmd.insert();
			auto sql = cmd.to_str();
			//CYNG_LOG_INFO(logger_, "db.insert.data " << sql);
			auto stmt = sp.create_statement();
			std::pair<int, bool> r = stmt->prepare(sql);
			BOOST_ASSERT(r.first == 8);	//	11 parameters to bind
			BOOST_ASSERT(r.second);
			boost::ignore_unused(r);	//	release version

			if (boost::algorithm::equals(schema_, "v4.0"))
			{
				//sml::obis code(cyng::value_cast(frame.at(3), tmp));
				stmt->push(cyng::make_object(pk), 36)	//	pk
					.push(cyng::make_object(sml::to_string(code)), 24)	//	OBIS
					.push(cyng::make_object(unit), 0)	//	unitCode
					.push(cyng::make_object(unit_name), 0)	//	unitName
					.push(cyng::make_object(type_name), 0)	//	SML data type
					.push(cyng::make_object(scaler), 0)	//	scaler
					.push(raw, 0)	//	value
					.push(value, 512)	//	result
					;
			}
			else
			{
				stmt->push(cyng::make_object(pk), 36)	//	pk
					.push(cyng::make_object(cyng::io::to_hex(code.to_buffer())), 24)	//	OBIS
					.push(cyng::make_object(unit), 0)	//	unitCode
					.push(cyng::make_object(unit_name), 0)	//	unitName
					.push(cyng::make_object(type_name), 0)	//	SML data type
					.push(cyng::make_object(scaler), 0)	//	scaler
					.push(raw, 0)	//	value
					.push(value, 512)	//	result
					;

			}

			const bool b = stmt->execute();
			stmt->clear();
			return b;

		}

	}	//	sml
}

max_stars_repo_name: solosTec/node

max_issues_repo_name: solosTec/node

max_forks_repo_name: solosTec/node


text:
/*
 * openRatSLAM
 *
 * utils - General purpose utility helper functions mainly for angles and readings settings
 *
 * Copyright (C) 2012
 * David Ball (david.ball@qut.edu.au) (1), Scott Heath (scott.heath@uqconnect.edu.au) (2)
 *
 * RatSLAM algorithm by:
 * Michael Milford (1) and Gordon Wyeth (1) ([michael.milford, gordon.wyeth]@qut.edu.au)
 *
 * 1. Queensland University of Technology, Australia
 * 2. The University of Queensland, Australia
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <iostream>
using namespace std;

#include "utils/utils.h"

#include <boost/property_tree/ini_parser.hpp>

#include <ros/ros.h>

#include "ratslam/posecell_network.h"
#include <ratslam_ros/TopologicalAction.h>
#include <nav_msgs/Odometry.h>
#include <ratslam_ros/ViewTemplate.h>

#if HAVE_IRRLICHT
#include "graphics/posecell_scene.h"
ratslam::PosecellScene *pcs;
bool use_graphics;
#endif

using namespace ratslam;

ratslam_ros::TopologicalAction pc_output;

void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::PosecellNetwork *pc, ros::Publisher * pub_pc)
{
  static ros::Time prev_time(0);
  /*
  ROS_DEBUG_STREAM("PC:odo_callback{" << ros::Time::now() <<
    "} seq=" << odo->header.seq <<
    " v=" << odo->twist.twist.linear.x <<
    " r=" << odo->twist.twist.angular.z);
  */

  if (prev_time.toSec() > 0)
  {
    double time_diff = (odo->header.stamp - prev_time).toSec();

    // Previous PoseCellExperience identifier.
    pc_output.src_id = pc->get_current_exp_id();

    // Apply odometry, excite/inhibit neural-net and integrate path.
    // FIXME: Broader odom message compatibility. Not linking correctly with wheel odom?
    pc->on_odo(odo->twist.twist.linear.x, odo->twist.twist.angular.z, time_diff);
    // Create or recall PoseCellExperience with current PosecellVisualTemplate.
    pc_output.action = pc->get_action();
    if (pc_output.action != ratslam::PosecellNetwork::NO_ACTION)
    {
      pc_output.header.stamp = ros::Time::now();
      pc_output.header.seq++;
      // New PoseCellExperience identifier.
      // Given an odometry and VisualTemplate now with further evidence, the current experience may be inside the loop again.
      pc_output.dest_id = pc->get_current_exp_id();
      pc_output.relative_rad = pc->get_relative_rad();
      // Pass the action to experience map for linking.
      pub_pc->publish(pc_output);

      ROS_DEBUG_STREAM("PC:action_publish{odo}{" << ros::Time::now() <<
        "} seq=" << pc_output.header.seq <<
        " action=" <<  pc_output.action <<
        " src=" << pc_output.src_id <<
        " dest=" << pc_output.dest_id <<
        " rad=" << pc_output.relative_rad);
    }

#ifdef HAVE_IRRLICHT
	if (use_graphics)
	{
		pcs->update_scene();
		pcs->draw_all();
	}
#endif
  }
  prev_time = odo->header.stamp;
}

void template_callback(ratslam_ros::ViewTemplateConstPtr vt, ratslam::PosecellNetwork *pc, ros::Publisher * pub_pc)
{
  ROS_DEBUG_STREAM("PC:vt_callback{" << ros::Time::now() <<
    "} seq=" << vt->header.seq <<
    " id=" << vt->current_id <<
    " rad=" << vt->relative_rad);
  pc->on_view_template(vt->current_id, vt->relative_rad);

#ifdef HAVE_IRRLICHT
	if (use_graphics)
	{
		pcs->update_scene();
		pcs->draw_all();
	}
#endif

}

int main(int argc, char * argv[])
{
  ROS_INFO_STREAM(argv[0] << " - openRatSLAM Copyright (C) 2012 David Ball and Scott Heath");
  ROS_INFO_STREAM("RatSLAM algorithm by Michael Milford and Gordon Wyeth");
  ROS_INFO_STREAM("Distributed under the GNU GPL v3, see the included license file.");

  if (argc < 2)
  {
    ROS_FATAL_STREAM("USAGE: " << argv[0] << " <config_file>");
    exit(-1);
  }
  std::string topic_root = "";
  boost::property_tree::ptree settings, ratslam_settings, general_settings;
  read_ini(argv[1], settings);

  get_setting_child(ratslam_settings, settings, "ratslam", true);
  get_setting_child(general_settings, settings, "general", true);
  get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) "");

  if (!ros::isInitialized())
  {
    ros::init(argc, argv, "RatSLAMPoseCells");
  }
  ros::NodeHandle node;

  ratslam::PosecellNetwork * pc = new ratslam::PosecellNetwork(ratslam_settings);
  ros::Publisher pub_pc = node.advertise<ratslam_ros::TopologicalAction>(topic_root + "/PoseCell/TopologicalAction", 0);

  ros::Subscriber sub_odometry = node.subscribe<nav_msgs::Odometry>(topic_root + "/odom", 0, boost::bind(odo_callback, _1, pc, &pub_pc), ros::VoidConstPtr(),
                                                                    ros::TransportHints().tcpNoDelay());
  ros::Subscriber sub_template = node.subscribe<ratslam_ros::ViewTemplate>(topic_root + "/LocalView/Template", 0, boost::bind(template_callback, _1, pc, &pub_pc),
                                                                           ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay());
#ifdef HAVE_IRRLICHT
  boost::property_tree::ptree draw_settings;
  get_setting_child(draw_settings, settings, "draw", true);
  get_setting_from_ptree(use_graphics, draw_settings, "enable", true);
  if (use_graphics)
  {
	pcs = new ratslam::PosecellScene(draw_settings, pc);
  }
#endif

  ros::spin();

  return 0;
}


text:
/* Author: Zachary Kingston */

#include <boost/program_options.hpp>

#include <ompl/base/Planner.h>

#include <robowflex_library/benchmarking.h>
#include <robowflex_library/builder.h>
#include <robowflex_library/detail/fetch.h>
#include <robowflex_library/planning.h>
#include <robowflex_library/robot.h>
#include <robowflex_library/scene.h>
#include <robowflex_library/util.h>
#include <robowflex_library/log.h>
#include <robowflex_library/io/gnuplot.h>
#include <robowflex_library/io/visualization.h>

#include <robowflex_ompl/ompl_interface.h>

using namespace robowflex;
namespace po = boost::program_options;

/* \file fetch_profile.cpp
 * A basic script that demonstrates using the plan profiler with the Fetch robot.
 * The plan profiler is a tool to instrument a single planning run and extract relevant progress properties or
 * metrics. For larger scale planner testing, check out the benchmarking tools.
 * This uses IO::GNUPlotHelper, which helps with live visualization of data. Make sure GNUPlot is installed.
 */

static const std::string GROUP = "arm_with_torso";
static const double TIME = 60.0;

/** \brief Creates a progress callback function to plot the progress property \a field live using
 * GNUPlot.
 */
Profiler::ProgressCallback getGNUPlotCallback(IO::GNUPlotHelper &plotter, const std::string &field)
{
    return [&, field](const PlannerPtr &planner,                             //
                      const SceneConstPtr &scene,                            //
                      const planning_interface::MotionPlanRequest &request,  //
                      const PlanData &result) {
        IO::GNUPlotHelper::TimeSeriesOptions tso;  // Plotting options for time series data
        tso.instance = field;
        tso.title = "Live Profiling";
        tso.x.label = "Time (s)";
        tso.x.min = 0.;
        tso.x.max = TIME;

        // Plot all progress collected so far.
        const auto &points = result.getProgressPropertiesAsPoints("time REAL", field);
        tso.points.emplace(field, points);
        plotter.timeseries(tso);
    };
}

/** \brief Get a custom progress property function allocator that extracts the planner data from the
 * underlying OMPL motion planner.
 */
Profiler::ProgressPropertyAllocator getNumVerticesAllocator()
{
    return [](const PlannerPtr &planner,   //
              const SceneConstPtr &scene,  //
              const planning_interface::MotionPlanRequest &request) -> Planner::ProgressProperty {
        // Extract OMPL level information
        const auto &ompl_planner = std::dynamic_pointer_cast<const OMPL::OMPLInterfacePlanner>(planner);
        const auto &ss = ompl_planner->getLastSimpleSetup();
        const auto &op = ss->getPlanner();

        return [op] {
            ompl::base::PlannerData pd(op->getSpaceInformation());
            op->getPlannerData(pd);

            return std::to_string(pd.numVertices());
        };
    };
}

/** \brief Get a custom progress callback allocator that allocates a function to visualize the current
 * planning graph of the OMPL planner.
 */
Profiler::ProgressCallbackAllocator getRVIZGraphVisualizationAllocator(IO::RVIZHelperPtr &rviz)
{
    return [rviz](const PlannerPtr &planner,   //
                  const SceneConstPtr &scene,  //
                  const planning_interface::MotionPlanRequest &request) -> Profiler::ProgressCallback {
        // Extract OMPL level information
        const auto &ompl_planner = std::dynamic_pointer_cast<const OMPL::OMPLInterfacePlanner>(planner);
        const auto &ss = ompl_planner->getLastSimpleSetup();
        const auto &op = ss->getPlanner();

        return [rviz, op](const PlannerPtr &planner,                             //
                          const SceneConstPtr &scene,                            //
                          const planning_interface::MotionPlanRequest &request,  //
                          const PlanData &result) {
            const auto &robot = planner->getRobot();
            const auto &ss = op->getSpaceInformation()->getStateSpace();

            // Get the planner data
            ompl::base::PlannerData pd(op->getSpaceInformation());
            op->getPlannerData(pd);

            // Compute the pose of the end-effector for all new vertices
            std::vector<double> reals;

            RobotPoseVector poses(pd.numVertices());
            for (std::size_t i = 0; i < pd.numVertices(); ++i)
            {
                const auto &pdv = pd.getVertex(i);
                const auto &state = pdv.getState();
                ss->copyToReals(reals, state);
                robot->setGroupState(GROUP, reals);

                poses[i] = robot->getLinkTF("wrist_roll_link");
            }

            // Color map for lines in graph. Vertices have tags used by some planners.
            const std::vector<Eigen::Vector4d> color_map = {{0, 0, 1, 0.4},  //
                                                            {0, 1, 0, 0.4},  //
                                                            {1, 0.37, 0.81, 0.4}};

            std::vector<Eigen::Vector3d> points;  // Points in the line
            std::vector<Eigen::Vector4d> colors;  // Color of the points in the line

            // Get the points used in the graph
            std::vector<unsigned int> edges;
            for (std::size_t i = 0; i < pd.numVertices(); ++i)
            {
                pd.getEdges(i, edges);
                const auto &src = pd.getVertex(i);
                for (const auto &edge : edges)
                {
                    const auto &dst = pd.getVertex(edge);

                    std::size_t si = src.getTag();
                    points.emplace_back(poses[i].translation());
                    colors.emplace_back((si < color_map.size()) ? color_map[si] : color_map[0]);

                    std::size_t di = dst.getTag();
                    points.emplace_back(poses[edge].translation());
                    colors.emplace_back((di < color_map.size()) ? color_map[di] : color_map[0]);
                }
            }

            // Update markers on RViz
            rviz->removeAllMarkers();
            rviz->addLineMarker("graph", points, colors, 0.005);
            rviz->updateMarkers();
        };
    };
}

/** \brief Get a custom metric that computes the distance to the goal for the best solution the planner found.
 */
Profiler::ComputeMetricCallback getGoalDistanceCallback()
{
    return [](const PlannerPtr &planner,                             //
              const SceneConstPtr &scene,                            //
              const planning_interface::MotionPlanRequest &request,  //
              const PlanData &run) -> PlannerMetric {
        const auto &ompl_planner = std::dynamic_pointer_cast<const OMPL::OMPLInterfacePlanner>(planner);

        const auto &pdef = ompl_planner->getLastSimpleSetup()->getProblemDefinition();
        double distance = pdef->getSolutionDifference();

        if (distance == -1)
        {
            const auto &start = pdef->getStartState(0);
            const auto &goal = std::dynamic_pointer_cast<ompl::base::GoalRegion>(pdef->getGoal());
            distance = goal->distanceGoal(start);
        }

        return distance;
    };
}

int main(int argc, char **argv)
{
    // Startup ROS
    ROS ros(argc, argv);

    // Program Arguments
    bool gnuplot;
    bool live_plotting;
    bool custom_progress;
    bool rviz_enable;
    double progress_rate;
    std::string planner_name;

    po::options_description description;
    description.add_options()                                                                            //
        ("help,h", "Produces this help message")                                                         //
        ("planner,p", po::value<std::string>(&planner_name)->default_value("RRTstar"),                   //
         "Which OMPL Planner to use.")                                                                   //
        ("rate,r", po::value<double>(&progress_rate)->default_value(0.5),                                //
         "Update rate of planner progress property collection in seconds.")                              //
        ("gnuplot,g", po::bool_switch(&gnuplot),                                                         //
         "Enables GNUPlot visualization of the best cost path (if a progress property)")                 //
        ("liveplot,l", po::bool_switch(&live_plotting),                                                  //
         "Enables live GNUPlot plotting of `best cost` and `num vertices` (if enabled and available).")  //
        ("rviz,v", po::bool_switch(&rviz_enable),                                                        //
         "Enables live visualization of the planning graph through the RViz MarkerArray.")               //
        ("vertices,n", po::bool_switch(&custom_progress),                                                //
         "Enables the number of vertices in the planning graph with a custom progress property.")        //
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, description), vm);

    if (vm.count("help"))
    {
        std::cout << description << std::endl;
        return false;
    }

    po::notify(vm);

    // Create the default Fetch robot.
    auto fetch = std::make_shared<FetchRobot>();
    fetch->initialize(false);

    // Create the default planner for the Fetch.
    auto planner = std::make_shared<OMPL::OMPLInterfacePlanner>(fetch);

    OMPL::Settings settings;
    settings.simplify_solutions = false;

    planner->initialize("package://robowflex_resources/fetch/config/ompl_planning.yaml", settings);

    // Load an example problem of reaching into a box.
    auto scene = std::make_shared<Scene>(fetch);
    scene->fromYAMLFile("package://robowflex_library/yaml/fetch_box/scene0001.yaml");

    // Load the motion planning request and configure for profiling.
    auto request = std::make_shared<MotionRequestBuilder>(planner, GROUP);
    request->fromYAMLFile("package://robowflex_library/yaml/fetch_box/request0001.yaml");
    request->setAllowedPlanningTime(TIME);
    request->setNumPlanningAttempts(1);
    request->setConfig(planner_name);

    scene->getCurrentState() = *request->getStartConfiguration();

    // Create the profiler. We will add some progress callbacks to plot progress properties while the planner
    // solves the problem.
    Profiler profiler;

    // Example of using plotting with the GNUPlot helper.
    IO::GNUPlotHelper gp;

    // Setup RViz
    auto rviz = std::make_shared<IO::RVIZHelper>(fetch);
    if (rviz_enable)
        rviz->updateScene(scene);

    // Plotting options for time series data to display after planning is complete.
    IO::GNUPlotHelper::TimeSeriesOptions tso;
    tso.title = "Best Cost";
    tso.x.min = 0.;
    tso.x.max = TIME;

    // Add a custom metric
    profiler.addProgressAllocator("num vertices INTEGER", getNumVerticesAllocator());

    // Add progress callbacks to plot progress data live while planning.
    if (live_plotting)
    {
        profiler.addProgressCallback(getGNUPlotCallback(gp, "best cost REAL"));
        profiler.addProgressCallback(getGNUPlotCallback(gp, "num vertices INTEGER"));
    }

    // Add a callback to visualize the planning graph in RViz.
    if (rviz_enable)
        profiler.addProgressCallbackAllocator(getRVIZGraphVisualizationAllocator(rviz));

    // Add a custom metric to compute the distance to go to the goal.
    profiler.addMetricCallback("goal_distance", getGoalDistanceCallback());

    // Profiler options.
    Profiler::Options options;
    options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH | Profiler::SMOOTHNESS;
    options.progress_update_rate = progress_rate;

    // 5 iterations of profiling the same problem
    for (std::size_t i = 0; i < 5; ++i)
    {
        PlanData result;
        if (profiler.profilePlan(planner, scene, request->getRequest(), options, result))
        {
            if (gnuplot)
            {
                tso.points.emplace(log::format("Trial %1%", i + 1),  //
                                   result.getProgressPropertiesAsPoints("time REAL", "best cost REAL"));

                // Plot all progress collected so far.
                gp.timeseries(tso);
            }

            if (rviz_enable)
                rviz->updateTrajectory(*result.trajectory);
        }

        RBX_INFO("Press Enter to Continue...");
        std::cin.ignore();
    }

    RBX_INFO("Press Enter to Exit...");
    std::cin.ignore();

    return 0;
}

max_stars_repo_name: servetb/robowflex

max_issues_repo_name: servetb/robowflex

max_forks_repo_name: servetb/robowflex


text:
// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2018 The University of Sydney
// All rights reserved.
//
// 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 Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  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.

#include <deque>
#include <limits>
#include <boost/circular_buffer.hpp>
#include <comma/io/stream.h>
#include <comma/csv/stream.h>
#include <comma/csv/traits.h>
#include "../../visiting/traits.h"

namespace {

void bash_completion( unsigned const ac, char const * const * av )
{
    static char const* arguments[] =
    {
        " turn decelerate",
        " --binary -b --delimiter -d --fields -f --flush --format --full-xpath --help -h --quote --precision --verbose -v",
        " --input-fields --output-fields --output-format",
        " --deceleration -c",
        " --max-acceleration",
        " --min-points-separation --min-gap -g",
        " --approach-speed -s",
        " --sharp-turn-angle -t",
        " --stop-on-sharp-turn --pivot -p"
    };
    std::cout << arguments << std::endl;
    exit( 0 );
}

void operations( unsigned const indent_count = 0 )
{
    auto const indent = std::string( indent_count, ' ' );
    std::cerr << indent << "decelerate; moderate sudden decreases in speed with given deceleration." << std::endl;
    std::cerr << indent << "turn; set the speed on curves in the plan based on given centripetal acceleration." << std::endl;
}

void usage( bool const verbose )
{
    static const char* const indent="    ";

    std::cerr << std::endl;
    std::cerr << "read trajectory points, append speed moderated according to turn angle, max lateral acceleration, etc" << std::endl;
    std::cerr << std::endl;
    std::cerr << "Usage:" << std::endl;
    std::cerr << indent << comma::verbose.app_name() << " <operation> [<options>...]" << std::endl;
    std::cerr << std::endl;
    std::cerr << "Operations:" << std::endl;
    operations(4);
    std::cerr << std::endl;
    std::cerr << "Options:" << std::endl;
    std::cerr << "    decelerate:" << std::endl;
    std::cerr << "        --deceleration, -c=<deceleration>; deceleration by which to moderate sudden decreases in speed." << std::endl;
    std::cerr << std::endl;
    std::cerr << "    turn:" << std::endl;
    std::cerr << "        --approach-speed, -a=<speed>; approach with this speed for waypoints with sharp turns." << std::endl;
    std::cerr << "        --max-acceleration, -c=<acceleration>; maximum centripetal acceleration allowed." << std::endl;
    std::cerr << "        --min-points-separation, --min-gap, -g=<distance>; default=0.01; minimum distance between points for their consideration in the operation." << std::endl;
    std::cerr << "        --sharp-turn-angle, -t=<angle>; default=1.57; assign 'approach-speed' for waypoints having turn angle greater than this." << std::endl;
    std::cerr << "        --speed, -s=[<speed>]; use this speed if it is not in input fields." << std::endl;
    std::cerr << "        --stop-on-sharp-turn, --pivot, -p; on sharp turns (i.e. waypoints set with 'approach-speed'), output extra point with relative heading and no speed." << std::endl;
    std::cerr << std::endl;
    std::cerr << "Info:" << std::endl;
    std::cerr << "    --input-fields; print input fields to stdout and exit." << std::endl;
    std::cerr << "    --output-fields; print output fields to stdout and exit." << std::endl;
    std::cerr << "    --output-format; print output fields to stdout and exit." << std::endl;
    std::cerr << std::endl;
    if( verbose ) { std::cerr << "Csv options:" << std::endl << comma::csv::options::usage( "x,y,z,speed" ) << std::endl; }
    else { std::cerr << "Csv options: run --help --verbose for more info..." << std::endl; }
    std::cerr << std::endl;
    std::cerr << "Examples:" << std::endl;
    std::cerr << "    ( echo '0.0,0.0'; echo '0.3,0.3'; echo '0.6,0.6'; echo '0.6,0.9'; echo '0.6,1.2'; echo '0.9,1.2'; echo '1.2,1.2'; echo '1.5,0.9'; echo '1.8,0.6' ) > trajectory.csv" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    control-speed turn --fields=x,y --max-acceleration=0.5 --approach-speed=0.2 --speed=1 --pivot < trajectory.csv > moderated.csv" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    control-speed decelerate --fields=x,y,speed --deceleration=0.5 < moderated.csv > decelerated.csv" << std::endl;
    std::cerr << std::endl;
}

namespace generic
{
struct input_t
{
    Eigen::Vector3d coordinates;
    double speed;

    input_t( double sp = 0.0 ) : coordinates( Eigen::Vector3d::Zero() ), speed( sp ) {}
};
}

namespace turn
{
struct output_t
{
    double moderated_speed;
    double relative_heading;

    output_t( double sp = 0.0 ) : moderated_speed( sp ), relative_heading( 0.0 ) {}
};

struct record_t
{
    generic::input_t input;
    output_t output;
    std::string line;
    //...debug...
    //double angle;
    //double radius;

    record_t( generic::input_t const& in, comma::csv::input_stream< generic::input_t > const& istrm, comma::csv::options const& csv )
        : input( in )
        , output( in.speed )
        , line()
        //...debug...
        //, angle( 0 )
        //, radius( 0 )
    {
        if( csv.binary() )
        {
            line.resize( csv.format().size() );
            ::memcpy( &line[ 0 ], istrm.binary().last(), csv.format().size() );
        }
        else
        {
            line = comma::join( istrm.ascii().last(), csv.delimiter );
        }
    }

    void write( comma::csv::output_stream< output_t >& ostrm, comma::csv::options const& csv )
    {
        ostrm.append( line, output );
        if( csv.binary() && csv.flush ) { std::cout.flush(); }
        //...debug...
        //std::cerr << line << csv.delimiter << output.moderated_speed << csv.delimiter << angle << csv.delimiter << radius << std::endl;
    }
};
}

namespace deceleration
{
struct record_t
{
    generic::input_t input;
    std::string line;
    //...debug...
    //double angle;
    //double radius;

    record_t( generic::input_t const& in, comma::csv::input_stream< generic::input_t > const& istrm, comma::csv::options const& csv )
        : input( in )
        , line()
        //...debug...
        //, angle( 0 )
        //, radius( 0 )
    {
        if( csv.binary() )
        {
            line.resize( csv.format().size() );
            ::memcpy( &line[ 0 ], istrm.binary().last(), csv.format().size() );
        }
        else { line = comma::join( istrm.ascii().last(), csv.delimiter ); }
    }

    void write( comma::csv::output_stream< generic::input_t >& ostrm, comma::csv::options const& csv )
    {
        ostrm.write( input, line );
        if( csv.binary() && csv.flush ) { std::cout.flush(); }
        //...debug...
        //std::cerr << line << csv.delimiter << input.speed << csv.delimiter << angle << csv.delimiter << radius << std::endl;
    }
};
}

void handle_info_options( comma::command_line_options const& options )
{
    if( options.exists( "--input-fields" ) ) { std::cout << comma::join( comma::csv::names< generic::input_t >( false ), ',' ) << std::endl; exit( 0 ); }
}

double radius_calc( Eigen::Vector3d const& p1, Eigen::Vector3d const& p2, Eigen::Vector3d const& p3 )
{
    auto const side1 = ( p2 - p1 ).norm();
    auto const side2 = ( p3 - p2 ).norm();
    auto const side3 = ( p3 - p1 ).norm();
    auto const perimeter_half = ( side1 + side2 + side3 ) / 2;
    auto area = perimeter_half * ( perimeter_half - side1 ) * ( perimeter_half - side2 ) * ( perimeter_half - side3 ); // Heron's Equation

    if( comma::math::less( 0.0, area ) )
    {
        area = std::sqrt( area );
        return ( side1 * side2 * side3 ) / ( 4.0 * area );
    }
    else
    {
        return comma::math::less( side1, side3 ) && comma::math::less( side2, side3 ) ? std::numeric_limits< double >::max() : 0.0;
    }
}

double angle_calc( Eigen::Vector3d const& p1, Eigen::Vector3d const& p2, Eigen::Vector3d const& p3 )
{
    auto angle_axis = Eigen::AngleAxis< double >( Eigen::Quaternion< double >::FromTwoVectors( p2 - p1, p3 - p2 ) );
    //...debug...
    //std::cerr << std::setprecision(16)
    //          << p1.x() << ',' << p1.y() << ',' << p1.z() << ','
    //          << p2.x() << ',' << p2.y() << ',' << p2.z() << ','
    //          << p3.x() << ',' << p3.y() << ',' << p3.z() << ','
    //          << angle_axis.angle() << ',' << angle_axis.axis().x() << ',' << angle_axis.axis().y() << ',' << angle_axis.axis().z() << std::endl;
    return angle_axis.angle() * ( comma::math::less( angle_axis.axis().z(), 0.0 ) ? -1 : 1 );
}

}

namespace comma { namespace visiting {

template <> struct traits< generic::input_t >
{
    template < typename K, typename V > static void visit ( const K&, generic::input_t& t, V& v )
    {
        v.apply( "coordinates", t.coordinates );
        v.apply( "speed", t.speed );
    }

    template < typename K, typename V > static void visit ( const K&, const generic::input_t& t, V& v )
    {
        v.apply( "coordinates", t.coordinates );
        v.apply( "speed", t.speed );
    }
};

template <> struct traits< turn::output_t >
{
    template < typename K, typename V > static void visit ( const K&, turn::output_t& t, V& v )
    {
        v.apply( "moderated_speed", t.moderated_speed );
        v.apply( "relative_heading", t.relative_heading );
    }

    template < typename K, typename V > static void visit ( const K&, const turn::output_t& t, V& v )
    {
        v.apply( "moderated_speed", t.moderated_speed );
        v.apply( "relative_heading", t.relative_heading );
    }
};

}}

int main( int ac, char* av[] )
{
    try
    {
        comma::command_line_options options( ac, av, usage );
        if( options.exists( "--bash-completion" ) ) bash_completion( ac, av );
        handle_info_options( options );

        std::vector< std::string > operations = options.unnamed( "","--binary,-b,--delimiter,-d,--fields,-f,--flush,--format,--full-xpath,--help,-h,--quote,--precision,--verbose,-v,--input-fields,--output-fields,--output-format,--deceleration,--max-acceleration,-c,--approach-speed,-a,--sharp-turn-angle,-t,--speed,-s,--stop-on-sharp-turn,--pivot,-p,--min-points-separation,--min-gap,-g" );
        if( operations.size() != 1 ) { std::cerr << comma::verbose.app_name() << ": expected one operation, got " << operations.size() << ": " << comma::join( operations, ' ' ) << std::endl; return 1; }

        comma::csv::options csv( options );
        if( csv.fields.empty() ) { csv.fields = "x,y,z,speed"; }
        csv.full_xpath = false;

        if( "turn" == operations[ 0 ] )
        {
            if( !( csv.has_field( "speed" ) || options.exists( "--speed,-s" ) ) )
            { std::cerr << comma::verbose.app_name() << ": speed must be present in input fields or command line options." << std::endl; return 1; }

            comma::csv::options out_csv( csv );
            auto const pivot = options.exists( "--stop-on-sharp-turn,--pivot,-p" );
            out_csv.fields = "moderated_speed"; if( pivot ) { out_csv.fields.append( ",relative_heading" ); }
            if( options.exists( "--output-fields" ) ) { std::cout << out_csv.fields << std::endl; return 0; }
            if( options.exists( "--output-format" ) ) { std::cout << comma::csv::format::value< turn::output_t >( out_csv.fields, true ) << std::endl; return 0; }

            auto const speed = options.value< double >( "--speed,-s", 0.0 );
            auto const max_acceleration = options.value< double >( "--max-acceleration,-c" );
            auto const approach_speed = options.value< double >( "--approach-speed,-a" );
            auto const sharp_turn_angle = options.value< double >( "--sharp-turn-angle,-t", 1.57 ); // near 90 degrees
            auto const min_points_gap = options.value< double >( "--min-points-separation,--min-gap,-g", 0.01 );
            auto const spot_turn_radius = approach_speed * approach_speed / max_acceleration;

            auto unique_points = 0U;
            std::deque< turn::record_t > que;

            comma::csv::input_stream< generic::input_t > istrm( std::cin, csv, generic::input_t( speed ) );
            comma::csv::output_stream< turn::output_t > ostrm( std::cout, out_csv );

            while( istrm.ready() || ( std::cin.good() && !std::cin.eof() ) )
            {
                auto input = istrm.read(); if( !input ) break;
                if( que.empty() || ( input->coordinates - que.back().input.coordinates ).norm() >= min_points_gap ) { unique_points++; }
                que.emplace_back( *input, istrm, csv );

                if( 3U == unique_points )
                {
                    auto relevant_index = que.size() - 2U;
                    auto const angle = angle_calc( que.front().input.coordinates, que[ relevant_index ].input.coordinates, que.back().input.coordinates );
                    auto const radius = radius_calc( que.front().input.coordinates, que[ relevant_index ].input.coordinates, que.back().input.coordinates );

                    if( comma::math::less( std::fabs( angle ), sharp_turn_angle ) && comma::math::less( spot_turn_radius, radius ) )
                    {
                        auto updated_speed = std::sqrt( max_acceleration * radius );
                        que[ relevant_index ].output.moderated_speed = std::max( approach_speed, std::min( que[ relevant_index ].input.speed, updated_speed ) );
                    }
                    else
                    {
                        que[ relevant_index ].output.moderated_speed = std::min( que[ relevant_index ].input.speed, approach_speed );
                        if( pivot )
                        {
                            auto temp = que.back();
                            que.back() = que[ relevant_index ];
                            que.back().output.moderated_speed = 0;
                            que.back().output.relative_heading = angle;
                            que.push_back( temp );
                        }
                    }
                    while( 2 < que.size() ) { que.front().write( ostrm, csv ); que.pop_front(); }
                    unique_points = 2;
                }
            }
            for( auto ii = 0U; ii < que.size(); ii++ ) { que[ ii ].write( ostrm, csv ); }
            return 0;
        }
        else if( "decelerate" == operations[ 0 ] )
        {
            comma::csv::input_stream< generic::input_t > istrm( std::cin, csv );

            comma::csv::output_stream< generic::input_t > ostrm( std::cout, csv );
            std::deque< deceleration::record_t > que;

            auto const deceleration = options.value< double >( "--deceleration,-c" );
            auto max_speed = 0.0;
            while( istrm.ready() || ( std::cin.good() && !std::cin.eof() ) )
            {
                auto input = istrm.read(); if( !input ) break;
                que.emplace_back( *input, istrm, csv );

                max_speed = std::max( que.back().input.speed, max_speed );
                auto const affected_distance = max_speed * max_speed / ( 2 * deceleration ); // kinematic equation with min_speed = 0.0

                auto curri = que.rbegin();
                auto previ = curri + 1;
                double updated_speed, distance;

                for( ; previ != que.rend()
                         && ( comma::math::less( 0.0, distance = ( curri->input.coordinates - previ->input.coordinates ).norm() )
                           || comma::math::less( 0.0, curri->input.speed ) )
                         && comma::math::less( updated_speed = std::sqrt( curri->input.speed * curri->input.speed + 2 * deceleration * distance )
                                             , previ->input.speed )
                     ; previ++, curri++ )
                {
                    previ->input.speed = updated_speed;
                }
                while(( que.back().input.coordinates - que.front().input.coordinates ).norm() > affected_distance )
                {
                    que.front().write( ostrm, csv );
                    que.pop_front();
                }
            }
            for( auto ii = 0U; ii < que.size(); ii++ ) { que[ ii ].write( ostrm, csv ); }
            return 0;
        }
        std::cerr << comma::verbose.app_name() << ": unknown operation: " <<  operations[ 0 ] << std::endl;
        return 1;
    }
    catch( std::exception& ex ) { std::cerr << comma::verbose.app_name() << ": " << ex.what() << std::endl; }
    catch( ... ) { std::cerr << comma::verbose.app_name() << ": unknown exception" << std::endl; }
    return 1;
}

max_stars_repo_name: mission-systems-pty-ltd/snark

max_issues_repo_name: NEU-LC/snark

max_forks_repo_name: NEU-LC/snark


text:
/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <boost/rational.hpp>
#include <sprig/cast_converter.hpp>

namespace sprig {
	namespace rational_detail {
		//
		// rational_arrange_digit_impl
		//
		template<typename IntType>
		SPRIG_INLINE IntType rational_arrange_digit_impl(
			IntType const& denom,
			IntType const& n,
			IntType const& current,
			IntType const& current_digit
			)
		{
			return (current % denom != 0)
				? rational_arrange_digit_impl(denom, n, current * n, current_digit + 1)
				: current_digit
				;
		}
		//
		// rational_arrange_factor_impl
		//
		template<typename IntType>
		SPRIG_INLINE IntType rational_arrange_factor_impl(
			IntType const& denom,
			IntType const& n,
			IntType const& current
			)
		{
			return (current % denom != 0)
				? rational_arrange_factor_impl(denom, n, current * n)
				: current
				;
		}
	}	// namespace rational_detail
	//
	// rational_cast
	//
	template<typename To, typename Converter, typename IntType>
	SPRIG_INLINE To rational_cast(boost::rational<IntType> const& from, Converter conv) {
		return conv(from.numerator()) / conv(from.denominator());
	}
	//
	// rational_converter_cast
	//
	template<typename Tag, typename To, typename IntType>
	SPRIG_INLINE To rational_converter_cast(boost::rational<IntType> const& from) {
		return sprig::converter_cast<Tag, To>(from.numerator()) / sprig::converter_cast<Tag, To>(from.denominator());
	}
	//
	// rational_arrange_digit
	//
	template<typename IntType>
	SPRIG_INLINE IntType rational_arrange_digit(
		boost::rational<IntType> const& v,
		IntType const& n = 10
		)
	{
		return rational_detail::rational_arrange_digit_impl<IntType>(v.denominator(), n, 1, 0);
	}
	//
	// rational_arrange_factor
	//
	template<typename IntType>
	SPRIG_INLINE IntType rational_arrange_factor(
		boost::rational<IntType> const& v,
		IntType const& n = 10
		)
	{
		return rational_detail::rational_arrange_factor_impl<IntType>(v.denominator(), n, 1);
	}
}	// namespace sprig

#endif	// #ifndef SPRIG_UTILITY_RATIONAL_HPP

ext: hpp

max_stars_repo_name: bolero-MURAKAMI/Sprig

max_issues_repo_name: bolero-MURAKAMI/Sprig

max_forks_repo_name: bolero-MURAKAMI/Sprig