text:
#ifndef N_BODY_PHYSICAL_HPP
#define N_BODY_PHYSICAL_HPP

#include "communication.hpp"
#include "config.hpp"
#include "data.hpp"
#include "logging.hpp"
#include "space.hpp"
#include "tree.hpp"
#include <boost/archive/xml_oarchive.hpp>
#include <boost/mpi.hpp>
#include <cmath>
#include <cstddef>

namespace n_body::physical {

using namespace n_body::data;

template <typename T, std::size_t Dimension>
void step(const config::Configuration<T> &config,
          const boost::mpi::communicator &comm, int root,
          data::Bodies<T, Dimension> &bodies,
          const data::tree::BodyTree<T, Dimension> &tree);

template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::tree::BodyTree<T, Dimension> &tree);

template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::Bodies<T, Dimension> &bodies);

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_tree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree,
    const data::Vector<T, Dimension> &position);

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_subtree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree, std::size_t root,
    const data::Vector<T, Dimension> &position);

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_position_to_position(
    const config::Configuration<T> &config,
    const data::Vector<T, Dimension> &other_position,
    data::Scalar<T> other_mass, const data::Vector<T, Dimension> &position);

// update bodies one step
template <typename T, std::size_t Dimension>
void step(const config::Configuration<T> &config,
          const boost::mpi::communicator &comm,
          data::Bodies<T, Dimension> &bodies,
          const data::tree::BodyTree<T, Dimension> &tree) {
  communication::Division division(comm, bodies.size());
  data::Bodies<T, Dimension> local_bodies(&bodies[division.begin],
                                          &bodies[division.end]);
  // step(config, local_bodies.begin(), local_bodies.end(), tree);
  step(config, local_bodies.begin(), local_bodies.end(), bodies);
  boost::mpi::all_gather(comm, local_bodies.data(), division.count, bodies);
}

// update bodies one step by iterator
template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::Bodies<T, Dimension> &bodies) {
  for (; first != last; ++first) {
    data::Vector<T, Dimension> acceleration{
        0,
        0,
        0,
    };
    for (const auto &body : bodies) {
      acceleration += gravity_per_unit_mass_position_to_position(
          config, body.position, body.mass, first->position);
    }
    first->velocity += config.time * acceleration;
    first->position += config.time * first->velocity;
  }
}

// update bodies one step by iterator
template <typename T, std::size_t Dimension, typename Iter>
void step(const config::Configuration<T> &config, Iter first, Iter last,
          const data::tree::BodyTree<T, Dimension> &tree) {
  for (; first != last; ++first) {
    auto acceleration =
        gravity_per_unit_mass_tree_to_position(config, tree, first->position);
    first->velocity += config.time * acceleration;
    first->position += config.time * first->velocity;
  }
}

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_tree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree,
    const data::Vector<T, Dimension> &position) {
  if (tree.tree.empty())
    return {0, 0, 0};
  return gravity_per_unit_mass_subtree_to_position(config, tree, 0, position);
}

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_subtree_to_position(
    const config::Configuration<T> &config,
    const data::tree::BodyTree<T, Dimension> &tree, std::size_t root,
    const data::Vector<T, Dimension> &position) {

  if (tree.node(root).node_type() == data::tree::NodeType::Inner) {
    auto position_in_space = space::contains(tree.node(root).space, position);

    auto space_size = space::size_of_space(tree.node(root).space);
    auto distance = data::module_of(position - tree.node(root).center_of_mass);
    if (!position_in_space && (space_size / distance) < config.theta) {
      return gravity_per_unit_mass_position_to_position(
          config, tree.node(root).center_of_mass, tree.node(root).mass,
          position);
    } else {
      data::Vector<T, Dimension> sum{
          0,
          0,
          0,
      };
      for (std::size_t i = 0;
           i < data::tree::BodyTreeInnerNode<T, Dimension>::CHILDREN_NUMBER;
           ++i) {
        if (auto subtree = tree.child_of_node(root, i)) {
          sum += gravity_per_unit_mass_subtree_to_position(config, tree,
                                                           *subtree, position);
        }
      }
      return sum;
    }
  } else {
    // node is a leaf
    return gravity_per_unit_mass_position_to_position(
        config, tree.node(root).center_of_mass, tree.node(root).mass, position);
  }
}

template <typename T, std::size_t Dimension>
data::Vector<T, Dimension> gravity_per_unit_mass_position_to_position(
    const config::Configuration<T> &config,
    const data::Vector<T, Dimension> &other_position,
    data::Scalar<T> other_mass, const data::Vector<T, Dimension> &position) {
  auto dp = other_position - position;
  auto distance = data::module_of(dp);
  if (distance == 0) {
    return {
        0,
        0,
        0,
    };
  } // singularity
  auto G = config.G;
  auto soften = config.soften_length;
  auto result = G * other_mass /
                std::pow((soften_length * soften_length + distance * distance),
                         static_cast<T>(3) / static_cast<T>(2)) *
                dp;
  return result;
}

} // namespace n_body::physical

#endif

ext: hpp

max_stars_repo_name: linyinfeng/n-body

max_issues_repo_name: linyinfeng/n-body

max_forks_repo_name: linyinfeng/n-body


text:
/* Copyright (c) 2013 by Wayne Wright, Round Rock, Texas.
** See license at http://github.com/w5xd/plm/blob/master/LICENSE.md */
#include <iomanip>
#include <sstream>
#include <fstream>
#include <functional>
#include "PlmMonitor.h"
#include "Dimmer.h"
#include "X10Dimmer.h"
#include "Fanlinc.h"
#include "Keypad.h"
#include <boost/date_time/posix_time/posix_time.hpp> // cuz c++11 chrono can't print times w/o warnings for statics

#if defined (WIN32)
#include "PlmMonitorWin.h"
#elif defined (LINUX32)
#include "PlmMonitorLinux.h"
#endif

#define DIM(x) (sizeof(x) / sizeof((x)[0]))

namespace w5xdInsteon {

static int gNextId = 10349;
enum {MAX_COMMAND_READS = 5, MAX_RETRIES = 8};
static const int COMMAND_DELAY_MSEC = 1000;
const unsigned char PlmMonitor::SET_ACQ_MSG_BYTE = 0x68;
const unsigned char PlmMonitor::SET_ACQ_MSG_2BYTE = 0x71;
const unsigned char PlmMonitor::SET_NAQ_MSG_BYTE = 0x70;

static const unsigned char OUT_OF_ORDER[] =
{
    PlmMonitor::SET_ACQ_MSG_BYTE,
    PlmMonitor::SET_ACQ_MSG_2BYTE,
    PlmMonitor::SET_NAQ_MSG_BYTE
};

static bool isOutOfOrder(unsigned char v)
{
    for (int i = 0 ; i < DIM(OUT_OF_ORDER); i++)
        if (v == OUT_OF_ORDER[i]) return true;
    return false;
}

void bufferToStream(std::ostream &st, const unsigned char *v, int s)
{
    st << std::hex ;
    for (int i = 0; i < s; i++)
    {
        if (i > 0) st << " ";
        st << "0x" << std::setfill('0') << std::setw(2) << (int)*v++;
    }
}

namespace {
	struct compPtr {
			bool operator()(const std::shared_ptr<X10Dimmer> &A,
				const std::shared_ptr<X10Dimmer> &B) const
			{
				if (!A || !B)
					return A < B;
				return *A.get() < *B.get();
			}
		};
}

class X10DimmerCollection : public std::set<std::shared_ptr<X10Dimmer>, compPtr> {};

void PlmMonitor::timeStamp(std::ostream &st)
{
    auto now(std::chrono::steady_clock::now());
    std::streamsize p = st.precision(6);
    double usec = static_cast<double>(std::chrono::duration_cast<std::chrono::microseconds>(now-m_startupTime).count());
    std::time_t n = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
    st << boost::posix_time::microsec_clock::universal_time() << " (" << std::setfill(' ') << std::setw(13) << std::fixed << std::showpoint
        << usec / 1e6 << ") ";
    st.precision(p);
}

/**********************************************************************
** member functions
*/

PlmMonitor::PlmMonitor(const char *commPortName, const char *logFileName) :
    m_which(gNextId++),
    m_shutdown(false),
    m_ThreadRunning(0),
    m_io(new PlmMonitorIO(commPortName)),
    m_verbosity(static_cast<int>(MESSAGE_QUIET)),
    m_startupTime(std::chrono::steady_clock::now()),
    m_multipleLinkingRequested(0),
    m_haveAllModemLinks(false),
    m_nextCommandId(0),
    m_queueNotifications(false),
    m_commandDelayMsec(COMMAND_DELAY_MSEC),
	m_X10Dimmers(new X10DimmerCollection())
{
    if (logFileName && *logFileName)
        m_errorFile.open(logFileName, std::ofstream::out | std::ofstream::app);
}

PlmMonitor::~PlmMonitor()
{
    m_shutdown = true;
    syncWithThreadState(false);
    if (m_thread.joinable())
        m_thread.join();
}

int PlmMonitor::printLogString(const char *s)
{
    if (s) cerr() << s;
    return 1;
}

const std::string &PlmMonitor::commPortName() const {return m_io->commPortName();}

void PlmMonitor::syncWithThreadState(bool v)
{
    std::unique_lock<std::mutex> l(m_mutex);
    while (v ^ (m_ThreadRunning != 0))
        m_condition.wait(l);
}

void PlmMonitor::signalThreadStartStop(bool v)
{
    {
        std::unique_lock<std::mutex> l(m_mutex);
        m_ThreadRunning += v ? 1 : -1;
    }
    m_condition.notify_all();
}

int PlmMonitor::shutdownModem()
{
    m_shutdown = true;
    m_condition.notify_all();
    return 0;
}

void PlmMonitor::reportErrorState(const unsigned char *command, int clen,
                                  std::shared_ptr<InsteonCommand> p)
{
    if (p->m_answerState < 0)
    {
        if (m_verbosity >= static_cast<int>(MESSAGE_ON))
        {
            timeStamp(cerr());
            cerr() << "PlmMonitor::sendCommandAndWait error on ";
            bufferToStream(cerr(), command, clen);
            cerr() << " answer ";
            if (!p->m_answer->empty())
                bufferToStream(cerr(), &(*p->m_answer)[0], static_cast<int>(p->m_answer->size()));
            cerr() << " answer_state: " << std::dec << p->m_answerState << std::endl;
        }
    }
}

std::shared_ptr<InsteonCommand> PlmMonitor::queueCommand(
    const unsigned char *v, unsigned s, unsigned resLen, bool retry, InsteonCommand::CompletionCb_t fcn)
{
    std::shared_ptr<InsteonCommand> p;
    p.reset(new InsteonCommand(resLen, retry ? MAX_RETRIES : 0, fcn));
    p->m_command.assign(v, v+s);
    {
        std::unique_lock<std::mutex> l(m_mutex);
        p->m_globalId = ++m_nextCommandId;
        if (m_writeQueue.empty() || (s <= 1) || !isOutOfOrder(v[1]))
            m_writeQueue.push_back(p);
        else
        {
            // scoot this particular command ahead of anything that is not the same command
            std::deque<std::shared_ptr<InsteonCommand> >::reverse_iterator itor = m_writeQueue.rbegin();
            while (itor != m_writeQueue.rend() &&
                    ((*itor)->m_command.size() > 1) &&
                    !isOutOfOrder((*itor)->m_command[1]))
                    itor++;
            m_writeQueue.insert(itor.base(), p);
        }
    }

    if (m_verbosity >= static_cast<int>(MESSAGE_EVERY_IO))
    {
        timeStamp(cerr());
        cerr() << "PlmMonitor::queueCommand " ;
        bufferToStream(cerr(), &p->m_command[0], static_cast<int>(p->m_command.size()));
        cerr() << std::endl;
    }
    return p;
}

std::shared_ptr<InsteonCommand>
PlmMonitor::sendCommandAndWait(const unsigned char *v, unsigned s, unsigned resLen, bool retry)
{
    std::shared_ptr<InsteonCommand> p = queueCommand(v, s, resLen, retry);
    {
        std::unique_lock<std::mutex> l(m_mutex);
        // wait for an answer
        while (p->m_answerState == 0)
            m_condition.wait(l);
    }
    reportErrorState(v, s, p);
    return p;
}

void
PlmMonitor::forceGetImInfo()
{
    static const unsigned char buf[] =  {0x02, 0x60};
    {
        std::unique_lock<std::mutex> l(m_mutex);
        if (
            m_writeQueue.empty() ||
            m_writeQueue.front()->m_command.size() != sizeof(buf) ||
            memcmp(buf, &m_writeQueue.front()->m_command[0], sizeof(buf)) != 0)
        {   // only push this if the first command is not already GetImInfo
            std::shared_ptr<InsteonCommand> p;
            p.reset(new InsteonCommand(9));
            p->m_command.assign(buf, buf + sizeof(buf));
            p->m_globalId = ++m_nextCommandId;
            m_writeQueue.push_front(p);
        }
    }
}

int PlmMonitor::MessageGetImInfo()
{
    // Get IM Info message
    int ret = 0;
    static const unsigned char buf[] =  {0x02, 0x60};
    std::shared_ptr<InsteonCommand> p = sendCommandAndWait(buf, sizeof(buf), 9);
    if (p->m_answerState < 0)
        return p->m_answerState;

    if ((*p->m_answer)[8] == 0x6)
    {
        memcpy(m_IMInsteonId, &(*p->m_answer)[2], sizeof(m_IMInsteonId));
        m_IMDeviceCat = (*p->m_answer)[5];
        m_IMDeviceSubCat = (*p->m_answer)[6];
        m_IMFirmwareVersion = (*p->m_answer)[7];
        ret = p->m_timesSent;
        if (m_verbosity >= static_cast<int>(MESSAGE_ON))
        {
            cerr() << "Plm modem id = ";
            bufferToStream(cerr(), &(*p->m_answer)[2], 3);
            cerr() << std::endl;
        }
    }
    else
        return -1;

    return ret;
}

int PlmMonitor::init()
{
    int status = m_io->OpenCommPort();
    if (status < 0) return status;
    m_thread = std::thread(std::bind(&PlmMonitor::ioThread, this));
    syncWithThreadState(true);
    {  // Set Modem mode
        static const unsigned char tempWrite[] = {0x02, 0x6b, 0x0};
        sendCommandAndWait(tempWrite, sizeof(tempWrite), 4, false);
        static const unsigned char getImConfig[]  = {0x02, 0x73};
        sendCommandAndWait(getImConfig, sizeof(getImConfig), 6, false);
    }
    MessageGetImInfo();
    return status;
}

static const char FirstIMCommand = 0x50;
static const char LastIMCommand = 0x58;
static const int ReceiveCommandSizes[] =
{
  11,  //0x50  Standard Message Received
  25,  //0x51  Extended Message Received
   4,  //0x52  X10 Received
  10,  //0x53  ALL-Linking completed
   3, //0x54  Button Event Report
   2, //0x55  User Reset Detected
   7, //0x56  All-Link Cleanup Failure Report
  10, //0x57  All link record response
   3, //0x58  all-link cleanup status report
};

static const int LargestMessageLength = 25;
static const int ReopenDeviceAfterFails = 100;

void PlmMonitor::ioThread()
{
    signalThreadStartStop(true);
    std::shared_ptr<InsteonCommand> p;
    std::shared_ptr<InsteonCommand> lastCommandAcqed;
    auto timeOfLastIncomingMessage = std::chrono::steady_clock::now();
    unsigned char rbuf[100];
    int sizeToRead = LargestMessageLength + 1;
    std::deque<unsigned char> leftOver;
    int readError = 0;
    int delayDequeue = 0;
    int failureMessageRepeatCount = 0;
    while (!m_shutdown)
    {
        std::shared_ptr<std::vector<unsigned char> > msg (
            new std::vector<unsigned char>(leftOver.begin(), leftOver.end()));
        std::vector<unsigned char> &curMessage(*msg.get());
        if (m_verbosity >= static_cast<int>(MESSAGE_MOST_IO))
        {
            if (!leftOver.empty())
            {
                timeStamp(cerr());
                cerr() << "leftOver: ";
                std::vector<unsigned char> b(leftOver.begin(), leftOver.end());
                bufferToStream(cerr(), &b[0], static_cast<int>(b.size()));
                cerr() << std::endl;
            }
        }
        leftOver.clear();
        // read next "message"
        // A "message" is defined to be a string of bytes that:
        //  start with 0x02
        //  next byte is either matches the corresponding byte in the last command
        //      we sent it, or
        //  is in the range FirstIMCommand to LastIMCommand
        //  Is of the length specified by the IM command or Host command
        //
        // If we reach a 500msec timeout without any of the above happening,
        // then discard what we have. (500msec = MAX_COMMAND_READS * 100msec read timeout)
        //
        // if we receive more characters than we need, then leave them in leftOver
        // looking for command responses is more tolerant that just polling for input
        int currentCommandReadLoopCount = p ? MAX_COMMAND_READS : 0;
        for (;;)
        {
            unsigned nr=0;
            if (m_io->Read(rbuf, sizeToRead, &nr)) // implementations each has 100msec read timeout
                readError = 0;
            else if (++readError >= ReopenDeviceAfterFails)
            {
                readError = 0;
                int status = m_io->OpenCommPort();
                if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                {
                   if (status >= 0)
                   {
                       cerr() << "Reopened comm port successfully" << std::endl;
                       failureMessageRepeatCount = 0;
                   }
                   else
                   {
                       if (failureMessageRepeatCount++ == 0)
                            cerr() << "Reopened comm port and failed again" << std::endl;
                   }
                }
                if (status < 0)
                {   // on failures, there was no wait in the Read call
                    std::this_thread::sleep_for(std::chrono::seconds(2));
                }
                else
                {
                    if (p)
                    {
                        m_writeQueue.push_front(p);
                        p.reset();
                    }
                    forceGetImInfo();
                }
            }
            if (nr != 0)    // if we got anything, ensure we go around again
                 currentCommandReadLoopCount = 1;
            for (unsigned i = 0; i < nr; i++)
                curMessage.push_back(rbuf[i]);

            if (m_verbosity >= static_cast<int>(MESSAGE_EVERY_IO))
            {
                if ((nr > 0) || (curMessage.size() > 0))
                {
                    timeStamp(cerr());
                    cerr() << "PlmMonitor::ioThread read" << std::endl;
                    bufferToStream(cerr(), rbuf, nr);
                    if (!curMessage.empty())
                    {
                        cerr() << std::endl << " with current message:" << std::endl;
                        bufferToStream(cerr(), &curMessage[0], static_cast<int>(curMessage.size()));
                    }
                    else
                        cerr() << std::endl << " with no current message";
                    cerr() << std::endl;
                }
            }

            // message must start with 0x02
            int charsDiscarded = 0;
            unsigned lastCharDiscarded = 0;
            while ((curMessage.size() > 0) && (curMessage[0] != 0x02))
            {
                charsDiscarded += 1;
                lastCharDiscarded = *curMessage.begin();
                curMessage.erase(curMessage.begin()); // discard until in sync
            }

            if (charsDiscarded && (m_verbosity >= static_cast<int>(MESSAGE_ON)))
                cerr() << "PlmMonitor::ioThread discarded " << std::dec << charsDiscarded << " characters ending with 0x" <<
                std::hex << lastCharDiscarded << std::endl;

            if (curMessage.size() >= 2)
            {
                unsigned char msgID = curMessage[1];
                unsigned expectedCount = 0;
                if ((msgID >= FirstIMCommand) && (msgID <= LastIMCommand))
                    expectedCount = ReceiveCommandSizes[msgID - FirstIMCommand];
                else if (p && (msgID == p->m_command[1]))
                    expectedCount = p->m_responseLength;
                else    // discard first character and try again
                    curMessage.erase(curMessage.begin());
                // put away left-overs
                while (curMessage.size() > expectedCount)
                {
                    leftOver.push_front(curMessage.back());
                    curMessage.resize(curMessage.size() - 1);
                }

                if (curMessage.size() == expectedCount)
                    break; // successful read of IM->Host

                sizeToRead = static_cast<int>(expectedCount - curMessage.size());
            }
            else
                sizeToRead -= nr;

            if (--currentCommandReadLoopCount < 0)
            {   // we read for a while and failed to receive what we wanted/expected
                if (p)
                {   // have a command outstanding and received nothing,
                    if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                    {
                        timeStamp(cerr());
                        cerr() << "PlmMonitor::ioThread loop count discard for message:" << std::endl;
                        bufferToStream(cerr(), &p->m_command[0], static_cast<int>(p->m_command.size()));
                        cerr() << " with answer ";
                        if (!curMessage.empty())
                            bufferToStream(cerr(), &curMessage[0], static_cast<int>(curMessage.size()));
                        else cerr() << "empty.";
                        cerr() << " Reopening tty port" << std::endl;
                    }
                    m_io->OpenCommPort();
                    if (--p->m_retry < 0)
                    {
                        if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                           cerr() << "Retry count exceeded" << std::endl;
                        // terminate this command--not getting an answer
                        p->m_answer = msg;
                        {
                            std::unique_lock<std::mutex> l(m_mutex);
                            p->m_answerState = -7;
                            m_condition.notify_all();
                        }
                        if (p->m_whenDone) p->m_whenDone(p.get());
                    }
                    else
                    {
                        if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                           cerr() << "Retrying after reopening TTY" << std::endl;
                        {
                            std::unique_lock<std::mutex> l(m_mutex);
                            //put it back in queue at the front
                            m_writeQueue.push_front(p);
                            delayDequeue = 5; // don't dequeue this command for a bit
                        }
                        forceGetImInfo();
                    }
                    p.reset();
               }
               if (curMessage.size())
                   if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                       cerr() << "Discarding " << std::dec << curMessage.size() << " characters." << std::endl;
               curMessage.clear();
               break;   // nothing here, but check for incoming command
            }
        }

        sizeToRead = LargestMessageLength + 1;

        if (!curMessage.empty())
        {
            if (p &&  // we have sent a command....
                  (curMessage.size() > 1) &&
                     (p->m_command[1] == curMessage[1]))
            {   // successful command completion
                bool print = m_verbosity >= static_cast<int>(MESSAGE_EVERY_IO);
                if ((p->m_command.size() > 1) &&
                    (p->m_command[1] == 0x73) &&
                    (m_verbosity >= static_cast<int>(MESSAGE_ON)))
                    print = true;
                if (print)
                {
                    timeStamp(cerr());
                    cerr() << "PlmMonitor::ioThread successful completion for" << std::endl;
                    bufferToStream(cerr(), &p->m_command[0], static_cast<int>(p->m_command.size()));
                    cerr() << " with answer" << std::endl;
                    bufferToStream(cerr(), &curMessage[0], static_cast<int>(curMessage.size()));
                    cerr() << std::endl;
                }
                p->m_answer = msg;

                unsigned char acq = (*p->m_answer)[p->m_responseLength-1];

                if (((p->m_answer->size() >= p->m_responseLength) &&
                    (acq == 0x6)) ||
                    (--p->m_retry < 0))
                {
                    {
                        std::unique_lock<std::mutex> l(m_mutex);
                        if (acq == 0x6)
                        {
                        	p->m_answerState = 1;
                            lastCommandAcqed = p;
                        }
                        else if (p->m_retry < 0)
                            p->m_answerState = -1;
                        else
                            p->m_answerState = -9;
                        m_condition.notify_all();
                    }
                    if (p->m_whenDone)  p->m_whenDone(p.get());
                    p.reset();
                }
                else
                {   // answer is not adequate
                    if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                    {
                        timeStamp(cerr());
                        cerr() << "PlmMonitor::ioThread retrying command " << std::endl;
                        bufferToStream(cerr(), &p->m_command[0], static_cast<int>(p->m_command.size()));
                        cerr() << std::endl;
                    }
                    std::unique_lock<std::mutex> l(m_mutex);
                    //put it back in queue at the front
                    m_writeQueue.push_front(p);
                    p.reset();
                    delayDequeue = 10;
                }
            }
            else
            {   // is not an answer to our command
		        if (m_verbosity >= static_cast<int>(MESSAGE_ON))
		        {
                    timeStamp(cerr());
                    cerr() << "Delivering remote message" << std::endl;
			        bufferToStream(cerr(), &(*msg)[0], static_cast<int>(msg->size()));
			        cerr() << std::endl;
		        }
                timeOfLastIncomingMessage = std::chrono::steady_clock::now();
                if (p)
                {   // delay it.
                    std::unique_lock<std::mutex> l(m_mutex);
                    m_writeQueue.push_front(p);
                    p.reset();
                    if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                        cerr() << "Delaying previously dequeued p" << std::endl;
                }
                deliverfromRemoteMessage(msg, lastCommandAcqed);
                curMessage.clear();
            }
        }
        if (curMessage.empty() && !p)    // only look for commands if nothing pending from modem
        {
            if (leftOver.empty() && (--delayDequeue <= 0))
            {// check for commands to write to modem
                std::unique_lock<std::mutex> l(m_mutex);
                if (!m_writeQueue.empty())
                {
                    std::shared_ptr<InsteonCommand> temp = m_writeQueue.front();
                    auto now(std::chrono::steady_clock::now());
                    if (
                        (temp->m_command.size() < 2) ||
                        // commands 0x62 and 0x63 cannot be sent until m_commandDelayMsec after the last thing we heard from the PLM
                        ((temp->m_command[1] != 0x62) && (temp->m_command[1] != 0x63)) ||
                        ((now - timeOfLastIncomingMessage) > std::chrono::milliseconds(m_commandDelayMsec))
                        )
                    {
                        m_writeQueue.pop_front();
                        p = temp;
                    }
                }
                delayDequeue = 0;
            }
            if (p)
            {
                timeOfLastIncomingMessage = std::chrono::steady_clock::now();
                writeCommand(p);
            }
        }
        if (p)
            sizeToRead = p->m_responseLength;
    }
    {
        std::unique_lock<std::mutex> l(m_mutex);
        while (!m_writeQueue.empty())
        {
            p = m_writeQueue.front();
            m_writeQueue.pop_front();
            p->m_answerState = -11;
        }
        m_condition.notify_all();
    }
    signalThreadStartStop(false);
    if (m_verbosity >= static_cast<int>(MESSAGE_ON))
    {
        timeStamp(cerr());
        cerr() << "PlmMonitor::ioThread exit" << std::endl;
    }
}

void PlmMonitor::deliverfromRemoteMessage(std::shared_ptr<std::vector<unsigned char> > v,
                                          std::shared_ptr<InsteonCommand> lastAcqued)
{
    std::vector<unsigned char> &raw = *v.get();
    switch (raw[1])
    {
    case 0x50:
    case 0x51:
        if (raw.size() >= 11)
        {
            unsigned char addr[3];
            addr[0] = raw[2]; addr[1] = raw[3]; addr[2] = raw[4];
            std::shared_ptr<InsteonDevice> p;
            {
                std::unique_lock<std::mutex> l(m_mutex);
                InsteonDeviceSet_t::iterator itor = m_insteonDeviceSet.find(addr);
                if (itor != m_insteonDeviceSet.end())
                    p = itor->m_p;
            }
            if (p)
            {
                p->incomingMessage(raw, lastAcqued);
                unsigned char flag = raw[8] & 0xf0;
                if (raw[1] == 0x50)
                {
                    unsigned char group = raw[7];
                    unsigned char cmd1 = raw[9];
                    unsigned char cmd2 = raw[10];
                    bool doNotify = (flag == 0xc0) && (raw[5] == 0) && (raw[6] == 0);
                    if (flag == 0x40)
                    {   // P2P Cleanup
                        if (memcmp(&m_IMInsteonId[0], &raw[5], sizeof(m_IMInsteonId)) == 0)
                        {
                            // addressed to me
                            doNotify = true;
                            group = cmd2;
                        }
                    }
                    if (doNotify)
                    {
                        std::unique_lock<std::mutex> l(m_mutex);
                        if (m_queueNotifications)
                        {
                            auto now(std::chrono::steady_clock::now());
                            // put together a NotificationEntry for this message
                            std::shared_ptr<NotificationEntry> nep (new NotificationEntry());
                            NotificationEntry &ne = *nep.get();
                            ne.m_device = p;
                            ne.group = group;
                            ne.cmd1 = cmd1;
                            ne.cmd2 = cmd2;
                            ne.m_received = now;
                            for (LinkTable_t::iterator itor = m_ModemLinks.begin();
                                itor != m_ModemLinks.end();
                                itor ++)
                            {
                                if ((itor->m_addr == p->addr()) &&
                                    (itor->m_group == ne.group) &&
                                    (itor->isResponder()))
                                {
                                    ne.ls1 = itor->m_LinkSpecific1;
                                    ne.ls2 = itor->m_LinkSpecific2;
                                    ne.ls3 = itor->m_LinkSpecific3;
                                    break;
                                }
                            }

                            bool duplicate = false;
                            for (NotificationEntryQueue_t::iterator itor = m_priorNotifications.begin();
                                    itor != m_priorNotifications.end();
                                    )
                            {
                                if ((now - itor->get()->m_received) >
                                    std::chrono::seconds(1))
                                {   // anything old in the notification queue is deleted
                                    itor = m_priorNotifications.erase(itor);
                                }
                                else
                                {   // anything not too told in the queue is compared with the new one
                                    if (*itor->get() == ne)
                                        duplicate = true;
                                    itor++;
                                }
                            }
                            m_priorNotifications.push_back(nep);
                            if (!duplicate)
                            {
                                m_notifications.push_back(nep);
                                m_condition.notify_all();
                            }
                        }
                        else
                            m_priorNotifications.clear();
                    }
                }
            }
        }
        break;
    case 0x57:
        if (m_verbosity >= static_cast<int>(MESSAGE_ON))
        {
            cerr() << "Modem all-link record is ";
            if (!v->empty()) bufferToStream(cerr(), &(*v)[0], static_cast<int>(v->size()));
            cerr() << std::endl;
        }
        if (raw.size() >= 10)
        {
            std::unique_lock<std::mutex> l(m_mutex);
            m_ModemLinks.push_back(InsteonLinkEntry(&raw[2]));
        }
        else
            if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                cerr() << "Received rejected Link data message because size = " << raw.size() << std::endl;
        break;
    case 0x53:
        if (raw.size() == 10)
        {
            if (m_verbosity >= static_cast<int>(MESSAGE_ON))
            {
                cerr() << "Device link received from ";
                bufferToStream(cerr(), &(*v)[4], 3);
                cerr() << std::endl;
            }
            unsigned char backToLinking = 0;
            {
                std::unique_lock<std::mutex> l(m_mutex);
                backToLinking = m_multipleLinkingRequested;
            }
            if (backToLinking != 0)
            {
                if (m_verbosity >= static_cast<int>(MESSAGE_ON))
                    cerr() << "Back to linking\n" << std::endl;
                startLinking(backToLinking);
            }
        }
        break;
    case 0x56:
        if (raw.size() >= 7)
        {
            if (m_verbosity >= static_cast<int>(MESSAGE_ON))
            {
                cerr() << "All-Linked-Cleanup failure report ";
                bufferToStream(cerr(), &raw[2], 2);
                cerr() << " addr ";
                bufferToStream(cerr(), &raw[4], 3);
                cerr() << std::endl;
            }
        }
    case 0x58:
        if (raw.size() >= 3)
        {
            if (m_verbosity >= static_cast<int>(MESSAGE_ON))
            {
                cerr() << "All-Linked-Cleanup status ";
                bufferToStream(cerr(), &raw[2], 1);
                cerr() << std::endl;
            }
        }
        break;
    }
}

void PlmMonitor::writeCommand(std::shared_ptr<InsteonCommand>p)
{
    p->m_timesSent += 1;
    if (m_verbosity >= static_cast<int>(MESSAGE_ON))
    {
        timeStamp(cerr());
        cerr() << "Writing command " << std::dec << p->m_globalId << " ";
        bufferToStream(cerr(), &p->m_command[0], static_cast<int>(p->m_command.size()));
        cerr() << std::endl;
    }
    if (!m_io->Write(&p->m_command[0], static_cast<int>(p->m_command.size())))
    {   // fail now if failed to write
        {
            std::unique_lock<std::mutex> l(m_mutex);
            p->m_answerState = -5;
            m_condition.notify_all();
        }
        if (p->m_whenDone) p->m_whenDone(p.get());
        p.reset();
        if (m_verbosity >= static_cast<int>(MESSAGE_ON))
        {
            timeStamp(cerr());
            cerr() << "Write failed" << std::endl;
        }
    }
}

void PlmMonitor::startLinking(unsigned char group)
{
        unsigned char buf[4] =  {0x02, 0x64, 0x01};
        buf[3] = group;
        queueCommand(buf, sizeof(buf), 5);
}

int PlmMonitor::startLinkingR(unsigned char group)
{
    unsigned char buf[4] =  {0x02, 0x64, 0};
    buf[3] = group;
    queueCommand(buf, sizeof(buf), 5);
    return 1;
}

template <class T>
T *PlmMonitor::getDeviceAccess(const char *p)
{
    unsigned char addr[3];
    memset(addr, 0, sizeof(addr));
    int i;
    for (i = 0; i < 3; i++)
    {
        if (!p || !*p)
            break;
        while (*p && *p != '.')
        {
            addr[i] <<= 4;
            unsigned char c = toupper(*p);
            if ((c >= 'A') && (c <= 'F'))
                addr[i] += c - 'A' + 10;
            else if (isdigit(c))
                addr[i] += c - '0';
            p += 1;
        }
        if (*p == '.') p++;
    }
    if (i < 3)
        return 0;
    return getDeviceAccess<T>(addr);
}

// access by insteon address
template <class T>
T *PlmMonitor::getDeviceAccess(const unsigned char addr[3])
{
    std::unique_lock<std::mutex> l(m_mutex);
    InsteonDeviceSet_t::iterator itor = m_insteonDeviceSet.find(addr);
    if (itor != m_insteonDeviceSet.end())
    {
        T *current = dynamic_cast<T *>(itor->m_p.get());
        if (current)
            return current;
        else
        {
            m_retiredDevices.push_back(*itor); // can't just delete--C clients might still use
            m_insteonDeviceSet.erase(itor);
        }
    }
    std::shared_ptr<InsteonDevice> v(new T(this, addr));
    m_insteonDeviceSet.insert(InsteonDevicePtr(v));
    return dynamic_cast<T *>(v.get());
}

X10Dimmer *PlmMonitor::getX10DimmerAccess(char houseCode, unsigned char wheelCode)
{
    std::unique_lock<std::mutex> l(m_mutex);
	std::shared_ptr<X10Dimmer> dimmer(new X10Dimmer(this, houseCode, wheelCode));
	X10DimmerCollection::iterator itor = m_X10Dimmers->find(dimmer);
	if (itor == m_X10Dimmers->end())
		m_X10Dimmers->insert(dimmer);
	else
		dimmer = *itor;
	return dimmer.get();
}

int PlmMonitor::startLinking(int group, bool multiple)
{
    startLinking((unsigned char)group);
    {
        std::unique_lock<std::mutex> l(m_mutex);
        m_multipleLinkingRequested = multiple ? (unsigned char)group : 0;
    }
    return 0;
}

int PlmMonitor::cancelLinking()
{
    static unsigned char buf[] =  {0x02, 0x65};
    std::shared_ptr<InsteonCommand> p = sendCommandAndWait(buf, sizeof(buf), 3);
    return 0;
}

int PlmMonitor::getModemLinkRecords()
{
    int ret = 0;
    {
        std::unique_lock<std::mutex> l(m_mutex);
        m_haveAllModemLinks = false;
        m_ModemLinks.clear();
    }
    {   // request first all-link record
        static const unsigned char reqAllLink[] =
        {0x02, 0x69};
        std::shared_ptr<InsteonCommand> q = sendCommandAndWait(reqAllLink, sizeof(reqAllLink), 3, false);
        reportErrorState(reqAllLink, sizeof(reqAllLink), q);
        getNextLinkRecordCompleted(q.get());
        std::unique_lock<std::mutex> l(m_mutex);
        while (!m_haveAllModemLinks)
            m_condition.wait(l);
        ret = static_cast<int>(m_ModemLinks.size());
    }
    return ret;
}

void PlmMonitor::getNextLinkRecordCompleted(InsteonCommand *q)
{
    if (q->m_answer && (q->m_answer->size() > 2) && q->m_answer->at(2) == 0x6)
    {
        static const unsigned char reqNextLink[] =
        {0x02, 0x6A};
        queueCommand(reqNextLink, sizeof(reqNextLink), 3, false,
            std::bind(&PlmMonitor::getNextLinkRecordCompleted, this, std::placeholders::_1));
    }
    else
    {
        std::unique_lock<std::mutex> l(m_mutex);
        m_haveAllModemLinks = true;
        m_condition.notify_all();
    }
}

int PlmMonitor::clearModemLinkData()
{
    static unsigned char buf[] =  {0x02, 0x67};
    std::shared_ptr<InsteonCommand> p = sendCommandAndWait(buf, sizeof(buf), 3);
    std::unique_lock<std::mutex> l(m_mutex);
    m_haveAllModemLinks = true;
    m_ModemLinks.clear();
    return 0;
}

int PlmMonitor::nextUnusedControlGroup() const
{
    {
        std::unique_lock<std::mutex> l(m_mutex);
        if (!m_haveAllModemLinks)
            return -1;
    }
    int grpNum = 2;
    while (grpNum < 254)
    {
        bool found = false;
        for (unsigned i = 0; i < m_ModemLinks.size(); i++)
        {
            if ((m_ModemLinks[i].m_flag & 0x40) && (grpNum == m_ModemLinks[i].m_group))
            {
                found = true;
                grpNum += 1;
                break;
            }
        }
        if (!found) break;
    }
    if (grpNum < 254)
        return grpNum;
    return -2;
}

int PlmMonitor::createLink(InsteonDevice *who, bool amControl, unsigned char grp,
                           unsigned char flag, unsigned char ls1, unsigned char ls2, unsigned char ls3)
{
    unsigned char buf[] =  {0x02, 0x6F, amControl ? 0x40u : 0x41u, flag, grp,
        who->addr()[0], who->addr()[1], who->addr()[2], ls1, ls2, ls3};
    std::shared_ptr<InsteonCommand> p = sendCommandAndWait(buf, sizeof(buf), 12);
    return p->m_answerState;
}

int PlmMonitor::removeLink(InsteonDevice *who, bool amControl, unsigned char grp)
{
    const InsteonDeviceAddr &addr = who->addr();
    unsigned char buf[11] = {
        0x02, 0x6f, 0x80, amControl ? 0xe2u : 0, grp,
            addr[0], addr[1], addr[2],
            0, 0, 0};
     std::shared_ptr<InsteonCommand> p = sendCommandAndWait(buf, sizeof(buf), 12);
     return p->m_answerState;
}

int PlmMonitor::deleteGroupLinks(unsigned char group)
{
    // removes all links for which I am the controller of the group
    std::vector<InsteonDevice *> devicesInvolved;
    std::vector<InsteonLinkEntry> toDelete;
    {
        LinkTable_t LinkTableCopy;
        {
            std::unique_lock<std::mutex> l(m_mutex);
            if (!m_haveAllModemLinks)
                return -1;
            LinkTableCopy = m_ModemLinks;
        }
        for (LinkTable_t::iterator itor = LinkTableCopy.begin(); itor != LinkTableCopy.end(); itor++)
        {
            if (itor->m_flag & 0x40) // is controller
            {
                if (itor->m_group == group)
                {
                    devicesInvolved.push_back(getDeviceAccess<InsteonDevice>(itor->m_addr));
                    toDelete.push_back(*itor);
                }
            }
        }
    }

    InsteonDeviceSet_t insteonDeviceSet;
    {
        std::unique_lock<std::mutex> l(m_mutex);
        insteonDeviceSet = m_insteonDeviceSet;
    }

    for (
        InsteonDeviceSet_t::iterator itor =  insteonDeviceSet.begin();
        itor != insteonDeviceSet.end();
        itor ++)
        {
            if (itor->m_p->numberOfLinks(10) > 0)
            {
                // if we are not already planning to deal with this device...
                bool found = false;
                for (unsigned i = 0; i < devicesInvolved.size(); i++)
                {
                    if (devicesInvolved[i]->addr() == itor->m_p->addr())
                    {
                        found = true;
                        break;
                    }
                }
                if (!found)
                    devicesInvolved.push_back(itor->m_p.get());
            }
        }

    for (unsigned i = 0; i < devicesInvolved.size(); i++)
    {
        InsteonDevice *dev = devicesInvolved[i];
        dev->startGatherLinkTable();
        if (dev->numberOfLinks() < 0)
            return -2;
        int res = dev->removeLinks(m_IMInsteonId, group, false, 0);
        if (res < 0)
            return -3;
    }

    {
        std::unique_lock<std::mutex> l(m_mutex);
        m_haveAllModemLinks = false;
    }

    for (unsigned i = 0; i < toDelete.size(); i++)
    {
        InsteonLinkEntry &le = toDelete[i];
        unsigned char buf[11] = {
            0x02, 0x6f, 0x80, le.m_flag, le.m_group,
                le.m_addr[0], le.m_addr[1], le.m_addr[2],
                le.m_LinkSpecific1, le.m_LinkSpecific2, le.m_LinkSpecific3};

         std::shared_ptr<InsteonCommand> p = sendCommandAndWait(buf, sizeof(buf), 12);
        if (p->m_answerState < 0)
            return p->m_answerState;
    }

    return 0;
}

int PlmMonitor::getDeviceLinkGroup(InsteonDevice *d) const
{
    int group = -1;
    {
        std::unique_lock<std::mutex> l(m_mutex);
        if (!m_haveAllModemLinks)
            return -1;
        for (LinkTable_t::const_iterator itor = m_ModemLinks.begin(); itor != m_ModemLinks.end(); itor ++)
        {
            const InsteonLinkEntry &link = *itor;
            if (link.m_addr == d->addr())
            {
                if (link.m_flag & 0x40)    // I am controller
                {
                    if ((link.m_LinkSpecific1 == 0) &&
                        (link.m_LinkSpecific2 == 0) &&
                        (link.m_LinkSpecific3 == 1)) // Values we put here when we created this link
                    {
                        group = link.m_group;
                        break;
                    }
                }
            }
        }
    }
    return group;
}

int PlmMonitor::setAllDevices(unsigned char grp, unsigned char v)
{
    unsigned char buf[5] = {0x02, 0x61, grp, v, 0};
    queueCommand(buf, sizeof(buf), 6);
    return 0;
}

int PlmMonitor::setIfLinked(InsteonDevice *d, unsigned char v)
{
    int group = getDeviceLinkGroup(d);
    if (group > 0)
        return setAllDevices(static_cast<unsigned char>(group), v != 0 ? 0x11 : 0x13);
    return -1;
}

int PlmMonitor::setFastIfLinked(InsteonDevice *d, int v)
{
    int group = getDeviceLinkGroup(d);
    if (group > 0)
        return setAllDevices(static_cast<unsigned char>(group), v > 0 ? 0x12 : 0x14);
    return -1;
}

int PlmMonitor::sendX10Command(char houseCode, unsigned short unitMask, enum X10Commands_t command)
{
	// X10 protocol:
	// have to send a message to select each unit
	unsigned char buf[4] = {0x02, 0x63, 0, 0};
	unsigned char hc = InsteonDevice::X10HouseLetterToBits[(houseCode - 'A') & 0xF] << 4;;
	unsigned char mask = 1;
	for (int i = 0; i < 16; i++)
	{
		if (unitMask & mask)
		{
			buf[2] = (InsteonDevice::X10WheelCodeToBits[i+1] >> 1) | hc;
			queueCommand(buf, sizeof(buf), 5);
		}
		mask <<= 1;
	}
	buf[3] = 0x80;
	buf[2] = hc | static_cast<unsigned char>(command);
	queueCommand(buf, sizeof(buf), 5);
	return 1;
}

const char * PlmMonitor::printModemLinkTable()
{
    std::unique_lock<std::mutex> l(m_mutex);
    if (!m_haveAllModemLinks)
    {
        cerr() << "Modem links not retrieved yet" << std::endl;
        return 0;
    }
    std::ostringstream linkTablePrinted;
    linkTablePrinted << "Modem links" << std::endl << "flag group   ID       ls1 ls2 ls3" << std::endl;
    for (LinkTable_t::const_iterator itor = m_ModemLinks.begin();
            itor != m_ModemLinks.end();
            itor++)
    {
        itor->print(linkTablePrinted);
        linkTablePrinted << std::endl;
    }
    m_linkTablePrinted = linkTablePrinted.str();
    return m_linkTablePrinted.c_str();
}

int PlmMonitor::monitor(int waitSeconds, InsteonDevice *&dimmer,
            unsigned char &group, unsigned char &cmd1, unsigned char &cmd2,
            unsigned char &ls1, unsigned char &ls2, unsigned char &ls3)
{
    std::unique_lock<std::mutex> l(m_mutex);
    if (waitSeconds < 0)
        m_queueNotifications = true;
    m_ThreadRunning += 1;
    while (!m_shutdown && m_queueNotifications && m_notifications.empty())
    {
        if (waitSeconds < 0)
            m_condition.wait(l);
        else if (waitSeconds > 0)
        {
            m_condition.wait_for(l, std::chrono::seconds(waitSeconds));
            break;
        }
        else
            break;
    }
    m_ThreadRunning -= 1;
    if (m_notifications.empty())
        return 0;
    NotificationEntry &ne = *m_notifications.front().get();
    dimmer = ne.m_device.get();
    group = ne.group;
    cmd1 = ne.cmd1;
    cmd2 = ne.cmd2;
    ls1 = ne.ls1;
    ls2 = ne.ls2;
    ls3 = ne.ls3;
    m_notifications.pop_front();
    return 1;
}

void PlmMonitor::queueNotifications(bool v)
{
    {
        std::unique_lock<std::mutex> l(m_mutex);
        m_queueNotifications = v;
        if (!v)
        {
            m_notifications.clear();
            m_priorNotifications.clear();
        }
    }
    m_condition.notify_all();
}

bool PlmMonitor::NotificationEntry::operator == (const NotificationEntry &other) const
{
    if (*m_device.get()->addr() != *other.m_device.get()->addr())
        return false;
    if (group != other.group)
        return false;
    if (other.cmd1 != cmd1)
        return false;
    if (other.cmd2 != cmd2)
        return false;
    return true;
}

int PlmMonitor::setCommandDelay(int delayMsec)
{
    if ((delayMsec >= 10) && (delayMsec <= 10000))
        m_commandDelayMsec = delayMsec;
    return (int) m_commandDelayMsec;
}

InsteonDevicePtr::InsteonDevicePtr(const unsigned char addr[3]): m_p(new InsteonDevice(0, addr)){}

bool InsteonDevicePtr::operator < (const InsteonDevicePtr &other) const {return *m_p < *other.m_p;}

// force a couple of template instantiations. These are currently used above, but show how to make more, if needed
template Dimmer *PlmMonitor::getDeviceAccess<Dimmer>(const char *);
template Keypad *PlmMonitor::getDeviceAccess<Keypad>(const char *);
template Fanlinc *PlmMonitor::getDeviceAccess<Fanlinc>(const char *);
template InsteonDevice *PlmMonitor::getDeviceAccess<InsteonDevice>(const char *);

}

ext: cpp


text:
// Copyright (c) 2021 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ScheduleHostOpTest

#include <../random_util.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/call.hpp>
#include <popart/op/dynamic/dynamicslice.hpp>
#include <popart/op/exchange/remote.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/init.hpp>
#include <popart/op/l1.hpp>
#include <popart/opmanager.hpp>
#include <popart/popx/devicex.hpp>
#include <popart/region.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

#include <algorithm>
#include <map>
#include <tuple>
#include <vector>

using namespace popart;

template <class T> std::vector<T *> getOpsOfType(Ir &ir) {
  std::vector<T *> ops;
  for (auto &id_op : ir.getMainGraphOps()) {
    auto op = id_op.second.get();
    if (op->isConvertibleTo<T>()) {
      ops.push_back(dynamic_cast<T *>(op));
    }
  }
  return ops;
}

void createAndRun(bool hostIO = false) {

  // Try to create a simple host copy
  auto builder = Builder::create();
  auto aiOnnx  = builder->aiOnnxOpset9();

  int bps   = 3;
  int steps = 10;

  // we will generate random initializations
  int seed = 1337;
  DefaultRandomEngine eng(seed);
  UniformRealDistribution<float> fdis(-1.f, 1.f);

  TensorInfo tinfo{"FLOAT", std::vector<int64_t>{2, 2}};
  std::vector<float> input(tinfo.nelms() * bps * steps);
  for (auto &val : input) {
    val = fdis(eng);
  }
  TensorInfo linfo{"INT32", std::vector<int64_t>{2}};

  std::vector<float> w_init(tinfo.nelms());
  for (auto &val : w_init) {
    val = fdis(eng);
  }

  auto i = builder->addInputTensor(tinfo, "A_input");
  auto l = builder->addInputTensor(linfo, "label");
  auto c = builder->addInitializedInputTensor({w_init.data(), tinfo}, "weight");

  auto a0 = aiOnnx.matmul({i, c});

  auto sm  = aiOnnx.softmax({a0}, 1);
  auto nll = builder->aiGraphcoreOpset1().nllloss({sm, l});

  auto out_id = nll;

  builder->addOutputTensor(out_id);

  SessionOptions opts;

  opts.useHostCopyOps = hostIO;

  auto proto      = builder->getModelProto();
  auto modelProto = io::getModelFromString(proto);
  auto art        = AnchorReturnType("All");
  auto dataFlow   = DataFlow(bps, {{out_id, art}, {i, art}});
  auto device     = createTestDevice(TEST_TARGET);

  // inputs:
  popart::NDArrayWrapper<float> A_wrapper(
      input.data(), TensorInfo("FLOAT", std::vector<int64_t>{bps, 2, 2}));

  auto carriedWrapper = A_wrapper;

  // prepare the anchors
  int32_t rawLabel[bps * 2];
  int32_t j = 1;
  for (auto &val : rawLabel) {
    val = j;
    j += 1;
  }
  popart::NDArrayWrapper<int32_t> lWrapper(rawLabel, {bps, 2});

  std::map<popart::TensorId, popart::IArray &> inputs = {{i, A_wrapper},
                                                         {l, lWrapper}};

  std::vector<float> raw_out(bps);
  popart::NDArrayWrapper<float> B_wrapper(raw_out.data(),
                                          std::vector<int64_t>{bps});

  std::map<popart::TensorId, popart::IArray &> anchors = {{out_id, B_wrapper},
                                                          {i, A_wrapper}};
  auto optimizer = popart::ConstSGD(0.01f);

  if (device != nullptr) {
    auto session = popart::TrainingSession::createFromOnnxModel(
        proto,
        dataFlow,
        out_id,
        optimizer,
        device,
        popart::InputShapeInfo(),
        opts,
        popart::Patterns(PatternsLevel::Default));
    session->prepareDevice();
    popart::StepIO stepio(inputs, anchors);

    std::vector<Op *> schedule =
        session->getIr().getOpSchedule({}, RequireOptimalSchedule::Yes);
    int initOpscount = 0;
    int hlOpscount   = 0;

    std::string schedule_str = "";
    for (auto op : schedule) {
      std::cout << op->debugName() << std::endl;
      if (op->opid == Onnx::CustomOperators::Init_1) {
        initOpscount += 1;
        schedule_str += "i";
      } else if (op->opid == Onnx::CustomOperators::HostLoad) {
        hlOpscount += 1;
        schedule_str += "l";
      } else {
        schedule_str += "_";
      }
    }
    if (hostIO) {
      BOOST_CHECK(hlOpscount == 2);
      BOOST_CHECK(initOpscount == 2);

      std::cout << schedule_str << std::endl;
      // Find occurences of il in schedule_str
      int count = 0;
      auto pos  = schedule_str.find("il", 0);
      while (pos != std::string::npos) {
        count++;
        pos = schedule_str.find("il", pos + 2);
      }
      BOOST_CHECK(count == 2);
    } else {
      BOOST_CHECK(hlOpscount == 0);
      BOOST_CHECK(initOpscount == 0);
    }
  }
}

BOOST_AUTO_TEST_CASE(ScheduleHostOpTest0) { createAndRun(true); }
// BOOST_AUTO_TEST_CASE(ScheduleHostOpTest1) { createAndRun(false); }


text:
// Copyright (c) Steinwurf ApS 2017.
// All Rights Reserved
//
// Distributed under the "BSD License". See the accompanying LICENSE.rst file.

#include <cassert>
#include <cstdint>
#include <ctime>
#include <memory>
#include <system_error>

#include <gauge/gauge.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
#include <mts/parser.hpp>

class parsing_benchmark : public gauge::time_benchmark
{
public:

    double measurement() override
    {
        // Get the time spent per iteration
        double time = gauge::time_benchmark::measurement();

        gauge::config_set cs = get_current_configuration();
        auto size = cs.get_value<uint32_t>("size");

        return size / time; // MB/s for each iteration
    }

    std::string unit_text() const override
    {
        return "MB/s";
    }

    void store_run(tables::table& results) override
    {
        if (!results.has_column("throughput"))
            results.add_column("throughput");

        results.set_value("throughput", measurement());
    }

    void get_options(gauge::po::variables_map& options) override
    {
        auto filename = options["filename"].as<std::string>();
        gauge::config_set cs;
        cs.set_value<std::string>("filename", filename);
        boost::iostreams::mapped_file_source file;
        file.open(filename);
        assert(file.is_open());
        cs.set_value<uint32_t>("size", file.size());
        file.close();

        add_configuration(cs);
    }

    void setup() override
    {
        if (m_buffer.empty())
        {
            gauge::config_set cs = get_current_configuration();
            auto filename = cs.get_value<std::string>("filename");
            boost::iostreams::mapped_file_source file;
            file.open(filename);
            assert(file.is_open());
            m_buffer.insert(m_buffer.begin(), file.data(), file.data() + file.size());
            file.close();
        }
    }

    void test_body() override
    {
        RUN
        {
            mts::parser parser;
            uint64_t offset = 0;
            const auto packets = m_buffer.size() / mts::parser::packet_size();
            for (uint32_t i = 0; i < packets; ++i)
            {
                std::error_code error;
                parser.read((uint8_t*)m_buffer.data() + offset, error);
                offset += mts::parser::packet_size();
                if (parser.has_pes())
                {
                    auto pid = parser.pes_pid();
                    mts::stream_type type = (mts::stream_type)parser.stream_type(pid);
                    if (type != mts::stream_type::avc_video_stream)
                        continue;

                    auto& pes_data = parser.pes_data();
                    std::error_code error;
                    auto pes = mts::pes::parse(pes_data.data(), pes_data.size(), error);
                    if (error)
                        continue;

                    assert(pes->payload_data() != nullptr);
                    assert(pes->payload_size() != 0U);
                }
            }
        }
    }

private:

    std::vector<uint8_t> m_buffer;
};

BENCHMARK_F(parsing_benchmark, parsing, h264, 5);

/// Using this macro we may specify options. For specifying options
/// we use the boost program options library. So you may additional
/// details on how to do it in the manual for that library.
BENCHMARK_OPTION(arithmetic_options)
{
    gauge::po::options_description options;

    options.add_options()
    ("filename", gauge::po::value<std::string>()->default_value("test.ts"),
     "Set the file name");

    gauge::runner::instance().register_options(options);
}

int main(int argc, const char* argv[])
{
    srand(static_cast<uint32_t>(time(0)));

    gauge::runner::add_default_printers();
    gauge::runner::run_benchmarks(argc, argv);

    return 0;
}

max_stars_repo_name: steinwurf/mts

max_issues_repo_name: steinwurf/mts

max_forks_repo_name: steinwurf/mts


text:
#include <actions/actions.h>
#include <actions/app.h>
#include <dependency_graph/graph.h>
#include <dependency_graph/metadata_register.h>

#include <boost/test/unit_test.hpp>
#include <dependency_graph/nodes.inl>
#include <dependency_graph/port.inl>

#include "common.h"

using namespace dependency_graph;

namespace {

dependency_graph::NodeBase& findNode(dependency_graph::Network& net, const std::string& name) {
	for(auto& n : net.nodes())
		if(n.name() == name)
			return n;

	BOOST_REQUIRE(false && "Node not found, fail");
	throw;
}

}  // namespace

BOOST_AUTO_TEST_CASE(nested) {
	possumwood::AppCore app;

	const MetadataHandle& addition = additionNode();
	const MetadataHandle& multiplication = multiplicationNode();

	auto networkFactoryIterator = MetadataRegister::singleton().find("network");
	BOOST_REQUIRE(networkFactoryIterator != MetadataRegister::singleton().end());

	auto inputFactoryIterator = MetadataRegister::singleton().find("input");
	BOOST_REQUIRE(inputFactoryIterator != MetadataRegister::singleton().end());

	auto outputFactoryIterator = MetadataRegister::singleton().find("output");
	BOOST_REQUIRE(outputFactoryIterator != MetadataRegister::singleton().end());

	////////////////////////////
	// build a simple graph for ((a + b) * c) + ((a + b) * d)
	// with each bracketed expression in a sub-network

	// (((a + b) * c) + ((a + b) * d))
	// ││└net4─┘    │   │└net5─┘    ││
	// │└── net2 ───┘   └─── net3 ──┘│
	// └─────────── net1 ────────────┘

	BOOST_REQUIRE_NO_THROW(
	    possumwood::actions::createNode(app.graph(), *networkFactoryIterator, "net1", possumwood::NodeData()));
	Network& net1 = dynamic_cast<Network&>(findNode(app.graph(), "net1"));
	BOOST_REQUIRE_NO_THROW(
	    possumwood::actions::createNode(net1, *networkFactoryIterator, "net2", possumwood::NodeData()));
	Network& net2 = dynamic_cast<Network&>(findNode(net1, "net2"));
	BOOST_REQUIRE_NO_THROW(
	    possumwood::actions::createNode(net1, *networkFactoryIterator, "net3", possumwood::NodeData()));
	Network& net3 = dynamic_cast<Network&>(findNode(net1, "net3"));
	BOOST_REQUIRE_NO_THROW(
	    possumwood::actions::createNode(net2, *networkFactoryIterator, "net4", possumwood::NodeData()));
	Network& net4 = dynamic_cast<Network&>(findNode(net2, "net4"));
	BOOST_REQUIRE_NO_THROW(
	    possumwood::actions::createNode(net3, *networkFactoryIterator, "net5", possumwood::NodeData()));
	Network& net5 = dynamic_cast<Network&>(findNode(net3, "net5"));

	{
		BOOST_REQUIRE_NO_THROW(possumwood::actions::createNode(net5, addition, "add5", possumwood::NodeData()));
		NodeBase& net5_add5 = findNode(net5, "add5");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net5, *inputFactoryIterator, "net5_in1", possumwood::NodeData()));
		NodeBase& net5_in1 = findNode(net5, "net5_in1");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net5, *inputFactoryIterator, "net5_in2", possumwood::NodeData()));
		NodeBase& net5_in2 = findNode(net5, "net5_in2");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net5, *outputFactoryIterator, "net5_out", possumwood::NodeData()));
		NodeBase& net5_out = findNode(net5, "net5_out");

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net5_in1.port(0), net5_add5.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net5_in2.port(0), net5_add5.port(1)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net5_add5.port(2), net5_out.port(0)));
	}

	{
		BOOST_REQUIRE_NO_THROW(possumwood::actions::createNode(net4, addition, "add4", possumwood::NodeData()));
		NodeBase& add4 = findNode(net4, "add4");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net4, *inputFactoryIterator, "net4_in1", possumwood::NodeData()));
		NodeBase& in1 = findNode(net4, "net4_in1");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net4, *inputFactoryIterator, "net4_in2", possumwood::NodeData()));
		NodeBase& in2 = findNode(net4, "net4_in2");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net4, *outputFactoryIterator, "net4_out", possumwood::NodeData()));
		NodeBase& out = findNode(net4, "net4_out");

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in1.port(0), add4.port(0)));
		BOOST_CHECK(net4.port(0).isLinked());

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in2.port(0), add4.port(1)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(add4.port(2), out.port(0)));
		BOOST_CHECK(net4.port(0).isLinked());
	}

	{
		BOOST_REQUIRE_NO_THROW(possumwood::actions::createNode(net3, multiplication, "mult3", possumwood::NodeData()));
		NodeBase& net3_mult3 = findNode(net3, "mult3");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net3, *inputFactoryIterator, "net3_in1", possumwood::NodeData()));
		NodeBase& net3_in1 = findNode(net3, "net3_in1");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net3, *inputFactoryIterator, "net3_in2", possumwood::NodeData()));
		NodeBase& net3_in2 = findNode(net3, "net3_in2");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net3, *inputFactoryIterator, "net3_in3", possumwood::NodeData()));
		NodeBase& net3_in3 = findNode(net3, "net3_in3");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net3, *outputFactoryIterator, "net3_out", possumwood::NodeData()));
		NodeBase& net3_out = findNode(net3, "net3_out");

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net3_in1.port(0), net5.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net3_in2.port(0), net5.port(1)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net3_in3.port(0), net3_mult3.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net5.port(2), net3_mult3.port(1)));

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net3_mult3.port(2), net3_out.port(0)));
	}

	{
		BOOST_REQUIRE_NO_THROW(possumwood::actions::createNode(net2, multiplication, "mult2", possumwood::NodeData()));
		NodeBase& mult2 = findNode(net2, "mult2");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net2, *inputFactoryIterator, "net2_in1", possumwood::NodeData()));
		NodeBase& net2_in1 = findNode(net2, "net2_in1");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net2, *inputFactoryIterator, "net2_in2", possumwood::NodeData()));
		NodeBase& net2_in2 = findNode(net2, "net2_in2");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net2, *inputFactoryIterator, "net2_in3", possumwood::NodeData()));
		NodeBase& net2_in3 = findNode(net2, "net2_in3");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net2, *outputFactoryIterator, "net2_out", possumwood::NodeData()));
		NodeBase& net2_out = findNode(net2, "net2_out");

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net2_in1.port(0), net4.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net2_in2.port(0), net4.port(1)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net2_in3.port(0), mult2.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net4.port(2), mult2.port(1)));

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(mult2.port(2), net2_out.port(0)));
	}

	{
		BOOST_REQUIRE_NO_THROW(possumwood::actions::createNode(net1, addition, "add1", possumwood::NodeData()));
		NodeBase& add1 = findNode(net1, "add1");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net1, *inputFactoryIterator, "net1_in1", possumwood::NodeData()));
		NodeBase& in1 = findNode(net1, "net1_in1");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net1, *inputFactoryIterator, "net1_in2", possumwood::NodeData()));
		NodeBase& in2 = findNode(net1, "net1_in2");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net1, *inputFactoryIterator, "net1_in3", possumwood::NodeData()));
		NodeBase& in3 = findNode(net1, "net1_in3");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net1, *inputFactoryIterator, "net1_in4", possumwood::NodeData()));
		NodeBase& in4 = findNode(net1, "net1_in4");
		BOOST_REQUIRE_NO_THROW(
		    possumwood::actions::createNode(net1, *outputFactoryIterator, "net1_out", possumwood::NodeData()));
		NodeBase& out = findNode(net1, "net1_out");

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in1.port(0), net2.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in1.port(0), net3.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in2.port(0), net2.port(1)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in2.port(0), net3.port(1)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in3.port(0), net2.port(2)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(in4.port(0), net3.port(2)));

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net2.port(3), add1.port(0)));
		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(net3.port(3), add1.port(1)));

		BOOST_REQUIRE_NO_THROW(possumwood::actions::connect(add1.port(2), out.port(0)));
	}

	// just after creation, the output should be 0
	BOOST_CHECK_EQUAL(net1.port(4).get<float>(), 0.0f);

	// setting a to 1 should give the value of net4 and net5 output to 1
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(0), 1.0f));

	BOOST_CHECK_EQUAL(net1.port(0).get<float>(), 1.0f);
	BOOST_CHECK(net1.port(0).isLinked());

	BOOST_CHECK_EQUAL(net2.port(0).get<float>(), 1.0f);
	BOOST_CHECK(net2.port(0).isLinked());

	BOOST_CHECK_EQUAL(net4.port(0).get<float>(), 1.0f);
	BOOST_CHECK(net4.port(0).isLinked());

	BOOST_CHECK_EQUAL(net4.port(0).get<float>(), 1.0f);
	BOOST_CHECK_EQUAL(net5.port(0).get<float>(), 1.0f);

	BOOST_CHECK_EQUAL(net1.port(4).get<float>(), 0.0f);

	// setting A to 1 and C to 1 should lead to 1.0f
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(0), 1.0f));
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(2), 1.0f));

	BOOST_CHECK_EQUAL(net1.port(4).get<float>(), 1.0f);

	// setting all to 1 should lead to 4.0f
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(0), 1.0f));
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(1), 1.0f));
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(2), 1.0f));
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(3), 1.0f));

	BOOST_CHECK_EQUAL(net1.port(4).get<float>(), 4.0f);

	// setting the inputs to 1,2,3,4 should lead to 21
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(0), 1.0f));
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(1), 2.0f));
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(2), 3.0f));
	BOOST_REQUIRE_NO_THROW(possumwood::actions::setValue(net1.port(3), 4.0f));

	BOOST_CHECK_EQUAL(net1.port(4).get<float>(), 21.0f);
}

max_stars_repo_name: martin-pr/possumwood

max_issues_repo_name: LIUJUN-liujun/possumwood

max_forks_repo_name: LIUJUN-liujun/possumwood


text:
#include "io/ssl.hh"
#include <boost/bind.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

void io::Service::Run() {
  loop->run();
}

io::ssl::context& io::Service::getContext() {
  return *(ssl_ctx.get());
}

io::asio::io_service& io::Service::getService() {
  return *(loop.get());
}

io::Service::Service() {
  loop = std::make_shared<io::asio::io_service>();
  resolver = std::make_shared<io::tcp::resolver>(getService());
  ssl_ctx = std::make_shared<io::ssl::context>(io::ssl::context::sslv23);
}

void io::Service::Resolve(const std::string &host, int port,
  std::function<void(const io::error_code&, io::tcp::resolver::iterator)> cb)
{
  io::tcp::resolver::query query(host.c_str(), std::to_string(port).c_str());
  resolver->async_resolve(query, cb);
}

io::Timer* io::Service::createTimer() {
  io::Timer *timer = new io::Timer(getService());
  return timer;
}

io::Timer* io::Service::spawn
(const long delay, void *data, const std::function<void(Timer*)> &callback)
{
  io::Timer *timer = new io::Timer(getService());
  timer->data = data;
  timer->async(delay, std::move(callback));
  return timer;
}

////////////////////////////////////////////////////////////////////////

io::Timer::Timer(io::asio::io_service &service) : timer(service), data(nullptr) {
}

void io::Timer::cancel() {
  timer.cancel();
}

void io::Timer::async(const long ms, const std::function<void(Timer*)> &callback) {
  timer_cb = std::move(callback);
  timer.expires_from_now(boost::posix_time::millisec(ms));
  timer.async_wait([this](const io::error_code& err){
    if (!err) timer_cb(this);
  });
}

////////////////////////////////////////////////////////////////////////

io::SSLClient::SSLClient(io::Service& loop) : service(loop) {
  sock = std::make_shared<io::ssl::stream<io::tcp::socket>>(
    service.getService(), service.getContext());
  sock->set_verify_mode(io::ssl::verify_none);

  on_close = [](const io::error_code& err){};
  on_connect = [](const io::error_code& err){};
  on_read = [](const std::vector<char>& data){};
}

const bool io::SSLClient::isConnected() const {
  return connected;
}

void io::SSLClient::onClose(std::function<void(const io::error_code&)> cb) {
  on_close = cb;
}

void io::SSLClient::onConnect(std::function<void(const io::error_code&)> cb) {
  on_connect = cb;
}

void io::SSLClient::onRead(std::function<void(const std::vector<char>&)> cb) {
  on_read = cb;
}

void io::SSLClient::Send(const char* data, const std::size_t len) {
  if (!connected) return;
  io::asio::async_write(*(sock.get()), io::asio::buffer(data, len),
    [this](const io::error_code& e, std::size_t written) { if (e) Close(e); });
}

void io::SSLClient::_connect(const io::tcp::endpoint& endpoint,
  io::tcp::resolver::iterator& it, std::function<void(const io::error_code&)> callback)
{
  sock->lowest_layer().async_connect(endpoint, boost::bind(
    &io::SSLClient::_connect_handler, this,
    io::asio::placeholders::error, ++it, callback));
}

void io::SSLClient::Close(const io::error_code& err, bool callback) {
  if (!connected) return;
  io::error_code ec;
  connected = false;
  sock->lowest_layer().cancel(ec);
  sock->async_shutdown([&](const io::error_code &error) {
    io::error_code e;
    sock->lowest_layer().close(e);
    if (callback)
      on_close(e ? e : (err ? err : ec));
  });
}

void io::SSLClient::_connect_handler(const io::error_code& err,
  io::tcp::resolver::iterator it, std::function<void(const io::error_code&)> callback)
{
  if (!err) {
    callback(io::Success);
  } else if (it != io::tcp::resolver::iterator()) {
    Close(io::Success, false);
    _connect(*it, ++it, callback);
  } else {
    callback(err);
  }
}

void io::SSLClient::_read_handler(const io::error_code& err, std::size_t size) {
  if (err) {
    Close(err);
  } else {
    builder.insert(builder.end(), buffer.data(), buffer.data() + size);
    if (size < buffer.max_size()) {
      on_read(builder);
      builder.clear();
    }
    sock->async_read_some(io::asio::buffer(buffer),
      boost::bind(&io::SSLClient::_read_handler, this,
        io::asio::placeholders::error,
        io::asio::placeholders::bytes_transferred));
  }
}

void io::SSLClient::Connect(const std::string& host, int port) {
  if (connected) return;
  service.Resolve(host, port,
  [&](const io::error_code& err, io::tcp::resolver::iterator it) {
    if (err) {
      on_connect(err);
      return;
    }
    _connect(*it, it, [this](const io::error_code &err) {
      sock->async_handshake(io::ssl::stream_base::client,
      [this](const io::error_code &err) {
        connected = true;
        on_connect(err);
        sock->async_read_some(io::asio::buffer(buffer),
          boost::bind(&io::SSLClient::_read_handler, this,
            io::asio::placeholders::error,
            io::asio::placeholders::bytes_transferred));
      });
    });
  });
}

ext: cc


text:
#include <fstream>
#include <filesystem>

#include <Eigen/Geometry>
#include <utility>

#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/norms.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>

#include "analysis.h"
#include "filter.h"

#define N_BINS 4

namespace fs = std::filesystem;

std::pair<float, float> calculate_rotation_and_translation_errors(const Eigen::Matrix4f &transformation,
                                                                  const Eigen::Matrix4f &transformation_gt) {
    Eigen::Matrix3f rotation_diff = transformation.block<3, 3>(0, 0).inverse() * transformation_gt.block<3, 3>(0, 0);
    Eigen::Vector3f translation_diff = transformation.block<3, 1>(0, 3) - transformation_gt.block<3, 1>(0, 3);
    float rotation_error = Eigen::AngleAxisf(rotation_diff).angle();
    float translation_error = translation_diff.norm();
    return {rotation_error, translation_error};
}

inline float dist2(const PointN &p1, const PointN &p2) {
    return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z);
};

float calculate_point_cloud_rmse(const PointNCloud::ConstPtr &pcd,
                                 const Eigen::Matrix4f &transformation,
                                 const Eigen::Matrix4f &transformation_gt) {
    PointNCloud pcd_transformed;
    Eigen::Matrix4f transformation_diff = transformation.inverse() * transformation_gt;
    pcl::transformPointCloud(*pcd, pcd_transformed, transformation_diff);

    float rmse = 0.f;
    for (int i = 0; i < pcd->size(); ++i) {
        rmse += dist2(pcd->points[i], pcd_transformed.points[i]);
    }
    rmse = std::sqrt(rmse / (float) pcd->size());
    return rmse;
}

float calculate_overlap_rmse(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                             const Eigen::Matrix4f &transformation,
                             const Eigen::Matrix4f &transformation_gt,
                             float inlier_threshold) {
    PointNCloud src_aligned, src_aligned_gt;
    pcl::transformPointCloud(*src, src_aligned, transformation);
    pcl::transformPointCloud(*src, src_aligned_gt, transformation_gt);

    pcl::KdTreeFLANN<PointN> tree_tgt;
    tree_tgt.setInputCloud(tgt);

    int overlap_size = 0;
    float rmse = 0.f;
    pcl::Indices nn_indices;
    std::vector<float> nn_dists;
    for (int i = 0; i < src->size(); ++i) {
        tree_tgt.nearestKSearch(src_aligned_gt[i], 1, nn_indices, nn_dists);
        if (nn_dists[0] < inlier_threshold * inlier_threshold) {    // ith point in overlap
            rmse += dist2(src_aligned.points[i], src_aligned_gt.points[i]);
            overlap_size++;
        }
    }
    if (overlap_size != 0) {
        rmse = std::sqrt(rmse / (float)overlap_size);
    } else {
        rmse = std::numeric_limits<float>::quiet_NaN();
    }
    return rmse;

}

float calculate_correspondence_uniformity(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                          const pcl::Correspondences &correct_correspondences,
                                          const AlignmentParameters &parameters,
                                          const Eigen::Matrix4f &transformation_gt) {
    PointNCloud::Ptr src_aligned(new PointNCloud);
    pcl::transformPointCloud(*src, *src_aligned, transformation_gt);
    pcl::KdTreeFLANN<PointN>::Ptr tree(new pcl::KdTreeFLANN<PointN>());
    tree->setInputCloud(tgt);

    pcl::Indices indices;
    std::vector<float> distances;
    // calculate bbox of overlapping area
    PointN min_point, max_point;
    float error_thr = parameters.distance_thr_coef * parameters.voxel_size;
    for (int i = 0; i < src_aligned->size(); ++i) {
        (float) tree->nearestKSearch(*src_aligned, i, 1, indices, distances);
        const PointN &p_src(src_aligned->points[i]), p_tgt(tgt->points[indices[0]]);
        if (std::sqrt(distances[0]) < error_thr && std::isfinite(p_src.normal_x) && std::isfinite(p_tgt.normal_x)) {
            min_point.x = std::min(min_point.x, p_src.x);
            min_point.y = std::min(min_point.y, p_src.y);
            min_point.z = std::min(min_point.z, p_src.z);
            max_point.x = std::max(max_point.x, p_src.x);
            max_point.y = std::max(max_point.y, p_src.y);
            max_point.z = std::max(max_point.z, p_src.z);
        }
    }
    int count[3][N_BINS][N_BINS]{0};

    for (auto const &corr: correct_correspondences) {
        const auto &point = src_aligned->points[corr.index_query];
        if (pointInBoundingBox(point, min_point, max_point)) {
            int bin[3];
            bin[0] = std::floor((point.x - min_point.x) / (max_point.x - min_point.x) * N_BINS);
            bin[1] = std::floor((point.y - min_point.y) / (max_point.y - min_point.y) * N_BINS);
            bin[2] = std::floor((point.z - min_point.z) / (max_point.z - min_point.z) * N_BINS);
            // count 3D points projected to YZ, ZX, XY and fallen in 2D bin
            for (int k = 0; k < 3; ++k) {
                count[k][bin[(k + 1) % 3]][bin[(k + 2) % 3]]++;
            }
        }
    }
    float entropy[3]{0.f};
    float n = correct_correspondences.size();
    for (int k = 0; k < 3; ++k) {
        for (int i = 0; i < N_BINS; ++i) {
            for (int j = 0; j < N_BINS; ++j) {
                float p = (float) count[k][i][j] / n;
                if (p == 0.f) continue;
                entropy[k] -= p * std::log(p);
            }
        }
        entropy[k] /= std::log((float) (N_BINS * N_BINS));
    }
    return std::cbrt(entropy[0] * entropy[1] * entropy[2]);
}

float calculate_normal_difference(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                  const AlignmentParameters &parameters, const Eigen::Matrix4f &transformation_gt) {
    PointNCloud::Ptr src_aligned(new PointNCloud);
    pcl::transformPointCloud(*src, *src_aligned, transformation_gt);
    pcl::KdTreeFLANN<PointN>::Ptr tree(new pcl::KdTreeFLANN<PointN>());
    tree->setInputCloud(tgt);

    pcl::Indices indices;
    std::vector<float> distances;
    float error_thr = parameters.distance_thr_coef * parameters.voxel_size;
    float difference = 0.f;
    int n_points_overlap = 0;
    for (int i = 0; i < src_aligned->size(); ++i) {
        (float) tree->nearestKSearch(*src_aligned, i, 1, indices, distances);
        const PointN &p_src(src_aligned->points[i]), p_tgt(tgt->points[indices[0]]);
        if (std::sqrt(distances[0]) < error_thr && std::isfinite(p_src.normal_x) && std::isfinite(p_tgt.normal_x)) {
            float cos = std::clamp(p_src.normal_x * p_tgt.normal_x + p_src.normal_y * p_tgt.normal_y +
                                   p_src.normal_z * p_tgt.normal_z, -1.f, 1.f);
            difference += std::abs(std::acos(cos));
            n_points_overlap++;
        }
    }
    return difference / (float) n_points_overlap;
}

void buildCorrectCorrespondences(const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                 const pcl::Correspondences &correspondences,
                                 pcl::Correspondences &correct_correspondences,
                                 const Eigen::Matrix4f &transformation_gt, float error_threshold) {
    correct_correspondences.clear();
    correct_correspondences.reserve(correspondences.size());

    PointNCloud input_transformed;
    input_transformed.resize(src->size());
    pcl::transformPointCloud(*src, input_transformed, transformation_gt);

    for (const auto &correspondence: correspondences) {
        PointN source_point(input_transformed.points[correspondence.index_query]);
        PointN target_point(tgt->points[correspondence.index_match]);
        float e = pcl::L2_Norm(source_point.data, target_point.data, 3);
        if (e < error_threshold) {
            correct_correspondences.push_back(correspondence);
        }
    }
}

AlignmentAnalysis::AlignmentAnalysis(const AlignmentParameters &parameters,
                                     const PointNCloud::ConstPtr &src, const PointNCloud::ConstPtr &tgt,
                                     const pcl::Correspondences &correspondences,
                                     int iterations, const Eigen::Matrix4f &transformation, double time) {
    parameters_ = parameters;
    src_ = src;
    tgt_ = tgt;
    correspondences_ = correspondences;
    iterations_ = iterations;
    transformation_ = transformation;
    time_ = time;
    has_converged_ = true;
    metric_estimator_ = getMetricEstimator(parameters);
    metric_estimator_->setSourceCloud(src);
    metric_estimator_->setTargetCloud(tgt);
    metric_estimator_->setCorrespondences(correspondences);
    metric_estimator_->setInlierThreshold(parameters.distance_thr_coef * parameters.voxel_size);
}

void AlignmentAnalysis::start(const Eigen::Matrix4f &transformation_gt, const std::string &testname) {
    testname_ = testname;
    float error_thr = parameters_.distance_thr_coef * parameters_.voxel_size;
    transformation_gt_ = transformation_gt;

    buildCorrectCorrespondences(src_, tgt_, correspondences_, correct_correspondences_, transformation_gt_, error_thr);
    metric_estimator_->buildInlierPairsAndEstimateMetric(transformation_, inlier_pairs_, rmse_, fitness_);
    metric_estimator_->buildCorrectInlierPairs(inlier_pairs_, correct_inlier_pairs_, transformation_gt_);
    pcd_error_ = calculate_point_cloud_rmse(src_, transformation_, transformation_gt_);
    overlap_error_ = calculate_overlap_rmse(src_, tgt_, transformation_, transformation_gt_, error_thr);
    normal_diff_ = calculate_normal_difference(src_, tgt_, parameters_, transformation_gt_);
    corr_uniformity_ = calculate_correspondence_uniformity(src_, tgt_, correct_correspondences_,
                                                           parameters_, transformation_gt_);
    std::tie(r_error_, t_error_) = calculate_rotation_and_translation_errors(transformation_, transformation_gt_);

    print();
    save(testname);
}

void AlignmentAnalysis::print() {
    // Print results
    printf("\n");
    printTransformation(transformation_);
    printTransformation(transformation_gt_);
    pcl::console::print_info("fitness: %0.7f\n", fitness_);
    pcl::console::print_info("inliers_rmse: %0.7f\n", rmse_);
    pcl::console::print_info("correct inliers: %i/%i\n", correct_inlier_pairs_.size(), inlier_pairs_.size());
    pcl::console::print_info("correct correspondences: %i/%i\n",
                             correct_correspondences_.size(), correspondences_.size());
    pcl::console::print_info("rotation error: %0.7f\n", r_error_);
    pcl::console::print_info("translation error: %0.7f\n", t_error_);
    pcl::console::print_info("point cloud mean error: %0.7f\n", pcd_error_);
    pcl::console::print_info("normal mean difference: %0.7f\n", normal_diff_);
    pcl::console::print_info("uniformity of correct correspondences' distribution: %0.7f\n", corr_uniformity_);
}

void AlignmentAnalysis::save(const std::string &testname) {
    // Save test parameters and results
    std::string filepath = constructPath("test", "results", "csv", false);
    bool file_exists = std::filesystem::exists(filepath);
    std::fstream fout;
    if (!file_exists) {
        fout.open(filepath, std::ios_base::out);
    } else {
        fout.open(filepath, std::ios_base::app);
    }
    if (fout.is_open()) {
        if (!file_exists) {
            printAnalysisHeader(fout);
        }
        fout << *this;
        fout.close();
    } else {
        perror(("error while opening file " + filepath).c_str());
    }
}

void printAnalysisHeader(std::ostream &out) {
    out << "version,descriptor,testname,fitness,rmse,correspondences,correct_correspondences,inliers,correct_inliers,";
    out << "voxel_size,normal_radius_coef,feature_radius_coef,distance_thr_coef,edge_thr,";
    out << "iteration,matching,randomness,filter,threshold,n_random,r_err,t_err,pcd_err,use_normals,";
    out << "normal_diff,corr_uniformity,lrf,metric,time,overlap_rmse\n";
}

std::ostream &operator<<(std::ostream &stream, const AlignmentAnalysis &analysis) {
    stream << VERSION << "," << analysis.parameters_.descriptor_id << "," << analysis.testname_ << ","
           << analysis.fitness_ << "," << analysis.rmse_ << ",";
    stream << analysis.correspondences_.size() << "," << analysis.correct_correspondences_.size() << ",";
    stream << analysis.inlier_pairs_.size() << "," << analysis.correct_inlier_pairs_.size() << ",";
    stream << analysis.parameters_.voxel_size << ",";
    stream << analysis.parameters_.normal_radius_coef << ",";
    stream << analysis.parameters_.feature_radius_coef << ",";
    stream << analysis.parameters_.distance_thr_coef << ",";
    stream << analysis.parameters_.edge_thr_coef << ",";
    stream << analysis.iterations_ << ",";
    stream << analysis.parameters_.matching_id << ",";
    stream << analysis.parameters_.randomness << ",";
    auto func = getUniquenessFunction(analysis.parameters_.func_id);
    if (func != nullptr) {
        stream << analysis.parameters_.func_id << "," << UNIQUENESS_THRESHOLD << "," << N_RANDOM_FEATURES << ",";
    } else {
        stream << ",,,";
    }
    stream << analysis.r_error_ << "," << analysis.t_error_ << "," << analysis.pcd_error_ << ",";
    stream << analysis.parameters_.use_normals << "," << analysis.normal_diff_ << ",";
    stream << analysis.corr_uniformity_ << "," << analysis.parameters_.lrf_id << ",";
    stream << analysis.parameters_.metric_id << "," << analysis.time_ << "," << analysis.overlap_error_ << "\n";
    return stream;
}


text:
#define TAOJSON
#include "relevance.h"

#include <pleg.h>
using namespace Pleg;
#include <algorithm>
#include <string>
#include <iostream>
#include <sstream>
using namespace std;
#include <boost/lexical_cast.hpp>
using namespace boost;
#include "thread.h"
#include "source.h"
#include "gstreamer.h"

namespace Pleg {

/**
 * @brief Relevance::Relevance : the default Relevance is falsy
 * @param untrue
 */
Relevance::Relevance(bool _true):argParity(false),queryParity(false)
{
    if(_true){
        source = (Sources::Source*)1;
        uuid = string_gen("ffffffffffffffffffffffffffffffff");
    }else{
        source = nullptr;
        uuid = nil_gen();
    }
}
/**
 * @brief Relevance::Relevance : construct from a given source
 * @param _source Sources::Source*
 */
Relevance::Relevance(Sources::Source *_source):argParity(false),queryParity(false)
{
    source = _source;
    if(!!_source){
        source_name = (const char*)*source;
        uuid = _source->getUuid();
    }else{
        source_name = nullptr;
        uuid = nil_gen();
    }
}

Relevance::Relevance(const char* _source_name):argParity(false),queryParity(false)
{
    operator=(_source_name);
}

/**
 * @brief Relevance::Relevance : move constructor
 * @param rel Relevance const&&
 */
Relevance::Relevance(const Relevance &&rel)
{
    operator =(std::move(rel));
}

/**
 * @brief Relevance::Relevance : copy constructor
 * @param rhs Relevance const&
 */
Relevance::Relevance(const Relevance &rhs)
{
    operator =(rhs);
}
/**
 * @brief Relevance::getSource
 * @return Sources::Source*
 */
Sources::Source *Relevance::getSource()const
{
    return source;
}

/**
 * @brief Relevance::setSource : setSource for the relevance
 * @param _source Sources::Source*
 */
void Relevance::setSource(Sources::Source *_source)
{
    source = _source;
    source_name = (const char*)*_source;
    uuid = _source->getUuid();
}

/**
 * @brief Relevance::getUuid
 * @return QUuid
 */
uuids::uuid Relevance::getUuid()const
{
    return uuid;
}

/**
 * @brief Relevance::setUuid
 * @param _uuid Relevance
 */
void Relevance::setUuid(uuids::uuid _uuid)
{
    uuid = _uuid;
}

/**
 * @brief Relevance::setUuid
 * @param string uuid
 */
void Relevance::setUuid(string &_uuid)
{
    uuid = string_gen(_uuid);
}

void Relevance::setQueryParams(arguments_type query_params)
{
    params.clear();
    std::copy(query_params.begin(),query_params.end(),std::inserter(params,params.begin()));
}

/**
 * @brief Relevance::compare : assess equality to the rhs
 * @param rhs Relevance const&
 * @return bool
 */
bool Relevance::compare(const Relevance &rhs)const
{
    return source_name == rhs.source_name;
}

bool Relevance::operator<(const Relevance &rhs)const
{
    if(*this==rhs)
        return false;
    return source_name < rhs.source_name;
}

void Relevance::operator=(const char *rhs)
{
    source = Sources::fromString<Sources::Source>(rhs);
    if(!!source){
        source_name = rhs;
        uuid = source->getUuid();
        return;
    }
    source_name = "none";
    uuid = string_gen("0f0f0f0f0f0f0f0f0f0f0f0f0f0f0f0f");
}

/**
 * @brief Relevance::operator = : copy assignment
 * @param rhs Relevance const&
 */
void Relevance::operator=(const Relevance &rhs)
{
    source = rhs.source;
    source_name = rhs.source_name;
    uuid = rhs.uuid;
    argParity = rhs.argParity;
    queryParity = rhs.queryParity;
    arguments.clear();
    std::copy(rhs.arguments.begin(),rhs.arguments.end(),std::inserter(arguments,arguments.begin()));
    params.clear();
    std::copy(rhs.params.begin(),rhs.params.end(),std::inserter(params,params.begin()));
}

/**
 * @brief Relevance::operator bool : assess constant truthiness
 */
bool Relevance::toBool()const
{
    return nullptr != source || !uuid.is_nil() || argParity || queryParity;
}

void Relevance::toJson(json::value *object)const
{
    json::object_t &obj(object->get_object());
    obj.insert({"source",source_name});
    stringstream ss;
    ss << uuid;
    obj.insert({"uuid",ss.str()});
}

/**
 * @brief fromArguments : derive Relevance from parsed URI
 * @param c_arguments UriParseFunc::arguments_type
 * @return Relevance
 */
Relevance Relevance::fromArguments(const UriParseFunc::arguments_type &c_arguments,bool argParity)
{
    typedef UriParseFunc::arguments_type arguments_type;
    arguments_type &arguments = const_cast<arguments_type&>(c_arguments);
    Relevance ret(true);
    if(arguments.end()!=arguments.find("source")){
        ret = arguments.at("source").c_str();
    }
    if(arguments.end()!=arguments.find("uuid")){
        ret.setUuid(arguments.at("uuid"));
    }
    for(auto arg : arguments){
        ret.arguments.insert(arg);
    }
    ret.argParity = argParity;
    return ret;
}

/**
 * @brief fromSource : utility function for assessing source relevance
 * @param source Sources::Source*
 * @return Relevance
 */
Relevance Relevance::fromSource(const Sources::Source *source)
{
    Relevance rel(source);
    return rel;
}

/**
 * @brief operator == : c++ technique
 * @param lhs Relevance const&
 * @param rhs Relevance const&
 * @return bool
 */
bool Relevance::operator==(const Pleg::Relevance &rhs)const
{
    return toBool() && rhs.toBool() && compare(rhs);
}

/**
 * @brief operator << : stream operator
 * @param stream std::ostream &
 * @param rhs Relevance const&
 * @return std::ostream &
 */
logger &operator<<(logger &stream,const Pleg::Relevance &rel)
{
    if(!rel.source){
        stream << " relevance is irrelevant (tm)";
        return stream;
    }
    stream << "{"
           << "name:" << rel.source_name
           << "uuid:" << rel.uuid
           << "}";
    return stream;
}

logger &operator<<(logger &strm,uuids::uuid const& uuid)
{
    strm.getStream() << uuid;
    return strm;
}

stringstream &operator<<(stringstream &strm,uuids::uuid const& uuid)
{
    strm << "0x";
    for(int i=0;i<16;i++){
        strm << ios_base::hex << uuid.data[i];
    }
    return strm;
}

ostream &operator<<(ostream &strm,uuids::uuid const& uuid)
{
    strm << "0x";
    for(int i=0;i<16;i++){
        strm << ios_base::hex << uuid.data[i];
    }
    return strm;
}

} // namespace Pleg

namespace drumlin {

template <>
PodEvent<Pleg::Relevance::arguments_type> *pod_event_cast(const Event *event,Pleg::Relevance::arguments_type *out)
{
    if(out){
        PodEvent<Relevance::arguments_type> *ret = static_cast<PodEvent<Relevance::arguments_type>*>(const_cast<Event*>(event));
        if(0==std::distance(out->begin(),out->end())){
            for(auto item : ret->getVal()){
                out->insert(item);
            }
        }
        return ret;
    }
    return static_cast<PodEvent<Relevance::arguments_type>*>(const_cast<Event*>(event));
}

} // namespace drumlin

max_stars_repo_name: cephalicmarble/pleg

max_issues_repo_name: cephalicmarble/pleg

max_forks_repo_name: cephalicmarble/pleg