<commit_before>/* Copyright (c) 2016 PaddlePaddle Authors. All Rights Reserve.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License. */

#include "paddle/pybind/protobuf.h"
#include <deque>
#include "paddle/framework/attribute.h"

namespace paddle {
namespace pybind {

using namespace paddle::framework;  // NOLINT

template <typename T>
inline std::vector<T> RepeatedToVector(
    const google::protobuf::RepeatedField<T> &repeated_field) {
  std::vector<T> ret;
  ret.reserve(repeated_field.size());
  std::copy(
      repeated_field.begin(), repeated_field.end(), std::back_inserter(ret));
  return ret;
}

template <typename T, typename RepeatedField>
inline void VectorToRepeated(const std::vector<T> &vec,
                             RepeatedField *repeated_field) {
  repeated_field->Reserve(vec.size());
  for (auto &elem : vec) {
    *repeated_field->Add() = elem;
  }
}

class ProgramDescBind;
class OpDescBind;
class BlockDescBind;
class VarDescBind;

class VarDescBind {
public:
  explicit VarDescBind(const std::string &name) { desc_.set_name(name); }

  VarDesc *Proto() { return &desc_; }

  void SetShape(const std::vector<int64_t> &dims) {
    VectorToRepeated(dims, desc_.mutable_lod_tensor()->mutable_dims());
  }

  void SetDataType(int type_id) {
    desc_.mutable_lod_tensor()->set_data_type(static_cast<DataType>(type_id));
  }

  std::vector<int64_t> Shape() {
    return RepeatedToVector(desc_.lod_tensor().dims());
  }

private:
  VarDesc desc_;
};

class OpDescBind {
public:
  OpDesc *Proto() {
    Sync();
    return &op_desc_;
  }

  std::string Type() const { return op_desc_.type(); }

  void SetType(const std::string &type) { op_desc_.set_type(type); }

  const std::vector<std::string> &Input(const std::string &name) const {
    auto it = inputs_.find(name);
    PADDLE_ENFORCE(
        it != inputs_.end(), "Input %s cannot be found in Op %s", name, Type());
    return it->second;
  }

  std::vector<std::string> InputNames() const {
    std::vector<std::string> retv;
    retv.reserve(this->inputs_.size());
    for (auto &ipt : this->inputs_) {
      retv.push_back(ipt.first);
    }
    return retv;
  }

  void SetInput(const std::string &param_name,
                const std::vector<std::string> &args) {
    need_update_ = true;
    inputs_[param_name] = args;
  }

  const std::vector<std::string> &Output(const std::string &name) const {
    auto it = outputs_.find(name);
    PADDLE_ENFORCE(it != outputs_.end(),
                   "Output %s cannot be found in Op %s",
                   name,
                   Type());
    return it->second;
  }

  std::vector<std::string> OutputNames() const {
    std::vector<std::string> retv;
    retv.reserve(this->outputs_.size());
    for (auto &ipt : this->outputs_) {
      retv.push_back(ipt.first);
    }
    return retv;
  }

  void SetOutput(const std::string &param_name,
                 const std::vector<std::string> &args) {
    need_update_ = true;
    this->outputs_[param_name] = args;
  }

  std::string DebugString() { return this->Proto()->DebugString(); }

  struct SetAttrDescVisitor : public boost::static_visitor<void> {
    explicit SetAttrDescVisitor(OpDesc::Attr *attr) : attr_(attr) {}
    OpDesc::Attr *attr_;
    void operator()(int v) { attr_->set_i(v); }
    void operator()(float v) { attr_->set_f(v); }
    void operator()(const std::string &v) { attr_->set_s(v); }
  };

  void Sync() {
    if (need_update_) {
      this->op_desc_.mutable_inputs()->Clear();
      for (auto &ipt : inputs_) {
        auto *input = op_desc_.add_inputs();
        input->set_parameter(ipt.first);
        VectorToRepeated(ipt.second, input->mutable_arguments());
      }

      this->op_desc_.mutable_outputs()->Clear();
      for (auto &opt : outputs_) {
        auto *output = op_desc_.add_outputs();
        output->set_parameter(opt.first);
        VectorToRepeated(opt.second, output->mutable_arguments());
      }

      this->op_desc_.mutable_attrs()->Clear();
      for (auto &attr : attrs_) {
        auto *attr_desc = op_desc_.add_attrs();
        attr_desc->set_name(attr.first);
        attr_desc->set_type(static_cast<AttrType>(attr.second.which() - 1));
      }

      need_update_ = false;
    }
  }

private:
  OpDesc op_desc_;
  std::unordered_map<std::string, std::vector<std::string>> inputs_;
  std::unordered_map<std::string, std::vector<std::string>> outputs_;
  std::unordered_map<std::string, Attribute> attrs_;

  bool need_update_{false};
};

class BlockDescBind {
public:
  BlockDescBind(ProgramDescBind *prog, BlockDesc *desc)
      : prog_(prog), desc_(desc), need_update_(false) {}

  BlockDescBind(const BlockDescBind &o) = delete;
  BlockDescBind &operator=(const BlockDescBind &o) = delete;

  int32_t id() const { return desc_->idx(); }

  int32_t Parent() const { return desc_->parent_idx(); }

  VarDescBind *NewVar(py::bytes name_bytes) {
    std::string name = name_bytes;
    need_update_ = true;
    auto it = vars_.find(name);
    PADDLE_ENFORCE(it == vars_.end(), "Duplicated variable %s", name);
    auto var = new VarDescBind(name);
    vars_[name].reset(var);
    return var;
  }

  BlockDescBind *ParentBlock() const;

  OpDescBind *AppendOp() {
    need_update_ = true;
    ops_.emplace_back(new OpDescBind());
    return ops_.back().get();
  }

  void Sync() {
    if (need_update_) {
      auto &op_field = *this->desc_->mutable_ops();
      op_field.Clear();
      op_field.Reserve(static_cast<int>(ops_.size()));
      for (auto &op_desc : ops_) {
        op_field.AddAllocated(op_desc->Proto());
      }
      need_update_ = false;
    }
  }

private:
  ProgramDescBind *prog_;  // not_own
  BlockDesc *desc_;        // not_own
  bool need_update_;

  std::deque<std::unique_ptr<OpDescBind>> ops_;
  std::unordered_map<std::string, std::unique_ptr<VarDescBind>> vars_;
};

using ProgDescMap =
    std::unordered_map<ProgramDesc *, std::unique_ptr<ProgramDescBind>>;
static ProgDescMap *g_bind_map = nullptr;

class ProgramDescBind {
public:
  static ProgramDescBind &Instance(ProgramDesc *prog) {
    if (g_bind_map == nullptr) {
      g_bind_map = new ProgDescMap();
    }
    auto &map = *g_bind_map;
    auto &ptr = map[prog];

    if (ptr == nullptr) {
      ptr.reset(new ProgramDescBind(prog));
    }
    return *ptr;
  }
  ProgramDescBind(const ProgramDescBind &o) = delete;
  ProgramDescBind &operator=(const ProgramDescBind &o) = delete;

  BlockDescBind *AppendBlock(const BlockDescBind &parent) {
    auto *b = prog_->add_blocks();
    b->set_parent_idx(parent.id());
    b->set_idx(prog_->blocks_size() - 1);
    blocks_.emplace_back(new BlockDescBind(this, b));
    return blocks_.back().get();
  }

  BlockDescBind *Block(size_t idx) { return blocks_[idx].get(); }

  std::string DebugString() { return Proto()->DebugString(); }

  size_t Size() const { return blocks_.size(); }

  ProgramDesc *Proto() {
    for (auto &block : blocks_) {
      block->Sync();
    }
    return prog_;
  }

private:
  explicit ProgramDescBind(ProgramDesc *prog) : prog_(prog) {
    for (auto &block : *prog->mutable_blocks()) {
      blocks_.emplace_back(new BlockDescBind(this, &block));
    }
  }

  // Not owned
  ProgramDesc *prog_;

  std::vector<std::unique_ptr<BlockDescBind>> blocks_;
};

BlockDescBind *BlockDescBind::ParentBlock() const {
  if (this->desc_->parent_idx() == -1) {
    return nullptr;
  }
  return prog_->Block(static_cast<size_t>(this->desc_->parent_idx()));
}

void BindProgramDesc(py::module &m) {
  py::class_<ProgramDescBind>(m, "ProgramDesc", "")
      .def_static("instance",
                  []() -> ProgramDescBind * {
                    return &ProgramDescBind::Instance(&GetProgramDesc());
                  },
                  py::return_value_policy::reference)
      .def_static("__create_program_desc__",
                  []() -> ProgramDescBind * {
                    // Only used for unit-test
                    auto *prog_desc = new ProgramDesc;
                    auto *block = prog_desc->mutable_blocks()->Add();
                    block->set_idx(0);
                    block->set_parent_idx(-1);
                    return &ProgramDescBind::Instance(prog_desc);
                  },
                  py::return_value_policy::reference)
      .def("append_block",
           &ProgramDescBind::AppendBlock,
           py::return_value_policy::reference)
      .def("block", &ProgramDescBind::Block, py::return_value_policy::reference)
      .def("__str__", &ProgramDescBind::DebugString)
      .def("num_blocks", &ProgramDescBind::Size);
}

void BindBlockDesc(py::module &m) {
  py::class_<BlockDescBind>(m, "BlockDesc", "")
      .def_property_readonly("id", &BlockDescBind::id)
      .def_property_readonly("parent", &BlockDescBind::Parent)
      .def("append_op",
           &BlockDescBind::AppendOp,
           py::return_value_policy::reference)
      .def("new_var",
           &BlockDescBind::NewVar,
           py::return_value_policy::reference);
}

void BindVarDsec(py::module &m) {
  py::class_<VarDescBind>(m, "VarDesc", "")
      .def("set_shape", &VarDescBind::SetShape)
      .def("set_data_type", &VarDescBind::SetDataType)
      .def("shape", &VarDescBind::Shape);
}

void BindOpDesc(py::module &m) {
  py::class_<OpDescBind>(m, "OpDesc", "")
      .def("type", &OpDescBind::Type)
      .def("set_type", &OpDescBind::SetType)
      .def("input", &OpDescBind::Input)
      .def("input_names", &OpDescBind::InputNames)
      .def("set_input", &OpDescBind::SetInput)
      .def("output", &OpDescBind::Output)
      .def("output_names", &OpDescBind::OutputNames)
      .def("set_output", &OpDescBind::SetOutput)
      .def("__str__", &OpDescBind::DebugString)
      .def("__repr__", &OpDescBind::DebugString);
}
}  // namespace pybind
}  // namespace paddle
<commit_msg>Refine Visitor<commit_after>/* Copyright (c) 2016 PaddlePaddle Authors. All Rights Reserve.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License. */

#include "paddle/pybind/protobuf.h"
#include <deque>
#include "paddle/framework/attribute.h"

namespace paddle {
namespace pybind {

using namespace paddle::framework;  // NOLINT

template <typename T>
inline std::vector<T> RepeatedToVector(
    const google::protobuf::RepeatedField<T> &repeated_field) {
  std::vector<T> ret;
  ret.reserve(repeated_field.size());
  std::copy(
      repeated_field.begin(), repeated_field.end(), std::back_inserter(ret));
  return ret;
}

template <typename T, typename RepeatedField>
inline void VectorToRepeated(const std::vector<T> &vec,
                             RepeatedField *repeated_field) {
  repeated_field->Reserve(vec.size());
  for (auto &elem : vec) {
    *repeated_field->Add() = elem;
  }
}

class ProgramDescBind;
class OpDescBind;
class BlockDescBind;
class VarDescBind;

class VarDescBind {
public:
  explicit VarDescBind(const std::string &name) { desc_.set_name(name); }

  VarDesc *Proto() { return &desc_; }

  void SetShape(const std::vector<int64_t> &dims) {
    VectorToRepeated(dims, desc_.mutable_lod_tensor()->mutable_dims());
  }

  void SetDataType(int type_id) {
    desc_.mutable_lod_tensor()->set_data_type(static_cast<DataType>(type_id));
  }

  std::vector<int64_t> Shape() {
    return RepeatedToVector(desc_.lod_tensor().dims());
  }

private:
  VarDesc desc_;
};

class OpDescBind {
public:
  OpDesc *Proto() {
    Sync();
    return &op_desc_;
  }

  std::string Type() const { return op_desc_.type(); }

  void SetType(const std::string &type) { op_desc_.set_type(type); }

  const std::vector<std::string> &Input(const std::string &name) const {
    auto it = inputs_.find(name);
    PADDLE_ENFORCE(
        it != inputs_.end(), "Input %s cannot be found in Op %s", name, Type());
    return it->second;
  }

  std::vector<std::string> InputNames() const {
    std::vector<std::string> retv;
    retv.reserve(this->inputs_.size());
    for (auto &ipt : this->inputs_) {
      retv.push_back(ipt.first);
    }
    return retv;
  }

  void SetInput(const std::string &param_name,
                const std::vector<std::string> &args) {
    need_update_ = true;
    inputs_[param_name] = args;
  }

  const std::vector<std::string> &Output(const std::string &name) const {
    auto it = outputs_.find(name);
    PADDLE_ENFORCE(it != outputs_.end(),
                   "Output %s cannot be found in Op %s",
                   name,
                   Type());
    return it->second;
  }

  std::vector<std::string> OutputNames() const {
    std::vector<std::string> retv;
    retv.reserve(this->outputs_.size());
    for (auto &ipt : this->outputs_) {
      retv.push_back(ipt.first);
    }
    return retv;
  }

  void SetOutput(const std::string &param_name,
                 const std::vector<std::string> &args) {
    need_update_ = true;
    this->outputs_[param_name] = args;
  }

  std::string DebugString() { return this->Proto()->DebugString(); }

  struct SetAttrDescVisitor : public boost::static_visitor<void> {
    explicit SetAttrDescVisitor(OpDesc::Attr *attr) : attr_(attr) {}
    mutable OpDesc::Attr *attr_;
    void operator()(int v) const { attr_->set_i(v); }
    void operator()(float v) const { attr_->set_f(v); }
    void operator()(const std::string &v) const { attr_->set_s(v); }
    void operator()(bool b) const { attr_->set_b(b); }

    void operator()(const std::vector<int> &v) const {
      VectorToRepeated(v, attr_->mutable_ints());
    }
    void operator()(const std::vector<float> &v) const {
      VectorToRepeated(v, attr_->mutable_floats());
    }
    void operator()(const std::vector<std::string> &v) const {
      VectorToRepeated(v, attr_->mutable_strings());
    }
    void operator()(const std::vector<bool> &v) const {
      VectorToRepeated(v, attr_->mutable_bools());
    }
  };

  void Sync() {
    if (need_update_) {
      this->op_desc_.mutable_inputs()->Clear();
      for (auto &ipt : inputs_) {
        auto *input = op_desc_.add_inputs();
        input->set_parameter(ipt.first);
        VectorToRepeated(ipt.second, input->mutable_arguments());
      }

      this->op_desc_.mutable_outputs()->Clear();
      for (auto &opt : outputs_) {
        auto *output = op_desc_.add_outputs();
        output->set_parameter(opt.first);
        VectorToRepeated(opt.second, output->mutable_arguments());
      }

      this->op_desc_.mutable_attrs()->Clear();
      for (auto &attr : attrs_) {
        auto *attr_desc = op_desc_.add_attrs();
        attr_desc->set_name(attr.first);
        attr_desc->set_type(static_cast<AttrType>(attr.second.which() - 1));
      }

      need_update_ = false;
    }
  }

private:
  OpDesc op_desc_;
  std::unordered_map<std::string, std::vector<std::string>> inputs_;
  std::unordered_map<std::string, std::vector<std::string>> outputs_;
  std::unordered_map<std::string, Attribute> attrs_;

  bool need_update_{false};
};

class BlockDescBind {
public:
  BlockDescBind(ProgramDescBind *prog, BlockDesc *desc)
      : prog_(prog), desc_(desc), need_update_(false) {}

  BlockDescBind(const BlockDescBind &o) = delete;
  BlockDescBind &operator=(const BlockDescBind &o) = delete;

  int32_t id() const { return desc_->idx(); }

  int32_t Parent() const { return desc_->parent_idx(); }

  VarDescBind *NewVar(py::bytes name_bytes) {
    std::string name = name_bytes;
    need_update_ = true;
    auto it = vars_.find(name);
    PADDLE_ENFORCE(it == vars_.end(), "Duplicated variable %s", name);
    auto var = new VarDescBind(name);
    vars_[name].reset(var);
    return var;
  }

  BlockDescBind *ParentBlock() const;

  OpDescBind *AppendOp() {
    need_update_ = true;
    ops_.emplace_back(new OpDescBind());
    return ops_.back().get();
  }

  void Sync() {
    if (need_update_) {
      auto &op_field = *this->desc_->mutable_ops();
      op_field.Clear();
      op_field.Reserve(static_cast<int>(ops_.size()));
      for (auto &op_desc : ops_) {
        op_field.AddAllocated(op_desc->Proto());
      }
      need_update_ = false;
    }
  }

private:
  ProgramDescBind *prog_;  // not_own
  BlockDesc *desc_;        // not_own
  bool need_update_;

  std::deque<std::unique_ptr<OpDescBind>> ops_;
  std::unordered_map<std::string, std::unique_ptr<VarDescBind>> vars_;
};

using ProgDescMap =
    std::unordered_map<ProgramDesc *, std::unique_ptr<ProgramDescBind>>;
static ProgDescMap *g_bind_map = nullptr;

class ProgramDescBind {
public:
  static ProgramDescBind &Instance(ProgramDesc *prog) {
    if (g_bind_map == nullptr) {
      g_bind_map = new ProgDescMap();
    }
    auto &map = *g_bind_map;
    auto &ptr = map[prog];

    if (ptr == nullptr) {
      ptr.reset(new ProgramDescBind(prog));
    }
    return *ptr;
  }
  ProgramDescBind(const ProgramDescBind &o) = delete;
  ProgramDescBind &operator=(const ProgramDescBind &o) = delete;

  BlockDescBind *AppendBlock(const BlockDescBind &parent) {
    auto *b = prog_->add_blocks();
    b->set_parent_idx(parent.id());
    b->set_idx(prog_->blocks_size() - 1);
    blocks_.emplace_back(new BlockDescBind(this, b));
    return blocks_.back().get();
  }

  BlockDescBind *Block(size_t idx) { return blocks_[idx].get(); }

  std::string DebugString() { return Proto()->DebugString(); }

  size_t Size() const { return blocks_.size(); }

  ProgramDesc *Proto() {
    for (auto &block : blocks_) {
      block->Sync();
    }
    return prog_;
  }

private:
  explicit ProgramDescBind(ProgramDesc *prog) : prog_(prog) {
    for (auto &block : *prog->mutable_blocks()) {
      blocks_.emplace_back(new BlockDescBind(this, &block));
    }
  }

  // Not owned
  ProgramDesc *prog_;

  std::vector<std::unique_ptr<BlockDescBind>> blocks_;
};

BlockDescBind *BlockDescBind::ParentBlock() const {
  if (this->desc_->parent_idx() == -1) {
    return nullptr;
  }
  return prog_->Block(static_cast<size_t>(this->desc_->parent_idx()));
}

void BindProgramDesc(py::module &m) {
  py::class_<ProgramDescBind>(m, "ProgramDesc", "")
      .def_static("instance",
                  []() -> ProgramDescBind * {
                    return &ProgramDescBind::Instance(&GetProgramDesc());
                  },
                  py::return_value_policy::reference)
      .def_static("__create_program_desc__",
                  []() -> ProgramDescBind * {
                    // Only used for unit-test
                    auto *prog_desc = new ProgramDesc;
                    auto *block = prog_desc->mutable_blocks()->Add();
                    block->set_idx(0);
                    block->set_parent_idx(-1);
                    return &ProgramDescBind::Instance(prog_desc);
                  },
                  py::return_value_policy::reference)
      .def("append_block",
           &ProgramDescBind::AppendBlock,
           py::return_value_policy::reference)
      .def("block", &ProgramDescBind::Block, py::return_value_policy::reference)
      .def("__str__", &ProgramDescBind::DebugString)
      .def("num_blocks", &ProgramDescBind::Size);
}

void BindBlockDesc(py::module &m) {
  py::class_<BlockDescBind>(m, "BlockDesc", "")
      .def_property_readonly("id", &BlockDescBind::id)
      .def_property_readonly("parent", &BlockDescBind::Parent)
      .def("append_op",
           &BlockDescBind::AppendOp,
           py::return_value_policy::reference)
      .def("new_var",
           &BlockDescBind::NewVar,
           py::return_value_policy::reference);
}

void BindVarDsec(py::module &m) {
  py::class_<VarDescBind>(m, "VarDesc", "")
      .def("set_shape", &VarDescBind::SetShape)
      .def("set_data_type", &VarDescBind::SetDataType)
      .def("shape", &VarDescBind::Shape);
}

void BindOpDesc(py::module &m) {
  py::class_<OpDescBind>(m, "OpDesc", "")
      .def("type", &OpDescBind::Type)
      .def("set_type", &OpDescBind::SetType)
      .def("input", &OpDescBind::Input)
      .def("input_names", &OpDescBind::InputNames)
      .def("set_input", &OpDescBind::SetInput)
      .def("output", &OpDescBind::Output)
      .def("output_names", &OpDescBind::OutputNames)
      .def("set_output", &OpDescBind::SetOutput)
      .def("__str__", &OpDescBind::DebugString)
      .def("__repr__", &OpDescBind::DebugString);
}
}  // namespace pybind
}  // namespace paddle
<|endoftext|>


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

#include "ros/transport/transport_tcp.h"
#include "ros/poll_set.h"
#include "ros/header.h"
#include "ros/file_log.h"

#include <ros/assert.h>

#include <sstream>

#include <boost/bind.hpp>

#include <sys/socket.h>
#include <netinet/tcp.h>

#include <sys/poll.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <fcntl.h>
#include <errno.h>

namespace ros
{

TransportTCP::TransportTCP(PollSet* poll_set, int flags)
: sock_(-1)
, closed_(false)
, expecting_read_(false)
, expecting_write_(false)
, is_server_(false)
, server_port_(-1)
, poll_set_(poll_set)
, flags_(flags)
{

}

TransportTCP::~TransportTCP()
{
  ROS_ASSERT_MSG(sock_ == -1, "TransportTCP socket [%d] was never closed", sock_);
}

bool TransportTCP::setSocket(int sock)
{
  sock_ = sock;
  return initializeSocket();
}

bool TransportTCP::initializeSocket()
{
  ROS_ASSERT(sock_ != -1);

  if (!(flags_ & SYNCHRONOUS))
  {
    // make the socket non-blocking
    if(fcntl(sock_, F_SETFL, O_NONBLOCK) == -1)
    {
      ROS_ERROR("fcntl (non-blocking) to socket [%d] failed with error [%s]", sock_, strerror(errno));

      close();
      return false;
    }
  }

  if (is_server_)
  {
    cached_remote_host_ = "TCPServer Socket";
  }
  else
  {
    std::stringstream ss;
    ss << getClientURI() << " on socket " << sock_;
    cached_remote_host_ = ss.str();
  }

#ifdef ROSCPP_USE_TCP_NODELAY
  setNoDelay(true);
#endif

  ROS_ASSERT(poll_set_ || (flags_ & SYNCHRONOUS));
  if (poll_set_)
  {
    poll_set_->addSocket(sock_, boost::bind(&TransportTCP::socketUpdate, this, _1), shared_from_this());
  }

  if (!(flags_ & SYNCHRONOUS))
  {
    enableRead();
  }

  return true;
}

void TransportTCP::parseHeader(const Header& header)
{
  std::string nodelay;
  if (header.getValue("tcp_nodelay", nodelay) && nodelay == "1")
  {
    ROS_INFO("Setting nodelay on socket");
    setNoDelay(true);
  }
}

void TransportTCP::setNoDelay(bool nodelay)
{
  int flag = nodelay ? 1 : 0;
  int result = setsockopt(sock_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, sizeof(int));
  if (result < 0)
  {
    ROS_ERROR("setsockopt failed to set TCP_NODELAY on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
  }
}

bool TransportTCP::connect(const std::string& host, int port)
{
  sock_ = socket(AF_INET, SOCK_STREAM, 0);

  if (sock_ == -1)
  {
    ROS_ERROR("socket() failed with error [%s]", strerror(errno));
    return false;
  }

  sockaddr_in sin;
  sin.sin_family = AF_INET;
  if (inet_addr(host.c_str()) == INADDR_NONE)
  {
    struct addrinfo* addr;
    if (getaddrinfo(host.c_str(), NULL, NULL, &addr) != 0)
    {
      close();
      ROS_ERROR("couldn't resolve publisher host [%s]", host.c_str());
      return false;
    }

    bool found = false;
    struct addrinfo* it = addr;
    for (; it; it = it->ai_next)
    {
      if (it->ai_family == AF_INET)
      {
        memcpy(&sin, it->ai_addr, it->ai_addrlen);
        sin.sin_family = it->ai_family;
        sin.sin_port = htons(port);

        found = true;
        break;
      }
    }

    freeaddrinfo(addr);

    if (!found)
    {
      ROS_ERROR("Couldn't find an AF_INET address for [%s]\n", host.c_str());
      return false;
    }

    ROSCPP_LOG_DEBUG("Resolved publisher host [%s] to [%s]", host.c_str(), inet_ntoa(sin.sin_addr));
  }
  else
  {
    sin.sin_addr.s_addr = inet_addr(host.c_str()); // already an IP addr
  }

  sin.sin_port = htons(port);

  if (::connect(sock_, (sockaddr *)&sin, sizeof(sin)))
  {
    ROSCPP_LOG_DEBUG("Connect to tcpros publisher [%s:%d] failed with error [%s]", host.c_str(), port, strerror(errno));
    close();

    return false;
  }

  if (!initializeSocket())
  {
    return false;
  }

  ROSCPP_LOG_DEBUG("Connect succeeded to [%s:%d] on socket [%d]", host.c_str(), port, sock_);

  return true;
}

bool TransportTCP::listen(int port, int backlog, const AcceptCallback& accept_cb)
{
  is_server_ = true;
  accept_cb_ = accept_cb;

  sock_ = socket(AF_INET, SOCK_STREAM, 0);

  if (sock_ <= 0)
  {
    ROS_ERROR("socket() failed with error [%s]", strerror(errno));
    return false;
  }

  server_address_.sin_family = AF_INET;
  server_address_.sin_port = htons(port);
  server_address_.sin_addr.s_addr = INADDR_ANY;
  if (bind(sock_, (sockaddr *)&server_address_, sizeof(server_address_)) < 0)
  {
    ROS_ERROR("bind() failed with error [%s]", strerror(errno));
    return false;
  }

  ::listen(sock_, backlog);
  socklen_t len = sizeof(server_address_);
  getsockname(sock_, (sockaddr *)&server_address_, &len);
  server_port_ = ntohs(server_address_.sin_port);

  if (!initializeSocket())
  {
    return false;
  }

  return true;
}

void TransportTCP::close()
{
  Callback disconnect_cb;

  if (!closed_)
  {
    {
      boost::recursive_mutex::scoped_lock lock(close_mutex_);

      if (!closed_)
      {
        closed_ = true;

        ROSCPP_LOG_DEBUG("TCP socket [%d] closed", sock_);

        ROS_ASSERT(sock_ != -1);

        if (poll_set_)
        {
          poll_set_->delSocket(sock_);
        }

        ::shutdown(sock_, SHUT_RDWR);
        if (::close(sock_) < 0)
        {
          ROS_ERROR("Error closing socket [%d]: [%s]", sock_, strerror(errno));
        }

        closed_ = true;

        sock_ = -1;

        disconnect_cb = disconnect_cb_;

        disconnect_cb_ = Callback();
        read_cb_ = Callback();
        write_cb_ = Callback();
        accept_cb_ = AcceptCallback();
      }
    }
  }

  if (disconnect_cb)
  {
    disconnect_cb(shared_from_this());
  }
}

int32_t TransportTCP::read(uint8_t* buffer, uint32_t size)
{
  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);
    if (closed_)
    {
      ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
      return -1;
    }
  }

  ROS_ASSERT((int32_t)size > 0);

  int num_bytes = ::recv(sock_, buffer, size, 0);
  if (num_bytes < 0)
  {
    if (errno != EAGAIN)
    {
      ROSCPP_LOG_DEBUG("recv() failed with error [%s]", strerror(errno));
    }
    else
    {
      num_bytes = 0;
    }
  }
  else if (num_bytes == 0)
  {
    ROSCPP_LOG_DEBUG("Socket [%d] received 0/%d bytes, closing", sock_, size);
    close();
    return -1;
  }

  return num_bytes;
}

int32_t TransportTCP::write(uint8_t* buffer, uint32_t size)
{
  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      ROSCPP_LOG_DEBUG("Tried to write on a closed socket [%d]", sock_);
      return -1;
    }
  }

  ROS_ASSERT((int32_t)size > 0);

  int num_bytes = ::send(sock_, buffer, size, 0);
  if (num_bytes < 0)
  {
    if(errno != EAGAIN)
    {
      ROSCPP_LOG_DEBUG("send() failed with error [%s]", strerror(errno));

      close();
    }
    else
    {
      num_bytes = 0;
    }
  }

  return num_bytes;
}

void TransportTCP::enableRead()
{
  ROS_ASSERT(!(flags_ & SYNCHRONOUS));

  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (!expecting_read_)
  {
    poll_set_->addEvents(sock_, POLLIN);
    expecting_read_ = true;
  }
}

void TransportTCP::enableWrite()
{
  ROS_ASSERT(!(flags_ & SYNCHRONOUS));

  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (!expecting_write_)
  {
    poll_set_->addEvents(sock_, POLLOUT);
    expecting_write_ = true;
  }
}

void TransportTCP::disableWrite()
{
  ROS_ASSERT(!(flags_ & SYNCHRONOUS));

  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (expecting_write_)
  {
    poll_set_->delEvents(sock_, POLLOUT);
    expecting_write_ = false;
  }
}

TransportTCPPtr TransportTCP::accept()
{
  ROS_ASSERT(is_server_);

  sockaddr client_address;
  socklen_t len = sizeof(client_address);
  int new_sock = ::accept(sock_, (sockaddr *)&client_address, &len);
  if (new_sock >= 0)
  {
    ROSCPP_LOG_DEBUG("Accepted connection on socket [%d]", new_sock);

    TransportTCPPtr transport(new TransportTCP(poll_set_, flags_));
    if (!transport->setSocket(new_sock))
    {
      ROS_ERROR("Failed to set socket on transport for socket %d", new_sock);
    }

    return transport;
  }
  else
  {
    ROS_ERROR("accept() on socket [%d] failed with error [%s]", sock_, strerror(errno));
  }

  return TransportTCPPtr();
}

void TransportTCP::socketUpdate(int events)
{
  boost::recursive_mutex::scoped_lock lock(close_mutex_);

  if (closed_)
  {
    return;
  }

  if((events & POLLERR) ||
     (events & POLLHUP) ||
     (events & POLLNVAL))
  {
    ROSCPP_LOG_DEBUG("Socket %d closed with (ERR|HUP|NVAL) events %d", sock_, events);
    close();
  }
  else
  {
    if ((events & POLLIN) && expecting_read_)
    {
      if (is_server_)
      {
        // Should not block here, because poll() said that it's ready
        // for reading
        TransportTCPPtr transport = accept();
        if (transport)
        {
          ROS_ASSERT(accept_cb_);
          accept_cb_(transport);
        }
      }
      else
      {
        if (read_cb_)
        {
          read_cb_(shared_from_this());
        }
      }
    }

    if ((events & POLLOUT) && expecting_write_)
    {
      if (write_cb_)
      {
        write_cb_(shared_from_this());
      }
    }
  }
}

std::string TransportTCP::getTransportInfo()
{
  return "TCPROS connection to [" + cached_remote_host_ + "]";
}

std::string TransportTCP::getClientURI()
{
  ROS_ASSERT(!is_server_);

  sockaddr_in addr;
  socklen_t len = sizeof(addr);
  getpeername(sock_, (sockaddr *)&addr, &len);
  int port = ntohs(addr.sin_port);
  std::string ip = inet_ntoa(addr.sin_addr);

  std::stringstream uri;
  uri << ip << ":" << port;

  return uri.str();
}

} // namespace ros
<commit_msg>Turn an old debug INFO into a debug<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#include "ros/transport/transport_tcp.h"
#include "ros/poll_set.h"
#include "ros/header.h"
#include "ros/file_log.h"

#include <ros/assert.h>

#include <sstream>

#include <boost/bind.hpp>

#include <sys/socket.h>
#include <netinet/tcp.h>

#include <sys/poll.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <fcntl.h>
#include <errno.h>

namespace ros
{

TransportTCP::TransportTCP(PollSet* poll_set, int flags)
: sock_(-1)
, closed_(false)
, expecting_read_(false)
, expecting_write_(false)
, is_server_(false)
, server_port_(-1)
, poll_set_(poll_set)
, flags_(flags)
{

}

TransportTCP::~TransportTCP()
{
  ROS_ASSERT_MSG(sock_ == -1, "TransportTCP socket [%d] was never closed", sock_);
}

bool TransportTCP::setSocket(int sock)
{
  sock_ = sock;
  return initializeSocket();
}

bool TransportTCP::initializeSocket()
{
  ROS_ASSERT(sock_ != -1);

  if (!(flags_ & SYNCHRONOUS))
  {
    // make the socket non-blocking
    if(fcntl(sock_, F_SETFL, O_NONBLOCK) == -1)
    {
      ROS_ERROR("fcntl (non-blocking) to socket [%d] failed with error [%s]", sock_, strerror(errno));

      close();
      return false;
    }
  }

  if (is_server_)
  {
    cached_remote_host_ = "TCPServer Socket";
  }
  else
  {
    std::stringstream ss;
    ss << getClientURI() << " on socket " << sock_;
    cached_remote_host_ = ss.str();
  }

#ifdef ROSCPP_USE_TCP_NODELAY
  setNoDelay(true);
#endif

  ROS_ASSERT(poll_set_ || (flags_ & SYNCHRONOUS));
  if (poll_set_)
  {
    poll_set_->addSocket(sock_, boost::bind(&TransportTCP::socketUpdate, this, _1), shared_from_this());
  }

  if (!(flags_ & SYNCHRONOUS))
  {
    enableRead();
  }

  return true;
}

void TransportTCP::parseHeader(const Header& header)
{
  std::string nodelay;
  if (header.getValue("tcp_nodelay", nodelay) && nodelay == "1")
  {
    ROSCPP_LOG_DEBUG("Setting nodelay on socket [%d]", sock_);
    setNoDelay(true);
  }
}

void TransportTCP::setNoDelay(bool nodelay)
{
  int flag = nodelay ? 1 : 0;
  int result = setsockopt(sock_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag, sizeof(int));
  if (result < 0)
  {
    ROS_ERROR("setsockopt failed to set TCP_NODELAY on socket [%d] [%s]", sock_, cached_remote_host_.c_str());
  }
}

bool TransportTCP::connect(const std::string& host, int port)
{
  sock_ = socket(AF_INET, SOCK_STREAM, 0);

  if (sock_ == -1)
  {
    ROS_ERROR("socket() failed with error [%s]", strerror(errno));
    return false;
  }

  sockaddr_in sin;
  sin.sin_family = AF_INET;
  if (inet_addr(host.c_str()) == INADDR_NONE)
  {
    struct addrinfo* addr;
    if (getaddrinfo(host.c_str(), NULL, NULL, &addr) != 0)
    {
      close();
      ROS_ERROR("couldn't resolve publisher host [%s]", host.c_str());
      return false;
    }

    bool found = false;
    struct addrinfo* it = addr;
    for (; it; it = it->ai_next)
    {
      if (it->ai_family == AF_INET)
      {
        memcpy(&sin, it->ai_addr, it->ai_addrlen);
        sin.sin_family = it->ai_family;
        sin.sin_port = htons(port);

        found = true;
        break;
      }
    }

    freeaddrinfo(addr);

    if (!found)
    {
      ROS_ERROR("Couldn't find an AF_INET address for [%s]\n", host.c_str());
      return false;
    }

    ROSCPP_LOG_DEBUG("Resolved publisher host [%s] to [%s]", host.c_str(), inet_ntoa(sin.sin_addr));
  }
  else
  {
    sin.sin_addr.s_addr = inet_addr(host.c_str()); // already an IP addr
  }

  sin.sin_port = htons(port);

  if (::connect(sock_, (sockaddr *)&sin, sizeof(sin)))
  {
    ROSCPP_LOG_DEBUG("Connect to tcpros publisher [%s:%d] failed with error [%s]", host.c_str(), port, strerror(errno));
    close();

    return false;
  }

  if (!initializeSocket())
  {
    return false;
  }

  ROSCPP_LOG_DEBUG("Connect succeeded to [%s:%d] on socket [%d]", host.c_str(), port, sock_);

  return true;
}

bool TransportTCP::listen(int port, int backlog, const AcceptCallback& accept_cb)
{
  is_server_ = true;
  accept_cb_ = accept_cb;

  sock_ = socket(AF_INET, SOCK_STREAM, 0);

  if (sock_ <= 0)
  {
    ROS_ERROR("socket() failed with error [%s]", strerror(errno));
    return false;
  }

  server_address_.sin_family = AF_INET;
  server_address_.sin_port = htons(port);
  server_address_.sin_addr.s_addr = INADDR_ANY;
  if (bind(sock_, (sockaddr *)&server_address_, sizeof(server_address_)) < 0)
  {
    ROS_ERROR("bind() failed with error [%s]", strerror(errno));
    return false;
  }

  ::listen(sock_, backlog);
  socklen_t len = sizeof(server_address_);
  getsockname(sock_, (sockaddr *)&server_address_, &len);
  server_port_ = ntohs(server_address_.sin_port);

  if (!initializeSocket())
  {
    return false;
  }

  return true;
}

void TransportTCP::close()
{
  Callback disconnect_cb;

  if (!closed_)
  {
    {
      boost::recursive_mutex::scoped_lock lock(close_mutex_);

      if (!closed_)
      {
        closed_ = true;

        ROSCPP_LOG_DEBUG("TCP socket [%d] closed", sock_);

        ROS_ASSERT(sock_ != -1);

        if (poll_set_)
        {
          poll_set_->delSocket(sock_);
        }

        ::shutdown(sock_, SHUT_RDWR);
        if (::close(sock_) < 0)
        {
          ROS_ERROR("Error closing socket [%d]: [%s]", sock_, strerror(errno));
        }

        closed_ = true;

        sock_ = -1;

        disconnect_cb = disconnect_cb_;

        disconnect_cb_ = Callback();
        read_cb_ = Callback();
        write_cb_ = Callback();
        accept_cb_ = AcceptCallback();
      }
    }
  }

  if (disconnect_cb)
  {
    disconnect_cb(shared_from_this());
  }
}

int32_t TransportTCP::read(uint8_t* buffer, uint32_t size)
{
  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);
    if (closed_)
    {
      ROSCPP_LOG_DEBUG("Tried to read on a closed socket [%d]", sock_);
      return -1;
    }
  }

  ROS_ASSERT((int32_t)size > 0);

  int num_bytes = ::recv(sock_, buffer, size, 0);
  if (num_bytes < 0)
  {
    if (errno != EAGAIN)
    {
      ROSCPP_LOG_DEBUG("recv() failed with error [%s]", strerror(errno));
    }
    else
    {
      num_bytes = 0;
    }
  }
  else if (num_bytes == 0)
  {
    ROSCPP_LOG_DEBUG("Socket [%d] received 0/%d bytes, closing", sock_, size);
    close();
    return -1;
  }

  return num_bytes;
}

int32_t TransportTCP::write(uint8_t* buffer, uint32_t size)
{
  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      ROSCPP_LOG_DEBUG("Tried to write on a closed socket [%d]", sock_);
      return -1;
    }
  }

  ROS_ASSERT((int32_t)size > 0);

  int num_bytes = ::send(sock_, buffer, size, 0);
  if (num_bytes < 0)
  {
    if(errno != EAGAIN)
    {
      ROSCPP_LOG_DEBUG("send() failed with error [%s]", strerror(errno));

      close();
    }
    else
    {
      num_bytes = 0;
    }
  }

  return num_bytes;
}

void TransportTCP::enableRead()
{
  ROS_ASSERT(!(flags_ & SYNCHRONOUS));

  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (!expecting_read_)
  {
    poll_set_->addEvents(sock_, POLLIN);
    expecting_read_ = true;
  }
}

void TransportTCP::enableWrite()
{
  ROS_ASSERT(!(flags_ & SYNCHRONOUS));

  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (!expecting_write_)
  {
    poll_set_->addEvents(sock_, POLLOUT);
    expecting_write_ = true;
  }
}

void TransportTCP::disableWrite()
{
  ROS_ASSERT(!(flags_ & SYNCHRONOUS));

  {
    boost::recursive_mutex::scoped_lock lock(close_mutex_);

    if (closed_)
    {
      return;
    }
  }

  if (expecting_write_)
  {
    poll_set_->delEvents(sock_, POLLOUT);
    expecting_write_ = false;
  }
}

TransportTCPPtr TransportTCP::accept()
{
  ROS_ASSERT(is_server_);

  sockaddr client_address;
  socklen_t len = sizeof(client_address);
  int new_sock = ::accept(sock_, (sockaddr *)&client_address, &len);
  if (new_sock >= 0)
  {
    ROSCPP_LOG_DEBUG("Accepted connection on socket [%d]", new_sock);

    TransportTCPPtr transport(new TransportTCP(poll_set_, flags_));
    if (!transport->setSocket(new_sock))
    {
      ROS_ERROR("Failed to set socket on transport for socket %d", new_sock);
    }

    return transport;
  }
  else
  {
    ROS_ERROR("accept() on socket [%d] failed with error [%s]", sock_, strerror(errno));
  }

  return TransportTCPPtr();
}

void TransportTCP::socketUpdate(int events)
{
  boost::recursive_mutex::scoped_lock lock(close_mutex_);

  if (closed_)
  {
    return;
  }

  if((events & POLLERR) ||
     (events & POLLHUP) ||
     (events & POLLNVAL))
  {
    ROSCPP_LOG_DEBUG("Socket %d closed with (ERR|HUP|NVAL) events %d", sock_, events);
    close();
  }
  else
  {
    if ((events & POLLIN) && expecting_read_)
    {
      if (is_server_)
      {
        // Should not block here, because poll() said that it's ready
        // for reading
        TransportTCPPtr transport = accept();
        if (transport)
        {
          ROS_ASSERT(accept_cb_);
          accept_cb_(transport);
        }
      }
      else
      {
        if (read_cb_)
        {
          read_cb_(shared_from_this());
        }
      }
    }

    if ((events & POLLOUT) && expecting_write_)
    {
      if (write_cb_)
      {
        write_cb_(shared_from_this());
      }
    }
  }
}

std::string TransportTCP::getTransportInfo()
{
  return "TCPROS connection to [" + cached_remote_host_ + "]";
}

std::string TransportTCP::getClientURI()
{
  ROS_ASSERT(!is_server_);

  sockaddr_in addr;
  socklen_t len = sizeof(addr);
  getpeername(sock_, (sockaddr *)&addr, &len);
  int port = ntohs(addr.sin_port);
  std::string ip = inet_ntoa(addr.sin_addr);

  std::stringstream uri;
  uri << ip << ":" << port;

  return uri.str();
}

} // namespace ros
<|endoftext|>


<commit_before>#ifndef __GLOBALS_HPP_INCLUDED
#define __GLOBALS_HPP_INCLUDED

#include "any_value.hpp"

// GCC (at least g++ 4.7.2) and Visual Studio 2015 do support
// setting default values of a struct using C++11 syntax.
// Clang 3.7.0 and Visual Studio 2013 do not support
// setting default values of a struct using C++11 syntax.
// Visual Studio 2013 fails to compile, whereas Clang-compiled
// executable with code with setting default values of a struct
// causes Segmentation fault upon execution of the program.
// Compilers that don't support setting default values of a struct
// are handled by setting the default values in a macro.
// http://stackoverflow.com/questions/16782103/initializing-default-values-in-a-struct/16783513#16783513
#ifdef __clang__
#elif defined(__GNUC__)
#define __STRUCT_DEFAULT_VALUES_ARE_ACCEPTED
#elif defined(_WIN32)
#if (_MSC_VER >= 1900)
#define __STRUCT_DEFAULT_VALUES_ARE_ACCEPTED
#endif
#endif

// Include GLEW
#ifndef __GL_GLEW_H_INCLUDED
#define __GL_GLEW_H_INCLUDED
#include <GL/glew.h> // GLfloat, GLuint etc.
#endif

// Include GLFW
#ifndef __GLFW3_H_INCLUDED
#define __GLFW3_H_INCLUDED
#include <glfw3.h>
#endif

// Include GLM
#ifndef __GLM_GLM_HPP_INCLUDED
#define __GLM_GLM_HPP_INCLUDED
#include <glm/glm.hpp> // glm
#endif

// Include standard headers
#include <cmath>    // NAN, std::isnan, std::pow
#include <stdint.h> // uint32_t etc.
#include <string>   // std::string
#include <vector>   // std::vector

#ifndef WINDOW_WIDTH
#define WINDOW_WIDTH (1600.0f)
#endif

#ifndef WINDOW_HEIGHT
#define WINDOW_HEIGHT (900.0f)
#endif

#ifndef ASPECT_RATIO
#define ASPECT_RATIO (WINDOW_WIDTH / WINDOW_HEIGHT)
#endif

#ifndef PI
#define PI 3.14159265359f
#endif

#ifndef EARTH_RADIUS
#define EARTH_RADIUS 6371000.0f
#endif

#ifndef TEXT_SIZE
#define TEXT_SIZE 40
#endif

#ifndef FONT_SIZE
#define FONT_SIZE 16
#endif

#ifndef MAX_FPS
#define MAX_FPS 60
#endif

extern glm::mat4 ProjectionMatrix;
extern glm::mat4 ViewMatrix;
extern glm::vec3 position;
extern GLfloat gravity;
extern bool is_flight_mode_in_use;
extern bool in_help_mode;
extern GLfloat fallSpeed;
extern double horizontalAngle;
extern double verticalAngle;
extern GLfloat initialFoV;
extern double earth_radius;
extern bool hasMouseEverMoved;
extern bool is_invert_mouse_in_use;
extern bool is_key_I_released;
extern bool is_key_F_released;
extern bool is_key_F1_released;
extern bool is_world_loaded;        // no more than one world object can be loaded. TODO: check that no more than one world is loaded!
extern bool is_world_spherical;

extern GLfloat speed;
extern GLfloat turbo_factor;
extern GLfloat twin_turbo_factor;
extern GLfloat mouseSpeed;

extern glm::vec3 camera_position;

namespace model
{
    class World;
    class Scene;
    class Shader;
    class Graph;
    class Material;
    class VectorFont;
    class Species;
    class Glyph;
    class Text3D;
}

typedef struct ShaderStruct
{
    ShaderStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    model::Scene* parent_pointer; // pointer to the scene (draw list).
    std::string vertex_shader;    // filename of vertex shader.
    std::string fragment_shader;  // filename of fragment shader.
} ShaderStruct;

typedef struct MaterialStruct
{
    MaterialStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    model::Shader* parent_pointer;   // pointer to the shader.
    std::string texture_file_format; // type of the texture file. supported file formats so far: `"bmp"`/`"BMP"`, `"dds"`/`"DDS"`.
    std::string texture_filename;    // filename of the model file.
    std::string image_path;
} MaterialStruct;

typedef struct NodeStruct
{
    NodeStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    GLuint nodeID;
    model::Graph* parent_pointer;
    glm::vec3 coordinate_vector;
    std::vector<uint32_t> neighbor_nodeIDs;
} NodeStruct;

typedef struct ObjectStruct
{
    ObjectStruct()
        : species_parent_pointer(nullptr), glyph_parent_pointer(nullptr), text3D_parent_pointer(nullptr), original_scale_vector(glm::vec3(1.0f, 1.0f, 1.0f)), rotate_angle(NAN), is_character(false)
    {
        // constructor.
    }
    model::Species* species_parent_pointer; // pointer to the parent `Species`.
    model::Glyph* glyph_parent_pointer;     // pointer to the parent `Glyph`.
    model::Text3D* text3D_parent_pointer;   // pointer to the parent `Text3D`.
    glm::vec3 original_scale_vector; // original scale vector.
    GLfloat rotate_angle;            // rotate angle.
    bool is_character;               // The parent of a character object is a Glyph. The parent of a regular object is a Species.
    glm::vec3 coordinate_vector;     // coordinate vector.
    glm::vec3 rotate_vector;         // rotate vector.
    glm::vec3 translate_vector;      // translate vector.
} ObjectStruct;

typedef struct SpeciesStruct
{
    SpeciesStruct()
        : parent_pointer(nullptr), is_world(false), world_radius(NAN), triangulation_type("bilinear_interpolation")
    {
        // constructor.
    }
    // used for all files (for all species).
    model::Material* parent_pointer;         // pointer to the material object.
    bool is_world;                           // worlds currently do not rotate nor translate.
    double world_radius;                     // radius of sea level in meters. used only for worlds.
    std::string model_file_format;           // type of the model file. supported file formats so far: `"bmp"`/`"BMP"`, `"obj"`/`"OBJ"`.
                                             // TODO: add support for `"SRTM"`.
    std::string model_filename;              // filename of the model file.
    // for `"bmp"` model files.
    std::string color_channel;               // color channel to use for altitude data.

    glm::vec3 light_position;                // light position.
    std::string coordinate_system;           // used only for worlds (`is_world` == `true`). valid values: `"cartesian"`.
                                             // TODO: add support for `"spherical"`. `"spherical"` is used eg. in SRTM heightmaps.
    std::string triangulation_type;
} SpeciesStruct;

#define DEFAULT_VERTEX_SCALING_FACTOR (0.001f)

typedef struct VectorFontStruct
{
    VectorFontStruct()
        : parent_pointer(nullptr), vertex_scaling_factor(DEFAULT_VERTEX_SCALING_FACTOR)
    {
        // constructor.
    }
    // used for all files (for all font).
    model::Material* parent_pointer;        // pointer to the material object.
    GLfloat vertex_scaling_factor;
    std::string font_file_format;           // type of the font file. supported file formats so far: `"svg"`/`"SVG"`.
    std::string font_filename;              // filename of the font file.
} VectorFontStruct;

typedef struct Text3DStruct
{
    Text3DStruct()
        : parent_pointer(nullptr), original_scale_vector(glm::vec3(1.0f, 1.0f, 1.0f)), rotate_angle(NAN)
    {
        // constructor.
    }
    model::VectorFont* parent_pointer; // pointer to the parent `VectorFont`.
    std::string text_string;
    glm::vec3 original_scale_vector;   // original scale vector.
    GLfloat rotate_angle;              // rotate angle.
    glm::vec3 coordinate_vector;       // coordinate vector.
    glm::vec3 rotate_vector;           // rotate vector.
    glm::vec3 translate_vector;        // translate vector.
} Text3DStruct;

typedef struct GlyphStruct
{
    GlyphStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    // used for all files (for all glyph).
    std::vector<std::vector<glm::vec2>>* glyph_vertex_data;
    std::string* glyph_name_pointer;         // we need only a pointer, because glyphs are always created by the `VectorFont` constructor.
    std::string* unicode_string_pointer;     // we need only a pointer, because glyphs are always created by the `VectorFont` constructor.
    model::VectorFont* parent_pointer;       // pointer to the font object.
    glm::vec3 light_position;                // light position.
} GlyphStruct;

typedef struct
{
    GLuint screen_width;
    GLuint screen_height;
    GLuint x;
    GLuint y;
    GLuint text_size;
    GLuint font_size;
    const char* text;
    const char* char_font_texture_file_format;
    const char* horizontal_alignment;
    const char* vertical_alignment;
} PrintingStruct;

typedef struct
{
    double rho;
    double theta;
    double phi;
} SphericalCoordinatesStruct;

typedef struct SphericalWorldStruct
{
    SphericalWorldStruct()
        :SRTM_latitude_step_in_degrees(1.0f/1200.0f), SRTM_longitude_step_in_degrees(1.0f/1200.0f)
    {
        // constructor.
    }
    double southern_latitude;
    double northern_latitude;
    double western_longitude;
    double eastern_longitude;
    double SRTM_latitude_step_in_degrees;
    double SRTM_longitude_step_in_degrees;
} SphericalWorldStruct;

typedef struct TriangulateQuadsStruct
{
    TriangulateQuadsStruct()
        : should_ylikuutio_use_real_texture_coordinates(true)
    {
        // constructor.
    }
    GLuint* input_vertex_pointer;
    uint32_t image_width;
    uint32_t image_height;
    std::string triangulation_type;
    bool should_ylikuutio_use_real_texture_coordinates;
    double sphere_radius;
    SphericalWorldStruct spherical_world_struct;
} TriangulateQuadsStruct;

typedef struct
{
    uint32_t* input_vertex_pointer;
    uint32_t image_width;
    uint32_t image_height;
    bool should_ylikuutio_use_real_texture_coordinates;
} BilinearInterpolationStruct;

typedef struct
{
    uint32_t image_width;
    uint32_t image_height;
    double sphere_radius;
    bool is_bilinear_interpolation_in_use;
    SphericalWorldStruct spherical_world_struct;
} TransformationStruct;

namespace callback_system
{
    class CallbackEngine;
    class CallbackParameter;
}
typedef datatypes::AnyValue* (*InputParametersToAnyValueCallback)(callback_system::CallbackEngine*, std::vector<callback_system::CallbackParameter*>);

extern SphericalCoordinatesStruct spherical_position;

#endif
<commit_msg>Modified `typedef struct Text3DStruct`.<commit_after>#ifndef __GLOBALS_HPP_INCLUDED
#define __GLOBALS_HPP_INCLUDED

#include "any_value.hpp"

// GCC (at least g++ 4.7.2) and Visual Studio 2015 do support
// setting default values of a struct using C++11 syntax.
// Clang 3.7.0 and Visual Studio 2013 do not support
// setting default values of a struct using C++11 syntax.
// Visual Studio 2013 fails to compile, whereas Clang-compiled
// executable with code with setting default values of a struct
// causes Segmentation fault upon execution of the program.
// Compilers that don't support setting default values of a struct
// are handled by setting the default values in a macro.
// http://stackoverflow.com/questions/16782103/initializing-default-values-in-a-struct/16783513#16783513
#ifdef __clang__
#elif defined(__GNUC__)
#define __STRUCT_DEFAULT_VALUES_ARE_ACCEPTED
#elif defined(_WIN32)
#if (_MSC_VER >= 1900)
#define __STRUCT_DEFAULT_VALUES_ARE_ACCEPTED
#endif
#endif

// Include GLEW
#ifndef __GL_GLEW_H_INCLUDED
#define __GL_GLEW_H_INCLUDED
#include <GL/glew.h> // GLfloat, GLuint etc.
#endif

// Include GLFW
#ifndef __GLFW3_H_INCLUDED
#define __GLFW3_H_INCLUDED
#include <glfw3.h>
#endif

// Include GLM
#ifndef __GLM_GLM_HPP_INCLUDED
#define __GLM_GLM_HPP_INCLUDED
#include <glm/glm.hpp> // glm
#endif

// Include standard headers
#include <cmath>    // NAN, std::isnan, std::pow
#include <stdint.h> // uint32_t etc.
#include <string>   // std::string
#include <vector>   // std::vector

#ifndef WINDOW_WIDTH
#define WINDOW_WIDTH (1600.0f)
#endif

#ifndef WINDOW_HEIGHT
#define WINDOW_HEIGHT (900.0f)
#endif

#ifndef ASPECT_RATIO
#define ASPECT_RATIO (WINDOW_WIDTH / WINDOW_HEIGHT)
#endif

#ifndef PI
#define PI 3.14159265359f
#endif

#ifndef EARTH_RADIUS
#define EARTH_RADIUS 6371000.0f
#endif

#ifndef TEXT_SIZE
#define TEXT_SIZE 40
#endif

#ifndef FONT_SIZE
#define FONT_SIZE 16
#endif

#ifndef MAX_FPS
#define MAX_FPS 60
#endif

extern glm::mat4 ProjectionMatrix;
extern glm::mat4 ViewMatrix;
extern glm::vec3 position;
extern GLfloat gravity;
extern bool is_flight_mode_in_use;
extern bool in_help_mode;
extern GLfloat fallSpeed;
extern double horizontalAngle;
extern double verticalAngle;
extern GLfloat initialFoV;
extern double earth_radius;
extern bool hasMouseEverMoved;
extern bool is_invert_mouse_in_use;
extern bool is_key_I_released;
extern bool is_key_F_released;
extern bool is_key_F1_released;
extern bool is_world_loaded;        // no more than one world object can be loaded. TODO: check that no more than one world is loaded!
extern bool is_world_spherical;

extern GLfloat speed;
extern GLfloat turbo_factor;
extern GLfloat twin_turbo_factor;
extern GLfloat mouseSpeed;

extern glm::vec3 camera_position;

namespace model
{
    class World;
    class Scene;
    class Shader;
    class Graph;
    class Material;
    class VectorFont;
    class Species;
    class Glyph;
    class Text3D;
}

typedef struct ShaderStruct
{
    ShaderStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    model::Scene* parent_pointer; // pointer to the scene (draw list).
    std::string vertex_shader;    // filename of vertex shader.
    std::string fragment_shader;  // filename of fragment shader.
} ShaderStruct;

typedef struct MaterialStruct
{
    MaterialStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    model::Shader* parent_pointer;   // pointer to the shader.
    std::string texture_file_format; // type of the texture file. supported file formats so far: `"bmp"`/`"BMP"`, `"dds"`/`"DDS"`.
    std::string texture_filename;    // filename of the model file.
    std::string image_path;
} MaterialStruct;

typedef struct NodeStruct
{
    NodeStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    GLuint nodeID;
    model::Graph* parent_pointer;
    glm::vec3 coordinate_vector;
    std::vector<uint32_t> neighbor_nodeIDs;
} NodeStruct;

typedef struct ObjectStruct
{
    ObjectStruct()
        : species_parent_pointer(nullptr), glyph_parent_pointer(nullptr), text3D_parent_pointer(nullptr), original_scale_vector(glm::vec3(1.0f, 1.0f, 1.0f)), rotate_angle(NAN), is_character(false)
    {
        // constructor.
    }
    model::Species* species_parent_pointer; // pointer to the parent `Species`.
    model::Glyph* glyph_parent_pointer;     // pointer to the parent `Glyph`.
    model::Text3D* text3D_parent_pointer;   // pointer to the parent `Text3D`.
    glm::vec3 original_scale_vector; // original scale vector.
    GLfloat rotate_angle;            // rotate angle.
    bool is_character;               // The parent of a character object is a Glyph. The parent of a regular object is a Species.
    glm::vec3 coordinate_vector;     // coordinate vector.
    glm::vec3 rotate_vector;         // rotate vector.
    glm::vec3 translate_vector;      // translate vector.
} ObjectStruct;

typedef struct SpeciesStruct
{
    SpeciesStruct()
        : parent_pointer(nullptr), is_world(false), world_radius(NAN), triangulation_type("bilinear_interpolation")
    {
        // constructor.
    }
    // used for all files (for all species).
    model::Material* parent_pointer;         // pointer to the material object.
    bool is_world;                           // worlds currently do not rotate nor translate.
    double world_radius;                     // radius of sea level in meters. used only for worlds.
    std::string model_file_format;           // type of the model file. supported file formats so far: `"bmp"`/`"BMP"`, `"obj"`/`"OBJ"`.
                                             // TODO: add support for `"SRTM"`.
    std::string model_filename;              // filename of the model file.
    // for `"bmp"` model files.
    std::string color_channel;               // color channel to use for altitude data.

    glm::vec3 light_position;                // light position.
    std::string coordinate_system;           // used only for worlds (`is_world` == `true`). valid values: `"cartesian"`.
                                             // TODO: add support for `"spherical"`. `"spherical"` is used eg. in SRTM heightmaps.
    std::string triangulation_type;
} SpeciesStruct;

#define DEFAULT_VERTEX_SCALING_FACTOR (0.001f)

typedef struct VectorFontStruct
{
    VectorFontStruct()
        : parent_pointer(nullptr), vertex_scaling_factor(DEFAULT_VERTEX_SCALING_FACTOR)
    {
        // constructor.
    }
    // used for all files (for all font).
    model::Material* parent_pointer;        // pointer to the material object.
    GLfloat vertex_scaling_factor;
    std::string font_file_format;           // type of the font file. supported file formats so far: `"svg"`/`"SVG"`.
    std::string font_filename;              // filename of the font file.
} VectorFontStruct;

typedef struct Text3DStruct
{
    Text3DStruct()
        : parent_pointer(nullptr), original_scale_vector(glm::vec3(1.0f, 1.0f, 1.0f)), rotate_angle(NAN)
    {
        // constructor.
    }
    model::VectorFont* parent_pointer; // pointer to the parent `VectorFont`.
    std::string text_string;
    const char* text_string_char;
    glm::vec3 original_scale_vector;   // original scale vector.
    GLfloat rotate_angle;              // rotate angle.
    glm::vec3 coordinate_vector;       // coordinate vector.
    glm::vec3 rotate_vector;           // rotate vector.
    glm::vec3 translate_vector;        // translate vector.
} Text3DStruct;

typedef struct GlyphStruct
{
    GlyphStruct()
        : parent_pointer(nullptr)
    {
        // constructor.
    }
    // used for all files (for all glyph).
    std::vector<std::vector<glm::vec2>>* glyph_vertex_data;
    std::string* glyph_name_pointer;         // we need only a pointer, because glyphs are always created by the `VectorFont` constructor.
    std::string* unicode_string_pointer;     // we need only a pointer, because glyphs are always created by the `VectorFont` constructor.
    model::VectorFont* parent_pointer;       // pointer to the font object.
    glm::vec3 light_position;                // light position.
} GlyphStruct;

typedef struct
{
    GLuint screen_width;
    GLuint screen_height;
    GLuint x;
    GLuint y;
    GLuint text_size;
    GLuint font_size;
    const char* text;
    const char* char_font_texture_file_format;
    const char* horizontal_alignment;
    const char* vertical_alignment;
} PrintingStruct;

typedef struct
{
    double rho;
    double theta;
    double phi;
} SphericalCoordinatesStruct;

typedef struct SphericalWorldStruct
{
    SphericalWorldStruct()
        :SRTM_latitude_step_in_degrees(1.0f/1200.0f), SRTM_longitude_step_in_degrees(1.0f/1200.0f)
    {
        // constructor.
    }
    double southern_latitude;
    double northern_latitude;
    double western_longitude;
    double eastern_longitude;
    double SRTM_latitude_step_in_degrees;
    double SRTM_longitude_step_in_degrees;
} SphericalWorldStruct;

typedef struct TriangulateQuadsStruct
{
    TriangulateQuadsStruct()
        : should_ylikuutio_use_real_texture_coordinates(true)
    {
        // constructor.
    }
    GLuint* input_vertex_pointer;
    uint32_t image_width;
    uint32_t image_height;
    std::string triangulation_type;
    bool should_ylikuutio_use_real_texture_coordinates;
    double sphere_radius;
    SphericalWorldStruct spherical_world_struct;
} TriangulateQuadsStruct;

typedef struct
{
    uint32_t* input_vertex_pointer;
    uint32_t image_width;
    uint32_t image_height;
    bool should_ylikuutio_use_real_texture_coordinates;
} BilinearInterpolationStruct;

typedef struct
{
    uint32_t image_width;
    uint32_t image_height;
    double sphere_radius;
    bool is_bilinear_interpolation_in_use;
    SphericalWorldStruct spherical_world_struct;
} TransformationStruct;

namespace callback_system
{
    class CallbackEngine;
    class CallbackParameter;
}
typedef datatypes::AnyValue* (*InputParametersToAnyValueCallback)(callback_system::CallbackEngine*, std::vector<callback_system::CallbackParameter*>);

extern SphericalCoordinatesStruct spherical_position;

#endif
<|endoftext|>


<commit_before>#include "ws2812b.h"
#include "string.h"

int WS2812B_init(WS2812B *self, int pin, Colour *pixels, int flags, unsigned int columns, unsigned int rows)
{
  if (digitalPinToPort(pin) == NOT_A_PIN) return 1;

  self->port = portOutputRegister(digitalPinToPort(pin));
  self->bit = digitalPinToBitMask(pin);
  self->pin = pin;
  self->count = columns*rows;
  self->pixels = pixels;
  self->flags = flags;
  self->columns = columns;
  self->rows = rows;

  return 0;
}

void WS2812B_sync(WS2812B *self)
{
  // https://cdn-shop.adafruit.com/datasheets/WS2812B.pdf
  volatile uint16_t count = self->count*3;
  uint8_t *ptr = (uint8_t*)self->pixels; // Next byte ptr (initialized to first here)
  uint8_t byte = *ptr++; // Current byte

  uint8_t hi = *self->port |  self->bit; // Cached high state
  uint8_t lo = *self->port & ~self->bit; // Cached low state

  volatile uint8_t next = lo;
  volatile uint8_t bit = 8;

  if (!count) return;

  cli(); // Disable interrupts

  // @TODO: Go through instruction set to see if we can relax the timing.
  // Note that the C code on the side is pseudo-pseudocode.
  // Some of the C code on the side would get optimsed, while some of it
  // can't be optimsed as effectively, which would significantly skew
  // the signal timing a WS281X chip expects. The timing here is on a very
  // tight schedule anyway, so we need to be careful.
  //
  //
  // See footnotes for extended descriptions.
  asm volatile (
    "__colour_bit:          \n\t"   // __colour_bit:
      "st %a[port], %[hi]   \n\t"   //   *port = hi;
      "sbrc %[byte], 7      \n\t\t" //   if (byte & 0x80)
        "mov %[next], %[hi] \n\t"   //     next = hi;
      "dec %[bit]           \n\t"   //   --bit;
      "st %a[port], %[next] \n\t"   //   *port = next;
      "mov %[next], %[lo]   \n\t"   //   next = lo;
      "breq __colour_byte   \n\t"   //   if (bit == 0) goto __colour_byte; // See [0]
      "lsl %[byte]          \n\t"   //   byte = (byte << 1);
      "nop; nop; nop        \n\t"   //   sleep_cycles(3);
      "st %a[port], %[lo]   \n\t"   //   *port = lo;
      "nop; nop; nop        \n\t"   //   sleep_cycles(3);
      "rjmp __colour_bit    \n"     //   goto __colour_bit;
                                    //
    "__colour_byte:         \n\t"   // __colour_byte:
      "ldi %[bit], 8        \n\t"   //   bit = 8;
      "ld %[byte], %a[ptr]+ \n\t"   //   byte = *ptr++;
      "st %a[port], %[lo]   \n\t"   //   *port = lo;
      "nop                  \n\t"   //   sleep_cycles(1);
      "sbiw %[count], 1     \n\t"   //   --count;
      "brne __colour_bit    \n"     //   if (bit != 0) goto __colour_bit; // See [1]

      // The benefit of extended ASM is that we can define registers and
      // operands elegantly:
    : [port]  "+e" (self->port),
      [count] "+w" (count),
      [bit]   "+r" (bit),
      [byte]  "+r" (byte),
      [next]  "+r" (next)
    : [ptr]    "e" (ptr),
      [hi]     "r" (hi),
      [lo]     "r" (lo)
  );
  // Enable interrupts
  sei();

  // Wait for *at least* 50µs; this causes the chip to latch the voltage onto
  // the individual LEDs contained within the RGB component.
  _delay_us(51);

  /**
   * # 1. `breq __colour_byte`
   * What this effectively does is test whether the Z flag in SREG register is
   * set to `1`, and if so, jumps to the defined symbol (`__colour_byte`).
   *
   * The Z flag (Zero flag) gets set when the previous arithmetic expression
   * evaluated to `0`. In our case, the last arithmetic expression was the
   * subtraction `dec %[bit]` (`--bit`).
   *
   * # 2. `brne __colour_bit`
   * This is similar to `breq`, but tests whether the Z flag in SREG is *not*
   * `1` before jumping. The flag gets cleared after the prevous arithmetic
   * expression evaluates to anything but `0`.
   *
   * ---
   * PS: "expression" is used as a synonym for "instruction"
   *
   * More detailed documentation can be found [here][doclink].
   *
   * [doclink]: http://www.atmel.com/webdoc/avrassembler/avrassembler.wb_instruction_list.html
   */
}

int WS2812B_init_palette(WS2812B *self, ColourPalette* palette, unsigned int size)
{
  self->palette_size = size;
  self->palette_used = 0;
  self->palette = palette;

  return 0;
}

int WS2812B_add_colour(WS2812B *self, char id, Colour c)
{
  if (self->palette_used >= self->palette_size) return 1;
  self->palette[self->palette_used++] = {id, c};

  return 0;
}

int WS2812B_set_pixel(WS2812B *self, unsigned int x, unsigned int y, Colour c)
{
  return _WS2812B_spf(self, x, y, _WS2812B_cc(c, self->flags & WS2812B_SCHEMES));
}

int WS2812B_set_pixel_at_offset(WS2812B *self, uint16_t offset, Colour c)
{
  if (offset >= self->count) return 1;
  self->pixels[offset] = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);
  return 0;
}

int WS2812B_set_all(WS2812B *self, Colour c)
{
  Colour _c = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);
  uint16_t count = self->count;

  while (count) self->pixels[--count] = _c;

  return 0;
}

Colour WS2812B_get_pixel(WS2812B *self, unsigned int x, unsigned int y)
{
  uint16_t offset = 0;
  if (x >= self->columns || y >= self->rows) return {0,0,0};

  if (self->flags & WS2812B_ZIGZAG_ODD && y % 2 != 0)
    x = self->columns - x - 1;

  if (self->flags & WS2812B_ZIGZAG_EVEN && y % 2 == 0)
    x = self->columns - x - 1;

  if(_WS2812B_addr(x, y, self->columns, &offset)) return {0,0,0};
  return self->pixels[offset];
}

int WS2812B_set_row(WS2812B *self, unsigned int row, Colour c)
{
  if (row >= self->rows) return 1;
  Colour _c = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);

  for (unsigned int i = 0; i < self->columns; i++)
    _WS2812B_spf(self, i, row, _c);

  return 0;
}

int WS2812B_set_column(WS2812B *self, unsigned int col, Colour c)
{
  if (col >= self->columns) return 1;
  Colour _c = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);

  for (unsigned int i = 0; i < self->rows; i++)
    _WS2812B_spf(self, col, i, _c);

  return 0;
}

int WS2812B_render(WS2812B *self, const char *ascii_art)
{
  size_t count = MIN(strlen(ascii_art), self->count);

  for (size_t i = 0; i < count; ++i) {
    unsigned int x, y;
    x = _WS2812B_x(i, self->columns);
    y = _WS2812B_y(i, self->columns);

    WS2812B_set_pixel(self, x, y, _WS2812B_lupc(self, ascii_art[i]));
  }

  return self->count - strlen(ascii_art);
}

/* Helper functions */
int _WS2812B_addr(unsigned int x, unsigned int y, unsigned int c, unsigned int *addr)
{
  if (x >= c) return 1;
  *addr = c * y + x;

  return 0;
}

int _WS2812B_spf(WS2812B *self, unsigned int x, unsigned int y, Colour c)
{
  uint16_t offset = 0;
  if (x >= self->columns || y >= self->rows) return 1;

  if (self->flags & WS2812B_ZIGZAG_ODD && y % 2 != 0)
    x = self->columns - x - 1;

  if (self->flags & WS2812B_ZIGZAG_EVEN && y % 2 == 0)
    x = self->columns - x - 1;

  if(_WS2812B_addr(x, y, self->columns, &offset)) return 1;

  self->pixels[offset] = c;
  return 0;
}

unsigned int _WS2812B_x(unsigned int addr, unsigned int c) {
  return addr % c;
}

unsigned int _WS2812B_y(unsigned int addr, unsigned int c) {
  return addr / c;
}

Colour _WS2812B_lupc(WS2812B *self, char id)
{
  for (size_t i = 0; i < self->palette_used; ++i) {
    ColourPalette p = self->palette[i];
    if (p.id == id) return p.col;
  }

  return {0,0,0};
}

Colour _WS2812B_cc(Colour c, int order)
{
  switch (order) {
    case WS2812B_RBG:  return {c.red, c.blue, c.green};
    case WS2812B_GRB:  return {c.green, c.red, c.blue};
    case WS2812B_GBR:  return {c.green, c.blue, c.red};
    case WS2812B_BRG:  return {c.blue, c.red, c.green};
    case WS2812B_BGR:  return {c.blue, c.green, c.red};
    default /* RGB */: return c;
  }
}
<commit_msg>Add support for redefining colours<commit_after>#include "ws2812b.h"
#include "string.h"

int WS2812B_init(WS2812B *self, int pin, Colour *pixels, int flags, unsigned int columns, unsigned int rows)
{
  if (digitalPinToPort(pin) == NOT_A_PIN) return 1;

  self->port = portOutputRegister(digitalPinToPort(pin));
  self->bit = digitalPinToBitMask(pin);
  self->pin = pin;
  self->count = columns*rows;
  self->pixels = pixels;
  self->flags = flags;
  self->columns = columns;
  self->rows = rows;

  return 0;
}

void WS2812B_sync(WS2812B *self)
{
  // https://cdn-shop.adafruit.com/datasheets/WS2812B.pdf
  volatile uint16_t count = self->count*3;
  uint8_t *ptr = (uint8_t*)self->pixels; // Next byte ptr (initialized to first here)
  uint8_t byte = *ptr++; // Current byte

  uint8_t hi = *self->port |  self->bit; // Cached high state
  uint8_t lo = *self->port & ~self->bit; // Cached low state

  volatile uint8_t next = lo;
  volatile uint8_t bit = 8;

  if (!count) return;

  cli(); // Disable interrupts

  // @TODO: Go through instruction set to see if we can relax the timing.
  // Note that the C code on the side is pseudo-pseudocode.
  // Some of the C code on the side would get optimsed, while some of it
  // can't be optimsed as effectively, which would significantly skew
  // the signal timing a WS281X chip expects. The timing here is on a very
  // tight schedule anyway, so we need to be careful.
  //
  //
  // See footnotes for extended descriptions.
  asm volatile (
    "__colour_bit:          \n\t"   // __colour_bit:
      "st %a[port], %[hi]   \n\t"   //   *port = hi;
      "sbrc %[byte], 7      \n\t\t" //   if (byte & 0x80)
        "mov %[next], %[hi] \n\t"   //     next = hi;
      "dec %[bit]           \n\t"   //   --bit;
      "st %a[port], %[next] \n\t"   //   *port = next;
      "mov %[next], %[lo]   \n\t"   //   next = lo;
      "breq __colour_byte   \n\t"   //   if (bit == 0) goto __colour_byte; // See [0]
      "lsl %[byte]          \n\t"   //   byte = (byte << 1);
      "nop; nop; nop        \n\t"   //   sleep_cycles(3);
      "st %a[port], %[lo]   \n\t"   //   *port = lo;
      "nop; nop; nop        \n\t"   //   sleep_cycles(3);
      "rjmp __colour_bit    \n"     //   goto __colour_bit;
                                    //
    "__colour_byte:         \n\t"   // __colour_byte:
      "ldi %[bit], 8        \n\t"   //   bit = 8;
      "ld %[byte], %a[ptr]+ \n\t"   //   byte = *ptr++;
      "st %a[port], %[lo]   \n\t"   //   *port = lo;
      "nop                  \n\t"   //   sleep_cycles(1);
      "sbiw %[count], 1     \n\t"   //   --count;
      "brne __colour_bit    \n"     //   if (bit != 0) goto __colour_bit; // See [1]

      // The benefit of extended ASM is that we can define registers and
      // operands elegantly:
    : [port]  "+e" (self->port),
      [count] "+w" (count),
      [bit]   "+r" (bit),
      [byte]  "+r" (byte),
      [next]  "+r" (next)
    : [ptr]    "e" (ptr),
      [hi]     "r" (hi),
      [lo]     "r" (lo)
  );
  // Enable interrupts
  sei();

  // Wait for *at least* 50µs; this causes the chip to latch the voltage onto
  // the individual LEDs contained within the RGB component.
  _delay_us(51);

  /**
   * # 1. `breq __colour_byte`
   * What this effectively does is test whether the Z flag in SREG register is
   * set to `1`, and if so, jumps to the defined symbol (`__colour_byte`).
   *
   * The Z flag (Zero flag) gets set when the previous arithmetic expression
   * evaluated to `0`. In our case, the last arithmetic expression was the
   * subtraction `dec %[bit]` (`--bit`).
   *
   * # 2. `brne __colour_bit`
   * This is similar to `breq`, but tests whether the Z flag in SREG is *not*
   * `1` before jumping. The flag gets cleared after the prevous arithmetic
   * expression evaluates to anything but `0`.
   *
   * ---
   * PS: "expression" is used as a synonym for "instruction"
   *
   * More detailed documentation can be found [here][doclink].
   *
   * [doclink]: http://www.atmel.com/webdoc/avrassembler/avrassembler.wb_instruction_list.html
   */
}

int WS2812B_init_palette(WS2812B *self, ColourPalette* palette, unsigned int size)
{
  self->palette_size = size;
  self->palette_used = 0;
  self->palette = palette;

  return 0;
}

int WS2812B_add_colour(WS2812B *self, char id, Colour c)
{
  if (self->palette_used >= self->palette_size) return 1;

  for (size_t i = 0; i < self->palette_used; ++i) {
    if (self->palette[i].id == id) {
      self->palette[i].c = c;
      return 0;
    }
  }

  self->palette[self->palette_used++] = {id, c};
  return 0;
}

int WS2812B_set_pixel(WS2812B *self, unsigned int x, unsigned int y, Colour c)
{
  return _WS2812B_spf(self, x, y, _WS2812B_cc(c, self->flags & WS2812B_SCHEMES));
}

int WS2812B_set_pixel_at_offset(WS2812B *self, uint16_t offset, Colour c)
{
  if (offset >= self->count) return 1;
  self->pixels[offset] = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);
  return 0;
}

int WS2812B_set_all(WS2812B *self, Colour c)
{
  Colour _c = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);
  uint16_t count = self->count;

  while (count) self->pixels[--count] = _c;

  return 0;
}

Colour WS2812B_get_pixel(WS2812B *self, unsigned int x, unsigned int y)
{
  uint16_t offset = 0;
  if (x >= self->columns || y >= self->rows) return {0,0,0};

  if (self->flags & WS2812B_ZIGZAG_ODD && y % 2 != 0)
    x = self->columns - x - 1;

  if (self->flags & WS2812B_ZIGZAG_EVEN && y % 2 == 0)
    x = self->columns - x - 1;

  if(_WS2812B_addr(x, y, self->columns, &offset)) return {0,0,0};
  return self->pixels[offset];
}

int WS2812B_set_row(WS2812B *self, unsigned int row, Colour c)
{
  if (row >= self->rows) return 1;
  Colour _c = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);

  for (unsigned int i = 0; i < self->columns; i++)
    _WS2812B_spf(self, i, row, _c);

  return 0;
}

int WS2812B_set_column(WS2812B *self, unsigned int col, Colour c)
{
  if (col >= self->columns) return 1;
  Colour _c = _WS2812B_cc(c, self->flags & WS2812B_SCHEMES);

  for (unsigned int i = 0; i < self->rows; i++)
    _WS2812B_spf(self, col, i, _c);

  return 0;
}

int WS2812B_render(WS2812B *self, const char *ascii_art)
{
  size_t count = MIN(strlen(ascii_art), self->count);

  for (size_t i = 0; i < count; ++i) {
    unsigned int x, y;
    x = _WS2812B_x(i, self->columns);
    y = _WS2812B_y(i, self->columns);

    WS2812B_set_pixel(self, x, y, _WS2812B_lupc(self, ascii_art[i]));
  }

  return self->count - strlen(ascii_art);
}

/* Helper functions */
int _WS2812B_addr(unsigned int x, unsigned int y, unsigned int c, unsigned int *addr)
{
  if (x >= c) return 1;
  *addr = c * y + x;

  return 0;
}

int _WS2812B_spf(WS2812B *self, unsigned int x, unsigned int y, Colour c)
{
  uint16_t offset = 0;
  if (x >= self->columns || y >= self->rows) return 1;

  if (self->flags & WS2812B_ZIGZAG_ODD && y % 2 != 0)
    x = self->columns - x - 1;

  if (self->flags & WS2812B_ZIGZAG_EVEN && y % 2 == 0)
    x = self->columns - x - 1;

  if(_WS2812B_addr(x, y, self->columns, &offset)) return 1;

  self->pixels[offset] = c;
  return 0;
}

unsigned int _WS2812B_x(unsigned int addr, unsigned int c) {
  return addr % c;
}

unsigned int _WS2812B_y(unsigned int addr, unsigned int c) {
  return addr / c;
}

Colour _WS2812B_lupc(WS2812B *self, char id)
{
  for (size_t i = 0; i < self->palette_used; ++i) {
    ColourPalette p = self->palette[i];
    if (p.id == id) return p.col;
  }

  return {0,0,0};
}

Colour _WS2812B_cc(Colour c, int order)
{
  switch (order) {
    case WS2812B_RBG:  return {c.red, c.blue, c.green};
    case WS2812B_GRB:  return {c.green, c.red, c.blue};
    case WS2812B_GBR:  return {c.green, c.blue, c.red};
    case WS2812B_BRG:  return {c.blue, c.red, c.green};
    case WS2812B_BGR:  return {c.blue, c.green, c.red};
    default /* RGB */: return c;
  }
}
<|endoftext|>


<commit_before>/*
 * File:  PPAxisAncestor.cpp
 * Copyright (C) 2004 The Institute for System Programming of the Russian Academy of Sciences (ISP RAS)
 */

#include "common/sedna.h"

#include "tr/executor/xqops/PPAxisAncestor.h"
#include "tr/crmutils/node_utils.h"
#include "tr/executor/base/PPUtils.h"
#include "tr/executor/base/dm_accessors.h"

void PPAxisAncestor::init_function()
{
	switch (nt_type)
    {
        case node_test_processing_instruction	: next_fun = &PPAxisAncestor::next_processing_instruction; break;
        case node_test_comment					: next_fun = &PPAxisAncestor::next_comment; break;
        case node_test_text						: next_fun = &PPAxisAncestor::next_text; break;
        case node_test_node						: next_fun = &PPAxisAncestor::next_node; break;
        case node_test_string					: next_fun = &PPAxisAncestor::next_string; break;
        case node_test_qname					: next_fun = &PPAxisAncestor::next_qname; break;
        case node_test_wildcard_star			: next_fun = &PPAxisAncestor::next_wildcard_star; break;
        case node_test_wildcard_ncname_star		: next_fun = &PPAxisAncestor::next_wildcard_ncname_star; break;
        case node_test_wildcard_star_ncname		: next_fun = &PPAxisAncestor::next_wildcard_star_ncname; break;
        case node_test_function_call			: next_fun = &PPAxisAncestor::next_function_call; break;
        case node_test_var_name					: next_fun = &PPAxisAncestor::next_var_name; break;
        default									: throw USER_EXCEPTION2(SE1003, "Unexpected node test");
    }
}
PPAxisAncestor::PPAxisAncestor(dynamic_context *_cxt_,
                         PPOpIn _child_,
                         NodeTestType _nt_type_,
						 NodeTestData _nt_data_):PPIterator(_cxt_),
                                                   child(_child_),
                                                   nt_type(_nt_type_),
                                                   nt_data(_nt_data_)
{
	self=false;
	init_function();
}
PPAxisAncestor::PPAxisAncestor(dynamic_context *_cxt_,
                         PPOpIn _child_,
                         NodeTestType _nt_type_,
                         NodeTestData _nt_data_,bool _self_) : PPIterator(_cxt_),
                                                   child(_child_),
                                                   nt_type(_nt_type_),
                                                   nt_data(_nt_data_),
												   self(_self_)
{
    init_function();
}

PPAxisAncestor::~PPAxisAncestor()
{
    delete child.op;
    child.op = NULL;
}

void PPAxisAncestor::open  ()
{
    child.op->open();

    cur = XNULL;
}

void PPAxisAncestor::reopen()
{
    child.op->reopen();

    cur = XNULL;
}

void PPAxisAncestor::close ()
{
    child.op->close();
}
PPIterator* PPAxisAncestor::copy(dynamic_context *_cxt_)
{
    PPAxisAncestor *res = self ? se_new PPAxisAncestor(_cxt_, child, nt_type, nt_data, true) :
                                 se_new PPAxisAncestor(_cxt_, child, nt_type, nt_data);
    res->child.op = child.op->copy(_cxt_);
    res->set_xquery_line(__xquery_line);
    return res;
}

bool PPAxisAncestor::result(PPIterator* cur, dynamic_context *cxt, void*& r)
{
	return true;
}

PPAxisAncestorOrSelf::PPAxisAncestorOrSelf(dynamic_context *_cxt_,
                         PPOpIn _child_,
                         NodeTestType _nt_type_,
						 NodeTestData _nt_data_):PPAxisAncestor(_cxt_, _child_, _nt_type_, _nt_data_,true)
{

}

void PPAxisAncestor::next_processing_instruction(tuple &t)
{
    while (true)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}
        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);
    }
}

void PPAxisAncestor::next_comment(tuple &t)
{
    while (true)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}
        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);
    }
}

void PPAxisAncestor::next_text(tuple &t)
{
    while (true)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}
        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);
    }
}

void PPAxisAncestor::next_node(tuple &t)
{
    while (cur == XNULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
		if (!self) cur = get_parent_node(cur);
		if (cur!=XNULL)
		{
			CHECKP(cur);
			if (GETSCHEMENODEX(cur)->type==virtual_root)
				cur=XNULL;
		}
    }

    t.copy(tuple_cell::node(cur));
    cur = get_parent_node(cur);
}
void PPAxisAncestor::next_string(tuple &t)
{
    throw USER_EXCEPTION2(SE1002, "PPAxisAncestor::next_string");
}
void PPAxisAncestor::next_qname(tuple &t)
{
    while (cur == XNULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
        if (!self) cur = get_parent_node(cur);
        while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_qname_type(GETSCHEMENODEX(cur),
                                nt_data.uri,
                                nt_data.ncname_local,
                                element))
                break;
			cur = get_parent_node(cur);
		}
    }
    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
	while (cur!=XNULL)
	{
		CHECKP(cur);
		if (comp_qname_type(GETSCHEMENODEX(cur),
                              nt_data.uri,
                              nt_data.ncname_local,
                              element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}

}

void PPAxisAncestor::next_wildcard_star(tuple &t)
{
    while (cur == NULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
		if (!self) cur = get_parent_node(cur);
		while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_type(GETSCHEMENODEX(cur),
                        NULL,
                        NULL,
                        element))
							  break;
			cur = get_parent_node(cur);
		}
    }

    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
    while (cur!=XNULL)
	{
		CHECKP(cur);
		if ( comp_type(GETSCHEMENODEX(cur),
                        NULL,
                        NULL,
                        element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}
}

void PPAxisAncestor::next_wildcard_ncname_star(tuple &t)
{
    while (cur == NULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
        if (!self) cur = get_parent_node(cur);

        while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_type(GETSCHEMENODEX(cur),
                        nt_data.uri,
                        NULL,
                        element))
							  break;
			cur = get_parent_node(cur);
		}
    }

    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
    while (cur!=XNULL)
	{
		CHECKP(cur);
		if (comp_type(GETSCHEMENODEX(cur),
                        nt_data.uri,
                        NULL,
                        element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}
}

void PPAxisAncestor::next_wildcard_star_ncname(tuple &t)
{
    while (cur == NULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
        if (!self) cur = get_parent_node(cur);
		while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_local_type(GETSCHEMENODEX(cur),
                              NULL,
                              nt_data.ncname_local,
                              element))
							  break;
			cur = get_parent_node(cur);
		}
    }

    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
    while (true)
	{
		CHECKP(cur);
		if (comp_local_type(GETSCHEMENODEX(cur),
                              NULL,
                              nt_data.ncname_local,
                              element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}
}

void PPAxisAncestor::next_function_call(tuple &t)
{
    throw USER_EXCEPTION2(SE1002, "PPAxisAncestor::next_function_call");
}

void PPAxisAncestor::next_var_name(tuple &t)
{
    throw USER_EXCEPTION2(SE1002, "PPAxisAncestor::next_var_name");
}
<commit_msg>ancestor or self fix<commit_after>/*
 * File:  PPAxisAncestor.cpp
 * Copyright (C) 2004 The Institute for System Programming of the Russian Academy of Sciences (ISP RAS)
 */

#include "common/sedna.h"

#include "tr/executor/xqops/PPAxisAncestor.h"
#include "tr/crmutils/node_utils.h"
#include "tr/executor/base/PPUtils.h"
#include "tr/executor/base/dm_accessors.h"

void PPAxisAncestor::init_function()
{
	switch (nt_type)
    {
        case node_test_processing_instruction	: next_fun = &PPAxisAncestor::next_processing_instruction; break;
        case node_test_comment					: next_fun = &PPAxisAncestor::next_comment; break;
        case node_test_text						: next_fun = &PPAxisAncestor::next_text; break;
        case node_test_node						: next_fun = &PPAxisAncestor::next_node; break;
        case node_test_string					: next_fun = &PPAxisAncestor::next_string; break;
        case node_test_qname					: next_fun = &PPAxisAncestor::next_qname; break;
        case node_test_wildcard_star			: next_fun = &PPAxisAncestor::next_wildcard_star; break;
        case node_test_wildcard_ncname_star		: next_fun = &PPAxisAncestor::next_wildcard_ncname_star; break;
        case node_test_wildcard_star_ncname		: next_fun = &PPAxisAncestor::next_wildcard_star_ncname; break;
        case node_test_function_call			: next_fun = &PPAxisAncestor::next_function_call; break;
        case node_test_var_name					: next_fun = &PPAxisAncestor::next_var_name; break;
        default									: throw USER_EXCEPTION2(SE1003, "Unexpected node test");
    }
}
PPAxisAncestor::PPAxisAncestor(dynamic_context *_cxt_,
                         PPOpIn _child_,
                         NodeTestType _nt_type_,
						 NodeTestData _nt_data_):PPIterator(_cxt_),
                                                   child(_child_),
                                                   nt_type(_nt_type_),
                                                   nt_data(_nt_data_)
{
	self=false;
	init_function();
}
PPAxisAncestor::PPAxisAncestor(dynamic_context *_cxt_,
                         PPOpIn _child_,
                         NodeTestType _nt_type_,
                         NodeTestData _nt_data_,bool _self_) : PPIterator(_cxt_),
                                                   child(_child_),
                                                   nt_type(_nt_type_),
                                                   nt_data(_nt_data_),
												   self(_self_)
{
    init_function();
}

PPAxisAncestor::~PPAxisAncestor()
{
    delete child.op;
    child.op = NULL;
}

void PPAxisAncestor::open  ()
{
    child.op->open();

    cur = XNULL;
}

void PPAxisAncestor::reopen()
{
    child.op->reopen();

    cur = XNULL;
}

void PPAxisAncestor::close ()
{
    child.op->close();
}
PPIterator* PPAxisAncestor::copy(dynamic_context *_cxt_)
{
    PPAxisAncestor *res = self ? se_new PPAxisAncestor(_cxt_, child, nt_type, nt_data, true) :
                                 se_new PPAxisAncestor(_cxt_, child, nt_type, nt_data);
    res->child.op = child.op->copy(_cxt_);
    res->set_xquery_line(__xquery_line);
    return res;
}

bool PPAxisAncestor::result(PPIterator* cur, dynamic_context *cxt, void*& r)
{
	return true;
}

PPAxisAncestorOrSelf::PPAxisAncestorOrSelf(dynamic_context *_cxt_,
                         PPOpIn _child_,
                         NodeTestType _nt_type_,
						 NodeTestData _nt_data_):PPAxisAncestor(_cxt_, _child_, _nt_type_, _nt_data_,true)
{

}

void PPAxisAncestor::next_processing_instruction(tuple &t)
{
    while (true)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}
        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);
    }
}

void PPAxisAncestor::next_comment(tuple &t)
{
    while (true)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}
        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);
    }
}

void PPAxisAncestor::next_text(tuple &t)
{
    while (true)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}
        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);
    }
}

void PPAxisAncestor::next_node(tuple &t)
{
    while (cur == XNULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
		if (!self) cur = get_parent_node(cur);
		if (cur!=XNULL)
		{
			CHECKP(cur);
			if (GETSCHEMENODEX(cur)->type==virtual_root)
				cur=XNULL;
		}
    }

    t.copy(tuple_cell::node(cur));
    cur = get_parent_node(cur);
	if (GETSCHEMENODEX(cur)->type==virtual_root)
				cur=XNULL;
}
void PPAxisAncestor::next_string(tuple &t)
{
    throw USER_EXCEPTION2(SE1002, "PPAxisAncestor::next_string");
}
void PPAxisAncestor::next_qname(tuple &t)
{
    while (cur == XNULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
        if (!self) cur = get_parent_node(cur);
        while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_qname_type(GETSCHEMENODEX(cur),
                                nt_data.uri,
                                nt_data.ncname_local,
                                element))
                break;
			cur = get_parent_node(cur);
		}
    }
    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
	while (cur!=XNULL)
	{
		CHECKP(cur);
		if (comp_qname_type(GETSCHEMENODEX(cur),
                              nt_data.uri,
                              nt_data.ncname_local,
                              element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}

}

void PPAxisAncestor::next_wildcard_star(tuple &t)
{
    while (cur == NULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
		if (!self) cur = get_parent_node(cur);
		while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_type(GETSCHEMENODEX(cur),
                        NULL,
                        NULL,
                        element))
							  break;
			cur = get_parent_node(cur);
		}
    }

    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
    while (cur!=XNULL)
	{
		CHECKP(cur);
		if ( comp_type(GETSCHEMENODEX(cur),
                        NULL,
                        NULL,
                        element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}
}

void PPAxisAncestor::next_wildcard_ncname_star(tuple &t)
{
    while (cur == NULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
        if (!self) cur = get_parent_node(cur);

        while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_type(GETSCHEMENODEX(cur),
                        nt_data.uri,
                        NULL,
                        element))
							  break;
			cur = get_parent_node(cur);
		}
    }

    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
    while (cur!=XNULL)
	{
		CHECKP(cur);
		if (comp_type(GETSCHEMENODEX(cur),
                        nt_data.uri,
                        NULL,
                        element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}
}

void PPAxisAncestor::next_wildcard_star_ncname(tuple &t)
{
    while (cur == NULL)
    {
        child.op->next(t);
        if (t.is_eos()) {RESTORE_CURRENT_PP; return;}

        if (!(child.get(t).is_node())) throw XQUERY_EXCEPTION(XPTY0020);

        cur = child.get(t).get_node();
        if (!self) cur = get_parent_node(cur);
		while (cur!=XNULL)
		{
			CHECKP(cur);
			if (comp_local_type(GETSCHEMENODEX(cur),
                              NULL,
                              nt_data.ncname_local,
                              element))
							  break;
			cur = get_parent_node(cur);
		}
    }

    t.copy(tuple_cell::node(cur));
	cur = get_parent_node(cur);
    while (true)
	{
		CHECKP(cur);
		if (comp_local_type(GETSCHEMENODEX(cur),
                              NULL,
                              nt_data.ncname_local,
                              element))
							  {RESTORE_CURRENT_PP; return;}
		cur = get_parent_node(cur);
	}
}

void PPAxisAncestor::next_function_call(tuple &t)
{
    throw USER_EXCEPTION2(SE1002, "PPAxisAncestor::next_function_call");
}

void PPAxisAncestor::next_var_name(tuple &t)
{
    throw USER_EXCEPTION2(SE1002, "PPAxisAncestor::next_var_name");
}
<|endoftext|>