<commit_before>#include "FOXFIRE_MPU9150.h"

////////////////////////////////////////////////////////////
///////// I2C functions to get all values easier ///////////
////////////////////////////////////////////////////////////

FOXFIRE_MPU9150::FOXFIRE_MPU9150()
{
  _addr_compass = 0x0C; //Digital Compass is at 0x0C
  _addr_motion = 0x68; //Default I2C address is 0x68, AD0 low = 0x68, AD0 high = 0x69
}

boolean FOXFIRE_MPU9150::begin(int addrI2C)
{
  Wire.beginTransmission(addrI2C);
  return ( Wire.endTransmission(true) == 0 ? true : false );
}

int FOXFIRE_MPU9150::readSensor(int addrI2C, int addrL, int addrH){
  Wire.beginTransmission(addrI2C);
  Wire.write(addrL);
  Wire.endTransmission(false);

  Wire.requestFrom(addrI2C, 1, true);
  byte L = Wire.read();

  Wire.beginTransmission(addrI2C);
  Wire.write(addrH);
  Wire.endTransmission(false);

  Wire.requestFrom(addrI2C, 1, true);
  byte H = Wire.read();

  return (int16_t)((H<<8)+L);
}

int FOXFIRE_MPU9150::readSensor(int addrI2C, int addr){
  Wire.beginTransmission(addrI2C);
  Wire.write(addr);
  Wire.endTransmission(false);

  Wire.requestFrom(addrI2C, 1, true);
  return Wire.read();
}

int FOXFIRE_MPU9150::writeSensor(int addrI2C, int addr, int data){
  Wire.beginTransmission(addrI2C);
  Wire.write(addr);
  Wire.write(data);
  Wire.endTransmission(true);

  return 1;
}
<commit_msg>Re-enable Wire.begin<commit_after>#include "FOXFIRE_MPU9150.h"

////////////////////////////////////////////////////////////
///////// I2C functions to get all values easier ///////////
////////////////////////////////////////////////////////////

FOXFIRE_MPU9150::FOXFIRE_MPU9150()
{
	_addr_compass = 0x0C; //Digital Compass is at 0x0C
	_addr_motion = 0x68; //Default I2C address is 0x68, AD0 low = 0x68, AD0 high = 0x69
}

boolean FOXFIRE_MPU9150::begin(int addrI2C)
{
	Wire.begin();
	Wire.beginTransmission(addrI2C);
	return ( Wire.endTransmission(true) == 0 ? true : false );
}

int FOXFIRE_MPU9150::readSensor(int addrI2C, int addrL, int addrH){
	Wire.beginTransmission(addrI2C);
	Wire.write(addrL);
	Wire.endTransmission(false);

	Wire.requestFrom(addrI2C, 1, true);
	byte L = Wire.read();

	Wire.beginTransmission(addrI2C);
	Wire.write(addrH);
	Wire.endTransmission(false);

	Wire.requestFrom(addrI2C, 1, true);
	byte H = Wire.read();

	return (int16_t)((H<<8)+L);
}

int FOXFIRE_MPU9150::readSensor(int addrI2C, int addr){
	Wire.beginTransmission(addrI2C);
	Wire.write(addr);
	Wire.endTransmission(false);

	Wire.requestFrom(addrI2C, 1, true);

	return Wire.read();
}

int FOXFIRE_MPU9150::writeSensor(int addrI2C, int addr, int data){
	Wire.beginTransmission(addrI2C);
	Wire.write(addr);
	Wire.write(data);
	Wire.endTransmission(true);

	return 1;
}
<|endoftext|>


<commit_before>/****************************************************************/
/*               DO NOT MODIFY THIS HEADER                      */
/* MOOSE - Multiphysics Object Oriented Simulation Environment  */
/*                                                              */
/*           (c) 2010 Battelle Energy Alliance, LLC             */
/*                   ALL RIGHTS RESERVED                        */
/*                                                              */
/*          Prepared by Battelle Energy Alliance, LLC           */
/*            Under Contract No. DE-AC07-05ID14517              */
/*            With the U. S. Department of Energy               */
/*                                                              */
/*            See COPYRIGHT for full restrictions               */
/****************************************************************/

#include "Problem.h"
#include "Factory.h"

template<>
InputParameters validParams<Problem>()
{
  InputParameters params;
  params.addParam<std::string>("name", "Problem", "The name of the object");
  return params;
}

Problem::Problem(const std::string & name, InputParameters parameters):
  _name(name),
  _pars(parameters),
  _output_initial(false)
{
  unsigned int n_threads = libMesh::n_threads();

  _real_zero.resize(n_threads, 0.);
  _zero.resize(n_threads);
  _grad_zero.resize(n_threads);
  _second_zero.resize(n_threads);

  _functions.resize(n_threads);
}

Problem::~Problem()
{
  unsigned int n_threads = libMesh::n_threads();
  for (unsigned int i = 0; i < n_threads; i++)
  {
    _zero[i].release();
    _grad_zero[i].release();
    _second_zero[i].release();
  }
  _real_zero.release();
  _zero.release();
  _grad_zero.release();
  _second_zero.release();

  for (unsigned int i = 0; i < n_threads; i++)
    for (std::map<std::string, Function *>::iterator it = _functions[i].begin(); it != _functions[i].end(); ++it)
      delete it->second;
}

void
Problem::addFunction(std::string type, const std::string & name, InputParameters parameters)
{
  parameters.set<Problem *>("_problem") = this;
  for (THREAD_ID tid = 0; tid < libMesh::n_threads(); tid++)
  {
    parameters.set<THREAD_ID>("_tid") = tid;
    Function * func = static_cast<Function *>(Factory::instance()->create(type, name, parameters));
    _functions[tid][name] = func;
  }
}

Function &
Problem::getFunction(const std::string & name, THREAD_ID tid)
{
  return *_functions[tid][name];
}

<commit_msg>In debug, check that function exists before dereferencing<commit_after>/****************************************************************/
/*               DO NOT MODIFY THIS HEADER                      */
/* MOOSE - Multiphysics Object Oriented Simulation Environment  */
/*                                                              */
/*           (c) 2010 Battelle Energy Alliance, LLC             */
/*                   ALL RIGHTS RESERVED                        */
/*                                                              */
/*          Prepared by Battelle Energy Alliance, LLC           */
/*            Under Contract No. DE-AC07-05ID14517              */
/*            With the U. S. Department of Energy               */
/*                                                              */
/*            See COPYRIGHT for full restrictions               */
/****************************************************************/

#include "Problem.h"
#include "Factory.h"

template<>
InputParameters validParams<Problem>()
{
  InputParameters params;
  params.addParam<std::string>("name", "Problem", "The name of the object");
  return params;
}

Problem::Problem(const std::string & name, InputParameters parameters):
  _name(name),
  _pars(parameters),
  _output_initial(false)
{
  unsigned int n_threads = libMesh::n_threads();

  _real_zero.resize(n_threads, 0.);
  _zero.resize(n_threads);
  _grad_zero.resize(n_threads);
  _second_zero.resize(n_threads);

  _functions.resize(n_threads);
}

Problem::~Problem()
{
  unsigned int n_threads = libMesh::n_threads();
  for (unsigned int i = 0; i < n_threads; i++)
  {
    _zero[i].release();
    _grad_zero[i].release();
    _second_zero[i].release();
  }
  _real_zero.release();
  _zero.release();
  _grad_zero.release();
  _second_zero.release();

  for (unsigned int i = 0; i < n_threads; i++)
    for (std::map<std::string, Function *>::iterator it = _functions[i].begin(); it != _functions[i].end(); ++it)
      delete it->second;
}

void
Problem::addFunction(std::string type, const std::string & name, InputParameters parameters)
{
  parameters.set<Problem *>("_problem") = this;
  for (THREAD_ID tid = 0; tid < libMesh::n_threads(); tid++)
  {
    parameters.set<THREAD_ID>("_tid") = tid;
    Function * func = static_cast<Function *>(Factory::instance()->create(type, name, parameters));
    _functions[tid][name] = func;
  }
}

Function &
Problem::getFunction(const std::string & name, THREAD_ID tid)
{
  Function * function = _functions[tid][name];
  mooseAssert(function, "Unable to find function " + name);
  return *function;
}

<|endoftext|>


<commit_before>#ifndef ENTT_POLY_POLY_HPP
#define ENTT_POLY_POLY_HPP

#include <cstddef>
#include <functional>
#include <tuple>
#include <type_traits>
#include <utility>
#include "../config/config.h"
#include "../core/any.hpp"
#include "../core/type_info.hpp"
#include "../core/type_traits.hpp"
#include "fwd.hpp"

namespace entt {

/*! @brief Inspector class used to infer the type of the virtual table. */
struct poly_inspector {
    /**
     * @brief Generic conversion operator (definition only).
     * @tparam Type Type to which conversion is requested.
     */
    template <class Type>
    operator Type &&() const;

    /**
     * @brief Dummy invocation function (definition only).
     * @tparam Member Index of the function to invoke.
     * @tparam Args Types of arguments to pass to the function.
     * @param args The arguments to pass to the function.
     * @return A poly inspector convertible to any type.
     */
    template<auto Member, typename... Args>
    poly_inspector invoke(Args &&... args) const;

    /*! @copydoc invoke */
    template<auto Member, typename... Args>
    poly_inspector invoke(Args &&... args);
};

/**
 * @brief Static virtual table factory.
 * @tparam Concept Concept descriptor.
 * @tparam Len Size of the storage reserved for the small buffer optimization.
 * @tparam Align Optional alignment requirement.
 */
template<typename Concept, std::size_t Len, std::size_t... Align>
class poly_vtable {
    using inspector = typename Concept::template type<poly_inspector>;

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(*)(inspector &, Args...)) -> Ret(*)(basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(*)(const inspector &, Args...)) -> Ret(*)(const basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(*)(Args...)) -> Ret(*)(const basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(inspector:: *)(Args...)) -> Ret(*)(basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(inspector:: *)(Args...) const) -> Ret(*)(const basic_any<Len, Align...> &, Args...);

    template<auto... Candidate>
    static auto make_vtable(value_list<Candidate...>)
    -> std::tuple<decltype(vtable_entry(Candidate))...>;

    template<typename... Func>
    [[nodiscard]] static constexpr auto make_vtable(type_list<Func...>)  {
        if constexpr(sizeof...(Func) == 0) {
            return decltype(make_vtable(typename Concept::template impl<inspector>{})){};
        } else if constexpr((std::is_function_v<Func> && ...)) {
            return decltype(std::make_tuple(vtable_entry(std::declval<Func inspector:: *>())...)){};
        }
    }

    template<typename Type, auto Candidate, typename Ret, typename Any, typename... Args>
    static void fill_vtable_entry(Ret(* &entry)(Any &, Args...)) {
        if constexpr(std::is_invocable_r_v<Ret, decltype(Candidate), Args...>) {
            entry = +[](Any &, Args... args) -> Ret {
                return std::invoke(Candidate, std::forward<Args>(args)...);
            };
        } else {
            entry = +[](Any &instance, Args... args) -> Ret {
                return static_cast<Ret>(std::invoke(Candidate, any_cast<constness_as_t<Type, Any> &>(instance), std::forward<Args>(args)...));
            };
        }
    }

    template<typename Type, auto... Index>
    [[nodiscard]] static auto fill_vtable(std::index_sequence<Index...>) {
        type impl{};
        (fill_vtable_entry<Type, value_list_element_v<Index, typename Concept::template impl<Type>>>(std::get<Index>(impl)), ...);
        return impl;
    }

public:
    /*! @brief Virtual table type. */
    using type = decltype(make_vtable(Concept{}));

    /**
     * @brief Returns a static virtual table for a specific concept and type.
     * @tparam Type The type for which to generate the virtual table.
     * @return A static virtual table for the given concept and type.
     */
    template<typename Type>
    [[nodiscard]] static const auto * instance() {
        static_assert(std::is_same_v<Type, std::decay_t<Type>>, "Type differs from its decayed form");
        static const auto vtable = fill_vtable<Type>(std::make_index_sequence<Concept::template impl<Type>::size>{});
        return &vtable;
    }
};

/**
 * @brief Poly base class used to inject functionalities into concepts.
 * @tparam Poly The outermost poly class.
 */
template<typename Poly>
struct poly_base {
    /**
     * @brief Invokes a function from the static virtual table.
     * @tparam Member Index of the function to invoke.
     * @tparam Args Types of arguments to pass to the function.
     * @param self A reference to the poly object that made the call.
     * @param args The arguments to pass to the function.
     * @return The return value of the invoked function, if any.
     */
    template<auto Member, typename... Args>
    [[nodiscard]] decltype(auto) invoke(const poly_base &self, Args &&... args) const {
        const auto &poly = static_cast<const Poly &>(self);
        return std::get<Member>(*poly.vtable)(poly.storage, std::forward<Args>(args)...);
    }

    /*! @copydoc invoke */
    template<auto Member, typename... Args>
    [[nodiscard]] decltype(auto) invoke(poly_base &self, Args &&... args) {
        auto &poly = static_cast<Poly &>(self);
        return std::get<Member>(*poly.vtable)(poly.storage, std::forward<Args>(args)...);
    }
};

/**
 * @brief Shortcut for calling `poly_base<Type>::invoke`.
 * @tparam Member Index of the function to invoke.
 * @tparam Poly A fully defined poly object.
 * @tparam Args Types of arguments to pass to the function.
 * @param self A reference to the poly object that made the call.
 * @param args The arguments to pass to the function.
 * @return The return value of the invoked function, if any.
 */
template<auto Member, typename Poly, typename... Args>
decltype(auto) poly_call(Poly &&self, Args &&... args) {
    return std::forward<Poly>(self).template invoke<Member>(self, std::forward<Args>(args)...);
}

/**
 * @brief Static polymorphism made simple and within everyone's reach.
 *
 * Static polymorphism is a very powerful tool in C++, albeit sometimes
 * cumbersome to obtain.

 * This class aims to make it simple and easy to use.
 *
 * @note
 * Both deduced and defined static virtual tables are supported.

 * Moreover, the `poly` class template also works with unmanaged objects.
 *
 * @tparam Concept Concept descriptor.
 * @tparam Len Size of the storage reserved for the small buffer optimization.
 * @tparam Align Optional alignment requirement.
 */
template<typename Concept, std::size_t Len, std::size_t... Align>
class basic_poly: private Concept::template type<poly_base<basic_poly<Concept, Len, Align...>>> {
    /*! @brief A poly base is allowed to snoop into a poly object. */
    friend struct poly_base<basic_poly>;

    using vtable_type = typename poly_vtable<Concept, Len, Align...>::type;

public:
    /*! @brief Concept type. */
    using concept_type = typename Concept::template type<poly_base<basic_poly>>;

    /*! @brief Default constructor. */
    basic_poly() ENTT_NOEXCEPT
        : storage{},
          vtable{}
    {}

    /**
     * @brief Constructs a poly by directly initializing the new object.
     * @tparam Type Type of object to use to initialize the poly.
     * @tparam Args Types of arguments to use to construct the new instance.
     * @param args Parameters to use to construct the instance.
     */
    template<typename Type, typename... Args>
    explicit basic_poly(std::in_place_type_t<Type>, Args &&... args)
        : storage{std::in_place_type<Type>, std::forward<Args>(args)...},
          vtable{poly_vtable<Concept, Len, Align...>::template instance<std::remove_const_t<std::remove_reference_t<Type>>>()}
    {}

    /**
     * @brief Constructs a poly that holds an unmanaged object.
     * @tparam Type Type of object to use to initialize the poly.
     * @param value An instance of an object to use to initialize the poly.
     */
    template<typename Type>
    basic_poly(std::reference_wrapper<Type> value)
        : basic_poly{std::in_place_type<Type &>, value.get()}
    {}

    /**
     * @brief Constructs a poly from a given value.
     * @tparam Type Type of object to use to initialize the poly.
     * @param value An instance of an object to use to initialize the poly.
     */
    template<typename Type, typename = std::enable_if_t<!std::is_same_v<std::remove_cv_t<std::remove_reference_t<Type>>, basic_poly>>>
    basic_poly(Type &&value) ENTT_NOEXCEPT
        : basic_poly{std::in_place_type<std::remove_cv_t<std::remove_reference_t<Type>>>, std::forward<Type>(value)}
    {}

    /**
     * @brief Copy constructor.
     * @param other The instance to copy from.
     */
    basic_poly(const basic_poly &other) = default;

    /**
     * @brief Move constructor.
     * @param other The instance to move from.
     */
    basic_poly(basic_poly &&other) ENTT_NOEXCEPT
        : basic_poly{}
    {
        swap(*this, other);
    }

    /**
     * @brief Assignment operator.
     * @param other The instance to assign from.
     * @return This poly object.
     */
    basic_poly & operator=(basic_poly other) {
        swap(other, *this);
        return *this;
    }

    /**
     * @brief Returns the type of the contained object.
     * @return The type of the contained object, if any.
     */
    [[nodiscard]] type_info type() const ENTT_NOEXCEPT {
        return storage.type();
    }

    /**
     * @brief Returns an opaque pointer to the contained instance.
     * @return An opaque pointer the contained instance, if any.
     */
    [[nodiscard]] const void * data() const ENTT_NOEXCEPT {
        return storage.data();
    }

    /*! @copydoc data */
    [[nodiscard]] void * data() ENTT_NOEXCEPT {
        return storage.data();
    }

    /**
     * @brief Replaces the contained object by creating a new instance directly.
     * @tparam Type Type of object to use to initialize the poly.
     * @tparam Args Types of arguments to use to construct the new instance.
     * @param args Parameters to use to construct the instance.
     */
    template<typename Type, typename... Args>
    void emplace(Args &&... args) {
        *this = basic_poly{std::in_place_type<Type>, std::forward<Args>(args)...};
    }

    /*! @brief Destroys contained object */
    void reset() {
        *this = basic_poly{};
    }

    /**
     * @brief Returns false if a poly is empty, true otherwise.
     * @return False if the poly is empty, true otherwise.
     */
    [[nodiscard]] explicit operator bool() const ENTT_NOEXCEPT {
        return !(vtable == nullptr);
    }

    /**
     * @brief Returns a pointer to the underlying concept.
     * @return A pointer to the underlying concept.
     */
    [[nodiscard]] concept_type * operator->() ENTT_NOEXCEPT {
        return this;
    }

    /*! @copydoc operator-> */
    [[nodiscard]] const concept_type * operator->() const ENTT_NOEXCEPT {
        return this;
    }

    /**
     * @brief Swaps two poly objects.
     * @param lhs A valid poly object.
     * @param rhs A valid poly object.
     */
    friend void swap(basic_poly &lhs, basic_poly &rhs) {
        using std::swap;
        swap(lhs.storage, rhs.storage);
        swap(lhs.vtable, rhs.vtable);
    }

    /**
     * @brief Aliasing constructor.
     * @return A poly that shares a reference to an unmanaged object.
     */
    [[nodiscard]] basic_poly as_ref() ENTT_NOEXCEPT {
        basic_poly ref = std::as_const(*this).as_ref();
        ref.storage = storage.as_ref();
        return ref;
    }

    /*! @copydoc as_ref */
    [[nodiscard]] basic_poly as_ref() const ENTT_NOEXCEPT {
        basic_poly ref{};
        ref.storage = storage.as_ref();
        ref.vtable = vtable;
        return ref;
    }

private:
    basic_any<Len, Align...> storage;
    const vtable_type *vtable;
};

}

#endif
<commit_msg>poly: make vs2017 happy again<commit_after>#ifndef ENTT_POLY_POLY_HPP
#define ENTT_POLY_POLY_HPP

#include <cstddef>
#include <functional>
#include <tuple>
#include <type_traits>
#include <utility>
#include "../config/config.h"
#include "../core/any.hpp"
#include "../core/type_info.hpp"
#include "../core/type_traits.hpp"
#include "fwd.hpp"

namespace entt {

/*! @brief Inspector class used to infer the type of the virtual table. */
struct poly_inspector {
    /**
     * @brief Generic conversion operator (definition only).
     * @tparam Type Type to which conversion is requested.
     */
    template <class Type>
    operator Type &&() const;

    /**
     * @brief Dummy invocation function (definition only).
     * @tparam Member Index of the function to invoke.
     * @tparam Args Types of arguments to pass to the function.
     * @param args The arguments to pass to the function.
     * @return A poly inspector convertible to any type.
     */
    template<auto Member, typename... Args>
    poly_inspector invoke(Args &&... args) const;

    /*! @copydoc invoke */
    template<auto Member, typename... Args>
    poly_inspector invoke(Args &&... args);
};

/**
 * @brief Static virtual table factory.
 * @tparam Concept Concept descriptor.
 * @tparam Len Size of the storage reserved for the small buffer optimization.
 * @tparam Align Optional alignment requirement.
 */
template<typename Concept, std::size_t Len, std::size_t... Align>
class poly_vtable {
    using inspector = typename Concept::template type<poly_inspector>;

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(*)(inspector &, Args...)) -> Ret(*)(basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(*)(const inspector &, Args...)) -> Ret(*)(const basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(*)(Args...)) -> Ret(*)(const basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(inspector:: *)(Args...)) -> Ret(*)(basic_any<Len, Align...> &, Args...);

    template<typename Ret, typename... Args>
    static auto vtable_entry(Ret(inspector:: *)(Args...) const) -> Ret(*)(const basic_any<Len, Align...> &, Args...);

    template<auto... Candidate>
    static auto make_vtable(value_list<Candidate...>)
    -> decltype(std::make_tuple(vtable_entry(Candidate)...));

    template<typename... Func>
    [[nodiscard]] static constexpr auto make_vtable(type_list<Func...>)  {
        if constexpr(sizeof...(Func) == 0) {
            return decltype(make_vtable(typename Concept::template impl<inspector>{})){};
        } else if constexpr((std::is_function_v<Func> && ...)) {
            return decltype(std::make_tuple(vtable_entry(std::declval<Func inspector:: *>())...)){};
        }
    }

    template<typename Type, auto Candidate, typename Ret, typename Any, typename... Args>
    static void fill_vtable_entry(Ret(* &entry)(Any &, Args...)) {
        if constexpr(std::is_invocable_r_v<Ret, decltype(Candidate), Args...>) {
            entry = +[](Any &, Args... args) -> Ret {
                return std::invoke(Candidate, std::forward<Args>(args)...);
            };
        } else {
            entry = +[](Any &instance, Args... args) -> Ret {
                return static_cast<Ret>(std::invoke(Candidate, any_cast<constness_as_t<Type, Any> &>(instance), std::forward<Args>(args)...));
            };
        }
    }

    template<typename Type, auto... Index>
    [[nodiscard]] static auto fill_vtable(std::index_sequence<Index...>) {
        type impl{};
        (fill_vtable_entry<Type, value_list_element_v<Index, typename Concept::template impl<Type>>>(std::get<Index>(impl)), ...);
        return impl;
    }

public:
    /*! @brief Virtual table type. */
    using type = decltype(make_vtable(Concept{}));

    /**
     * @brief Returns a static virtual table for a specific concept and type.
     * @tparam Type The type for which to generate the virtual table.
     * @return A static virtual table for the given concept and type.
     */
    template<typename Type>
    [[nodiscard]] static const auto * instance() {
        static_assert(std::is_same_v<Type, std::decay_t<Type>>, "Type differs from its decayed form");
        static const auto vtable = fill_vtable<Type>(std::make_index_sequence<Concept::template impl<Type>::size>{});
        return &vtable;
    }
};

/**
 * @brief Poly base class used to inject functionalities into concepts.
 * @tparam Poly The outermost poly class.
 */
template<typename Poly>
struct poly_base {
    /**
     * @brief Invokes a function from the static virtual table.
     * @tparam Member Index of the function to invoke.
     * @tparam Args Types of arguments to pass to the function.
     * @param self A reference to the poly object that made the call.
     * @param args The arguments to pass to the function.
     * @return The return value of the invoked function, if any.
     */
    template<auto Member, typename... Args>
    [[nodiscard]] decltype(auto) invoke(const poly_base &self, Args &&... args) const {
        const auto &poly = static_cast<const Poly &>(self);
        return std::get<Member>(*poly.vtable)(poly.storage, std::forward<Args>(args)...);
    }

    /*! @copydoc invoke */
    template<auto Member, typename... Args>
    [[nodiscard]] decltype(auto) invoke(poly_base &self, Args &&... args) {
        auto &poly = static_cast<Poly &>(self);
        return std::get<Member>(*poly.vtable)(poly.storage, std::forward<Args>(args)...);
    }
};

/**
 * @brief Shortcut for calling `poly_base<Type>::invoke`.
 * @tparam Member Index of the function to invoke.
 * @tparam Poly A fully defined poly object.
 * @tparam Args Types of arguments to pass to the function.
 * @param self A reference to the poly object that made the call.
 * @param args The arguments to pass to the function.
 * @return The return value of the invoked function, if any.
 */
template<auto Member, typename Poly, typename... Args>
decltype(auto) poly_call(Poly &&self, Args &&... args) {
    return std::forward<Poly>(self).template invoke<Member>(self, std::forward<Args>(args)...);
}

/**
 * @brief Static polymorphism made simple and within everyone's reach.
 *
 * Static polymorphism is a very powerful tool in C++, albeit sometimes
 * cumbersome to obtain.

 * This class aims to make it simple and easy to use.
 *
 * @note
 * Both deduced and defined static virtual tables are supported.

 * Moreover, the `poly` class template also works with unmanaged objects.
 *
 * @tparam Concept Concept descriptor.
 * @tparam Len Size of the storage reserved for the small buffer optimization.
 * @tparam Align Optional alignment requirement.
 */
template<typename Concept, std::size_t Len, std::size_t... Align>
class basic_poly: private Concept::template type<poly_base<basic_poly<Concept, Len, Align...>>> {
    /*! @brief A poly base is allowed to snoop into a poly object. */
    friend struct poly_base<basic_poly>;

    using vtable_type = typename poly_vtable<Concept, Len, Align...>::type;

public:
    /*! @brief Concept type. */
    using concept_type = typename Concept::template type<poly_base<basic_poly>>;

    /*! @brief Default constructor. */
    basic_poly() ENTT_NOEXCEPT
        : storage{},
          vtable{}
    {}

    /**
     * @brief Constructs a poly by directly initializing the new object.
     * @tparam Type Type of object to use to initialize the poly.
     * @tparam Args Types of arguments to use to construct the new instance.
     * @param args Parameters to use to construct the instance.
     */
    template<typename Type, typename... Args>
    explicit basic_poly(std::in_place_type_t<Type>, Args &&... args)
        : storage{std::in_place_type<Type>, std::forward<Args>(args)...},
          vtable{poly_vtable<Concept, Len, Align...>::template instance<std::remove_const_t<std::remove_reference_t<Type>>>()}
    {}

    /**
     * @brief Constructs a poly that holds an unmanaged object.
     * @tparam Type Type of object to use to initialize the poly.
     * @param value An instance of an object to use to initialize the poly.
     */
    template<typename Type>
    basic_poly(std::reference_wrapper<Type> value)
        : basic_poly{std::in_place_type<Type &>, value.get()}
    {}

    /**
     * @brief Constructs a poly from a given value.
     * @tparam Type Type of object to use to initialize the poly.
     * @param value An instance of an object to use to initialize the poly.
     */
    template<typename Type, typename = std::enable_if_t<!std::is_same_v<std::remove_cv_t<std::remove_reference_t<Type>>, basic_poly>>>
    basic_poly(Type &&value) ENTT_NOEXCEPT
        : basic_poly{std::in_place_type<std::remove_cv_t<std::remove_reference_t<Type>>>, std::forward<Type>(value)}
    {}

    /**
     * @brief Copy constructor.
     * @param other The instance to copy from.
     */
    basic_poly(const basic_poly &other) = default;

    /**
     * @brief Move constructor.
     * @param other The instance to move from.
     */
    basic_poly(basic_poly &&other) ENTT_NOEXCEPT
        : basic_poly{}
    {
        swap(*this, other);
    }

    /**
     * @brief Assignment operator.
     * @param other The instance to assign from.
     * @return This poly object.
     */
    basic_poly & operator=(basic_poly other) {
        swap(other, *this);
        return *this;
    }

    /**
     * @brief Returns the type of the contained object.
     * @return The type of the contained object, if any.
     */
    [[nodiscard]] type_info type() const ENTT_NOEXCEPT {
        return storage.type();
    }

    /**
     * @brief Returns an opaque pointer to the contained instance.
     * @return An opaque pointer the contained instance, if any.
     */
    [[nodiscard]] const void * data() const ENTT_NOEXCEPT {
        return storage.data();
    }

    /*! @copydoc data */
    [[nodiscard]] void * data() ENTT_NOEXCEPT {
        return storage.data();
    }

    /**
     * @brief Replaces the contained object by creating a new instance directly.
     * @tparam Type Type of object to use to initialize the poly.
     * @tparam Args Types of arguments to use to construct the new instance.
     * @param args Parameters to use to construct the instance.
     */
    template<typename Type, typename... Args>
    void emplace(Args &&... args) {
        *this = basic_poly{std::in_place_type<Type>, std::forward<Args>(args)...};
    }

    /*! @brief Destroys contained object */
    void reset() {
        *this = basic_poly{};
    }

    /**
     * @brief Returns false if a poly is empty, true otherwise.
     * @return False if the poly is empty, true otherwise.
     */
    [[nodiscard]] explicit operator bool() const ENTT_NOEXCEPT {
        return !(vtable == nullptr);
    }

    /**
     * @brief Returns a pointer to the underlying concept.
     * @return A pointer to the underlying concept.
     */
    [[nodiscard]] concept_type * operator->() ENTT_NOEXCEPT {
        return this;
    }

    /*! @copydoc operator-> */
    [[nodiscard]] const concept_type * operator->() const ENTT_NOEXCEPT {
        return this;
    }

    /**
     * @brief Swaps two poly objects.
     * @param lhs A valid poly object.
     * @param rhs A valid poly object.
     */
    friend void swap(basic_poly &lhs, basic_poly &rhs) {
        using std::swap;
        swap(lhs.storage, rhs.storage);
        swap(lhs.vtable, rhs.vtable);
    }

    /**
     * @brief Aliasing constructor.
     * @return A poly that shares a reference to an unmanaged object.
     */
    [[nodiscard]] basic_poly as_ref() ENTT_NOEXCEPT {
        basic_poly ref = std::as_const(*this).as_ref();
        ref.storage = storage.as_ref();
        return ref;
    }

    /*! @copydoc as_ref */
    [[nodiscard]] basic_poly as_ref() const ENTT_NOEXCEPT {
        basic_poly ref{};
        ref.storage = storage.as_ref();
        ref.vtable = vtable;
        return ref;
    }

private:
    basic_any<Len, Align...> storage;
    const vtable_type *vtable;
};

}

#endif
<|endoftext|>


<commit_before>/** \copyright
 * Copyright (c) 2014, Balazs Racz
 * 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.
 *
 * 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.
 *
 * \file UpdateLoop.hxx
 *
 * Interface to the global update loop.
 *
 * @author Balazs Racz
 * @date 10 May 2014
 */

#ifndef _DCC_UPDATELOOP_HXX_
#define _DCC_UPDATELOOP_HXX_

#include "utils/Singleton.hxx"
#include "dcc/PacketFlowInterface.hxx"

namespace dcc
{

class PacketSource;

/** Notifies the update processor that a modification happened to one of the
 * packet sources. Update processors will typically prioritize this packet
 * source in the packet generation loop.
 * @param source is the packet source that experienced a change
 * @param code is a source-specific value that will be sent to the source in
 * the get_next_packet callback. It should not be zero.*/
void packet_processor_notify_update(PacketSource *source, unsigned code);

/** Adds a new refresh source to the background refresh loop.
 * @param source is the packet source to add
 * @param priority represents the packet source priority. If at least
 * EXCLUSIVE_MIN_PRIORITY, then all packet slots will be assigned to this
 * packet source, effectively stopping all background refresh. Only the largest
 * priority will receive slots, until it gets unregistered.
 * @return true if there was no higher priority exclusive source than the one
 * added now. In case of false, the source was still added but will not receive
 * polling until the higher priority source goes away.
*/
bool packet_processor_add_refresh_source(
    PacketSource *source, unsigned priority = 0);

/** Removes a refresh source from the background refresh loop. */
void packet_processor_remove_refresh_source(PacketSource *source);

/** Base class for implementing update loops.
 *
 * Usage: make your update loop implementation private-derived from this base
 * class. Create an instance of your class in the main binary.
 *
 * Note: you may only ever have one live instance of all descendants of this
 * object in a process. */
class UpdateLoopBase : public Singleton<UpdateLoopBase>
{
public:
    virtual ~UpdateLoopBase();
    virtual void notify_update(PacketSource *source, unsigned code) = 0;
    virtual bool add_refresh_source(
        PacketSource *source, unsigned priority = 0) = 0;
    virtual void remove_refresh_source(PacketSource *source) = 0;

    /// Priority value for exclusive sources.
    static constexpr unsigned EXCLUSIVE_MIN_PRIORITY = 0x100;
    /// Priority value to be used for service mode programming source.
    static constexpr unsigned PROGRAMMING_PRIORITY = 0x108;
    /// Priority value to be used for global emergency stop packet source.
    static constexpr unsigned ESTOP_PRIORITY = 0x110;
};

} // namespace dcc

#endif //_DCC_UPDATELOOP_HXX_
<commit_msg>Inverts the priority for ESTOP and PROGRAMMING priority. This allows using the program track even if the mainline is on emergency stop.<commit_after>/** \copyright
 * Copyright (c) 2014, Balazs Racz
 * 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.
 *
 * 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.
 *
 * \file UpdateLoop.hxx
 *
 * Interface to the global update loop.
 *
 * @author Balazs Racz
 * @date 10 May 2014
 */

#ifndef _DCC_UPDATELOOP_HXX_
#define _DCC_UPDATELOOP_HXX_

#include "utils/Singleton.hxx"
#include "dcc/PacketFlowInterface.hxx"

namespace dcc
{

class PacketSource;

/** Notifies the update processor that a modification happened to one of the
 * packet sources. Update processors will typically prioritize this packet
 * source in the packet generation loop.
 * @param source is the packet source that experienced a change
 * @param code is a source-specific value that will be sent to the source in
 * the get_next_packet callback. It should not be zero.*/
void packet_processor_notify_update(PacketSource *source, unsigned code);

/** Adds a new refresh source to the background refresh loop.
 * @param source is the packet source to add
 * @param priority represents the packet source priority. If at least
 * EXCLUSIVE_MIN_PRIORITY, then all packet slots will be assigned to this
 * packet source, effectively stopping all background refresh. Only the largest
 * priority will receive slots, until it gets unregistered.
 * @return true if there was no higher priority exclusive source than the one
 * added now. In case of false, the source was still added but will not receive
 * polling until the higher priority source goes away.
*/
bool packet_processor_add_refresh_source(
    PacketSource *source, unsigned priority = 0);

/** Removes a refresh source from the background refresh loop. */
void packet_processor_remove_refresh_source(PacketSource *source);

/** Base class for implementing update loops.
 *
 * Usage: make your update loop implementation private-derived from this base
 * class. Create an instance of your class in the main binary.
 *
 * Note: you may only ever have one live instance of all descendants of this
 * object in a process. */
class UpdateLoopBase : public Singleton<UpdateLoopBase>
{
public:
    virtual ~UpdateLoopBase();
    virtual void notify_update(PacketSource *source, unsigned code) = 0;
    virtual bool add_refresh_source(
        PacketSource *source, unsigned priority = 0) = 0;
    virtual void remove_refresh_source(PacketSource *source) = 0;

    /// Priority value for exclusive sources.
    static constexpr unsigned EXCLUSIVE_MIN_PRIORITY = 0x100;
    /// Priority value to be used for service mode programming source.
    static constexpr unsigned PROGRAMMING_PRIORITY = 0x110;
    /// Priority value to be used for global emergency stop packet source.
    static constexpr unsigned ESTOP_PRIORITY = 0x108;
};

} // namespace dcc

#endif //_DCC_UPDATELOOP_HXX_
<|endoftext|>


<commit_before>#include <iostream>
#include <sys/types.h>
#include <sys/wait.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <sysexits.h>
#include <unistd.h>

#include "execution_step.h"
#include "program.h"

ExecutionStep::ExecutionStep(Program* program) {
  this->program = program;
}

void ExecutionStep::setPipe(ExecutionStep* step) {
  this->toPipe = step;
}

pid_t ExecutionStep::execute(int* fds) {
  pid_t pid;
  if ((pid = fork()) == -1) {
    fprintf(stderr, "ish: can't fork: %s\n", strerror(errno));
    throw std::string();
  }

  if (pid != 0) {
    return pid;
  }

  pipe(fds);

  const char* executable = this->program->getExecutable().c_str();
  char** argv = this->program->argv();
  execvp(executable, argv);
  fprintf(stderr, "ish: couldn't exec %s: %s\n", this->program->toString().c_str(), strerror(errno));
  exit(EX_DATAERR);
  return 0;
}
<commit_msg>cleanup<commit_after>#include <iostream>
#include <sys/types.h>
#include <sys/wait.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <sysexits.h>
#include <unistd.h>

#include "execution_step.h"
#include "program.h"

ExecutionStep::ExecutionStep(Program* program) {
  this->program = program;
}

void ExecutionStep::setPipe(ExecutionStep* step) {
  this->toPipe = step;
}

pid_t ExecutionStep::execute(int* fds) {
  if (pipe(fds) == -1) {
    throw std::string("Pipe failed");
  }

  pid_t pid = fork();

  // error check
  if (pid == -1) {
    throw std::string("can't fork");
  }

  // return child pid
  if (pid != 0) {
    return pid;
  }

  const char* executable = this->program->getExecutable().c_str();
  char** argv = this->program->argv();
  execvp(executable, argv);
  fprintf(stderr, "ish: couldn't exec %s: %s\n", this->program->toString().c_str(), strerror(errno));
  exit(EX_DATAERR);
  return 0;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * 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 copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Ali Saidi
 */

#include "base/trace.hh"
#include "dev/arm/rv_ctrl.hh"
#include "mem/packet.hh"
#include "mem/packet_access.hh"

RealViewCtrl::RealViewCtrl(Params *p)
    : BasicPioDevice(p)
{
    pioSize = 0xD4;
}

Tick
RealViewCtrl::read(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);
    assert(pkt->getSize() == 4);
    Addr daddr = pkt->getAddr() - pioAddr;
    pkt->allocate();

    switch(daddr) {
      case ProcId:
        pkt->set(params()->proc_id);
        break;
      case Clock24:
        Tick clk;
        clk = (Tick)(curTick() / (24 * SimClock::Float::MHz));
        pkt->set((uint32_t)(clk));
        break;
      case Flash:
        pkt->set<uint32_t>(0);
        break;
      case Clcd:
        pkt->set<uint32_t>(0x00001F00);
        break;
      case Osc0:
        pkt->set<uint32_t>(0x00012C5C);
        break;
      case Osc1:
        pkt->set<uint32_t>(0x00002CC0);
        break;
      case Osc2:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Osc3:
        pkt->set<uint32_t>(0x00020211);
        break;
      case Osc4:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Lock:
        pkt->set<uint32_t>(sysLock);
        break;
      default:
        panic("Tried to read RealView I/O at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;

}

Tick
RealViewCtrl::write(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);

    Addr daddr = pkt->getAddr() - pioAddr;
    switch (daddr) {
      case Flash:
      case Clcd:
      case Osc0:
      case Osc1:
      case Osc2:
      case Osc3:
      case Osc4:
        break;
      case Lock:
        sysLock.lockVal = pkt->get<uint16_t>();
        break;
      default:
        panic("Tried to write RVIO at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;
}

void
RealViewCtrl::serialize(std::ostream &os)
{
}

void
RealViewCtrl::unserialize(Checkpoint *cp, const std::string &section)
{
}

RealViewCtrl *
RealViewCtrlParams::create()
{
    return new RealViewCtrl(this);
}
<commit_msg>ARM: Add support for read of 100MHz clock in system controller.<commit_after>/*
 * Copyright (c) 2010 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * 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 copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Ali Saidi
 */

#include "base/trace.hh"
#include "dev/arm/rv_ctrl.hh"
#include "mem/packet.hh"
#include "mem/packet_access.hh"

RealViewCtrl::RealViewCtrl(Params *p)
    : BasicPioDevice(p)
{
    pioSize = 0xD4;
}

Tick
RealViewCtrl::read(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);
    assert(pkt->getSize() == 4);
    Addr daddr = pkt->getAddr() - pioAddr;
    pkt->allocate();

    switch(daddr) {
      case ProcId:
        pkt->set(params()->proc_id);
        break;
      case Clock24:
        Tick clk;
        clk = (Tick)(curTick() / (24 * SimClock::Float::MHz));
        pkt->set((uint32_t)(clk));
        break;
      case Clock100:
        Tick clk100;
        clk100 = (Tick)(curTick() / (100 * SimClock::Float::MHz));
        pkt->set((uint32_t)(clk100));
        break;
      case Flash:
        pkt->set<uint32_t>(0);
        break;
      case Clcd:
        pkt->set<uint32_t>(0x00001F00);
        break;
      case Osc0:
        pkt->set<uint32_t>(0x00012C5C);
        break;
      case Osc1:
        pkt->set<uint32_t>(0x00002CC0);
        break;
      case Osc2:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Osc3:
        pkt->set<uint32_t>(0x00020211);
        break;
      case Osc4:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Lock:
        pkt->set<uint32_t>(sysLock);
        break;
      default:
        panic("Tried to read RealView I/O at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;

}

Tick
RealViewCtrl::write(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);

    Addr daddr = pkt->getAddr() - pioAddr;
    switch (daddr) {
      case Flash:
      case Clcd:
      case Osc0:
      case Osc1:
      case Osc2:
      case Osc3:
      case Osc4:
        break;
      case Lock:
        sysLock.lockVal = pkt->get<uint16_t>();
        break;
      default:
        panic("Tried to write RVIO at offset %#x that doesn't exist\n", daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;
}

void
RealViewCtrl::serialize(std::ostream &os)
{
}

void
RealViewCtrl::unserialize(Checkpoint *cp, const std::string &section)
{
}

RealViewCtrl *
RealViewCtrlParams::create()
{
    return new RealViewCtrl(this);
}
<|endoftext|>


<commit_before>#include <cmath>
#include <string>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <jaguar/diff_drive.h>
#include <jaguar/JaguarConfig.h>

using namespace can;
using namespace jaguar;

static ros::Subscriber sub_twist;
static ros::Publisher pub_odom;
static ros::Publisher pub_estop;
static ros::Publisher pub_temp_left, pub_temp_right;
static ros::Publisher pub_voltage_left, pub_voltage_right;
static ros::Publisher pub_vleft, pub_vright, pub_wheel;

static DiffDriveSettings settings;
static boost::shared_ptr<DiffDriveRobot> robot;
static boost::shared_ptr<tf::TransformBroadcaster> pub_tf;
static std::string frame_parent;
static std::string frame_child;
static int heartbeat_rate;
static double wheel_separation, alpha;

static void callback_odom(double x, double y, double theta,
                          double velocity, double omega,
                          double v_left, double v_right)
{
    ros::Time now = ros::Time::now();

    ROS_INFO("(%f, %f, %f) facing %f rad", x, y, 0.0, theta);

    // odom TF Frame
    geometry_msgs::TransformStamped msg_tf;
    msg_tf.header.stamp = now;
    msg_tf.header.frame_id = frame_parent;
    msg_tf.child_frame_id  = frame_child;
    msg_tf.transform.translation.x = x;
    msg_tf.transform.translation.y = y;
    msg_tf.transform.rotation = tf::createQuaternionMsgFromYaw(theta);
    pub_tf->sendTransform(msg_tf);

    // Odometry Message
    nav_msgs::Odometry msg_odom;
    msg_odom.header.stamp = now;
    msg_odom.header.frame_id = frame_parent;
    msg_odom.child_frame_id  = frame_child;
    msg_odom.pose.pose.position.x = x;
    msg_odom.pose.pose.position.y = y;
    msg_odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
    msg_odom.twist.twist.linear.x = velocity;
    msg_odom.twist.twist.linear.y = 0;
    msg_odom.twist.twist.angular.z = omega;
    pub_odom.publish(msg_odom);

    robot_kf::WheelOdometry msg_wheel;
    msg_wheel.header.stamp = now;
    msg_wheel.header.frame_id = frame_child;
    msg_wheel.separation = wheel_separation;
    msg_wheel.left.movement = v_left;
    msg_wheel.left.variance = alpha * fabs(v_left);
    msg_wheel.right.movement = v_right;
    msg_wheel.right.variance = alpha * fabs(v_right);
    pub_wheel.publish(msg_wheel);
}

static void callback_estop(bool stopped)
{
    std_msgs::Bool msg;
    msg.data = stopped;
    pub_estop.publish(msg);
}

static void callback_diag_left(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_left.publish(msg_voltage);
    pub_temp_left.publish(msg_temperature);
}

static void callback_diag_right(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_right.publish(msg_voltage);
    pub_temp_right.publish(msg_temperature);
}

void callback_speed(DiffDriveRobot::Side side, double speed)
{
    std_msgs::Float64 msg;
    msg.data = speed;
    switch (side) {
    case DiffDriveRobot::kLeft:
        pub_vleft.publish(msg);
        break;

    case DiffDriveRobot::kRight:
        pub_vright.publish(msg);
        break;

    default:
        ROS_WARN_THROTTLE(10, "Invalid speed callback.");
    }
}

void callback_cmd(geometry_msgs::Twist const &twist)
{
    if (twist.linear.y  != 0.0 || twist.linear.z  != 0
     || twist.angular.x != 0.0 || twist.angular.y != 0) {
        ROS_WARN_THROTTLE(10.0, "Ignoring non-zero component of velocity command.");
    }
    robot->drive(twist.linear.x, twist.angular.z);
}

void callback_reconfigure(jaguar::JaguarConfig &config, uint32_t level)
{
    // Speed Control Gains
    if (level & 1) {
        robot->speed_set_p(config.gain_p);
        ROS_INFO("Reconfigure, P = %f", config.gain_p);
    }
    if (level & 2) {
        robot->speed_set_i(config.gain_i);
        ROS_INFO("Reconfigure, I = %f", config.gain_i);
    }
    if (level & 4) {
        robot->speed_set_d(config.gain_d);
        ROS_INFO("Reconfigure, D = %f", config.gain_d);
    }
    if (level & 8) {
        robot->drive_brake(config.brake);
        ROS_INFO("Reconfigure, Braking = %d", config.brake);
    }
    if (level & 16) {
        if (0 < config.cpr && config.cpr <= std::numeric_limits<uint16_t>::max()) {
            robot->odom_set_encoders(config.cpr);
            ROS_INFO("Reconfigure, CPR = %d", config.cpr);
        } else {
            ROS_WARN("CPR must be a positive 16-bit unsigned integer.");
        }
    }
    if (level & 32) {
        if (config.wheel_diameter <= 0) {
            ROS_WARN("Wheel diameter must be positive.");
        } else {
            robot->odom_set_circumference(M_PI * config.wheel_diameter);
            ROS_INFO("Reconfigure, Wheel Diameter = %f m", config.wheel_diameter);
        }
    }
    if (level & 64) {
        if (config.wheel_separation <= 0) {
            ROS_WARN("Wheel separation must be positive.");
        } else {
            robot->odom_set_separation(config.wheel_separation);
            wheel_separation = config.wheel_separation;
            ROS_INFO("Reconfigure, Wheel Separation = %f m", config.wheel_separation);
        }
    }
    if (level & 128) {
        if (config.odom_rate <= 0 || config.odom_rate > 255) {
            ROS_WARN("Odometry update rate must be positive.");
        } else {
            robot->odom_set_rate(config.odom_rate);
            ROS_INFO("Reconfigure, Odometry Update Rate = %d ms", config.odom_rate);
        }
    }
    if (level & 256) {
        if (config.diag_rate <= 0 || config.diag_rate > 255) {
            ROS_WARN("Diagnostics update rate must be positive.");
        } else {
            robot->diag_set_rate(config.diag_rate);
            ROS_INFO("Reconfigure, Diagnostics Update Rate = %d ms", config.diag_rate);
        }
    }
    if (level & 512) {
        if (config.heartbeat_rate <= 0 || config.heartbeat_rate > 255) {
            ROS_WARN("Heartbeat rate must be in the range 1-255.");
        } else if (config.heartbeat_rate > 100) {
            ROS_WARN("Heartbeat rate is dangerously high.");
        } else {
            heartbeat_rate = config.heartbeat_rate;
            ROS_INFO("Reconfigure, Heartbeat Rate = %d ms", config.heartbeat_rate);
        }
    }
    if (level & 1024) {
        robot->drive_raw(config.setpoint, config.setpoint);
        ROS_INFO("Reconfigure, Setpoint = %f", config.setpoint);
    }
    if (level & 2048) {
        if (config.alpha < 0.0) {
            ROS_WARN("Alpha must be positive");
        } else {
            alpha = config.alpha;
            ROS_INFO("Reconfigure, alpha = %f", alpha);
        }
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "diff_drive_node");
    ros::NodeHandle nh;

    ros::param::get("~port", settings.port);
    ros::param::get("~id_left", settings.id_left);
    ros::param::get("~id_right", settings.id_right);
    ros::param::get("~frame_parent", frame_parent);
    ros::param::get("~frame_child", frame_child);
    ros::param::get("~accel_max", settings.accel_max_mps2);

    // TODO: Read this from a parameter.
    settings.brake = BrakeCoastSetting::kOverrideCoast;

    if (!(1 <= settings.id_left  && settings.id_left  <= 63)
     || !(1 <= settings.id_right && settings.id_right <= 63)) {
        ROS_FATAL("Invalid CAN device id. Must be in the range 1-63.");
        return 1;
    } else if (settings.id_left == settings.id_right){
        ROS_FATAL("Invalid CAN device ID. Left and right IDs must be unique.");
        return 1;
    }
    ROS_INFO("Communicating to IDs %d and %d over %s", settings.id_left,
             settings.id_right, settings.port.c_str());

    robot = boost::make_shared<DiffDriveRobot>(settings);

    // Use dynamic reconfigure for all remaining parameters.
    dynamic_reconfigure::Server<jaguar::JaguarConfig> server;
    dynamic_reconfigure::Server<jaguar::JaguarConfig>::CallbackType f;
    f = boost::bind(&callback_reconfigure, _1, _2);
    server.setCallback(f);

    sub_twist = nh.subscribe("cmd_vel", 1, &callback_cmd);
    pub_odom  = nh.advertise<nav_msgs::Odometry>("odom", 100);
    pub_estop = nh.advertise<std_msgs::Bool>("estop", 1, true);
    pub_vleft  = nh.advertise<std_msgs::Float64>("encoder_left", 10);
    pub_vright = nh.advertise<std_msgs::Float64>("encoder_right", 10);
    pub_wheel  = nh.advertise<robot_kf::WheelOdometry>("wheel_odom", 10);
    pub_temp_left  = nh.advertise<std_msgs::Float64>("temperature_left", 10);
    pub_temp_right = nh.advertise<std_msgs::Float64>("temperature_right", 10);
    pub_voltage_left  = nh.advertise<std_msgs::Float64>("voltage_left", 10);
    pub_voltage_right = nh.advertise<std_msgs::Float64>("voltage_right", 10);
    pub_tf = boost::make_shared<tf::TransformBroadcaster>();

    // These must be registered after the publishers are initialized. Otherwise
    // there is a race condition in the callbacks.
    robot->odom_attach(&callback_odom);
    robot->diag_attach(&callback_diag_left, &callback_diag_right);
    robot->estop_attach(&callback_estop);

    // TODO: Read this heartbeat rate from a parameter.
    ros::Rate heartbeat_rate(50);
    while (ros::ok()) {
        robot->drive_spin(1 / 50.);
        robot->heartbeat();
        ros::spinOnce();
        heartbeat_rate.sleep();
    }
    return 0;
}
<commit_msg>flipped another sign<commit_after>#include <cmath>
#include <string>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <jaguar/diff_drive.h>
#include <jaguar/JaguarConfig.h>

using namespace can;
using namespace jaguar;

static ros::Subscriber sub_twist;
static ros::Publisher pub_odom;
static ros::Publisher pub_estop;
static ros::Publisher pub_temp_left, pub_temp_right;
static ros::Publisher pub_voltage_left, pub_voltage_right;
static ros::Publisher pub_vleft, pub_vright, pub_wheel;

static DiffDriveSettings settings;
static boost::shared_ptr<DiffDriveRobot> robot;
static boost::shared_ptr<tf::TransformBroadcaster> pub_tf;
static std::string frame_parent;
static std::string frame_child;
static int heartbeat_rate;
static double wheel_separation, alpha;
static volatile bool spinlock = false;

static void callback_odom(double x, double y, double theta,
                          double velocity, double omega,
                          double v_left, double v_right)
{
    ros::Time now = ros::Time::now();

    ROS_INFO("(%f, %f, %f) facing %f rad", x, y, 0.0, theta);

    // odom TF Frame
    geometry_msgs::TransformStamped msg_tf;
    msg_tf.header.stamp = now;
    msg_tf.header.frame_id = frame_parent;
    msg_tf.child_frame_id  = frame_child;
    msg_tf.transform.translation.x = x;
    msg_tf.transform.translation.y = y;
    msg_tf.transform.rotation = tf::createQuaternionMsgFromYaw(theta);
    pub_tf->sendTransform(msg_tf);

    // Odometry Message
    nav_msgs::Odometry msg_odom;
    msg_odom.header.stamp = now;
    msg_odom.header.frame_id = frame_parent;
    msg_odom.child_frame_id  = frame_child;
    msg_odom.pose.pose.position.x = x;
    msg_odom.pose.pose.position.y = y;
    msg_odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
    msg_odom.twist.twist.linear.x = velocity;
    msg_odom.twist.twist.linear.y = 0;
    msg_odom.twist.twist.angular.z = omega;
    pub_odom.publish(msg_odom);

    // TODO: Why are these flipped?
    robot_kf::WheelOdometry msg_wheel;
    msg_wheel.header.stamp = now;
    msg_wheel.header.frame_id = frame_child;
    msg_wheel.separation = wheel_separation;
    msg_wheel.left.movement = v_right;
    msg_wheel.left.variance = alpha * fabs(v_right);
    msg_wheel.right.movement = v_left;
    msg_wheel.right.variance = alpha * fabs(v_left);
    pub_wheel.publish(msg_wheel);
}

static void callback_estop(bool stopped)
{
    std_msgs::Bool msg;
    msg.data = stopped;
    pub_estop.publish(msg);
}

static void callback_diag_left(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_left.publish(msg_voltage);
    pub_temp_left.publish(msg_temperature);
}

static void callback_diag_right(double voltage, double temperature)
{
    std_msgs::Float64 msg_voltage, msg_temperature;
    msg_voltage.data = voltage;
    msg_temperature.data = temperature;
    pub_voltage_right.publish(msg_voltage);
    pub_temp_right.publish(msg_temperature);
}

void callback_speed(DiffDriveRobot::Side side, double speed)
{
    std_msgs::Float64 msg;
    msg.data = speed;
    switch (side) {
    case DiffDriveRobot::kLeft:
        pub_vleft.publish(msg);
        break;

    case DiffDriveRobot::kRight:
        pub_vright.publish(msg);
        break;

    default:
        ROS_WARN_THROTTLE(10, "Invalid speed callback.");
    }
}

void callback_cmd(geometry_msgs::Twist const &twist)
{
    if (twist.linear.y  != 0.0 || twist.linear.z  != 0
     || twist.angular.x != 0.0 || twist.angular.y != 0) {
        ROS_WARN_THROTTLE(10.0, "Ignoring non-zero component of velocity command.");
    }
    robot->drive(twist.linear.x, twist.angular.z);
}

void callback_reconfigure(jaguar::JaguarConfig &config, uint32_t level)
{
    // Speed Control Gains
    if (level & 1) {
        robot->speed_set_p(config.gain_p);
        ROS_INFO("Reconfigure, P = %f", config.gain_p);
    }
    if (level & 2) {
        robot->speed_set_i(config.gain_i);
        ROS_INFO("Reconfigure, I = %f", config.gain_i);
    }
    if (level & 4) {
        robot->speed_set_d(config.gain_d);
        ROS_INFO("Reconfigure, D = %f", config.gain_d);
    }
    if (level & 8) {
        robot->drive_brake(config.brake);
        ROS_INFO("Reconfigure, Braking = %d", config.brake);
    }
    if (level & 16) {
        if (0 < config.cpr && config.cpr <= std::numeric_limits<uint16_t>::max()) {
            robot->odom_set_encoders(config.cpr);
            ROS_INFO("Reconfigure, CPR = %d", config.cpr);
        } else {
            ROS_WARN("CPR must be a positive 16-bit unsigned integer.");
        }
    }
    if (level & 32) {
        if (config.wheel_diameter <= 0) {
            ROS_WARN("Wheel diameter must be positive.");
        } else {
            robot->odom_set_circumference(M_PI * config.wheel_diameter);
            ROS_INFO("Reconfigure, Wheel Diameter = %f m", config.wheel_diameter);
        }
    }
    if (level & 64) {
        if (config.wheel_separation <= 0) {
            ROS_WARN("Wheel separation must be positive.");
        } else {
            robot->odom_set_separation(config.wheel_separation);
            wheel_separation = config.wheel_separation;
            ROS_INFO("Reconfigure, Wheel Separation = %f m", config.wheel_separation);
        }
    }
    if (level & 128) {
        if (config.odom_rate <= 0 || config.odom_rate > 255) {
            ROS_WARN("Odometry update rate must be positive.");
        } else {
            robot->odom_set_rate(config.odom_rate);
            ROS_INFO("Reconfigure, Odometry Update Rate = %d ms", config.odom_rate);
        }
    }
    if (level & 256) {
        if (config.diag_rate <= 0 || config.diag_rate > 255) {
            ROS_WARN("Diagnostics update rate must be positive.");
        } else {
            robot->diag_set_rate(config.diag_rate);
            ROS_INFO("Reconfigure, Diagnostics Update Rate = %d ms", config.diag_rate);
        }
    }
    if (level & 512) {
        if (config.heartbeat_rate <= 0 || config.heartbeat_rate > 255) {
            ROS_WARN("Heartbeat rate must be in the range 1-255.");
        } else if (config.heartbeat_rate > 100) {
            ROS_WARN("Heartbeat rate is dangerously high.");
        } else {
            heartbeat_rate = config.heartbeat_rate;
            ROS_INFO("Reconfigure, Heartbeat Rate = %d ms", config.heartbeat_rate);
        }
    }
    if (level & 1024) {
        robot->drive_raw(config.setpoint, config.setpoint);
        ROS_INFO("Reconfigure, Setpoint = %f", config.setpoint);
    }
    if (level & 2048) {
        if (config.alpha < 0.0) {
            ROS_WARN("Alpha must be positive");
        } else {
            alpha = config.alpha;
            ROS_INFO("Reconfigure, alpha = %f", alpha);
        }
    }
    spinlock = true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "diff_drive_node");
    ros::NodeHandle nh;

    ros::param::get("~port", settings.port);
    ros::param::get("~id_left", settings.id_left);
    ros::param::get("~id_right", settings.id_right);
    ros::param::get("~frame_parent", frame_parent);
    ros::param::get("~frame_child", frame_child);
    ros::param::get("~accel_max", settings.accel_max_mps2);

    // TODO: Read this from a parameter.
    settings.brake = BrakeCoastSetting::kOverrideCoast;

    if (!(1 <= settings.id_left  && settings.id_left  <= 63)
     || !(1 <= settings.id_right && settings.id_right <= 63)) {
        ROS_FATAL("Invalid CAN device id. Must be in the range 1-63.");
        return 1;
    } else if (settings.id_left == settings.id_right){
        ROS_FATAL("Invalid CAN device ID. Left and right IDs must be unique.");
        return 1;
    }
    ROS_INFO("Communicating to IDs %d and %d over %s", settings.id_left,
             settings.id_right, settings.port.c_str());

    robot = boost::make_shared<DiffDriveRobot>(settings);

    // Use dynamic reconfigure for all remaining parameters.
    dynamic_reconfigure::Server<jaguar::JaguarConfig> server;
    dynamic_reconfigure::Server<jaguar::JaguarConfig>::CallbackType f;
    f = boost::bind(&callback_reconfigure, _1, _2);
    server.setCallback(f);

    sub_twist = nh.subscribe("cmd_vel", 1, &callback_cmd);
    pub_odom  = nh.advertise<nav_msgs::Odometry>("odom", 100);
    pub_estop = nh.advertise<std_msgs::Bool>("estop", 1, true);
    pub_vleft  = nh.advertise<std_msgs::Float64>("encoder_left", 10);
    pub_vright = nh.advertise<std_msgs::Float64>("encoder_right", 10);
    pub_wheel  = nh.advertise<robot_kf::WheelOdometry>("wheel_odom", 10);
    pub_temp_left  = nh.advertise<std_msgs::Float64>("temperature_left", 10);
    pub_temp_right = nh.advertise<std_msgs::Float64>("temperature_right", 10);
    pub_voltage_left  = nh.advertise<std_msgs::Float64>("voltage_left", 10);
    pub_voltage_right = nh.advertise<std_msgs::Float64>("voltage_right", 10);
    pub_tf = boost::make_shared<tf::TransformBroadcaster>();

    // These must be registered after the publishers are initialized. Otherwise
    // there is a race condition in the callbacks.
    robot->odom_attach(&callback_odom);
    robot->diag_attach(&callback_diag_left, &callback_diag_right);
    robot->estop_attach(&callback_estop);

    while (!spinlock);

    // TODO: Read this heartbeat rate from a parameter.
    ros::Rate heartbeat_rate(50);
    while (ros::ok()) {
        robot->drive_spin(1 / 50.);
        robot->heartbeat();
        ros::spinOnce();
        heartbeat_rate.sleep();
    }
    return 0;
}
<|endoftext|>


<commit_before>
#include <limits>
#include <stdexcept>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>

#include "vec2d.h"
#include "trashproblem.h"

double TrashProblem::distance(int n1, int n2) const {
    return datanodes[n1].distance(datanodes[n2]);
}

void TrashProblem::loadproblem(std::string& file) {
    std::ifstream in( file.c_str() );
    std::string line;

    // read the nodes
    int cnt = 0;
    while ( std::getline(in, line) ) {
        cnt++;
        // skip comment lines
        if (line[0] == '#') continue;

        Trashnode node( line );
        if (!node.isvalid())
            std::cout << "ERROR: line: " << cnt << ": " << line << std::endl;

        datanodes.push_back(node);

        if (node.ispickup())
            pickups.push_back(node.getnid());
        else if (node.isdepot())
            depots.push_back(node.getnid());
        else if (node.isdump())
            dumps.push_back(node.getnid());
    }

    in.close();

    buildDistanceMatrix();

    for (int i=0; i<datanodes.size(); i++)
        setNodeDistances(datanodes[i]);
}

void TrashProblem::setNodeDistances(Trashnode& n) {
    double dist = -1.0;
    int nid = -1;
    double dist2 = -1.0;
    int nid2 = -1;

    if (n.isdepot()) {
        n.setdepotdist(n.getnid(), 0.0, -1, -1.0);
        for (int i=0; i<dumps.size(); i++) {
            double d = dMatrix[n.getnid()][dumps[i]];
            if (nid == -1 or d < dist) {
                dist = d;
                nid = dumps[i];
            }
        }
        n.setdumpdist(nid, dist);
    }
    else if (n.isdump()) {
        n.setdumpdist(n.getnid(), 0.0);
        for (int i=0; i<depots.size(); i++) {
            double d = dMatrix[n.getnid()][depots[i]];
            if (nid == -1 or d < dist) {
                dist = d;
                nid = depots[i];
            }
        }
        n.setdepotdist(nid, dist, -1, -1.0);
    }
    else if (n.ispickup()) {
        for (int i=0; i<dumps.size(); i++) {
            double d = dMatrix[n.getnid()][dumps[i]];
            if (nid == -1 or d < dist) {
                dist = d;
                nid = dumps[i];
            }
        }
        n.setdumpdist(nid, dist);

        nid = -1;
        for (int i=0; i<depots.size(); i++) {
            double d = dMatrix[n.getnid()][depots[i]];
            if (nid == -1 or d < dist) {
                dist2 = dist;
                nid2 = nid;
                dist = d;
                nid = depots[i];
            }
        }
        n.setdepotdist(nid, dist, nid2, dist2);
    }
}

void TrashProblem::buildDistanceMatrix() {
    dMatrix.clear();
    dMatrix.resize(datanodes.size());
    for (int i=0; i<datanodes.size(); i++) {
        dMatrix[i].clear();
        dMatrix[i].resize(datanodes.size());
        for (int j=0; j<datanodes.size(); j++) {
            dMatrix[i][j] = datanodes[i].distance(datanodes[j]);
        }
    }
}

// search for node methods

// selector is a bit mask (TODO: make these an enum)
// selector: 0 - anyi                       ANY
//           1 - must be unassigned         UNASSIGNED
//           2 - in nid's cluster1          CLUSTER1
//           4 - in nid's cluster2          CLUSTER2
//           8 - with demand <= demandLimit LIMITDEMAND
//          16 - must be pickup nodes       PICKUP
//          32 - must be depot nodes        DEPOT
//          64 - must be dump nodes         DUMP

bool TrashProblem::filterNode(const Trashnode &tn, int i, int selector, int demandLimit) {
        bool select = false;

        // filter out nodes where the demand > demandLimit
        if (selector & 8 and datanodes[i].getdemand() > demandLimit)
            return true;

        // if select any
        if (!selector)
            select = true;
        // is pickup node
        else if (selector & 16 and datanodes[i].ispickup())
            select = true;
        // is depot node
        else if (selector & 32 and datanodes[i].isdepot())
            select = true;
        // is dump node
        else if (selector & 64 and datanodes[i].isdump())
            select = true;
        // belongs to cluster1
        else if (selector & 2 and (
                 tn.getdepotnid() == datanodes[i].getdepotnid() or
                 tn.getdepotnid() == datanodes[i].getdepotnid2() ) )
                select = true;
        // belongs to cluster2
        else if (selector & 4 and (
                 tn.getdepotnid2() == datanodes[i].getdepotnid() or
                 tn.getdepotnid2() == datanodes[i].getdepotnid2() ) )
                select = true;

        // is unassigned node
        if (selector & 1 and ! unassigned[i])
            return true;

        if (!select)
            return true;

        return ! select;
}

int TrashProblem::findNearestNodeTo(int nid, int selector, int demandLimit) {
    Trashnode &tn(datanodes[nid]);
    int nn = -1;    // init to not found
    double dist = -1;    // dist to nn

    for (int i=0; i<datanodes.size(); i++) {

        if (filterNode(tn, i, selector, demandLimit)) continue;

        double d = dMatrix[tn.getnid()][i];
        if (nn == -1 or d < dist) {
            dist = d;
            nn = i;
        }
    }
    std::cout << "TrashProblem::findNearestNodeTo(" << nid << ", " << selector << ") = " << nn << " at dist = " << dist << std::endl;
    return nn;
}

int TrashProblem::findNearestNodeTo(Vehicle &v, int selector, int demandLimit, int *pos) {
    Trashnode depot(v.getdepot());
    Trashnode dump(v.getdumpsite());
    int nn = -1;        // init to not found
    int loc = 0;        // position in path to insert
    double dist = -1;   // dist to nn
    double qx, qy;

    for (int i=0; i<datanodes.size(); i++) {

        if (filterNode(depot, i, selector, demandLimit)) continue;

        double d;
        Trashnode &last = depot;

        for (int j=0; j<v.size(); j++) {
            d = distanceFromLineSegmentToPoint(
                last.getx(), last.gety(), v[j].getx(), v[j].gety(),
                datanodes[i].getx(), datanodes[i].gety(), &qx, &qy);
            if (nn == -1 or d < dist) {
                dist = d;
                loc = j;
                nn = i;
            }
            last = v[j];
        }
        d = distanceFromLineSegmentToPoint(
            last.getx(), last.gety(), dump.getx(), dump.gety(),
            datanodes[i].getx(), datanodes[i].gety(), &qx, &qy);
        if (nn == -1 or d < dist) {
            dist = d;
            loc = v.size();
            nn = i;
        }
    }

    std::cout << "TrashProblem::findNearestNodeTo(V" << depot.getnid() << ", " << selector << ") = " << nn << " at dist = " << dist << " at pos = " << loc << std::endl;

    *pos = loc;
    return nn;
}

std::string TrashProblem::solutionAsText() {
    std::stringstream ss;;
    std::vector<int> s = solutionAsVector();
    for (int i=0; i<s.size(); i++) {
        if (i) ss << ",";
        ss << s[i];
    }
    return ss.str();
}

std::vector<int>  TrashProblem::solutionAsVector() {
    std::vector<int> s;
    for (int i=0; i<fleet.size(); i++) {
        if (fleet[i].size() == 0) continue;
        for (int j=0; j<fleet[i].size(); j++) {
            s.push_back(fleet[i][j].getnid());
        }
        s.push_back(-1);
    }
    return s;
}

void TrashProblem::nearestNeighbor() {
    // create a list of all pickup nodes and make them unassigned
    unassigned = std::vector<int>(datanodes.size(), 1);

    clearFleet();

    for (int i=0; i<depots.size(); i++) {
        Vehicle truck;

        // add this depot as the vehicles home location
        Trashnode& depot(datanodes[depots[i]]);
        truck.setdepot(depot);
        // add the closest dump for now, this might change later
        truck.setdumpsite(datanodes[depot.getdumpnid()]);

        // remember the last node we inserted
        Trashnode& last_node = datanodes[depots[i]];

        truck.evaluate();

        while (truck.getcurcapacity() <= truck.getmaxcapacity()) {

            int nnid = findNearestNodeTo(depot.getnid(),
                            UNASSIGNED|PICKUP|LIMITDEMAND,
                            truck.getmaxcapacity() - truck.getcurcapacity());

            // if we did not find a node we can break
            if (nnid == -1) break;

            // add node to route
            unassigned[nnid] = 0;
            truck.push_back(datanodes[nnid]);
            truck.evaluate();
        }
        std::cout << "nearestNeighbor: depot: " << i << std::endl;
        truck.dump();
        fleet.push_back(truck);
    }
    // report unassigned nodes
    std::cout << "-------- Unassigned after TrashProblem::nearestNeighbor\n";
    for (int i=0; i<pickups.size(); i++) {
        if (unassigned[pickups[i]])
            std::cout << "    " << pickups[i] << std::endl;
    }
}

void TrashProblem::nearestInsertion() {

}

void TrashProblem::farthestInsertion() {

}

void TrashProblem::assignmentSweep() {
    // create a list of all pickup nodes and make them unassigned
    unassigned = std::vector<int>(datanodes.size(), 1);

    clearFleet();

    for (int i=0; i<depots.size(); i++) {
        Vehicle truck;

        // add this depot as the vehicles home location
        Trashnode& depot(datanodes[depots[i]]);
        truck.setdepot(depot);
        // add the closest dump for now, this might change later
        truck.setdumpsite(datanodes[depot.getdumpnid()]);
        truck.evaluate();

        int pos;
        int nid = findNearestNodeTo(truck, UNASSIGNED|PICKUP|CLUSTER1, 0, &pos);
        if (nid == -1) {
            std::cout << "TrashProblem::assignmentSweep failed to find an initial node for depot: " << depots[i] << std::endl;
            continue;
        }
        truck.push_back(datanodes[nid]);
        unassigned[nid] = 0;

        while (truck.getcurcapacity() <= truck.getmaxcapacity()) {

            int nnid = findNearestNodeTo(truck,
                            UNASSIGNED|PICKUP|CLUSTER1|CLUSTER2|LIMITDEMAND,
                            truck.getmaxcapacity() - truck.getcurcapacity(),
                            &pos);

            // if we did not find a node we can break
            if (nnid == -1) break;

            // add node to route
            unassigned[nnid] = 0;
            truck.push_back(datanodes[nnid]);
            if (pos == 0)
                truck.push_front(datanodes[nnid]);
            else if (pos == truck.size())
                truck.push_back(datanodes[nnid]);
            else
                truck.insert(truck.begin()+pos, datanodes[nnid]);

            truck.evaluate();
        }
        std::cout << "assignmentSweep: depot: " << i << std::endl;
        truck.dump();
        fleet.push_back(truck);
    }
    // report unassigned nodes
    std::cout << "-------- Unassigned after TrashProblem::assignmentSweep\n";
    for (int i=0; i<pickups.size(); i++) {
        if (unassigned[pickups[i]])
            std::cout << "    " << pickups[i] << std::endl;
    }
}

void TrashProblem::opt_2opt() {

}

void TrashProblem::dumpDmatrix() const {
    std::cout << "--------- dMatrix ------------" << std::endl;
    for (int i=0; i<dMatrix.size(); i++) {
        for (int j=0; j<dMatrix[i].size(); j++) {
            std::cout << i << "\t" << j << "\t" << dMatrix[i][j] << std::endl;
        }
    }
}

void TrashProblem::dumpFleet() {
    std::cout << "--------- Fleet ------------" << std::endl;
    for (int i=0; i<fleet.size(); i++)
        fleet[i].dump();
}

void TrashProblem::dumpdataNodes() const {
    std::cout << "--------- Nodes ------------" << std::endl;
    for (int i=0; i<datanodes.size(); i++)
        datanodes[i].dump();
}

void TrashProblem::dumpDepots() const {
    std::cout << "--------- Depots ------------" << std::endl;
    for (int i=0; i<depots.size(); i++)
        datanodes[depots[i]].dump();
}

void TrashProblem::dumpDumps() const {
    std::cout << "--------- Dumps ------------" << std::endl;
    for (int i=0; i<dumps.size(); i++)
        datanodes[dumps[i]].dump();
}

void TrashProblem::dumpPickups() const {
    std::cout << "--------- Pickups ------------" << std::endl;
    for (int i=0; i<pickups.size(); i++)
        datanodes[pickups[i]].dump();
}

void TrashProblem::dump() {
    dumpDepots();
    dumpDumps();
    dumpPickups();
    dumpFleet();
    std::cout << "Solution: " << solutionAsText() << std::endl;
}

<commit_msg>start debugging assignmentSweep<commit_after>
#include <limits>
#include <stdexcept>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>

#include "vec2d.h"
#include "trashproblem.h"

double TrashProblem::distance(int n1, int n2) const {
    return datanodes[n1].distance(datanodes[n2]);
}

void TrashProblem::loadproblem(std::string& file) {
    std::ifstream in( file.c_str() );
    std::string line;

    // read the nodes
    int cnt = 0;
    while ( std::getline(in, line) ) {
        cnt++;
        // skip comment lines
        if (line[0] == '#') continue;

        Trashnode node( line );
        if (!node.isvalid())
            std::cout << "ERROR: line: " << cnt << ": " << line << std::endl;

        datanodes.push_back(node);

        if (node.ispickup())
            pickups.push_back(node.getnid());
        else if (node.isdepot())
            depots.push_back(node.getnid());
        else if (node.isdump())
            dumps.push_back(node.getnid());
    }

    in.close();

    buildDistanceMatrix();

    for (int i=0; i<datanodes.size(); i++)
        setNodeDistances(datanodes[i]);
}

void TrashProblem::setNodeDistances(Trashnode& n) {
    double dist = -1.0;
    int nid = -1;
    double dist2 = -1.0;
    int nid2 = -1;

    if (n.isdepot()) {
        n.setdepotdist(n.getnid(), 0.0, -1, -1.0);
        for (int i=0; i<dumps.size(); i++) {
            double d = dMatrix[n.getnid()][dumps[i]];
            if (nid == -1 or d < dist) {
                dist = d;
                nid = dumps[i];
            }
        }
        n.setdumpdist(nid, dist);
    }
    else if (n.isdump()) {
        n.setdumpdist(n.getnid(), 0.0);
        for (int i=0; i<depots.size(); i++) {
            double d = dMatrix[n.getnid()][depots[i]];
            if (nid == -1 or d < dist) {
                dist = d;
                nid = depots[i];
            }
        }
        n.setdepotdist(nid, dist, -1, -1.0);
    }
    else if (n.ispickup()) {
        for (int i=0; i<dumps.size(); i++) {
            double d = dMatrix[n.getnid()][dumps[i]];
            if (nid == -1 or d < dist) {
                dist = d;
                nid = dumps[i];
            }
        }
        n.setdumpdist(nid, dist);

        nid = -1;
        for (int i=0; i<depots.size(); i++) {
            double d = dMatrix[n.getnid()][depots[i]];
            if (nid == -1 or d < dist) {
                dist2 = dist;
                nid2 = nid;
                dist = d;
                nid = depots[i];
            }
        }
        n.setdepotdist(nid, dist, nid2, dist2);
    }
}

void TrashProblem::buildDistanceMatrix() {
    dMatrix.clear();
    dMatrix.resize(datanodes.size());
    for (int i=0; i<datanodes.size(); i++) {
        dMatrix[i].clear();
        dMatrix[i].resize(datanodes.size());
        for (int j=0; j<datanodes.size(); j++) {
            dMatrix[i][j] = datanodes[i].distance(datanodes[j]);
        }
    }
}

// search for node methods

// selector is a bit mask (TODO: make these an enum)
// selector: 0 - anyi                       ANY
//           1 - must be unassigned         UNASSIGNED
//           2 - in nid's cluster1          CLUSTER1
//           4 - in nid's cluster2          CLUSTER2
//           8 - with demand <= demandLimit LIMITDEMAND
//          16 - must be pickup nodes       PICKUP
//          32 - must be depot nodes        DEPOT
//          64 - must be dump nodes         DUMP

bool TrashProblem::filterNode(const Trashnode &tn, int i, int selector, int demandLimit) {
        bool select = false;

        // filter out nodes where the demand > demandLimit
        if (selector & LIMITDEMAND and datanodes[i].getdemand() > demandLimit)
            return true;

        // if select any
        if (!selector)
            select = true;
        // is pickup node
        else if (selector & PICKUP and datanodes[i].ispickup())
            select = true;
        // is depot node
        else if (selector & DEPOT and datanodes[i].isdepot())
            select = true;
        // is dump node
        else if (selector & DUMP and datanodes[i].isdump())
            select = true;
        // belongs to cluster1
        else if (selector & CLUSTER1 and (
                 tn.getdepotnid() == datanodes[i].getdepotnid() or
                 tn.getdepotnid() == datanodes[i].getdepotnid2() ) )
                select = true;
        // belongs to cluster2
        else if (selector & CLUSTER2 and (
                 tn.getdepotnid2() == datanodes[i].getdepotnid() or
                 tn.getdepotnid2() == datanodes[i].getdepotnid2() ) )
                select = true;

        // is unassigned node
        if (selector & UNASSIGNED and ! unassigned[i])
            return true;

        if (!select)
            return true;

        return ! select;
}

int TrashProblem::findNearestNodeTo(int nid, int selector, int demandLimit) {
    Trashnode &tn(datanodes[nid]);
    int nn = -1;    // init to not found
    double dist = -1;    // dist to nn

    for (int i=0; i<datanodes.size(); i++) {

        if (filterNode(tn, i, selector, demandLimit)) continue;

        double d = dMatrix[tn.getnid()][i];
        if (nn == -1 or d < dist) {
            dist = d;
            nn = i;
        }
    }
    std::cout << "TrashProblem::findNearestNodeTo(" << nid << ", " << selector << ") = " << nn << " at dist = " << dist << std::endl;
    return nn;
}

int TrashProblem::findNearestNodeTo(Vehicle &v, int selector, int demandLimit, int *pos) {
    Trashnode depot(v.getdepot());
    Trashnode dump(v.getdumpsite());
    int nn = -1;        // init to not found
    int loc = 0;        // position in path to insert
    double dist = -1;   // dist to nn
    double qx, qy;

    std::cout << "TrashProblem::findNearestNodeTo(V" << depot.getnid() << ", " << selector << ")\n";

    for (int i=0; i<datanodes.size(); i++) {

        if (filterNode(depot, i, selector, demandLimit)) {
            std::cout << "FILTERED: ";
            datanodes[i].dump();
            continue;
        }

        double d;
        Trashnode &last = depot;

        for (int j=0; j<v.size(); j++) {
            d = distanceFromLineSegmentToPoint(
                last.getx(), last.gety(), v[j].getx(), v[j].gety(),
                datanodes[i].getx(), datanodes[i].gety(), &qx, &qy);
            if (nn == -1 or d < dist) {
                dist = d;
                loc = j;
                nn = i;
            }
            last = v[j];
        }
        d = distanceFromLineSegmentToPoint(
            last.getx(), last.gety(), dump.getx(), dump.gety(),
            datanodes[i].getx(), datanodes[i].gety(), &qx, &qy);
        if (nn == -1 or d < dist) {
            dist = d;
            loc = v.size();
            nn = i;
        }
    }

    std::cout << "TrashProblem::findNearestNodeTo(V" << depot.getnid() << ", " << selector << ") = " << nn << " at dist = " << dist << " at pos = " << loc << std::endl;

    *pos = loc;
    return nn;
}

std::string TrashProblem::solutionAsText() {
    std::stringstream ss;;
    std::vector<int> s = solutionAsVector();
    for (int i=0; i<s.size(); i++) {
        if (i) ss << ",";
        ss << s[i];
    }
    return ss.str();
}

std::vector<int>  TrashProblem::solutionAsVector() {
    std::vector<int> s;
    for (int i=0; i<fleet.size(); i++) {
        if (fleet[i].size() == 0) continue;
        for (int j=0; j<fleet[i].size(); j++) {
            s.push_back(fleet[i][j].getnid());
        }
        s.push_back(-1);
    }
    return s;
}

void TrashProblem::nearestNeighbor() {
    // create a list of all pickup nodes and make them unassigned
    unassigned = std::vector<int>(datanodes.size(), 1);

    clearFleet();

    for (int i=0; i<depots.size(); i++) {
        Vehicle truck;

        // add this depot as the vehicles home location
        Trashnode& depot(datanodes[depots[i]]);
        truck.setdepot(depot);
        // add the closest dump for now, this might change later
        truck.setdumpsite(datanodes[depot.getdumpnid()]);

        // remember the last node we inserted
        Trashnode& last_node = datanodes[depots[i]];

        truck.evaluate();

        while (truck.getcurcapacity() <= truck.getmaxcapacity()) {

            int nnid = findNearestNodeTo(depot.getnid(),
                            UNASSIGNED|PICKUP|LIMITDEMAND,
                            truck.getmaxcapacity() - truck.getcurcapacity());

            // if we did not find a node we can break
            if (nnid == -1) break;

            // add node to route
            unassigned[nnid] = 0;
            truck.push_back(datanodes[nnid]);
            truck.evaluate();
        }
        std::cout << "nearestNeighbor: depot: " << i << std::endl;
        truck.dump();
        fleet.push_back(truck);
    }
    // report unassigned nodes
    std::cout << "-------- Unassigned after TrashProblem::nearestNeighbor\n";
    for (int i=0; i<pickups.size(); i++) {
        if (unassigned[pickups[i]])
            std::cout << "    " << pickups[i] << std::endl;
    }
}

void TrashProblem::nearestInsertion() {

}

void TrashProblem::farthestInsertion() {

}

void TrashProblem::assignmentSweep() {
    // create a list of all pickup nodes and make them unassigned
    unassigned = std::vector<int>(datanodes.size(), 1);

    clearFleet();

    for (int i=0; i<depots.size(); i++) {
        Vehicle truck;

        // add this depot as the vehicles home location
        Trashnode& depot(datanodes[depots[i]]);
        truck.setdepot(depot);
        // add the closest dump for now, this might change later
        truck.setdumpsite(datanodes[depot.getdumpnid()]);
        truck.evaluate();

        int pos;
        int nid = findNearestNodeTo(truck, UNASSIGNED|PICKUP|CLUSTER1, 0, &pos);
        if (nid == -1) {
            std::cout << "TrashProblem::assignmentSweep failed to find an initial node for depot: " << depots[i] << std::endl;
            continue;
        }
        truck.push_back(datanodes[nid]);
        unassigned[nid] = 0;

        while (truck.getcurcapacity() <= truck.getmaxcapacity()) {

            int nnid = findNearestNodeTo(truck,
                            UNASSIGNED|PICKUP|CLUSTER1|CLUSTER2|LIMITDEMAND,
                            truck.getmaxcapacity() - truck.getcurcapacity(),
                            &pos);

            // if we did not find a node we can break
            if (nnid == -1) break;

            // add node to route
            unassigned[nnid] = 0;
            truck.push_back(datanodes[nnid]);
            if (pos == 0)
                truck.push_front(datanodes[nnid]);
            else if (pos == truck.size())
                truck.push_back(datanodes[nnid]);
            else
                truck.insert(truck.begin()+pos, datanodes[nnid]);

            truck.evaluate();
        }
        std::cout << "assignmentSweep: depot: " << i << std::endl;
        truck.dump();
        fleet.push_back(truck);
    }
    // report unassigned nodes
    std::cout << "-------- Unassigned after TrashProblem::assignmentSweep\n";
    for (int i=0; i<pickups.size(); i++) {
        if (unassigned[pickups[i]])
            std::cout << "    " << pickups[i] << std::endl;
    }
}

void TrashProblem::opt_2opt() {

}

void TrashProblem::dumpDmatrix() const {
    std::cout << "--------- dMatrix ------------" << std::endl;
    for (int i=0; i<dMatrix.size(); i++) {
        for (int j=0; j<dMatrix[i].size(); j++) {
            std::cout << i << "\t" << j << "\t" << dMatrix[i][j] << std::endl;
        }
    }
}

void TrashProblem::dumpFleet() {
    std::cout << "--------- Fleet ------------" << std::endl;
    for (int i=0; i<fleet.size(); i++)
        fleet[i].dump();
}

void TrashProblem::dumpdataNodes() const {
    std::cout << "--------- Nodes ------------" << std::endl;
    for (int i=0; i<datanodes.size(); i++)
        datanodes[i].dump();
}

void TrashProblem::dumpDepots() const {
    std::cout << "--------- Depots ------------" << std::endl;
    for (int i=0; i<depots.size(); i++)
        datanodes[depots[i]].dump();
}

void TrashProblem::dumpDumps() const {
    std::cout << "--------- Dumps ------------" << std::endl;
    for (int i=0; i<dumps.size(); i++)
        datanodes[dumps[i]].dump();
}

void TrashProblem::dumpPickups() const {
    std::cout << "--------- Pickups ------------" << std::endl;
    for (int i=0; i<pickups.size(); i++)
        datanodes[pickups[i]].dump();
}

void TrashProblem::dump() {
    dumpDepots();
    dumpDumps();
    dumpPickups();
    dumpFleet();
    std::cout << "Solution: " << solutionAsText() << std::endl;
}

<|endoftext|>