<commit_before>
#include "Engine.h"
#include "GLSLGenerator.h"
#include "HLSLParser.h"
#include "HLSLTree.h"

#include <fstream>
#include <sstream>
#include <iostream>

M4::Allocator allocator;

std::string ReadFile(const char* fileName)
{
    std::ifstream ifs(fileName);
    std::stringstream buffer;
    buffer << ifs.rdbuf();
    return buffer.str();
}

void PrintUsage()
{
    std::cerr << "usage: hlslparser [-h] [-fs | -vs] FILENAME ENTRYNAME\n"
              << "\n"
              << "Translate HLSL shader to GLSL shader.\n"
              << "\n"
              << "positional arguments:\n"
              << " FILENAME    input file name\n"
              << " ENTRYNAME   entry point of the shader\n"
              << "\n"
              << "optional arguments:\n"
              << " -h, --help  show this help message and exit\n"
              << " -fs         generate fragment shader (default)\n"
              << " -vs         generate vertex shader\n";
}

int main(int argc, char* argv[])
{
    using namespace M4;

    // Parse arguments
    const char* fileName = NULL;
    const char* entryName = NULL;
    GLSLGenerator::Target target = GLSLGenerator::Target_FragmentShader;

    for (int argn = 1; argn < argc; ++argn)
    {
        const char* const arg = argv[argn];

        if (String_Equal(arg, "-h") || String_Equal(arg, "--help"))
        {
            PrintUsage();
            return 0;
        }
        else if (String_Equal(arg, "-fs"))
        {
           target = GLSLGenerator::Target_FragmentShader;
        }
        else if (String_Equal(arg, "-vs"))
        {
            target = GLSLGenerator::Target_VertexShader;
        }
        else if (fileName == NULL)
        {
            fileName = arg;
        }
        else if (entryName == NULL)
        {
            entryName = arg;
        }
        else
        {
            Log_Error("Too many arguments");
            PrintUsage();
            return 1;
        }
    }

    if (fileName == NULL || entryName == NULL)
    {
        Log_Error("Missing arguments");
        PrintUsage();
        return 1;
    }

    // Read input file
    const std::string source = ReadFile(fileName);

    // Parse input file
    Allocator allocator;
    HLSLParser parser(&allocator, fileName, source.data(), source.size());
    HLSLTree tree(&allocator);
    if (!parser.Parse(&tree))
    {
        Log_Error("Parsing failed, aborting");
        return 1;
    }

    // Generate output
    GLSLGenerator generator(&allocator);
    generator.Generate(&tree, target, entryName);
    std::cout << generator.GetResult();

    return 0;
}

<commit_msg>Initial extern function for emscripten to be called by JS<commit_after>
#include "Engine.h"
#include "GLSLGenerator.h"
#include "HLSLParser.h"
#include "HLSLTree.h"

/*
TODO
- type
- filename
- file contents
- entry name

parseHLSL2GLSL
*/

extern "C" {

    int parse(char* source, char* fileName, char* entryName)
    {
        using namespace M4;

        Log_Error(source);
        Log_Error("..fileName...\n");

        GLSLGenerator::Target target = GLSLGenerator::Target_FragmentShader;

        // Parse input file
        Allocator allocator;
        HLSLParser parser(&allocator, fileName, source, strlen(source));
        HLSLTree tree(&allocator);
        if (!parser.Parse(&tree))
        {
            Log_Error("Parsing failed, aborting");
            return 1;
        }

        // Generate output
        GLSLGenerator generator(&allocator);
        if (!generator.Generate(&tree, target, entryName)) {
            Log_Error("Generation error.\n");
            return EXIT_FAILURE;
        }
        Log_Error(generator.GetResult());

        return 0;
    }

}

<|endoftext|>


<commit_before>/*
 * Copyright (c) 2013, Ford Motor Company
 * 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 the Ford Motor Company nor the names of its contributors
 * may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "application_manager/message.h"

namespace {
bool BinaryDataPredicate(uint8_t i, uint8_t j) {
  return (i == j);
}
}

namespace application_manager {

MessageType MessageTypeFromRpcType(protocol_handler::RpcType rpc_type) {
  switch (rpc_type) {
    case protocol_handler::kRpcTypeRequest:
      return kRequest;
    case protocol_handler::kRpcTypeResponse:
      return kResponse;
    case protocol_handler::kRpcTypeNotification:
      return kNotification;
    case protocol_handler::kRpcTypeReserved:
    default:
      DCHECK(false);
      return kUnknownType;
  }
}

Message::Message(protocol_handler::MessagePriority priority)
    : function_id_(0),
      correlation_id_(0),
      type_(kUnknownType),
      priority_(priority),
      connection_key_(0),
      binary_data_(NULL),
      data_size_(0),
      payload_size_(0),
      version_(kUnknownProtocol) {
}

Message::Message(const Message& message)
    : priority_(message.priority_) {
  *this = message;
}

Message& Message::operator=(const Message& message) {
  set_function_id(message.function_id_);
  set_correlation_id(message.correlation_id_);
  set_connection_key(message.connection_key_);
  set_message_type(message.type_);
  set_data_size(message.data_size_);
  set_payload_size(message.payload_size_);
  if (message.binary_data_) {
    set_binary_data(message.binary_data_);
  }
  set_json_message(message.json_message_);
  set_protocol_version(message.protocol_version());
  priority_ = message.priority_;

  return *this;
}

bool Message::operator==(const Message& message) {
  bool function_id = function_id_ == message.function_id_;
  bool correlation_id = correlation_id_ == message.correlation_id_;
  bool connection_key = connection_key_ == message.connection_key_;
  bool type = type_ == message.type_;
  bool json_message = json_message_ == message.json_message_;
  bool version = version_ == message.version_;
  bool data_size = data_size_ == message.data_size_;
  bool payload_size = payload_size_ == message.payload_size_;

  bool binary_data = std::equal(binary_data_->begin(), binary_data_->end(),
                                message.binary_data_->begin(),
                                BinaryDataPredicate);

  return function_id && correlation_id && connection_key && type && binary_data
      && json_message && version && data_size && payload_size;
}

Message::~Message() {
  if (binary_data_) {
    delete binary_data_;
  }
}

int32_t Message::function_id() const {
  return function_id_;
}

int32_t Message::correlation_id() const {
  return correlation_id_;
}

int32_t Message::connection_key() const {
  return connection_key_;
}

MessageType Message::type() const {
  return type_;
}

ProtocolVersion Message::protocol_version() const {
  return version_;
}

const std::string& Message::json_message() const {
  return json_message_;
}

const BinaryData* Message::binary_data() const {
  return binary_data_;
}

bool Message::has_binary_data() const {
  return (binary_data_ != NULL);
}

size_t Message::data_size() const {
  return data_size_;
}

size_t Message::payload_size() const {
  return payload_size_;
}

void Message::set_function_id(int32_t id) {
  function_id_ = id;
}

void Message::set_correlation_id(int32_t id) {
  correlation_id_ = id;
}

void Message::set_connection_key(int32_t key) {
  connection_key_ = key;
}

void Message::set_message_type(MessageType type) {
  type_ = type;
}

void Message::set_binary_data(BinaryData* data) {
  if (NULL == data) {
    NOTREACHED();
    return;
  }

  if (binary_data_) {
    delete binary_data_;
  }

  binary_data_ = data;
}

void Message::set_json_message(const std::string& json_message) {
  json_message_ = json_message;
}

void Message::set_protocol_version(ProtocolVersion version) {
  version_ = version;
}

const smart_objects::SmartObject &Message::smart_object() const {
  return smart_object_;
}

void Message::set_smart_object(const smart_objects::SmartObject& object) {
  smart_object_ = object;
}

void Message::set_data_size(size_t data_size) {
  data_size_ = data_size;
}

void Message::set_payload_size(size_t payload_size) {
  payload_size_ = payload_size;
}
}  // namespace application_manager
<commit_msg>Initialize binary_data_ to NULL (CID 80088)<commit_after>/*
 * Copyright (c) 2013, Ford Motor Company
 * 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 the Ford Motor Company nor the names of its contributors
 * may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "application_manager/message.h"

namespace {
bool BinaryDataPredicate(uint8_t i, uint8_t j) {
  return (i == j);
}
}

namespace application_manager {

MessageType MessageTypeFromRpcType(protocol_handler::RpcType rpc_type) {
  switch (rpc_type) {
    case protocol_handler::kRpcTypeRequest:
      return kRequest;
    case protocol_handler::kRpcTypeResponse:
      return kResponse;
    case protocol_handler::kRpcTypeNotification:
      return kNotification;
    case protocol_handler::kRpcTypeReserved:
    default:
      DCHECK(false);
      return kUnknownType;
  }
}

Message::Message(protocol_handler::MessagePriority priority)
    : function_id_(0),
      correlation_id_(0),
      type_(kUnknownType),
      priority_(priority),
      connection_key_(0),
      binary_data_(NULL),
      data_size_(0),
      payload_size_(0),
      version_(kUnknownProtocol) {
}

Message::Message(const Message& message)
    : priority_(message.priority_),
      binary_data_(NULL) {
  *this = message;
}

Message& Message::operator=(const Message& message) {
  set_function_id(message.function_id_);
  set_correlation_id(message.correlation_id_);
  set_connection_key(message.connection_key_);
  set_message_type(message.type_);
  set_data_size(message.data_size_);
  set_payload_size(message.payload_size_);
  if (message.binary_data_) {
    set_binary_data(message.binary_data_);
  }
  set_json_message(message.json_message_);
  set_protocol_version(message.protocol_version());
  priority_ = message.priority_;

  return *this;
}

bool Message::operator==(const Message& message) {
  bool function_id = function_id_ == message.function_id_;
  bool correlation_id = correlation_id_ == message.correlation_id_;
  bool connection_key = connection_key_ == message.connection_key_;
  bool type = type_ == message.type_;
  bool json_message = json_message_ == message.json_message_;
  bool version = version_ == message.version_;
  bool data_size = data_size_ == message.data_size_;
  bool payload_size = payload_size_ == message.payload_size_;

  bool binary_data = std::equal(binary_data_->begin(), binary_data_->end(),
                                message.binary_data_->begin(),
                                BinaryDataPredicate);

  return function_id && correlation_id && connection_key && type && binary_data
      && json_message && version && data_size && payload_size;
}

Message::~Message() {
  if (binary_data_) {
    delete binary_data_;
  }
}

int32_t Message::function_id() const {
  return function_id_;
}

int32_t Message::correlation_id() const {
  return correlation_id_;
}

int32_t Message::connection_key() const {
  return connection_key_;
}

MessageType Message::type() const {
  return type_;
}

ProtocolVersion Message::protocol_version() const {
  return version_;
}

const std::string& Message::json_message() const {
  return json_message_;
}

const BinaryData* Message::binary_data() const {
  return binary_data_;
}

bool Message::has_binary_data() const {
  return (binary_data_ != NULL);
}

size_t Message::data_size() const {
  return data_size_;
}

size_t Message::payload_size() const {
  return payload_size_;
}

void Message::set_function_id(int32_t id) {
  function_id_ = id;
}

void Message::set_correlation_id(int32_t id) {
  correlation_id_ = id;
}

void Message::set_connection_key(int32_t key) {
  connection_key_ = key;
}

void Message::set_message_type(MessageType type) {
  type_ = type;
}

void Message::set_binary_data(BinaryData* data) {
  if (NULL == data) {
    NOTREACHED();
    return;
  }

  if (binary_data_) {
    delete binary_data_;
  }

  binary_data_ = data;
}

void Message::set_json_message(const std::string& json_message) {
  json_message_ = json_message;
}

void Message::set_protocol_version(ProtocolVersion version) {
  version_ = version;
}

const smart_objects::SmartObject &Message::smart_object() const {
  return smart_object_;
}

void Message::set_smart_object(const smart_objects::SmartObject& object) {
  smart_object_ = object;
}

void Message::set_data_size(size_t data_size) {
  data_size_ = data_size;
}

void Message::set_payload_size(size_t payload_size) {
  payload_size_ = payload_size;
}
}  // namespace application_manager
<|endoftext|>


<commit_before>// This file is distributed under the MIT license.
// See the LICENSE file for details.

#include <algorithm>
#include <cstring>
#include <exception>
#include <fstream>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <sstream>
#include <utility>

#include <boost/algorithm/string.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/utility/string_ref.hpp>
#include <boost/filesystem.hpp>
#include <boost/optional.hpp>

#include <visionaray/detail/macros.h>
#include <visionaray/math/math.h>
#include <visionaray/texture/texture.h>

#include "image.h"
#include "obj_loader.h"

namespace qi = boost::spirit::qi;

//-------------------------------------------------------------------------------------------------
// boost::fusion-adapt some structs for parsing
//

namespace visionaray
{
namespace detail
{

struct face_index_t
{
    int vertex_index;
    boost::optional<int> tex_coord_index;
    boost::optional<int> normal_index;
};

} // detail
} // visionaray

BOOST_FUSION_ADAPT_STRUCT
(
    visionaray::detail::face_index_t,
    (int, vertex_index)
    (boost::optional<int>, tex_coord_index)
    (boost::optional<int>, normal_index)
)

BOOST_FUSION_ADAPT_STRUCT
(
    visionaray::vec2,
    (float, x)
    (float, y)
)

BOOST_FUSION_ADAPT_STRUCT
(
    visionaray::vec3,
    (float, x)
    (float, y)
    (float, z)
)

using boost::string_ref;

namespace boost
{
namespace spirit
{
namespace traits
{

template <typename Iterator, typename Enable>
struct assign_to_attribute_from_iterators<string_ref, Iterator, Enable>
{
    static void call(Iterator const& first, Iterator const& last, string_ref& attr)
    {
        attr = { first, static_cast<size_t>(last - first) };
    }
};

} // traits
} // spirit
}

namespace visionaray
{

typedef basic_triangle<3, float> triangle_type;

namespace detail
{

//-------------------------------------------------------------------------------------------------
// aabb of a list of triangles
//

aabb bounds(detail::triangle_list const& tris)
{
    aabb result( vec3(std::numeric_limits<float>::max()), -vec3(std::numeric_limits<float>::max()) );

    for (auto const& tri : tris)
    {
        auto v1 = tri.v1;
        auto v2 = tri.v1 + tri.e1;
        auto v3 = tri.v1 + tri.e2;

        result = combine(result, aabb(v1, v1));
        result = combine(result, aabb(v2, v2));
        result = combine(result, aabb(v3, v3));
    }

    return result;
}

} // detail

struct mtl
{
    vec3 ka;
    vec3 kd;
    vec3 ks;
    float ns;
    std::string map_kd;
};

void parse_mtl(std::string const& filename, std::map<std::string, mtl>* matlib)
{
    std::ifstream ifstr(filename.c_str(), std::ifstream::in);
    std::string line;
    std::map<std::string, mtl>::iterator it;

    while (ifstr.good() && !ifstr.eof() && std::getline(ifstr, line))
    {
        boost::algorithm::trim(line);
        std::string identifier;
        std::istringstream str(line);
        str >> identifier >> std::ws;

        if (identifier == "newmtl")
        {
            std::string name;
            str >> name;
            auto mtl_pair = std::make_pair(name, mtl());
            matlib->insert(mtl_pair);
            it = matlib->find(name);
        }
        else if (identifier == "Ka")
        {
            str >> (*it).second.ka.x >> std::ws >> (*it).second.ka.y >> std::ws >> (*it).second.ka.z >> std::ws;
        }
        else if (identifier == "Kd")
        {
            str >> (*it).second.kd.x >> std::ws >> (*it).second.kd.y >> std::ws >> (*it).second.kd.z >> std::ws;
        }
        else if (identifier == "Ks")
        {
            str >> (*it).second.ks.x >> std::ws >> (*it).second.ks.y >> std::ws >> (*it).second.ks.z >> std::ws;
        }
        else if (identifier == "Ns")
        {
            str >> (*it).second.ns >> std::ws;
        }
        else if (identifier == "map_Kd")
        {
            str >> (*it).second.map_kd;
        }
    }
}

detail::obj_scene load_obj(std::string const& filename)
{
    std::map<std::string, mtl> matlib;

    std::ifstream ifstr(filename.c_str(), std::ifstream::in);
    boost::iostreams::mapped_file_source file(filename);

    unsigned geom_id = 0;

    using namespace detail;

    obj_scene result;

    using vertex_vector     = aligned_vector<vec3>;
    using tex_coord_vector  = aligned_vector<vec2>;
    using normal_vector     = aligned_vector<vec3>;
    using face_vector       = aligned_vector<face_index_t>;

    vertex_vector       vertices;
    tex_coord_vector    tex_coords;
    normal_vector       normals;
    face_vector         faces;

    // intermediate containers

    string_ref comment;
    string_ref mtl_file;
    string_ref mtl_name;

    // helper functions

    auto remap_index = [](int idx, int size) -> int
    {
        return idx < 0 ? static_cast<int>(size) + idx : idx - 1;
    };

    auto store_triangle = [&](int i1, int i2, int i3)
    {
        triangle_type tri;
        tri.prim_id = static_cast<unsigned>(result.primitives.size());
        tri.geom_id = geom_id;
        tri.v1 = vertices[i1];
        tri.e1 = vertices[i2] - tri.v1;
        tri.e2 = vertices[i3] - tri.v1;
        result.primitives.push_back(tri);
    };

    auto store_faces = [&](int vertices_size, int tex_coords_size, int normals_size)
    {
        size_t last = 2;
        auto i1 = remap_index(faces[0].vertex_index, vertices_size);

        while (last != faces.size())
        {
            // triangle
            auto i2 = remap_index(faces[last - 1].vertex_index, vertices_size);
            auto i3 = remap_index(faces[last].vertex_index, vertices_size);
            store_triangle(i1, i2, i3);

            // texture coordinates
            if (faces[0].tex_coord_index && faces[last - 1].tex_coord_index && faces[last].tex_coord_index)
            {
                auto ti1 = remap_index(*faces[0].tex_coord_index, tex_coords_size);
                auto ti2 = remap_index(*faces[last - 1].tex_coord_index, tex_coords_size);
                auto ti3 = remap_index(*faces[last].tex_coord_index, tex_coords_size);

                result.tex_coords.push_back( tex_coords[ti1] );
                result.tex_coords.push_back( tex_coords[ti2] );
                result.tex_coords.push_back( tex_coords[ti3] );
            }

            // normals
            if (faces[0].normal_index && faces[last - 1].normal_index && faces[last].normal_index)
            {
                auto ni1 = remap_index(*faces[0].normal_index, normals_size);
                auto ni2 = remap_index(*faces[last - 1].normal_index, normals_size);
                auto ni3 = remap_index(*faces[last].normal_index, normals_size);

                result.normals.push_back( normals[ni1] );
                result.normals.push_back( normals[ni2] );
                result.normals.push_back( normals[ni3] );
            }

            ++last;
        }
    };

    // obj grammar

    using It = string_ref::const_iterator;
    using space_type = decltype(qi::blank);

    qi::rule<It> r_unhandled                                    = *(qi::char_ - qi::eol)                                                    >> qi::eol;
    qi::rule<It, string_ref()> r_text_to_eol                    = qi::raw[*(qi::char_ - qi::eol)];

    qi::rule<It, string_ref()> r_comment                        = "#" >> r_text_to_eol                                                      >> qi::eol;
    qi::rule<It, string_ref(), space_type> r_mtllib             = "mtllib" >> r_text_to_eol                                                 >> qi::eol;
    qi::rule<It, string_ref(), space_type> r_usemtl             = "usemtl" >> r_text_to_eol                                                 >> qi::eol;

    qi::rule<It, vec3(), space_type> r_v                        = "v" >> qi::float_ >> qi::float_ >> qi::float_                             >> qi::eol;
    qi::rule<It, vec2(), space_type> r_vt                       = "vt" >> qi::float_ >> qi::float_ >> -qi::float_                           >> qi::eol;// TODO: parse optional 3rd (w)
    qi::rule<It, vec3(), space_type> r_vn                       = "vn" >> qi::float_ >> qi::float_ >> qi::float_                            >> qi::eol;

    qi::rule<It, vertex_vector(), space_type> r_vertices        = r_v >> *r_v;
    qi::rule<It, tex_coord_vector(), space_type> r_tex_coords   = r_vt >> *r_vt;
    qi::rule<It, normal_vector(), space_type> r_normals         = r_vn >> *r_vn;

    qi::rule<It, detail::face_index_t()> r_face_vertex          = qi::int_ >> -qi::lit("/") >> -qi::int_ >> -qi::lit("/") >> -qi::int_;
    qi::rule<It, face_vector(), space_type> r_face              = "f" >> r_face_vertex >> r_face_vertex >> r_face_vertex >> *r_face_vertex  >> qi::eol;

    string_ref text(file.data(), file.size());
    auto it = text.cbegin();

    while (it != text.cend())
    {
        faces.clear();

        if ( qi::phrase_parse(it, text.cend(), r_comment, qi::blank, comment) )
        {
            VSNRAY_UNUSED(comment);
            continue;
        }
        else if ( qi::phrase_parse(it, text.cend(), r_mtllib, qi::blank, mtl_file) )
        {
            boost::filesystem::path p(filename);
            std::string mtl_dir = p.parent_path().string();

            std::string mtl_path = mtl_dir + "/" + std::string(mtl_file);

            parse_mtl(mtl_path, &matlib);
        }
        else if ( qi::phrase_parse(it, text.cend(), r_usemtl, qi::blank, mtl_name) )
        {
            auto mat_it = matlib.find(std::string(mtl_name));
            if (mat_it != matlib.end())
            {
                phong<float> mat;
                mat.set_cd( mat_it->second.kd );
                mat.set_kd( 1.0f );
                mat.set_ks( 1.0f );
                mat.set_specular_exp( mat_it->second.ns );
                result.materials.push_back(mat);

                typedef tex_list::value_type tex_type;
                boost::filesystem::path p(filename);
                std::string tex_filename = p.parent_path().string() + "/" + mat_it->second.map_kd;

                static const std::string extensions[] = { ".jpg", ".jpeg", ".JPG", ".JPEG" };
                auto tex_path = boost::filesystem::path(tex_filename);
                auto has_jpg_ext = ( std::find(extensions, extensions + 4, tex_path.extension()) != extensions + 4 );

                if (!mat_it->second.map_kd.empty() && boost::filesystem::exists(tex_filename) && has_jpg_ext)
                {
#if defined(VSNRAY_HAVE_JPEG)
                    jpeg_image jpg(tex_filename);

                    tex_type tex(jpg.width(), jpg.height());
                    tex.set_address_mode( Clamp );
                    tex.set_filter_mode( Linear );

                    auto data_ptr = reinterpret_cast<tex_type::value_type const*>(jpg.data());
                    tex.set_data(data_ptr);

                    result.textures.push_back(std::move(tex));
#endif
                }
                else
                {
                    result.textures.push_back(tex_type(0, 0));
                }
            }
            geom_id = result.materials.size() == 0 ? 0 : static_cast<unsigned>(result.materials.size() - 1);
        }
        else if ( qi::phrase_parse(it, text.cend(), r_vertices, qi::blank, vertices) )
        {
        }
        else if ( qi::phrase_parse(it, text.cend(), r_tex_coords, qi::blank, tex_coords) )
        {
        }
        else if ( qi::phrase_parse(it, text.cend(), r_normals, qi::blank, normals) )
        {
        }
        else if ( qi::phrase_parse(it, text.cend(), r_face, qi::blank, faces) )
        {
            store_faces(static_cast<int>(vertices.size()), static_cast<int>(tex_coords.size()), static_cast<int>(normals.size()));
        }
        else if ( qi::phrase_parse(it, text.cend(), r_unhandled, qi::blank) )
        {
            continue;
        }
    }

// TODO

    result.normals.resize(0); // TODO: support for vertex normals
    if (result.normals.size() == 0) // have no default normals
    {
        for (auto const& tri : result.primitives)
        {
            vec3 n = normalize( cross(tri.e1, tri.e2) );
            result.normals.push_back(n);
        }
    }

    if (result.materials.size() == 0)
    {
        for (unsigned i = 0; i <= geom_id; ++i)
        {
            phong<float> m;
            m.set_cd( vec3(0.8f, 0.8f, 0.8f) );
            m.set_kd( 1.0f );
            m.set_ks( 1.0f );
            m.set_specular_exp( 32.0f );
            result.materials.push_back(m);
        }
    }
// TODO

    result.bbox = bounds(result.primitives);
    return result;
}

} // visionaray
<commit_msg>Obj allows vec4 vertices<commit_after>// This file is distributed under the MIT license.
// See the LICENSE file for details.

#include <algorithm>
#include <cstring>
#include <exception>
#include <fstream>
#include <iostream>
#include <limits>
#include <map>
#include <memory>
#include <sstream>
#include <utility>

#include <boost/algorithm/string.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/iostreams/device/mapped_file.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/utility/string_ref.hpp>
#include <boost/filesystem.hpp>
#include <boost/optional.hpp>

#include <visionaray/detail/macros.h>
#include <visionaray/math/math.h>
#include <visionaray/texture/texture.h>

#include "image.h"
#include "obj_loader.h"

namespace qi = boost::spirit::qi;

//-------------------------------------------------------------------------------------------------
// boost::fusion-adapt some structs for parsing
//

namespace visionaray
{
namespace detail
{

struct face_index_t
{
    int vertex_index;
    boost::optional<int> tex_coord_index;
    boost::optional<int> normal_index;
};

} // detail
} // visionaray

BOOST_FUSION_ADAPT_STRUCT
(
    visionaray::detail::face_index_t,
    (int, vertex_index)
    (boost::optional<int>, tex_coord_index)
    (boost::optional<int>, normal_index)
)

BOOST_FUSION_ADAPT_STRUCT
(
    visionaray::vec2,
    (float, x)
    (float, y)
)

BOOST_FUSION_ADAPT_STRUCT
(
    visionaray::vec3,
    (float, x)
    (float, y)
    (float, z)
)

using boost::string_ref;

namespace boost
{
namespace spirit
{
namespace traits
{

template <typename Iterator, typename Enable>
struct assign_to_attribute_from_iterators<string_ref, Iterator, Enable>
{
    static void call(Iterator const& first, Iterator const& last, string_ref& attr)
    {
        attr = { first, static_cast<size_t>(last - first) };
    }
};

} // traits
} // spirit
}

namespace visionaray
{

typedef basic_triangle<3, float> triangle_type;

namespace detail
{

//-------------------------------------------------------------------------------------------------
// aabb of a list of triangles
//

aabb bounds(detail::triangle_list const& tris)
{
    aabb result( vec3(std::numeric_limits<float>::max()), -vec3(std::numeric_limits<float>::max()) );

    for (auto const& tri : tris)
    {
        auto v1 = tri.v1;
        auto v2 = tri.v1 + tri.e1;
        auto v3 = tri.v1 + tri.e2;

        result = combine(result, aabb(v1, v1));
        result = combine(result, aabb(v2, v2));
        result = combine(result, aabb(v3, v3));
    }

    return result;
}

} // detail

struct mtl
{
    vec3 ka;
    vec3 kd;
    vec3 ks;
    float ns;
    std::string map_kd;
};

void parse_mtl(std::string const& filename, std::map<std::string, mtl>* matlib)
{
    std::ifstream ifstr(filename.c_str(), std::ifstream::in);
    std::string line;
    std::map<std::string, mtl>::iterator it;

    while (ifstr.good() && !ifstr.eof() && std::getline(ifstr, line))
    {
        boost::algorithm::trim(line);
        std::string identifier;
        std::istringstream str(line);
        str >> identifier >> std::ws;

        if (identifier == "newmtl")
        {
            std::string name;
            str >> name;
            auto mtl_pair = std::make_pair(name, mtl());
            matlib->insert(mtl_pair);
            it = matlib->find(name);
        }
        else if (identifier == "Ka")
        {
            str >> (*it).second.ka.x >> std::ws >> (*it).second.ka.y >> std::ws >> (*it).second.ka.z >> std::ws;
        }
        else if (identifier == "Kd")
        {
            str >> (*it).second.kd.x >> std::ws >> (*it).second.kd.y >> std::ws >> (*it).second.kd.z >> std::ws;
        }
        else if (identifier == "Ks")
        {
            str >> (*it).second.ks.x >> std::ws >> (*it).second.ks.y >> std::ws >> (*it).second.ks.z >> std::ws;
        }
        else if (identifier == "Ns")
        {
            str >> (*it).second.ns >> std::ws;
        }
        else if (identifier == "map_Kd")
        {
            str >> (*it).second.map_kd;
        }
    }
}

detail::obj_scene load_obj(std::string const& filename)
{
    std::map<std::string, mtl> matlib;

    std::ifstream ifstr(filename.c_str(), std::ifstream::in);
    boost::iostreams::mapped_file_source file(filename);

    unsigned geom_id = 0;

    using namespace detail;

    obj_scene result;

    using vertex_vector     = aligned_vector<vec3>;
    using tex_coord_vector  = aligned_vector<vec2>;
    using normal_vector     = aligned_vector<vec3>;
    using face_vector       = aligned_vector<face_index_t>;

    vertex_vector       vertices;
    tex_coord_vector    tex_coords;
    normal_vector       normals;
    face_vector         faces;

    // intermediate containers

    string_ref comment;
    string_ref mtl_file;
    string_ref mtl_name;

    // helper functions

    auto remap_index = [](int idx, int size) -> int
    {
        return idx < 0 ? static_cast<int>(size) + idx : idx - 1;
    };

    auto store_triangle = [&](int i1, int i2, int i3)
    {
        triangle_type tri;
        tri.prim_id = static_cast<unsigned>(result.primitives.size());
        tri.geom_id = geom_id;
        tri.v1 = vertices[i1];
        tri.e1 = vertices[i2] - tri.v1;
        tri.e2 = vertices[i3] - tri.v1;
        result.primitives.push_back(tri);
    };

    auto store_faces = [&](int vertices_size, int tex_coords_size, int normals_size)
    {
        size_t last = 2;
        auto i1 = remap_index(faces[0].vertex_index, vertices_size);

        while (last != faces.size())
        {
            // triangle
            auto i2 = remap_index(faces[last - 1].vertex_index, vertices_size);
            auto i3 = remap_index(faces[last].vertex_index, vertices_size);
            store_triangle(i1, i2, i3);

            // texture coordinates
            if (faces[0].tex_coord_index && faces[last - 1].tex_coord_index && faces[last].tex_coord_index)
            {
                auto ti1 = remap_index(*faces[0].tex_coord_index, tex_coords_size);
                auto ti2 = remap_index(*faces[last - 1].tex_coord_index, tex_coords_size);
                auto ti3 = remap_index(*faces[last].tex_coord_index, tex_coords_size);

                result.tex_coords.push_back( tex_coords[ti1] );
                result.tex_coords.push_back( tex_coords[ti2] );
                result.tex_coords.push_back( tex_coords[ti3] );
            }

            // normals
            if (faces[0].normal_index && faces[last - 1].normal_index && faces[last].normal_index)
            {
                auto ni1 = remap_index(*faces[0].normal_index, normals_size);
                auto ni2 = remap_index(*faces[last - 1].normal_index, normals_size);
                auto ni3 = remap_index(*faces[last].normal_index, normals_size);

                result.normals.push_back( normals[ni1] );
                result.normals.push_back( normals[ni2] );
                result.normals.push_back( normals[ni3] );
            }

            ++last;
        }
    };

    // obj grammar

    using It = string_ref::const_iterator;
    using space_type = decltype(qi::blank);

    qi::rule<It> r_unhandled                                    = *(qi::char_ - qi::eol)                                                    >> qi::eol;
    qi::rule<It, string_ref()> r_text_to_eol                    = qi::raw[*(qi::char_ - qi::eol)];

    qi::rule<It, string_ref()> r_comment                        = "#" >> r_text_to_eol                                                      >> qi::eol;
    qi::rule<It, string_ref(), space_type> r_mtllib             = "mtllib" >> r_text_to_eol                                                 >> qi::eol;
    qi::rule<It, string_ref(), space_type> r_usemtl             = "usemtl" >> r_text_to_eol                                                 >> qi::eol;

    qi::rule<It, vec3(), space_type> r_v                        = "v" >> qi::float_ >> qi::float_ >> qi::float_ >> -qi::float_              >> qi::eol; // TODO: mind w
    qi::rule<It, vec2(), space_type> r_vt                       = "vt" >> qi::float_ >> qi::float_ >> -qi::float_                           >> qi::eol; // TODO: mind w
    qi::rule<It, vec3(), space_type> r_vn                       = "vn" >> qi::float_ >> qi::float_ >> qi::float_                            >> qi::eol;

    qi::rule<It, vertex_vector(), space_type> r_vertices        = r_v >> *r_v;
    qi::rule<It, tex_coord_vector(), space_type> r_tex_coords   = r_vt >> *r_vt;
    qi::rule<It, normal_vector(), space_type> r_normals         = r_vn >> *r_vn;

    qi::rule<It, detail::face_index_t()> r_face_vertex          = qi::int_ >> -qi::lit("/") >> -qi::int_ >> -qi::lit("/") >> -qi::int_;
    qi::rule<It, face_vector(), space_type> r_face              = "f" >> r_face_vertex >> r_face_vertex >> r_face_vertex >> *r_face_vertex  >> qi::eol;

    string_ref text(file.data(), file.size());
    auto it = text.cbegin();

    while (it != text.cend())
    {
        faces.clear();

        if ( qi::phrase_parse(it, text.cend(), r_comment, qi::blank, comment) )
        {
            VSNRAY_UNUSED(comment);
            continue;
        }
        else if ( qi::phrase_parse(it, text.cend(), r_mtllib, qi::blank, mtl_file) )
        {
            boost::filesystem::path p(filename);
            std::string mtl_dir = p.parent_path().string();

            std::string mtl_path = mtl_dir + "/" + std::string(mtl_file);

            parse_mtl(mtl_path, &matlib);
        }
        else if ( qi::phrase_parse(it, text.cend(), r_usemtl, qi::blank, mtl_name) )
        {
            auto mat_it = matlib.find(std::string(mtl_name));
            if (mat_it != matlib.end())
            {
                phong<float> mat;
                mat.set_cd( mat_it->second.kd );
                mat.set_kd( 1.0f );
                mat.set_ks( 1.0f );
                mat.set_specular_exp( mat_it->second.ns );
                result.materials.push_back(mat);

                typedef tex_list::value_type tex_type;
                boost::filesystem::path p(filename);
                std::string tex_filename = p.parent_path().string() + "/" + mat_it->second.map_kd;

                static const std::string extensions[] = { ".jpg", ".jpeg", ".JPG", ".JPEG" };
                auto tex_path = boost::filesystem::path(tex_filename);
                auto has_jpg_ext = ( std::find(extensions, extensions + 4, tex_path.extension()) != extensions + 4 );

                if (!mat_it->second.map_kd.empty() && boost::filesystem::exists(tex_filename) && has_jpg_ext)
                {
#if defined(VSNRAY_HAVE_JPEG)
                    jpeg_image jpg(tex_filename);

                    tex_type tex(jpg.width(), jpg.height());
                    tex.set_address_mode( Clamp );
                    tex.set_filter_mode( Linear );

                    auto data_ptr = reinterpret_cast<tex_type::value_type const*>(jpg.data());
                    tex.set_data(data_ptr);

                    result.textures.push_back(std::move(tex));
#endif
                }
                else
                {
                    result.textures.push_back(tex_type(0, 0));
                }
            }
            geom_id = result.materials.size() == 0 ? 0 : static_cast<unsigned>(result.materials.size() - 1);
        }
        else if ( qi::phrase_parse(it, text.cend(), r_vertices, qi::blank, vertices) )
        {
        }
        else if ( qi::phrase_parse(it, text.cend(), r_tex_coords, qi::blank, tex_coords) )
        {
        }
        else if ( qi::phrase_parse(it, text.cend(), r_normals, qi::blank, normals) )
        {
        }
        else if ( qi::phrase_parse(it, text.cend(), r_face, qi::blank, faces) )
        {
            store_faces(static_cast<int>(vertices.size()), static_cast<int>(tex_coords.size()), static_cast<int>(normals.size()));
        }
        else if ( qi::phrase_parse(it, text.cend(), r_unhandled, qi::blank) )
        {
            continue;
        }
    }

// TODO

    result.normals.resize(0); // TODO: support for vertex normals
    if (result.normals.size() == 0) // have no default normals
    {
        for (auto const& tri : result.primitives)
        {
            vec3 n = normalize( cross(tri.e1, tri.e2) );
            result.normals.push_back(n);
        }
    }

    if (result.materials.size() == 0)
    {
        for (unsigned i = 0; i <= geom_id; ++i)
        {
            phong<float> m;
            m.set_cd( vec3(0.8f, 0.8f, 0.8f) );
            m.set_kd( 1.0f );
            m.set_ks( 1.0f );
            m.set_specular_exp( 32.0f );
            result.materials.push_back(m);
        }
    }
// TODO

    result.bbox = bounds(result.primitives);
    return result;
}

} // visionaray
<|endoftext|>


<commit_before>#pragma once

#include "console_user_server_client.hpp"
#include "constants.hpp"
#include "input_source_manager.hpp"
#include "local_datagram/server_manager.hpp"
#include "shell_utility.hpp"
#include "types.hpp"
#include <vector>

namespace krbn {
class receiver final {
public:
  // Signals

  boost::signals2::signal<void(void)> bound;
  boost::signals2::signal<void(const boost::system::error_code&)> bind_failed;
  boost::signals2::signal<void(void)> closed;

  // Methods

  receiver(const receiver&) = delete;

  receiver(void) : last_select_input_source_time_stamp_(0) {
  }

  void start(void) {
    auto uid = getuid();
    auto socket_file_path = console_user_server_client::make_console_user_server_socket_file_path(uid);

    unlink(socket_file_path.c_str());

    size_t buffer_size = 32 * 1024;
    std::chrono::milliseconds server_check_interval(3000);
    std::chrono::milliseconds reconnect_interval(1000);

    server_manager_ = std::make_unique<local_datagram::server_manager>(socket_file_path,
                                                                       buffer_size,
                                                                       server_check_interval,
                                                                       reconnect_interval);

    server_manager_->bound.connect([this] {
      bound();
    });

    server_manager_->bind_failed.connect([this](auto&& error_code) {
      bind_failed(error_code);
    });

    server_manager_->closed.connect([this] {
      closed();
    });

    server_manager_->received.connect([this](auto&& buffer) {
      if (auto type = types::find_operation_type(buffer.data(), buffer.size())) {
        switch (*type) {
          case operation_type::shell_command_execution:
            if (buffer.size() != sizeof(operation_type_shell_command_execution_struct)) {
              logger::get_logger().error("invalid size for operation_type::shell_command_execution");
            } else {
              auto p = reinterpret_cast<operation_type_shell_command_execution_struct*>(buffer.data());

              // Ensure shell_command is null-terminated string even if corrupted data is sent.
              p->shell_command[sizeof(p->shell_command) - 1] = '\0';

              std::string background_shell_command = shell_utility::make_background_command(p->shell_command);
              dispatch_async(dispatch_get_main_queue(), ^{
                system(background_shell_command.c_str());
              });
            }
            break;

          case operation_type::select_input_source:
            if (buffer.size() != sizeof(operation_type_select_input_source_struct)) {
              logger::get_logger().error("invalid size for operation_type::select_input_source");
            } else {
              auto p = reinterpret_cast<operation_type_select_input_source_struct*>(buffer.data());

              // Ensure input_source_selector's strings are null-terminated string even if corrupted data is sent.
              p->language[sizeof(p->language) - 1] = '\0';
              p->input_source_id[sizeof(p->input_source_id) - 1] = '\0';
              p->input_mode_id[sizeof(p->input_mode_id) - 1] = '\0';

              uint64_t time_stamp = p->time_stamp;
              boost::optional<std::string> language(std::string(p->language));
              boost::optional<std::string> input_source_id(std::string(p->input_source_id));
              boost::optional<std::string> input_mode_id(std::string(p->input_mode_id));
              if (language && language->empty()) {
                language = boost::none;
              }
              if (input_source_id && input_source_id->empty()) {
                input_source_id = boost::none;
              }
              if (input_mode_id && input_mode_id->empty()) {
                input_mode_id = boost::none;
              }

              input_source_selector input_source_selector(language,
                                                          input_source_id,
                                                          input_mode_id);

              dispatch_async(dispatch_get_main_queue(), ^{
                if (last_select_input_source_time_stamp_ == time_stamp) {
                  return;
                }
                if (input_source_manager_.select(input_source_selector)) {
                  last_select_input_source_time_stamp_ = time_stamp;
                }
              });
            }
            break;

          default:
            break;
        }
      }
    });

    server_manager_->start();

    logger::get_logger().info("receiver is initialized");
  }

  ~receiver(void) {
    server_manager_ = nullptr;

    logger::get_logger().info("receiver is terminated");
  }

private:
  std::unique_ptr<local_datagram::server_manager> server_manager_;

  input_source_manager input_source_manager_;
  uint64_t last_select_input_source_time_stamp_;
};
} // namespace krbn
<commit_msg>update for received changes<commit_after>#pragma once

#include "console_user_server_client.hpp"
#include "constants.hpp"
#include "input_source_manager.hpp"
#include "local_datagram/server_manager.hpp"
#include "shell_utility.hpp"
#include "types.hpp"
#include <vector>

namespace krbn {
class receiver final {
public:
  // Signals

  boost::signals2::signal<void(void)> bound;
  boost::signals2::signal<void(const boost::system::error_code&)> bind_failed;
  boost::signals2::signal<void(void)> closed;

  // Methods

  receiver(const receiver&) = delete;

  receiver(void) : last_select_input_source_time_stamp_(0) {
  }

  void start(void) {
    auto uid = getuid();
    auto socket_file_path = console_user_server_client::make_console_user_server_socket_file_path(uid);

    unlink(socket_file_path.c_str());

    size_t buffer_size = 32 * 1024;
    std::chrono::milliseconds server_check_interval(3000);
    std::chrono::milliseconds reconnect_interval(1000);

    server_manager_ = std::make_unique<local_datagram::server_manager>(socket_file_path,
                                                                       buffer_size,
                                                                       server_check_interval,
                                                                       reconnect_interval);

    server_manager_->bound.connect([this] {
      bound();
    });

    server_manager_->bind_failed.connect([this](auto&& error_code) {
      bind_failed(error_code);
    });

    server_manager_->closed.connect([this] {
      closed();
    });

    server_manager_->received.connect([this](auto&& buffer) {
      if (auto type = types::find_operation_type(*buffer)) {
        switch (*type) {
          case operation_type::shell_command_execution:
            if (buffer->size() != sizeof(operation_type_shell_command_execution_struct)) {
              logger::get_logger().error("invalid size for operation_type::shell_command_execution");
            } else {
              auto p = reinterpret_cast<operation_type_shell_command_execution_struct*>(&((*buffer)[0]));

              // Ensure shell_command is null-terminated string even if corrupted data is sent.
              p->shell_command[sizeof(p->shell_command) - 1] = '\0';

              std::string background_shell_command = shell_utility::make_background_command(p->shell_command);
              dispatch_async(dispatch_get_main_queue(), ^{
                system(background_shell_command.c_str());
              });
            }
            break;

          case operation_type::select_input_source:
            if (buffer->size() != sizeof(operation_type_select_input_source_struct)) {
              logger::get_logger().error("invalid size for operation_type::select_input_source");
            } else {
              auto p = reinterpret_cast<operation_type_select_input_source_struct*>(&((*buffer)[0]));

              // Ensure input_source_selector's strings are null-terminated string even if corrupted data is sent.
              p->language[sizeof(p->language) - 1] = '\0';
              p->input_source_id[sizeof(p->input_source_id) - 1] = '\0';
              p->input_mode_id[sizeof(p->input_mode_id) - 1] = '\0';

              uint64_t time_stamp = p->time_stamp;
              boost::optional<std::string> language(std::string(p->language));
              boost::optional<std::string> input_source_id(std::string(p->input_source_id));
              boost::optional<std::string> input_mode_id(std::string(p->input_mode_id));
              if (language && language->empty()) {
                language = boost::none;
              }
              if (input_source_id && input_source_id->empty()) {
                input_source_id = boost::none;
              }
              if (input_mode_id && input_mode_id->empty()) {
                input_mode_id = boost::none;
              }

              input_source_selector input_source_selector(language,
                                                          input_source_id,
                                                          input_mode_id);

              dispatch_async(dispatch_get_main_queue(), ^{
                if (last_select_input_source_time_stamp_ == time_stamp) {
                  return;
                }
                if (input_source_manager_.select(input_source_selector)) {
                  last_select_input_source_time_stamp_ = time_stamp;
                }
              });
            }
            break;

          default:
            break;
        }
      }
    });

    server_manager_->start();

    logger::get_logger().info("receiver is initialized");
  }

  ~receiver(void) {
    server_manager_ = nullptr;

    logger::get_logger().info("receiver is terminated");
  }

private:
  std::unique_ptr<local_datagram::server_manager> server_manager_;

  input_source_manager input_source_manager_;
  uint64_t last_select_input_source_time_stamp_;
};
} // namespace krbn
<|endoftext|>


<commit_before>/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * Port.cpp
 * Base implementation of the Port class.
 * Copyright (C) 2005-2009 Simon Newton
 *
 * Unfortunately this file contains a lot of code duplication.
 */

#include <string>
#include "ola/Logging.h"
#include "olad/Device.h"
#include "olad/Port.h"

namespace ola {

/*
 * Create a new basic input port
 */
BasicInputPort::BasicInputPort(AbstractDevice *parent,
                               unsigned int port_id,
                               const TimeStamp *wake_time):
    m_port_id(port_id),
    m_priority(DmxSource::PRIORITY_DEFAULT),
    m_priority_mode(PRIORITY_MODE_INHERIT),
    m_port_string(""),
    m_universe(NULL),
    m_device(parent),
    m_wakeup_time(wake_time) {
}

bool BasicInputPort::SetUniverse(Universe *new_universe) {
  Universe *old_universe = GetUniverse();
  if (old_universe == new_universe)
    return true;

  if (PreSetUniverse(old_universe, new_universe)) {
    m_universe = new_universe;
    PostSetUniverse(old_universe, new_universe);
    return true;
  }
  return false;
}

string BasicInputPort::UniqueId() const {
  if (m_port_string.empty()) {
    std::stringstream str;
    if (m_device)
      str << m_device->UniqueId() << "-I-" << m_port_id;
    m_port_string = str.str();
  }
  return m_port_string;
}

bool BasicInputPort::SetPriority(uint8_t priority) {
  if (priority > DmxSource::PRIORITY_MAX)
    return false;

  m_priority = priority;
  return true;
}

/*
 * Called when there is new data for this port
 */
void BasicInputPort::DmxChanged() {
  if (GetUniverse()) {
    const DmxBuffer &buffer = ReadDMX();
    uint8_t priority = (PriorityCapability() == CAPABILITY_FULL &&
                        GetPriorityMode() == PRIORITY_MODE_INHERIT ?
                        InheritedPriority() :
                        GetPriority());
    m_dmx_source.UpdateData(buffer, *m_wakeup_time, priority);
    GetUniverse()->PortDataChanged(this);
  }
}

/*
 * Handle an RDM Request on this port.
 * @param request the RDMRequest object, ownership is transferred to us
 */
bool BasicInputPort::HandleRDMRequest(const ola::rdm::RDMRequest *request) {
  if (m_universe)
    return m_universe->HandleRDMRequest(this, request);
  else
    delete request;
    return false;
}

/*
 * Handle a response message
 * @param response, the RDMResponse object, ownership is transferred to us
 */
bool BasicInputPort::HandleRDMResponse(
    const ola::rdm::RDMResponse *response) {
  OLA_WARN << "In base HandleRDMResponse, something has gone wrong with RDM" <<
    " request routing";
  delete response;
  return true;
}

/*
 * Trigger the RDM Discovery procedure for this universe
 */
void BasicInputPort::TriggerRDMDiscovery() {
  if (m_universe)
    m_universe->RunRDMDiscovery();
}

void BasicOutputPort::NewUIDList(const ola::rdm::UIDSet &uids) {
  if (m_universe)
    m_universe->NewUIDList(uids, this);
}

/*
 * Create a new BasicOutputPort
 */
BasicOutputPort::BasicOutputPort(AbstractDevice *parent,
                                 unsigned int port_id,
                                 bool start_rdm_discovery_on_patch):
    m_port_id(port_id),
    m_discover_on_patch(start_rdm_discovery_on_patch),
    m_priority(DmxSource::PRIORITY_DEFAULT),
    m_priority_mode(PRIORITY_MODE_INHERIT),
    m_port_string(""),
    m_universe(NULL),
    m_device(parent) {
}

bool BasicOutputPort::SetUniverse(Universe *new_universe) {
  Universe *old_universe = GetUniverse();
  if (old_universe == new_universe)
    return true;

  if (PreSetUniverse(old_universe, new_universe)) {
    m_universe = new_universe;
    PostSetUniverse(old_universe, new_universe);
    if (m_discover_on_patch)
      RunRDMDiscovery();
    return true;
  }
  return false;
}

string BasicOutputPort::UniqueId() const {
  if (m_port_string.empty()) {
    std::stringstream str;
    if (m_device)
      str << m_device->UniqueId() << "-O-" << m_port_id;
    m_port_string = str.str();
  }
  return m_port_string;
}

bool BasicOutputPort::SetPriority(uint8_t priority) {
  if (priority > DmxSource::PRIORITY_MAX)
    return false;

  m_priority = priority;
  return true;
}

/*
 * Handle an RDMRequest, subclasses can implement this to support RDM
 */
bool BasicOutputPort::HandleRDMRequest(const ola::rdm::RDMRequest *request) {
  OLA_WARN << "In base HandleRDMRequest, something has gone wrong with RDM" <<
    " request routing";
  delete request;
  return true;
}

/*
 * Handle a response message
 */
bool BasicOutputPort::HandleRDMResponse(
    const ola::rdm::RDMResponse *response) {
  if (m_universe)
    return m_universe->HandleRDMResponse(this, response);
  else
    delete response;
    return false;
}

/*
 * This is a noop for ports that don't support RDM
 */
void BasicOutputPort::RunRDMDiscovery() {
}

/*
 * This allows switching based on Port type.
 */
template<class PortClass>
bool IsInputPort() {
  return true;
}

template<>
bool IsInputPort<OutputPort>() {
  return false;
}

template<>
bool IsInputPort<InputPort>() {
  return true;
}
}  // ola
<commit_msg> * remove the warning for bcast messages<commit_after>/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Library General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * Port.cpp
 * Base implementation of the Port class.
 * Copyright (C) 2005-2009 Simon Newton
 *
 * Unfortunately this file contains a lot of code duplication.
 */

#include <string>
#include "ola/Logging.h"
#include "olad/Device.h"
#include "olad/Port.h"

namespace ola {

/*
 * Create a new basic input port
 */
BasicInputPort::BasicInputPort(AbstractDevice *parent,
                               unsigned int port_id,
                               const TimeStamp *wake_time):
    m_port_id(port_id),
    m_priority(DmxSource::PRIORITY_DEFAULT),
    m_priority_mode(PRIORITY_MODE_INHERIT),
    m_port_string(""),
    m_universe(NULL),
    m_device(parent),
    m_wakeup_time(wake_time) {
}

bool BasicInputPort::SetUniverse(Universe *new_universe) {
  Universe *old_universe = GetUniverse();
  if (old_universe == new_universe)
    return true;

  if (PreSetUniverse(old_universe, new_universe)) {
    m_universe = new_universe;
    PostSetUniverse(old_universe, new_universe);
    return true;
  }
  return false;
}

string BasicInputPort::UniqueId() const {
  if (m_port_string.empty()) {
    std::stringstream str;
    if (m_device)
      str << m_device->UniqueId() << "-I-" << m_port_id;
    m_port_string = str.str();
  }
  return m_port_string;
}

bool BasicInputPort::SetPriority(uint8_t priority) {
  if (priority > DmxSource::PRIORITY_MAX)
    return false;

  m_priority = priority;
  return true;
}

/*
 * Called when there is new data for this port
 */
void BasicInputPort::DmxChanged() {
  if (GetUniverse()) {
    const DmxBuffer &buffer = ReadDMX();
    uint8_t priority = (PriorityCapability() == CAPABILITY_FULL &&
                        GetPriorityMode() == PRIORITY_MODE_INHERIT ?
                        InheritedPriority() :
                        GetPriority());
    m_dmx_source.UpdateData(buffer, *m_wakeup_time, priority);
    GetUniverse()->PortDataChanged(this);
  }
}

/*
 * Handle an RDM Request on this port.
 * @param request the RDMRequest object, ownership is transferred to us
 */
bool BasicInputPort::HandleRDMRequest(const ola::rdm::RDMRequest *request) {
  if (m_universe)
    return m_universe->HandleRDMRequest(this, request);
  else
    delete request;
    return false;
}

/*
 * Handle a response message
 * @param response, the RDMResponse object, ownership is transferred to us
 */
bool BasicInputPort::HandleRDMResponse(
    const ola::rdm::RDMResponse *response) {
  OLA_WARN << "In base HandleRDMResponse, something has gone wrong with RDM" <<
    " request routing";
  delete response;
  return true;
}

/*
 * Trigger the RDM Discovery procedure for this universe
 */
void BasicInputPort::TriggerRDMDiscovery() {
  if (m_universe)
    m_universe->RunRDMDiscovery();
}

void BasicOutputPort::NewUIDList(const ola::rdm::UIDSet &uids) {
  if (m_universe)
    m_universe->NewUIDList(uids, this);
}

/*
 * Create a new BasicOutputPort
 */
BasicOutputPort::BasicOutputPort(AbstractDevice *parent,
                                 unsigned int port_id,
                                 bool start_rdm_discovery_on_patch):
    m_port_id(port_id),
    m_discover_on_patch(start_rdm_discovery_on_patch),
    m_priority(DmxSource::PRIORITY_DEFAULT),
    m_priority_mode(PRIORITY_MODE_INHERIT),
    m_port_string(""),
    m_universe(NULL),
    m_device(parent) {
}

bool BasicOutputPort::SetUniverse(Universe *new_universe) {
  Universe *old_universe = GetUniverse();
  if (old_universe == new_universe)
    return true;

  if (PreSetUniverse(old_universe, new_universe)) {
    m_universe = new_universe;
    PostSetUniverse(old_universe, new_universe);
    if (m_discover_on_patch)
      RunRDMDiscovery();
    return true;
  }
  return false;
}

string BasicOutputPort::UniqueId() const {
  if (m_port_string.empty()) {
    std::stringstream str;
    if (m_device)
      str << m_device->UniqueId() << "-O-" << m_port_id;
    m_port_string = str.str();
  }
  return m_port_string;
}

bool BasicOutputPort::SetPriority(uint8_t priority) {
  if (priority > DmxSource::PRIORITY_MAX)
    return false;

  m_priority = priority;
  return true;
}

/*
 * Handle an RDMRequest, subclasses can implement this to support RDM
 */
bool BasicOutputPort::HandleRDMRequest(const ola::rdm::RDMRequest *request) {
  // broadcasts go to every port
  if (!request->DestinationUID().IsBroadcast())
    OLA_WARN << "In base HandleRDMRequest, something has gone wrong with RDM"
      << " request routing";
  delete request;
  return true;
}

/*
 * Handle a response message
 */
bool BasicOutputPort::HandleRDMResponse(
    const ola::rdm::RDMResponse *response) {
  if (m_universe)
    return m_universe->HandleRDMResponse(this, response);
  else
    delete response;
    return false;
}

/*
 * This is a noop for ports that don't support RDM
 */
void BasicOutputPort::RunRDMDiscovery() {
}

/*
 * This allows switching based on Port type.
 */
template<class PortClass>
bool IsInputPort() {
  return true;
}

template<>
bool IsInputPort<OutputPort>() {
  return false;
}

template<>
bool IsInputPort<InputPort>() {
  return true;
}
}  // ola
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (C) 2012 Nokia Corporation and/or its subsidiary(-ies).
** Contact: http://www.qt-project.org/
**
** This file is part of the QtDeclarative module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** GNU Lesser General Public License Usage
** This file may be used under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation and
** appearing in the file LICENSE.LGPL included in the packaging of this
** file. Please review the following information to ensure the GNU Lesser
** General Public License version 2.1 requirements will be met:
** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights. These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU General
** Public License version 3.0 as published by the Free Software Foundation
** and appearing in the file LICENSE.GPL included in the packaging of this
** file. Please review the following information to ensure the GNU General
** Public License version 3.0 requirements will be met:
** http://www.gnu.org/copyleft/gpl.html.
**
** Other Usage
** Alternatively, this file may be used in accordance with the terms and
** conditions contained in a signed written agreement between you and Nokia.
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qdebugmessageservice_p.h"
#include "qdeclarativedebugservice_p_p.h"

QT_BEGIN_NAMESPACE

Q_GLOBAL_STATIC(QDebugMessageService, declarativeDebugMessageService)

void DebugMessageHandler(QtMsgType type, const QMessageLogContext &ctxt,
                         const char *buf)
{
    QDebugMessageService::instance()->sendDebugMessage(type, ctxt, buf);
}

class QDebugMessageServicePrivate : public QDeclarativeDebugServicePrivate
{
public:
    QDebugMessageServicePrivate()
        : oldMsgHandler(0)
        , prevState(QDeclarativeDebugService::NotConnected)
    {
    }

    QMessageHandler oldMsgHandler;
    QDeclarativeDebugService::State prevState;
};

QDebugMessageService::QDebugMessageService(QObject *parent) :
    QDeclarativeDebugService(*(new QDebugMessageServicePrivate()),
                                   QLatin1String("DebugMessages"), 2, parent)
{
    Q_D(QDebugMessageService);

    registerService();
    if (state() == Enabled) {
        d->oldMsgHandler = qInstallMessageHandler(DebugMessageHandler);
        d->prevState = Enabled;
    }
}

QDebugMessageService *QDebugMessageService::instance()
{
    return declarativeDebugMessageService();
}

void QDebugMessageService::sendDebugMessage(QtMsgType type,
                                            const QMessageLogContext &ctxt,
                                            const char *buf)
{
    Q_D(QDebugMessageService);

    //We do not want to alter the message handling mechanism
    //We just eavesdrop and forward the messages to a port
    //only if a client is connected to it.
    QByteArray message;
    QDataStream ws(&message, QIODevice::WriteOnly);
    ws << QByteArray("MESSAGE") << type << QString::fromLocal8Bit(buf).toUtf8();
    ws << QString::fromLatin1(ctxt.file).toUtf8();
    ws << ctxt.line << QString::fromLatin1(ctxt.function).toUtf8();

    sendMessage(message);
    if (d->oldMsgHandler)
        (*d->oldMsgHandler)(type, ctxt, buf);
}

void QDebugMessageService::stateChanged(State state)
{
    Q_D(QDebugMessageService);

    if (state != Enabled && d->prevState == Enabled) {
        QMessageHandler handler = qInstallMessageHandler(d->oldMsgHandler);
        // has our handler been overwritten in between?
        if (handler != DebugMessageHandler)
            qInstallMessageHandler(handler);

    } else if (state == Enabled && d->prevState != Enabled) {
        d->oldMsgHandler = qInstallMessageHandler(DebugMessageHandler);

    }

    d->prevState = state;
}

QT_END_NAMESPACE
<commit_msg>Add missing include<commit_after>/****************************************************************************
**
** Copyright (C) 2012 Nokia Corporation and/or its subsidiary(-ies).
** Contact: http://www.qt-project.org/
**
** This file is part of the QtDeclarative module of the Qt Toolkit.
**
** $QT_BEGIN_LICENSE:LGPL$
** GNU Lesser General Public License Usage
** This file may be used under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation and
** appearing in the file LICENSE.LGPL included in the packaging of this
** file. Please review the following information to ensure the GNU Lesser
** General Public License version 2.1 requirements will be met:
** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights. These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU General
** Public License version 3.0 as published by the Free Software Foundation
** and appearing in the file LICENSE.GPL included in the packaging of this
** file. Please review the following information to ensure the GNU General
** Public License version 3.0 requirements will be met:
** http://www.gnu.org/copyleft/gpl.html.
**
** Other Usage
** Alternatively, this file may be used in accordance with the terms and
** conditions contained in a signed written agreement between you and Nokia.
**
**
**
**
**
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qdebugmessageservice_p.h"
#include "qdeclarativedebugservice_p_p.h"

#include <QDataStream>

QT_BEGIN_NAMESPACE

Q_GLOBAL_STATIC(QDebugMessageService, declarativeDebugMessageService)

void DebugMessageHandler(QtMsgType type, const QMessageLogContext &ctxt,
                         const char *buf)
{
    QDebugMessageService::instance()->sendDebugMessage(type, ctxt, buf);
}

class QDebugMessageServicePrivate : public QDeclarativeDebugServicePrivate
{
public:
    QDebugMessageServicePrivate()
        : oldMsgHandler(0)
        , prevState(QDeclarativeDebugService::NotConnected)
    {
    }

    QMessageHandler oldMsgHandler;
    QDeclarativeDebugService::State prevState;
};

QDebugMessageService::QDebugMessageService(QObject *parent) :
    QDeclarativeDebugService(*(new QDebugMessageServicePrivate()),
                                   QLatin1String("DebugMessages"), 2, parent)
{
    Q_D(QDebugMessageService);

    registerService();
    if (state() == Enabled) {
        d->oldMsgHandler = qInstallMessageHandler(DebugMessageHandler);
        d->prevState = Enabled;
    }
}

QDebugMessageService *QDebugMessageService::instance()
{
    return declarativeDebugMessageService();
}

void QDebugMessageService::sendDebugMessage(QtMsgType type,
                                            const QMessageLogContext &ctxt,
                                            const char *buf)
{
    Q_D(QDebugMessageService);

    //We do not want to alter the message handling mechanism
    //We just eavesdrop and forward the messages to a port
    //only if a client is connected to it.
    QByteArray message;
    QDataStream ws(&message, QIODevice::WriteOnly);
    ws << QByteArray("MESSAGE") << type << QString::fromLocal8Bit(buf).toUtf8();
    ws << QString::fromLatin1(ctxt.file).toUtf8();
    ws << ctxt.line << QString::fromLatin1(ctxt.function).toUtf8();

    sendMessage(message);
    if (d->oldMsgHandler)
        (*d->oldMsgHandler)(type, ctxt, buf);
}

void QDebugMessageService::stateChanged(State state)
{
    Q_D(QDebugMessageService);

    if (state != Enabled && d->prevState == Enabled) {
        QMessageHandler handler = qInstallMessageHandler(d->oldMsgHandler);
        // has our handler been overwritten in between?
        if (handler != DebugMessageHandler)
            qInstallMessageHandler(handler);

    } else if (state == Enabled && d->prevState != Enabled) {
        d->oldMsgHandler = qInstallMessageHandler(DebugMessageHandler);

    }

    d->prevState = state;
}

QT_END_NAMESPACE
<|endoftext|>


<commit_before>/*
 * Node.cpp
 *
 *  Created on: Mar 16, 2015
 *      Author: jonno
 */

#include "Node.h"
#include "Chord.h"
#include <iomanip>
#include <sys/socket.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include "utils/csapp.h"
#include <iostream>
#include <sstream>
#include <assert.h>
#include <tuple>

using namespace std;

//	struct sockaddr_in address;
//	socklen_t length;
//	getpeername(file_descriptor, (sockaddr*)&address, &length);

Node::Node(std::string ip_addr, int port) :
		Node(0, ip_addr, port) {
}

Node::Node(int file_descriptor, std::string ip_addr, int port) {
	myFD = file_descriptor;
	if (myFD > 0) {
		myRIOBuffer = shared_ptr<RIOBuffered>(new RIOBuffered(myFD));
	}

	myIPAddress = ip_addr;
	myPort = port;
	myKey = Chord::hashKey(myIPAddress + ":" + to_string(myPort));
}

Node::~Node() {
	if (myFD > 0) {
		// let them know we are done.
		// myRIOBuffer->writeLine(&(Node::EXIT_MSG));
		shutdown(myFD, 0);
		Close(myFD);
	}
}

bool Node::Connect() {
	myFD = Open_clientfd(myIPAddress.c_str(), myPort);
	if (myFD > 0) {
		myRIOBuffer = shared_ptr<RIOBuffered>(new RIOBuffered(myFD));

		// other node sends hello message
		myRIOBuffer->readLine();
		// node's identity
		string message = myRIOBuffer->readLine();

		message = "Node " + this->toString();
		this->send(&message);
		message = myRIOBuffer->readLine();

		return message.find("Hello") == 0;
	}
	return false;
}

unsigned int Node::getKey() {
	return myKey;
}

std::string Node::readLine() {
	return myRIOBuffer->readLine();
}

size_t Node::send(const std::string* message) {
	assert(myFD > 0);
	return RIO::writeString(myFD, message);
}

void Node::processCommunication(std::shared_ptr<RIOBuffered> rio) {
	myRIOBuffer = rio;
	myFD = myRIOBuffer->getFD();

	auto chord = Chord::getInstance();

	stringstream stream;
	stream << "Hello " << std::hex << myKey << "\n";
	string message(stream.str());
	this->send(&message);

	while (true) {
		// block until it reads a line.
		message = this->readLine();
		cout << message;

		if (message.length() == 0) {
			// we have been shutdown by the other side. Bye!
			return;
		}
		stringstream str(message);
		string command;
		str >> command;

		// SET Queries - all specific to the instance of Chord
		if (command.compare("GET") == 0) {
			// get next part of get request
			str >> command;

			if (command.compare("SUCCESSOR") == 0) {
				size_t index = -1;
				str >> index;
				string response;

				if (chord->Successors.size() > index - 1) {
					response = chord->Successors[index - 1]->toString();
				} else {
					if (index == 1) {
						// this happens when there are no successors because it's the only one.
						response = chord->NodeInfo->toString();
					} else {
						response = Node::NOT_FOUND;
					}
				}
				this->send(&response);
			} else if (command.compare("PREDECESSOR") == 0) {
				size_t index;
				str >> index;
				string response;

				if (chord->Predecessors.size() > index - 1) {
					response = chord->Predecessors[index - 1]->toString();
				} else {
					if (index == 1) {
						// this happens when there are no successors because it's the only one.
						response = chord->NodeInfo->toString();
					} else {
						response = Node::NOT_FOUND;
					}
				}
				this->send(&response);
			} else if (command.compare("SUCCESSORS") == 0) {
				stringstream s;

				for (size_t i = 0; i < chord->Successors.size(); ++i) {
					s << i + 1<< ": " << chord->Successors[i]->toString();
				}

				string msg = s.str();
				this->send(&msg);
			} else if (command.compare("PREDECESSORS") == 0) {
				stringstream s;

				for (size_t i = 0; i < chord->Predecessors.size(); ++i) {
					s << i + 1 << ": " << chord->Predecessors[i]->toString();
				}

				string msg = s.str();
				this->send(&msg);
			} else if (command.compare("INFO") == 0) {
				string s = chord->toString();
				this->send(&s);
			} else if (command.compare("RANGE") == 0) {
				auto range = chord->getRange();
				stringstream msg_s;
				msg_s << hex << get<0>(range) << " " << hex << get<1>(range) << endl;
				string msg(msg_s.str());
				this->send(&msg);
			}

		}
		// Get Queries - all specific to the instance of Chord
		else if (command.compare("SET") == 0) {
			str >> command;

			if (command.compare("SUCCESSOR") == 0 || command.compare("PREDECESSOR") == 0) {
				int index;
				str >> index;

				string info;
				// get the Node info
				getline(str, info);
				auto node = Node::createFromInfo(info);
				if (node->getKey() == myKey) {
					// it is this node trying to make us point to it
					node = shared_ptr<Node>(this);
				}
				if (command.compare("SUCCESSOR") == 0) {
					// we don't want to reset up a link to the node.
					// they just told us to set it up.
					chord->setSuccessor(index, node, false);
				} else {
					chord->setPredecessor(index, node, false);
				}
			}
		}
		// Find queries
		else if (command.compare("FIND") == 0) {
			str >> command;
			if (command.compare("SUCCESSOR") == 0) {
				unsigned int key;
				str >> hex >> key;
				// find the closest predecessor
				// return successor of that
				auto node = chord->findSuccessor(key);
				string msg = node->toString();
				myRIOBuffer->writeLine(&msg);
			}
		}
		// Find queries
		else if (command.compare("SEARCH") == 0) {
			str >> command;
			if (command.compare("SUCCESSOR") == 0) {
				unsigned int key;
				str >> hex >> key;
				// get best option we know of
				auto node = chord->findSuccessor(key);

				while (true) {
					// check this entry for validity
					// pred is less than key
					auto pred = node->getPredecessor();
					cout << "Key Range (" << hex << pred->getKey() << ", " << hex << node->getKey() << "]" << endl;

					// check if key between predecessor and provided successor
					if (Chord::inRange(pred->getKey(), node->getKey(), key)) {
						// if so, then this is the successor!
						string msg = node->toString();
						myRIOBuffer->writeLine(&msg);
						break;
					} else {
						cout << "We have " << hex << node->getKey() << ". We are looking for " << hex << key << endl;
						// get the next best attempt
						node = node->FindSuccessor(key);
					}
				}

//				if (node->getKey() == Chord::getInstance()->NodeInfo->getKey()) {
//					// we just return ourselves
//					string msg = node->toString();
//					myRIOBuffer->writeLine(&msg);
//				}

				// perform iterative lookup of successor for a key
				// use FIND SUCCESSOR on nodes to get the actual value
				// find predecessor to the id
				// return successor of that
			}
		}
		// Find queries
		else if (command.compare("REQUEST") == 0) {
			// Join
		} else {
			if (command.compare(Node::EXIT_MSG) == 0) {
				// quit and close
				string goodbye = "Goodbye Friend!\n";
				myRIOBuffer->writeLine(&goodbye);
				return;
			}
			if (command.compare("LEAVE") == 0) {
				chord->LeaveRing();
			}
			cout << "Unknown Request: " << message;
		}
	}
}

std::string Node::toString() {
	stringstream result;
	result << hex << myKey << " " << myIPAddress << ":" << dec << myPort << endl;
	return result.str();
}

std::shared_ptr<Node> Node::getSuccessor(int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "GET SUCCESSOR " << index << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

std::shared_ptr<Node> Node::getPredecessor(int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "GET PREDECESSOR " << index << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

bool Node::isConnected() {
	return (myFD > 0);
}

std::shared_ptr<Node> Node::SearchSuccessor(unsigned int key) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "SEARCH SUCCESSOR " << hex << key << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

std::shared_ptr<Node> Node::FindSuccessor(unsigned int key) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "FIND SUCCESSOR " << hex << key << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

void Node::setSuccessor(Node* node, int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "SET SUCCESSOR " << index << " " << node->toString();
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
}

void Node::setPredecessor(Node* node, int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "SET PREDECESSOR " << index << " " << node->toString();
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
}

std::tuple<unsigned int, unsigned int> Node::getRange() {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "GET RANGE" << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();

	stringstream response(msg);

	unsigned int lower;
	unsigned int upper;

	response >> lower >> upper;
	return tuple<int, int>(lower, upper);
}

std::shared_ptr<Node> Node::createFromInfo(std::string info) {
	// this is in the same format as toString
	unsigned int key;
	std::string ip;
	int port;

	stringstream result(info);
	result >> hex >> key >> ip;

	// validate key and ip:port
	// TODO: remove && false
	if (key != Chord::hashKey(ip) && false) {
		// invalid key
		cout << "Node " << hex << key << " had an invalid key." << endl;
		return nullptr;
	}

	Chord::parseIPPort(ip, &ip, &port);

	return shared_ptr<Node>(new Node(ip, port));
}
<commit_msg>Enabled MP<commit_after>/*
 * Node.cpp
 *
 *  Created on: Mar 16, 2015
 *      Author: jonno
 */

#include "Node.h"
#include "Chord.h"
#include <iomanip>
#include <sys/socket.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include "utils/csapp.h"
#include <iostream>
#include <sstream>
#include <assert.h>
#include "MessageProcessor.h"
#include <tuple>

using namespace std;

//	struct sockaddr_in address;
//	socklen_t length;
//	getpeername(file_descriptor, (sockaddr*)&address, &length);

Node::Node(std::string ip_addr, int port) :
		Node(0, ip_addr, port) {
}

Node::Node(int file_descriptor, std::string ip_addr, int port) {
	myFD = file_descriptor;
	if (myFD > 0) {
		myRIOBuffer = shared_ptr<RIOBuffered>(new RIOBuffered(myFD));
	}

	myIPAddress = ip_addr;
	myPort = port;
	myKey = Chord::hashKey(myIPAddress + ":" + to_string(myPort));
}

Node::~Node() {
	if (myFD > 0) {
		// let them know we are done.
		// myRIOBuffer->writeLine(&(Node::EXIT_MSG));
		shutdown(myFD, 0);
		Close(myFD);
	}
}

bool Node::Connect() {
	myFD = Open_clientfd(myIPAddress.c_str(), myPort);
	if (myFD > 0) {
		myRIOBuffer = shared_ptr<RIOBuffered>(new RIOBuffered(myFD));

		// other node sends hello message
		myRIOBuffer->readLine();
		// node's identity
		string message = myRIOBuffer->readLine();

		message = "Node " + this->toString();
		this->send(&message);
		message = myRIOBuffer->readLine();

		return message.find("Hello") == 0;
	}
	return false;
}

unsigned int Node::getKey() {
	return myKey;
}

std::string Node::readLine() {
	return myRIOBuffer->readLine();
}

size_t Node::send(const std::string* message) {
	assert(myFD > 0);
	return RIO::writeString(myFD, message);
}

void Node::processCommunication(std::shared_ptr<RIOBuffered> rio) {
	myRIOBuffer = rio;
	myFD = myRIOBuffer->getFD();

	auto chord = Chord::getInstance();

	stringstream stream;
	stream << "Hello " << std::hex << myKey << "\n";
	string message(stream.str());
	this->send(&message);

	MessageProcessor processor(this);

	while (true) {
		// block until it reads a line.
		message = this->readLine();
		cout << message;

		if (message.length() == 0) {
			// we have been shutdown by the other side. Bye!
			return;
		}

		processor.handleMessage(message);

		stringstream str(message);
		string command;
		str >> command;

		// SET Queries - all specific to the instance of Chord
		if (command.compare("GET") == 0) {
			// get next part of get request
			str >> command;

			if (command.compare("SUCCESSOR") == 0) {
				size_t index = -1;
				str >> index;
				string response;

				if (chord->Successors.size() > index - 1) {
					response = chord->Successors[index - 1]->toString();
				} else {
					if (index == 1) {
						// this happens when there are no successors because it's the only one.
						response = chord->NodeInfo->toString();
					} else {
						response = Node::NOT_FOUND;
					}
				}
				this->send(&response);
			} else if (command.compare("PREDECESSOR") == 0) {
				size_t index;
				str >> index;
				string response;

				if (chord->Predecessors.size() > index - 1) {
					response = chord->Predecessors[index - 1]->toString();
				} else {
					if (index == 1) {
						// this happens when there are no successors because it's the only one.
						response = chord->NodeInfo->toString();
					} else {
						response = Node::NOT_FOUND;
					}
				}
				this->send(&response);
			} else if (command.compare("SUCCESSORS") == 0) {
				stringstream s;

				for (size_t i = 0; i < chord->Successors.size(); ++i) {
					s << i + 1<< ": " << chord->Successors[i]->toString();
				}

				string msg = s.str();
				this->send(&msg);
			} else if (command.compare("PREDECESSORS") == 0) {
				stringstream s;

				for (size_t i = 0; i < chord->Predecessors.size(); ++i) {
					s << i + 1 << ": " << chord->Predecessors[i]->toString();
				}

				string msg = s.str();
				this->send(&msg);
			} else if (command.compare("INFO") == 0) {
				string s = chord->toString();
				this->send(&s);
			} else if (command.compare("RANGE") == 0) {
				auto range = chord->getRange();
				stringstream msg_s;
				msg_s << hex << get<0>(range) << " " << hex << get<1>(range) << endl;
				string msg(msg_s.str());
				this->send(&msg);
			}

		}
		// Get Queries - all specific to the instance of Chord
		else if (command.compare("SET") == 0) {
			str >> command;

			if (command.compare("SUCCESSOR") == 0 || command.compare("PREDECESSOR") == 0) {
				int index;
				str >> index;

				string info;
				// get the Node info
				getline(str, info);
				auto node = Node::createFromInfo(info);
				if (node->getKey() == myKey) {
					// it is this node trying to make us point to it
					node = shared_ptr<Node>(this);
				}
				if (command.compare("SUCCESSOR") == 0) {
					// we don't want to reset up a link to the node.
					// they just told us to set it up.
					chord->setSuccessor(index, node, false);
				} else {
					chord->setPredecessor(index, node, false);
				}
			}
		}
		// Find queries
		else if (command.compare("FIND") == 0) {
			str >> command;
			if (command.compare("SUCCESSOR") == 0) {
				unsigned int key;
				str >> hex >> key;
				// find the closest predecessor
				// return successor of that
				auto node = chord->findSuccessor(key);
				string msg = node->toString();
				myRIOBuffer->writeLine(&msg);
			}
		}
		// Find queries
		else if (command.compare("SEARCH") == 0) {
			str >> command;
			if (command.compare("SUCCESSOR") == 0) {
				unsigned int key;
				str >> hex >> key;
				// get best option we know of
				auto node = chord->findSuccessor(key);

				while (true) {
					// check this entry for validity
					// pred is less than key
					auto pred = node->getPredecessor();
					cout << "Key Range (" << hex << pred->getKey() << ", " << hex << node->getKey() << "]" << endl;

					// check if key between predecessor and provided successor
					if (Chord::inRange(pred->getKey(), node->getKey(), key)) {
						// if so, then this is the successor!
						string msg = node->toString();
						myRIOBuffer->writeLine(&msg);
						break;
					} else {
						cout << "We have " << hex << node->getKey() << ". We are looking for " << hex << key << endl;
						// get the next best attempt
						node = node->FindSuccessor(key);
					}
				}

//				if (node->getKey() == Chord::getInstance()->NodeInfo->getKey()) {
//					// we just return ourselves
//					string msg = node->toString();
//					myRIOBuffer->writeLine(&msg);
//				}

				// perform iterative lookup of successor for a key
				// use FIND SUCCESSOR on nodes to get the actual value
				// find predecessor to the id
				// return successor of that
			}
		}
		// Find queries
		else if (command.compare("REQUEST") == 0) {
			// Join
		} else {
			if (command.compare(Node::EXIT_MSG) == 0) {
				// quit and close
				string goodbye = "Goodbye Friend!\n";
				myRIOBuffer->writeLine(&goodbye);
				return;
			}
			if (command.compare("LEAVE") == 0) {
				chord->LeaveRing();
			}
			cout << "Unknown Request: " << message;
		}
	}
}

std::string Node::toString() {
	stringstream result;
	result << hex << myKey << " " << myIPAddress << ":" << dec << myPort << endl;
	return result.str();
}

std::shared_ptr<Node> Node::getSuccessor(int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "GET SUCCESSOR " << index << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

std::shared_ptr<Node> Node::getPredecessor(int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "GET PREDECESSOR " << index << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

bool Node::isConnected() {
	return (myFD > 0);
}

std::shared_ptr<Node> Node::SearchSuccessor(unsigned int key) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "SEARCH SUCCESSOR " << hex << key << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

std::shared_ptr<Node> Node::FindSuccessor(unsigned int key) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "FIND SUCCESSOR " << hex << key << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();
	if (msg.compare(Node::NOT_FOUND) == 0) {
		return nullptr;
	}
	return Node::createFromInfo(msg);
}

void Node::setSuccessor(Node* node, int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "SET SUCCESSOR " << index << " " << node->toString();
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
}

void Node::setPredecessor(Node* node, int index) {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "SET PREDECESSOR " << index << " " << node->toString();
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
}

std::tuple<unsigned int, unsigned int> Node::getRange() {
	if (!this->isConnected()) {
		this->Connect();
	}
	stringstream s;
	s << "GET RANGE" << endl;
	string msg = s.str();
	myRIOBuffer->writeLine(&msg);
	msg = myRIOBuffer->readLine();

	stringstream response(msg);

	unsigned int lower;
	unsigned int upper;

	response >> lower >> upper;
	return tuple<int, int>(lower, upper);
}

std::shared_ptr<Node> Node::createFromInfo(std::string info) {
	// this is in the same format as toString
	unsigned int key;
	std::string ip;
	int port;

	stringstream result(info);
	result >> hex >> key >> ip;

	// validate key and ip:port
	// TODO: remove && false
	if (key != Chord::hashKey(ip) && false) {
		// invalid key
		cout << "Node " << hex << key << " had an invalid key." << endl;
		return nullptr;
	}

	Chord::parseIPPort(ip, &ip, &port);

	return shared_ptr<Node>(new Node(ip, port));
}
<|endoftext|>


<commit_before>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude = ( S.Latitude*Measurement.Latitude.Variance + Measurement.Latitude*S.Latitude.Variance ) / (S.Latitude.Variance+Measurement.Latitude.Variance);
    result.Longitude = ( S.Longitude*Measurement.Longitude.Variance + Measurement.Longitude*S.Longitude.Variance ) / (S.Longitude.Variance+Measurement.Longitude.Variance);
    result.Heading = ( S.Heading*Measurement.Heading.Variance + Measurement.Heading*S.Heading.Variance ) / (S.Heading.Variance+Measurement.Heading.Variance);

    result.Latitude.Variance = ( 1.0 / (1.0/S.Latitude.Variance + 1.0 / Measurement.Latitude.Variance) );
    result.Longitude.Variance = ( 1.0 / (1.0/S.Longitude.Variance + 1.0 / Measurement.Longitude.Variance) );
    result.Heading.Variance = ( 1.0 / (1.0/S.Heading.Variance + 1.0 / Measurement.Heading.Variance) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude = S.Latitude + Delta.Latitude;
    result.Longitude = S.Longitude + Delta.Longitude;
    result.Heading = S.Heading + Delta.Heading;

    result.Latitude.Variance = S.Latitude.Variance + Delta.Latitude.Variance;
    result.Longitude.Variance = S.Longitude.Variance + Delta.Longitude.Variance;
    result.Heading.Variance = S.Heading.Variance + Delta.Heading.Variance;
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::Instance().getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading + W*t) * M_PI/180.0;
    delta.Heading = W*t;
    delta.Latitude = R*(cos(theta) - cos(thetaPrime));
    delta.Longitude = R*(sin(thetaPrime) - sin(theta));
    // TODO - handle variances
    return delta;
}

Position PositionTracker::MeasurementFromIMUData(IMUData data)
{
    // Uses the "Destination point given distance and bearing from start point" described here:
    // http://www.movable-type.co.uk/scripts/latlong.html#destPoint
    Position measurement;
    double lat1 = _current_estimate.Latitude;
    double lon1 = _current_estimate.Latitude;
    double hed1 = _current_estimate.Heading;
    double dx = data.X /* * time*time */;
    double dy = data.Y /* * time*time */;
    double d = sqrt(dx*dx + dy*dy); // Distance travelled
    double R = 6378137; // radius of Earth
    measurement.Latitude = asin(sin(lat1)*cos(d/R)+cos(lat1)*sin(d/R)*cos(hed1));
    measurement.Longitude = (lon1 + atan2(sin(hed1)*sin(d/R)*cos(lat1),cos(d/R)-sin(lat1)*sin(measurement.Latitude)));
    measurement.Heading = (atan2((measurement.Longitude-lon1)*sin(lat1), cos(measurement.Latitude)*sin(lat1)-sin(measurement.Latitude)*cos(lat1)*cos(measurement.Longitude-lon1)));
    measurement.Heading = (measurement.Heading+180);
    while(measurement.Heading >= 360)
        measurement.Heading = (measurement.Heading - 360);
    while(measurement.Heading < 0)
        measurement.Heading = (measurement.Heading + 360);
    // TODO - handle variances
    return measurement;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude = data.Lat();
    measurement.Longitude = data.Long();
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading = data.Heading();
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{
    _current_estimate = UpdateWithMeasurement(_current_estimate, MeasurementFromIMUData(data));
}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}
<commit_msg>Adds some TODO comments to PositionTracker.<commit_after>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude = ( S.Latitude*Measurement.Latitude.Variance + Measurement.Latitude*S.Latitude.Variance ) / (S.Latitude.Variance+Measurement.Latitude.Variance);
    result.Longitude = ( S.Longitude*Measurement.Longitude.Variance + Measurement.Longitude*S.Longitude.Variance ) / (S.Longitude.Variance+Measurement.Longitude.Variance);
    result.Heading = ( S.Heading*Measurement.Heading.Variance + Measurement.Heading*S.Heading.Variance ) / (S.Heading.Variance+Measurement.Heading.Variance);

    result.Latitude.Variance = ( 1.0 / (1.0/S.Latitude.Variance + 1.0 / Measurement.Latitude.Variance) );
    result.Longitude.Variance = ( 1.0 / (1.0/S.Longitude.Variance + 1.0 / Measurement.Longitude.Variance) );
    result.Heading.Variance = ( 1.0 / (1.0/S.Heading.Variance + 1.0 / Measurement.Heading.Variance) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude = S.Latitude + Delta.Latitude;
    result.Longitude = S.Longitude + Delta.Longitude;
    result.Heading = S.Heading + Delta.Heading;

    result.Latitude.Variance = S.Latitude.Variance + Delta.Latitude.Variance;
    result.Longitude.Variance = S.Longitude.Variance + Delta.Longitude.Variance;
    result.Heading.Variance = S.Heading.Variance + Delta.Heading.Variance;
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    /*
     * TODO - Need to make sure this accounts for the time it takes to actually get to where we're going.
     * Perhaps fire this after the motion command is done.
     */
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::Instance().getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading + W*t) * M_PI/180.0;
    delta.Heading = W*t;
    delta.Latitude = R*(cos(theta) - cos(thetaPrime));
    delta.Longitude = R*(sin(thetaPrime) - sin(theta));
    // TODO - handle variances
    return delta;
}

Position PositionTracker::MeasurementFromIMUData(IMUData data)
{
    // Uses the "Destination point given distance and bearing from start point" described here:
    // http://www.movable-type.co.uk/scripts/latlong.html#destPoint
    Position measurement;
    double lat1 = _current_estimate.Latitude;
    double lon1 = _current_estimate.Latitude;
    double hed1 = _current_estimate.Heading;
    double dx = data.X /* * time*time */;
    double dy = data.Y /* * time*time */;
    double d = sqrt(dx*dx + dy*dy); // Distance travelled
    double R = 6378137; // radius of Earth
    measurement.Latitude = asin(sin(lat1)*cos(d/R)+cos(lat1)*sin(d/R)*cos(hed1));
    measurement.Longitude = (lon1 + atan2(sin(hed1)*sin(d/R)*cos(lat1),cos(d/R)-sin(lat1)*sin(measurement.Latitude)));
    measurement.Heading = (atan2((measurement.Longitude-lon1)*sin(lat1), cos(measurement.Latitude)*sin(lat1)-sin(measurement.Latitude)*cos(lat1)*cos(measurement.Longitude-lon1)));
    measurement.Heading = (measurement.Heading+180);
    while(measurement.Heading >= 360)
        measurement.Heading = (measurement.Heading - 360);
    while(measurement.Heading < 0)
        measurement.Heading = (measurement.Heading + 360);
    // TODO - handle variances
    return measurement;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude = data.Lat();
    measurement.Longitude = data.Long();
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading = data.Heading();
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{
    _current_estimate = UpdateWithMeasurement(_current_estimate, MeasurementFromIMUData(data));
}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    /*
     * TODO - Need to make sure this accounts for the time it takes to actually get to where we're going.
     * Perhaps fire this after the motion command is done.
     */
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}
<|endoftext|>