<commit_before>//---------------------------------------------------------------------------
/// \file   es/storage.hpp
/// \brief  The entity/component data store
//
// Copyright 2013, nocte@hippie.nu            Released under the MIT License.
//---------------------------------------------------------------------------
#pragma once

#include <algorithm>
#include <bitset>
#include <cassert>
#include <cstdint>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <vector>
#include <unordered_map>

#include "component.hpp"
#include "entity.hpp"
#include "traits.hpp"

namespace es {

/** A storage ties entities and components together.
 * Storage associates two other bits of data with every entity:
 * - A 64-bit mask that keeps track of which components are defined
 * - A vector of bytes, holding the actual data
 *
 * The vector of bytes tries to pack the component data as tightly as
 * possible.  It is really fast for plain old datatypes, but it also
 * handles nontrivial types safely.  It packs a virtual table and a
 * pointer to some heap space in the vector, and calls the constructor
 * and destructor as needed.
 */
class storage
{
    // It is assumed the first few components will be accessed the
    // most often.  We keep a cache of the first 12.
    static constexpr uint32_t cache_size = 12;
    static constexpr uint32_t cache_mask = (1 << cache_size) - 1;

    /** This data gets associated with every entity. */
    struct elem
    {
        /** Bitmask to keep track of which components are held in \a data. */
        std::bitset<64>     components;
        /** Component data for this entity. */
        std::vector<char>   data;
    };

    typedef component::placeholder placeholder;

    /** Data types that do not have a flat memory layout are kept in the
     ** elem::data buffer in a placeholder object. */
    template <typename t>
    class holder : public placeholder
    {
    public:
        holder () { }
        holder (t init) : held_(std::move(init)) { }

        const t& held() const { return held_; }
        t&       held()       { return held_; }

        placeholder* clone() const { return new holder<t>(held_); }

    private:
        t held_;
    };

    typedef std::unordered_map<uint32_t, elem> stor_impl;

public:
    typedef uint8_t     component_id;

    typedef stor_impl::iterator         iterator;
    typedef stor_impl::const_iterator   const_iterator;

public:
    /** Variable references are used by systems to access an entity's data.
     *  It keeps track of the elem::data buffer and the offset inside that
     *  buffer. */
    template <typename type>
    class var_ref
    {
        friend class storage;

        type& get()
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

    public:
        operator const type& () const
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

        var_ref& operator= (type assign)
        {
            if (is_flat<type>::value)
            {
                new (&*e_.data.begin() + offset_) type(std::move(assign));
            }
            else
            {
                auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
                new (ptr) holder<type>(std::move(assign));
            }
            return *this;
        }

        template <typename s>
        var_ref& operator+= (s val)
            { get() += val; return *this; }

        template <typename s>
        var_ref& operator-= (s val)
            { get() -= val; return *this; }

        template <typename s>
        var_ref& operator*= (s val)
            { get() *= val; return *this; }

        template <typename s>
        var_ref& operator/= (s val)
            { get() /= val; return *this; }

    protected:
        var_ref (size_t offset, elem& e)
            : offset_(offset), e_(e)
            { }

    private:
        size_t offset_;
        elem&  e_;
    };

public:
    storage()
        : next_id_(0)
        {
            component_offsets_.push_back(0);
        }

    template <typename type>
    component_id register_component (std::string name)
        {
            size_t size;

            if (is_flat<type>::value)
            {
                size = sizeof(type);
                components_.emplace_back(name, size, nullptr);
            }
            else
            {
                flat_mask_.set(components_.size());
                size = sizeof(holder<type>);
                components_.emplace_back(name, size,
                                         std::unique_ptr<placeholder>(new holder<type>()));
            }

            if (components_.size() < cache_size)
            {
                size_t i (component_offsets_.size());
                component_offsets_.resize(i * 2);
                for (size_t j (i); j != i * 2; ++j)
                    component_offsets_[j] = component_offsets_[j-i] + size;
            }

            return components_.size() - 1;
        }

    component_id find_component (const std::string& name) const
        {
            auto found (std::find(components_.begin(), components_.end(), name));
            if (found == components_.end())
                throw std::logic_error("component does not exist");

            return std::distance(components_.begin(), found);
        }

    const component& operator[] (component_id id) const
        { return components_[id]; }

    const std::vector<component>& components() const
        { return components_; }

public:
    entity new_entity()
        {
            entities_[next_id_++];
            return next_id_ - 1;
        }

    /** Create a whole bunch of empty entities in one go.
     * @param count     The number of entities to create
     * @return The range of entities created */
    std::pair<entity, entity> new_entities (size_t count)
        {
            auto range_begin (next_id_);
            for (; count > 0; --count)
                entities_[next_id_++];

            return std::make_pair(range_begin, next_id_);
        }

    entity clone_entity (iterator f)
        {
            elem& e (f->second);
            entities_[next_id_++] = e;

            // Quick check if we need to make deep copies
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int c_id (0); c_id < 64 && off < e.data.size(); ++c_id)
                {
                    if (e.components[c_id])
                    {
                        if (!components_[c_id].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr = ptr->clone();
                        }
                        off += components_[c_id].size();
                    }
                }
            }
            return next_id_ - 1;
        }

    iterator find (entity en)
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    const_iterator find (entity en) const
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    void delete_entity (entity en)
        { delete_entity(find(en)); }

    void delete_entity (iterator f)
        {
            elem& e (f->second);

            // Quick check if we'll have to call any destructors.
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int search (0); search < 64 && off < e.data.size(); ++search)
                {
                    if (e.components[search])
                    {
                        if (!components_[search].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr->~placeholder();
                        }
                        off += components_[search].size();
                    }
                }
            }

            entities_.erase(f);
        }

    void remove_component_from_entity (iterator en, component_id c)
        {
            auto& e (en->second);
            if (!e.components[c])
                return;

            size_t off (offset(e, c));
            auto& comp_info (components_[c]);
            if (!comp_info.is_flat())
            {
                auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                ptr->~placeholder();
            }
            auto o (e.data.begin() + off);
            e.data.erase(o, o + comp_info.size());
            e.components.reset(c);
        }

    /** Call a function for every entity that has a given component.
     *  The callee can then query and change the value of the component through
     *  a var_ref object, or remove the entity.
     * @param c     The component to look for.
     * @param func  The function to call.  This function will be passed an
     *              iterator to the current entity, and a var_ref corresponding
     *              to the component value in this entity. */
    template <typename t>
    void for_each (component_id c, std::function<void(iterator, var_ref<t>)> func)
        {
            std::bitset<64> mask;
            mask.set(c);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                    func(i, var_ref<t>(offset(e, c), e));

                i = next;
            }
        }

    template <typename t1, typename t2>
    void for_each (component_id c1, component_id c2,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e));
                }
                i = next;
            }
        }

    template <typename t1, typename t2, typename t3>
    void for_each (component_id c1, component_id c2, component_id c3,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>, var_ref<t3>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            mask.set(c3);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e),
                            var_ref<t3>(offset(e, c3), e));
                }
                i = next;
            }
        }

    bool exists (entity en) const
        { return entities_.count(en.id()); }

    template <typename type>
    void set (entity en, component_id c_id, type val)
        { return set<type>(find(en), c_id, std::move(val)); }

    template <typename type>
    void set (iterator en, component_id c_id, type val)
        {
            const component& c (components_[c_id]);
            elem&  e   (en->second);
            size_t off (offset(e, c_id));

            if (!e.components[c_id])
            {
                if (e.data.size() < off)
                    e.data.resize(off + c.size());
                else
                    e.data.insert(e.data.begin() + off, c.size(), 0);

                e.components.set(c_id);
            }

            if (is_flat<type>::value)
            {
                assert(e.data.size() >= off + sizeof(type));
                new (&*e.data.begin() + off) type(std::move(val));
            }
            else
            {
                assert(e.data.size() >= off + sizeof(holder<type>));
                auto ptr (reinterpret_cast<holder<type>*>(&*e.data.begin() + off));
                new (ptr) holder<type>(std::move(val));
            }
        }

    template <typename type>
    const type& get (entity en, component_id c_id) const
        { return get<type>(find(en), c_id); }

    template <typename type>
    const type& get (const_iterator en, component_id c_id) const
        {
            auto& e (en->second);
            if (!e.components[c_id])
                throw std::logic_error("entity does not have component");

            return get<type>(e, c_id);
        }

private:
    template <typename type>
    const type& get (const elem& e, component_id c_id) const
        {
            size_t off (offset(e, c_id));
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e.data.begin() + off);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e.data.begin() + off));

            return ptr->held();
        }

    size_t offset (const elem& e, component_id c) const
        {
            assert(c < components_.size());
            assert((c & cache_mask) < component_offsets_.size());
            size_t result (component_offsets_[c & cache_mask]);

            for (component_id search (cache_size); search < c; ++search)
            {
                if (e.components[search])
                    result += components_[search].size();
            }
            return result;
        }

private:
    /** Keeps track of entity IDs to give out. */
    uint32_t                            next_id_;
    /** The list of registered components. */
    std::vector<component>              components_;
    /** Mapping entity IDs to their data. */
    std::unordered_map<uint32_t, elem>  entities_;
    /** A lookup table for the data offsets of components.
     *  The index is the bitmask as used in elem::components. */
    std::vector<size_t>                 component_offsets_;
    /** A bitmask to quickly determine whether a certain combination of
     ** components has a flat memory layout or not. */
    std::bitset<64>                     flat_mask_;
};

} // namespace es
<commit_msg>Added make()<commit_after>//---------------------------------------------------------------------------
/// \file   es/storage.hpp
/// \brief  The entity/component data store
//
// Copyright 2013, nocte@hippie.nu            Released under the MIT License.
//---------------------------------------------------------------------------
#pragma once

#include <algorithm>
#include <bitset>
#include <cassert>
#include <cstdint>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <vector>
#include <unordered_map>

#include "component.hpp"
#include "entity.hpp"
#include "traits.hpp"

namespace es {

/** A storage ties entities and components together.
 * Storage associates two other bits of data with every entity:
 * - A 64-bit mask that keeps track of which components are defined
 * - A vector of bytes, holding the actual data
 *
 * The vector of bytes tries to pack the component data as tightly as
 * possible.  It is really fast for plain old datatypes, but it also
 * handles nontrivial types safely.  It packs a virtual table and a
 * pointer to some heap space in the vector, and calls the constructor
 * and destructor as needed.
 */
class storage
{
    // It is assumed the first few components will be accessed the
    // most often.  We keep a cache of the first 12.
    static constexpr uint32_t cache_size = 12;
    static constexpr uint32_t cache_mask = (1 << cache_size) - 1;

    /** This data gets associated with every entity. */
    struct elem
    {
        /** Bitmask to keep track of which components are held in \a data. */
        std::bitset<64>     components;
        /** Component data for this entity. */
        std::vector<char>   data;
    };

    typedef component::placeholder placeholder;

    /** Data types that do not have a flat memory layout are kept in the
     ** elem::data buffer in a placeholder object. */
    template <typename t>
    class holder : public placeholder
    {
    public:
        holder () { }
        holder (t init) : held_(std::move(init)) { }

        const t& held() const { return held_; }
        t&       held()       { return held_; }

        placeholder* clone() const { return new holder<t>(held_); }

    private:
        t held_;
    };

    typedef std::unordered_map<uint32_t, elem> stor_impl;

public:
    typedef uint8_t     component_id;

    typedef stor_impl::iterator         iterator;
    typedef stor_impl::const_iterator   const_iterator;

public:
    /** Variable references are used by systems to access an entity's data.
     *  It keeps track of the elem::data buffer and the offset inside that
     *  buffer. */
    template <typename type>
    class var_ref
    {
        friend class storage;

        type& get()
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

    public:
        operator const type& () const
        {
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e_.data.begin() + offset_);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e_.data.begin() + offset_));
            return ptr->held();
        }

        var_ref& operator= (type assign)
        {
            if (is_flat<type>::value)
            {
                new (&*e_.data.begin() + offset_) type(std::move(assign));
            }
            else
            {
                auto ptr (reinterpret_cast<holder<type>*>(&*e_.data.begin() + offset_));
                new (ptr) holder<type>(std::move(assign));
            }
            return *this;
        }

        template <typename s>
        var_ref& operator+= (s val)
            { get() += val; return *this; }

        template <typename s>
        var_ref& operator-= (s val)
            { get() -= val; return *this; }

        template <typename s>
        var_ref& operator*= (s val)
            { get() *= val; return *this; }

        template <typename s>
        var_ref& operator/= (s val)
            { get() /= val; return *this; }

    protected:
        var_ref (size_t offset, elem& e)
            : offset_(offset), e_(e)
            { }

    private:
        size_t offset_;
        elem&  e_;
    };

public:
    storage()
        : next_id_(0)
        {
            component_offsets_.push_back(0);
        }

    template <typename type>
    component_id register_component (std::string name)
        {
            size_t size;

            if (is_flat<type>::value)
            {
                size = sizeof(type);
                components_.emplace_back(name, size, nullptr);
            }
            else
            {
                flat_mask_.set(components_.size());
                size = sizeof(holder<type>);
                components_.emplace_back(name, size,
                                         std::unique_ptr<placeholder>(new holder<type>()));
            }

            if (components_.size() < cache_size)
            {
                size_t i (component_offsets_.size());
                component_offsets_.resize(i * 2);
                for (size_t j (i); j != i * 2; ++j)
                    component_offsets_[j] = component_offsets_[j-i] + size;
            }

            return components_.size() - 1;
        }

    component_id find_component (const std::string& name) const
        {
            auto found (std::find(components_.begin(), components_.end(), name));
            if (found == components_.end())
                throw std::logic_error("component does not exist");

            return std::distance(components_.begin(), found);
        }

    const component& operator[] (component_id id) const
        { return components_[id]; }

    const std::vector<component>& components() const
        { return components_; }

public:
    entity new_entity()
        {
            entities_[next_id_++];
            return next_id_ - 1;
        }

    /** Get an entity with a given ID, or create it if it didn't exist yet. */
    iterator make (uint32_t id)
        {
            if (next_id_ <= id)
                next_id_ = id + 1;

            return entities_.insert(entities_.end(), std::make_pair(id, elem()));
        }

    /** Create a whole bunch of empty entities in one go.
     * @param count     The number of entities to create
     * @return The range of entities created */
    std::pair<entity, entity> new_entities (size_t count)
        {
            auto range_begin (next_id_);
            for (; count > 0; --count)
                entities_[next_id_++];

            return std::make_pair(range_begin, next_id_);
        }

    entity clone_entity (iterator f)
        {
            elem& e (f->second);
            entities_[next_id_++] = e;

            // Quick check if we need to make deep copies
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int c_id (0); c_id < 64 && off < e.data.size(); ++c_id)
                {
                    if (e.components[c_id])
                    {
                        if (!components_[c_id].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr = ptr->clone();
                        }
                        off += components_[c_id].size();
                    }
                }
            }
            return next_id_ - 1;
        }

    iterator find (entity en)
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    const_iterator find (entity en) const
        {
            auto found (entities_.find(en.id()));
            if (found == entities_.end())
                throw std::logic_error("unknown entity");

            return found;
        }

    void delete_entity (entity en)
        { delete_entity(find(en)); }

    void delete_entity (iterator f)
        {
            elem& e (f->second);

            // Quick check if we'll have to call any destructors.
            if ((e.components & flat_mask_).any())
            {
                size_t off (0);
                for (int search (0); search < 64 && off < e.data.size(); ++search)
                {
                    if (e.components[search])
                    {
                        if (!components_[search].is_flat())
                        {
                            auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                            ptr->~placeholder();
                        }
                        off += components_[search].size();
                    }
                }
            }

            entities_.erase(f);
        }

    void remove_component_from_entity (iterator en, component_id c)
        {
            auto& e (en->second);
            if (!e.components[c])
                return;

            size_t off (offset(e, c));
            auto& comp_info (components_[c]);
            if (!comp_info.is_flat())
            {
                auto ptr (reinterpret_cast<placeholder*>(&*e.data.begin() + off));
                ptr->~placeholder();
            }
            auto o (e.data.begin() + off);
            e.data.erase(o, o + comp_info.size());
            e.components.reset(c);
        }

    /** Call a function for every entity that has a given component.
     *  The callee can then query and change the value of the component through
     *  a var_ref object, or remove the entity.
     * @param c     The component to look for.
     * @param func  The function to call.  This function will be passed an
     *              iterator to the current entity, and a var_ref corresponding
     *              to the component value in this entity. */
    template <typename t>
    void for_each (component_id c, std::function<void(iterator, var_ref<t>)> func)
        {
            std::bitset<64> mask;
            mask.set(c);
            for (auto i (entities_.begin()); i != entities_.end(); )
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                    func(i, var_ref<t>(offset(e, c), e));

                i = next;
            }
        }

    template <typename t1, typename t2>
    void for_each (component_id c1, component_id c2,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            for (auto i (entities_.begin()); i != entities_.end(); ++i)
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e));
                }
                i = next;
            }
        }

    template <typename t1, typename t2, typename t3>
    void for_each (component_id c1, component_id c2, component_id c3,
                   std::function<void(iterator, var_ref<t1>, var_ref<t2>, var_ref<t3>)> func)
        {
            std::bitset<64> mask;
            mask.set(c1);
            mask.set(c2);
            mask.set(c3);
            for (auto i (entities_.begin()); i != entities_.end(); ++i)
            {
                auto next (std::next(i));
                elem& e (i->second);
                if ((e.components & mask) == mask)
                {
                    func(i, var_ref<t1>(offset(e, c1), e),
                            var_ref<t2>(offset(e, c2), e),
                            var_ref<t3>(offset(e, c3), e));
                }
                i = next;
            }
        }

    bool exists (entity en) const
        { return entities_.count(en.id()); }

    template <typename type>
    void set (entity en, component_id c_id, type val)
        { return set<type>(find(en), c_id, std::move(val)); }

    template <typename type>
    void set (iterator en, component_id c_id, type val)
        {
            const component& c (components_[c_id]);
            elem&  e   (en->second);
            size_t off (offset(e, c_id));

            if (!e.components[c_id])
            {
                if (e.data.size() < off)
                    e.data.resize(off + c.size());
                else
                    e.data.insert(e.data.begin() + off, c.size(), 0);

                e.components.set(c_id);
            }

            if (is_flat<type>::value)
            {
                assert(e.data.size() >= off + sizeof(type));
                new (&*e.data.begin() + off) type(std::move(val));
            }
            else
            {
                assert(e.data.size() >= off + sizeof(holder<type>));
                auto ptr (reinterpret_cast<holder<type>*>(&*e.data.begin() + off));
                new (ptr) holder<type>(std::move(val));
            }
        }

    template <typename type>
    const type& get (entity en, component_id c_id) const
        { return get<type>(find(en), c_id); }

    template <typename type>
    const type& get (const_iterator en, component_id c_id) const
        {
            auto& e (en->second);
            if (!e.components[c_id])
                throw std::logic_error("entity does not have component");

            return get<type>(e, c_id);
        }

private:
    template <typename type>
    const type& get (const elem& e, component_id c_id) const
        {
            size_t off (offset(e, c_id));
            if (is_flat<type>::value)
                return *reinterpret_cast<const type*>(&*e.data.begin() + off);

            auto ptr (reinterpret_cast<const holder<type>*>(&*e.data.begin() + off));

            return ptr->held();
        }

    size_t offset (const elem& e, component_id c) const
        {
            assert(c < components_.size());
            assert((c & cache_mask) < component_offsets_.size());
            size_t result (component_offsets_[c & cache_mask]);

            for (component_id search (cache_size); search < c; ++search)
            {
                if (e.components[search])
                    result += components_[search].size();
            }
            return result;
        }

private:
    /** Keeps track of entity IDs to give out. */
    uint32_t                            next_id_;
    /** The list of registered components. */
    std::vector<component>              components_;
    /** Mapping entity IDs to their data. */
    std::unordered_map<uint32_t, elem>  entities_;
    /** A lookup table for the data offsets of components.
     *  The index is the bitmask as used in elem::components. */
    std::vector<size_t>                 component_offsets_;
    /** A bitmask to quickly determine whether a certain combination of
     ** components has a flat memory layout or not. */
    std::bitset<64>                     flat_mask_;
};

} // namespace es
<|endoftext|>


<commit_before>
/*
 * Copyright 2008 The Android Open Source Project
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "SkColorPriv.h"

#include "SkImageDecoder.h"
#include "SkImageEncoder.h"
#include "SkMovie.h"
#include "SkStream.h"
#include "SkTemplates.h"
#include "SkCGUtils.h"

#ifdef SK_BUILD_FOR_MAC
#include <ApplicationServices/ApplicationServices.h>
#endif

#ifdef SK_BUILD_FOR_IOS
#include <CoreGraphics/CoreGraphics.h>
#include <ImageIO/ImageIO.h>
#include <MobileCoreServices/MobileCoreServices.h>
#endif

static void malloc_release_proc(void* info, const void* data, size_t size) {
    sk_free(info);
}

static CGDataProviderRef SkStreamToDataProvider(SkStream* stream) {
    // TODO: use callbacks, so we don't have to load all the data into RAM
    size_t len = stream->getLength();
    void* data = sk_malloc_throw(len);
    stream->read(data, len);

    return CGDataProviderCreateWithData(data, data, len, malloc_release_proc);
}

static CGImageSourceRef SkStreamToCGImageSource(SkStream* stream) {
    CGDataProviderRef data = SkStreamToDataProvider(stream);
    CGImageSourceRef imageSrc = CGImageSourceCreateWithDataProvider(data, 0);
    CGDataProviderRelease(data);
    return imageSrc;
}

class SkImageDecoder_CG : public SkImageDecoder {
protected:
    virtual bool onDecode(SkStream* stream, SkBitmap* bm, Mode);
};

#define BITMAP_INFO (kCGBitmapByteOrder32Big | kCGImageAlphaPremultipliedLast)

bool SkImageDecoder_CG::onDecode(SkStream* stream, SkBitmap* bm, Mode mode) {
    CGImageSourceRef imageSrc = SkStreamToCGImageSource(stream);

    if (NULL == imageSrc) {
        return false;
    }
    SkAutoTCallVProc<const void, CFRelease> arsrc(imageSrc);

    CGImageRef image = CGImageSourceCreateImageAtIndex(imageSrc, 0, NULL);
    if (NULL == image) {
        return false;
    }
    SkAutoTCallVProc<CGImage, CGImageRelease> arimage(image);

    const int width = CGImageGetWidth(image);
    const int height = CGImageGetHeight(image);
    bm->setConfig(SkBitmap::kARGB_8888_Config, width, height);
    if (SkImageDecoder::kDecodeBounds_Mode == mode) {
        return true;
    }

    if (!this->allocPixelRef(bm, NULL)) {
        return false;
    }

    bm->lockPixels();
    bm->eraseColor(0);

    // use the same colorspace, so we don't change the pixels at all
    CGColorSpaceRef cs = CGImageGetColorSpace(image);
    CGContextRef cg = CGBitmapContextCreate(bm->getPixels(), width, height,
                                            8, bm->rowBytes(), cs, BITMAP_INFO);
    CGContextDrawImage(cg, CGRectMake(0, 0, width, height), image);
    CGContextRelease(cg);

    // since CGImage won't tell us if it is opaque, we have to compute it.
    bm->computeAndSetOpaquePredicate();
    bm->unlockPixels();
    return true;
}

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

SkImageDecoder* SkImageDecoder::Factory(SkStream* stream) {
    return SkNEW(SkImageDecoder_CG);
}

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

SkMovie* SkMovie::DecodeStream(SkStream* stream) {
    return NULL;
}

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

static size_t consumer_put(void* info, const void* buffer, size_t count) {
    SkWStream* stream = reinterpret_cast<SkWStream*>(info);
    return stream->write(buffer, count) ? count : 0;
}

static void consumer_release(void* info) {
    // we do nothing, since by design we don't "own" the stream (i.e. info)
}

static CGDataConsumerRef SkStreamToCGDataConsumer(SkWStream* stream) {
    CGDataConsumerCallbacks procs;
    procs.putBytes = consumer_put;
    procs.releaseConsumer = consumer_release;
    // we don't own/reference the stream, so it our consumer must not live
    // longer that our caller's ownership of the stream
    return CGDataConsumerCreate(stream, &procs);
}

static CGImageDestinationRef SkStreamToImageDestination(SkWStream* stream,
                                                        CFStringRef type) {
    CGDataConsumerRef consumer = SkStreamToCGDataConsumer(stream);
    if (NULL == consumer) {
        return NULL;
    }
    SkAutoTCallVProc<const void, CFRelease> arconsumer(consumer);

    return CGImageDestinationCreateWithDataConsumer(consumer, type, 1, NULL);
}

class SkImageEncoder_CG : public SkImageEncoder {
public:
    SkImageEncoder_CG(Type t) : fType(t) {}

protected:
    virtual bool onEncode(SkWStream* stream, const SkBitmap& bm, int quality);

private:
    Type fType;
};

/*  Encode bitmaps via CGImageDestination. We setup a DataConsumer which writes
    to our SkWStream. Since we don't reference/own the SkWStream, our consumer
    must only live for the duration of the onEncode() method.
 */
bool SkImageEncoder_CG::onEncode(SkWStream* stream, const SkBitmap& bm,
                                 int quality) {
    // Used for converting a bitmap to 8888.
    const SkBitmap* bmPtr = &bm;
    SkBitmap bitmap8888;

    CFStringRef type;
    switch (fType) {
        case kJPEG_Type:
            type = kUTTypeJPEG;
            break;
        case kPNG_Type:
            // PNG encoding an ARGB_4444 bitmap gives the following errors in GM:
            // <Error>: CGImageDestinationAddImage image could not be converted to destination
            // format.
            // <Error>: CGImageDestinationFinalize image destination does not have enough images
            // So instead we copy to 8888.
            if (bm.getConfig() == SkBitmap::kARGB_4444_Config) {
                bm.copyTo(&bitmap8888, SkBitmap::kARGB_8888_Config);
                bmPtr = &bitmap8888;
            }
            type = kUTTypePNG;
            break;
        default:
            return false;
    }

    CGImageDestinationRef dst = SkStreamToImageDestination(stream, type);
    if (NULL == dst) {
        return false;
    }
    SkAutoTCallVProc<const void, CFRelease> ardst(dst);

    CGImageRef image = SkCreateCGImageRef(*bmPtr);
    if (NULL == image) {
        return false;
    }
    SkAutoTCallVProc<CGImage, CGImageRelease> agimage(image);

    CGImageDestinationAddImage(dst, image, NULL);
    return CGImageDestinationFinalize(dst);
}

SkImageEncoder* SkImageEncoder::Create(Type t) {
    switch (t) {
        case kJPEG_Type:
        case kPNG_Type:
            break;
        default:
            return NULL;
    }
    return SkNEW_ARGS(SkImageEncoder_CG, (t));
}

<commit_msg>check to see if CGImage already knows if we're opaque Review URL: https://codereview.appspot.com/6838043<commit_after>
/*
 * Copyright 2008 The Android Open Source Project
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "SkColorPriv.h"

#include "SkImageDecoder.h"
#include "SkImageEncoder.h"
#include "SkMovie.h"
#include "SkStream.h"
#include "SkTemplates.h"
#include "SkCGUtils.h"

#ifdef SK_BUILD_FOR_MAC
#include <ApplicationServices/ApplicationServices.h>
#endif

#ifdef SK_BUILD_FOR_IOS
#include <CoreGraphics/CoreGraphics.h>
#include <ImageIO/ImageIO.h>
#include <MobileCoreServices/MobileCoreServices.h>
#endif

static void malloc_release_proc(void* info, const void* data, size_t size) {
    sk_free(info);
}

static CGDataProviderRef SkStreamToDataProvider(SkStream* stream) {
    // TODO: use callbacks, so we don't have to load all the data into RAM
    size_t len = stream->getLength();
    void* data = sk_malloc_throw(len);
    stream->read(data, len);

    return CGDataProviderCreateWithData(data, data, len, malloc_release_proc);
}

static CGImageSourceRef SkStreamToCGImageSource(SkStream* stream) {
    CGDataProviderRef data = SkStreamToDataProvider(stream);
    CGImageSourceRef imageSrc = CGImageSourceCreateWithDataProvider(data, 0);
    CGDataProviderRelease(data);
    return imageSrc;
}

class SkImageDecoder_CG : public SkImageDecoder {
protected:
    virtual bool onDecode(SkStream* stream, SkBitmap* bm, Mode);
};

#define BITMAP_INFO (kCGBitmapByteOrder32Big | kCGImageAlphaPremultipliedLast)

bool SkImageDecoder_CG::onDecode(SkStream* stream, SkBitmap* bm, Mode mode) {
    CGImageSourceRef imageSrc = SkStreamToCGImageSource(stream);

    if (NULL == imageSrc) {
        return false;
    }
    SkAutoTCallVProc<const void, CFRelease> arsrc(imageSrc);

    CGImageRef image = CGImageSourceCreateImageAtIndex(imageSrc, 0, NULL);
    if (NULL == image) {
        return false;
    }
    SkAutoTCallVProc<CGImage, CGImageRelease> arimage(image);

    const int width = CGImageGetWidth(image);
    const int height = CGImageGetHeight(image);
    bm->setConfig(SkBitmap::kARGB_8888_Config, width, height);
    if (SkImageDecoder::kDecodeBounds_Mode == mode) {
        return true;
    }

    if (!this->allocPixelRef(bm, NULL)) {
        return false;
    }

    bm->lockPixels();
    bm->eraseColor(0);

    // use the same colorspace, so we don't change the pixels at all
    CGColorSpaceRef cs = CGImageGetColorSpace(image);
    CGContextRef cg = CGBitmapContextCreate(bm->getPixels(), width, height,
                                            8, bm->rowBytes(), cs, BITMAP_INFO);
    CGContextDrawImage(cg, CGRectMake(0, 0, width, height), image);
    CGContextRelease(cg);

    CGImageAlphaInfo info = CGImageGetAlphaInfo(image);
    switch (info) {
        case kCGImageAlphaNone:
        case kCGImageAlphaNoneSkipLast:
        case kCGImageAlphaNoneSkipFirst:
            SkASSERT(SkBitmap::ComputeIsOpaque(*bm));
            bm->setIsOpaque(true);
            break;
        default:
            // we don't know if we're opaque or not, so compute it.
            bm->computeAndSetOpaquePredicate();
    }
    bm->unlockPixels();
    return true;
}

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

SkImageDecoder* SkImageDecoder::Factory(SkStream* stream) {
    return SkNEW(SkImageDecoder_CG);
}

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

SkMovie* SkMovie::DecodeStream(SkStream* stream) {
    return NULL;
}

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

static size_t consumer_put(void* info, const void* buffer, size_t count) {
    SkWStream* stream = reinterpret_cast<SkWStream*>(info);
    return stream->write(buffer, count) ? count : 0;
}

static void consumer_release(void* info) {
    // we do nothing, since by design we don't "own" the stream (i.e. info)
}

static CGDataConsumerRef SkStreamToCGDataConsumer(SkWStream* stream) {
    CGDataConsumerCallbacks procs;
    procs.putBytes = consumer_put;
    procs.releaseConsumer = consumer_release;
    // we don't own/reference the stream, so it our consumer must not live
    // longer that our caller's ownership of the stream
    return CGDataConsumerCreate(stream, &procs);
}

static CGImageDestinationRef SkStreamToImageDestination(SkWStream* stream,
                                                        CFStringRef type) {
    CGDataConsumerRef consumer = SkStreamToCGDataConsumer(stream);
    if (NULL == consumer) {
        return NULL;
    }
    SkAutoTCallVProc<const void, CFRelease> arconsumer(consumer);

    return CGImageDestinationCreateWithDataConsumer(consumer, type, 1, NULL);
}

class SkImageEncoder_CG : public SkImageEncoder {
public:
    SkImageEncoder_CG(Type t) : fType(t) {}

protected:
    virtual bool onEncode(SkWStream* stream, const SkBitmap& bm, int quality);

private:
    Type fType;
};

/*  Encode bitmaps via CGImageDestination. We setup a DataConsumer which writes
    to our SkWStream. Since we don't reference/own the SkWStream, our consumer
    must only live for the duration of the onEncode() method.
 */
bool SkImageEncoder_CG::onEncode(SkWStream* stream, const SkBitmap& bm,
                                 int quality) {
    // Used for converting a bitmap to 8888.
    const SkBitmap* bmPtr = &bm;
    SkBitmap bitmap8888;

    CFStringRef type;
    switch (fType) {
        case kJPEG_Type:
            type = kUTTypeJPEG;
            break;
        case kPNG_Type:
            // PNG encoding an ARGB_4444 bitmap gives the following errors in GM:
            // <Error>: CGImageDestinationAddImage image could not be converted to destination
            // format.
            // <Error>: CGImageDestinationFinalize image destination does not have enough images
            // So instead we copy to 8888.
            if (bm.getConfig() == SkBitmap::kARGB_4444_Config) {
                bm.copyTo(&bitmap8888, SkBitmap::kARGB_8888_Config);
                bmPtr = &bitmap8888;
            }
            type = kUTTypePNG;
            break;
        default:
            return false;
    }

    CGImageDestinationRef dst = SkStreamToImageDestination(stream, type);
    if (NULL == dst) {
        return false;
    }
    SkAutoTCallVProc<const void, CFRelease> ardst(dst);

    CGImageRef image = SkCreateCGImageRef(*bmPtr);
    if (NULL == image) {
        return false;
    }
    SkAutoTCallVProc<CGImage, CGImageRelease> agimage(image);

    CGImageDestinationAddImage(dst, image, NULL);
    return CGImageDestinationFinalize(dst);
}

SkImageEncoder* SkImageEncoder::Create(Type t) {
    switch (t) {
        case kJPEG_Type:
        case kPNG_Type:
            break;
        default:
            return NULL;
    }
    return SkNEW_ARGS(SkImageEncoder_CG, (t));
}

<|endoftext|>


<commit_before>#include "PositionModule.hpp"

#include <assert.h>
#include <sys/stat.h>
#include <errno.h>
#include <stdlib.h>
#include <string>
#include <algorithm>
#include <iostream>

#include <ros/console.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "sensor_msgs/Image.h"
#include "api_application/Ping.h"
#include "api_application/Announce.h"

#include "../matlab/Vector.h"

// Use this to test if images are saved properly, when you only have one camera.
//#define SINGLE_CAMERA_CALIBRATION

PositionModule::PositionModule(IPositionReceiver* receiver) :
	pictureCache(50), // Assume we never have 50 or more modules running on the network.
	pictureTimes(50)
{
	assert(receiver != 0);
	this->receiver = receiver;

	ROS_DEBUG("Initializing PositionModule");

	_isInitialized = true;
	isCalibrating = false;
	isRunning = false;

	ros::NodeHandle n;

	this->pictureSendingActivationPublisher = n.advertise<camera_application::PictureSendingActivation>("PictureSendingActivation", 4);
	this->pingPublisher = n.advertise<api_application::Ping>("Ping", 4);

	this->pictureSubscriber = n.subscribe("Picture", 128, &PositionModule::pictureCallback, this);
	this->systemSubscriber = n.subscribe("System", 4, &PositionModule::systemCallback, this);
	this->rawPositionSubscriber = n.subscribe("RawPosition", 1024, &PositionModule::rawPositionCallback, this);

	this->startCalibrationService = n.advertiseService("StartCalibration", &PositionModule::startCalibrationCallback, this);
	this->takeCalibrationPictureService = n.advertiseService("TakeCalibrationPicture", &PositionModule::takeCalibrationPictureCallback, this);
	this->calculateCalibrationService = n.advertiseService("CalculateCalibration", &PositionModule::calculateCalibrationCallback, this);

	// Advertise myself to API
	ros::ServiceClient announceClient = n.serviceClient<api_application::Announce>("announce");

	api_application::Announce announce;
	announce.request.type = 3; // 3 means position module

	if (announceClient.call(announce))
	{
		rosId = announce.response.id;

		if (rosId == ~0 /* -1 */)
		{
			ROS_ERROR("Error! Got id -1");
			_isInitialized = false;
		}
		else
		{
			ROS_INFO("Position module successfully announced. Got id %d", rosId);
		}
	}
	else
	{
		ROS_ERROR("Error! Could not announce myself to API!");
		_isInitialized = false;
	}

	msg = new KitrokopterMessages(rosId);

	if (_isInitialized)
	{
		ROS_DEBUG("PositionModule initialized");
	} else {
		ROS_ERROR("Could not initialize PositionModule!");
	}
}

PositionModule::~PositionModule()
{
	msg->~KitrokopterMessages();

	// TODO: Free picture cache.

	ROS_DEBUG("PositionModule destroyed");
}

// Service
bool PositionModule::startCalibrationCallback(control_application::StartCalibration::Request &req, control_application::StartCalibration::Response &res)
{
	res.ok = !isCalibrating;

	if (!isCalibrating)
	{
		ROS_INFO("Starting multi camera calibration process");

		setPictureSendingActivated(true);
		calibrationPictureCount = 0;
		boardSize = cv::Size(req.chessboardWidth, req.chessboardHeight);
		realSize = cv::Size(req.chessboardRealWidth, req.chessboardRealHeight);
	}

	isCalibrating = true;
	return true;
}

// Service
bool PositionModule::takeCalibrationPictureCallback(control_application::TakeCalibrationPicture::Request &req, control_application::TakeCalibrationPicture::Response &res)
{
	ROS_DEBUG("Taking calibration picture. Have %ld cameras.", camNoToNetId.size());

	pictureCacheMutex.lock();

	// Build net id -> cam no map.
	if (camNoToNetId.size() != netIdToCamNo.size()) {
		// If already built, there are already images on the disk with wrong ids, so return an error.
		if (netIdToCamNo.size() != 0) {
			ROS_ERROR("Got new cameras after taking first calibration picture!");
			return false;
		}

		std::sort(camNoToNetId.begin(), camNoToNetId.end());
		netIdToCamNo.clear();

		for (int i = 0; i < camNoToNetId.size(); i++) {
			netIdToCamNo[camNoToNetId[i]] = i;
		}

		ROS_INFO("Got %ld cameras", netIdToCamNo.size());
	}

	int id = 0;
	std::map<int, cv::Mat*> goodPictures;

	for (std::vector<cv::Mat*>::iterator it = pictureCache.begin(); it != pictureCache.end(); it++, id++)
	{
		if (*it != 0)
		{
			std::vector<cv::Point2f> corners;
			bool foundAllCorners = cv::findChessboardCorners(**it, boardSize, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);

			if (!foundAllCorners)
			{
				ROS_INFO("Took bad picture (id %d)", id);
				delete *it;
				*it = 0;
				continue;
			}
			else
			{
				ROS_INFO("Took good picture (id %d)", id);
				goodPictures[id] = *it;

				// Remove image from image cache.
				*it = 0;
			}
		}
	}

	pictureCacheMutex.unlock();

	#ifdef SINGLE_CAMERA_CALIBRATION
	if (goodPictures.size() >= 1) {
	#else
	if (goodPictures.size() >= 2) {
	#endif
		// Create directory for images.
		int error = mkdir("/tmp/calibrationImages", 0777);

		if (error != 0 && errno != EEXIST)
		{
			ROS_ERROR("Could not create directory for calibration images (/tmp/calibrationImages): %d", errno);

			// Delete images.
			for (std::map<int, cv::Mat*>::iterator it = goodPictures.begin(); it != goodPictures.end(); it++) {
				delete it->second;
			}

			return false;
		}

		error = mkdir("/tmp/calibrationResult", 0777);

		if (error != 0 && errno != EEXIST)
		{
			ROS_ERROR("Could not create directory for calibration images (/tmp/calibrationResult): %d", errno);

			// Delete images.
			for (std::map<int, cv::Mat*>::iterator it = goodPictures.begin(); it != goodPictures.end(); it++) {
				delete it->second;
			}

			return false;
		}

		for (std::map<int, cv::Mat*>::iterator it = goodPictures.begin(); it != goodPictures.end(); it++) {
			std::stringstream ss;
			ss << "/tmp/calibrationImages/cam" << netIdToCamNo[it->first] << "_image" << calibrationPictureCount << ".png";

			// Save picture on disk for amcctoolbox.
			std::cout << "Saving picture: " << cv::imwrite(ss.str(), *(it->second)) << std::endl;

			sensor_msgs::Image img;
			img.width = 640;
			img.height = 480;
			img.step = 3 * 640;
			img.data.reserve(img.step * img.height);

			for (int i = 0; i < 640 * 480 * 3; i++)
			{
				img.data[i] = it->second->data[i];
			}

			res.images.push_back(img);
			res.ids.push_back(it->first);

			delete it->second;
		}

		calibrationPictureCount++;
	}

	return true;
}

// Service
bool PositionModule::calculateCalibrationCallback(control_application::CalculateCalibration::Request &req, control_application::CalculateCalibration::Response &res)
{
	if (!isCalibrating) {
		ROS_ERROR("Cannot calculate calibration! Start calibration first!");
		return false;
	}

	if (netIdToCamNo.size() < 2) {
		ROS_ERROR("Have not enough images for calibration (Have %ld)!", netIdToCamNo.size());
	}

	ROS_INFO("Calculating multi camera calibration. This could take up to 2 hours");
	ChessboardData data(boardSize.width, boardSize.height, realSize.width, realSize.height);

	pictureCacheMutex.lock();
	int camNumber = camNoToNetId.size();
	pictureCacheMutex.unlock();

	// Delete old calibration results.
	system("rm -rf /tmp/calibrationResult/*");
	bool ok = tracker.calibrate(&data, camNoToNetId.size());

	if (ok) {
		ROS_INFO("Finished multi camera calibration");
	} else {
		ROS_ERROR("Calibration failed!");
	}

	isCalibrating = false;

	// TODO make not uncommented
	system("rm -rf /tmp/calibrationImages/*");

	return ok;
}

// Topic
void PositionModule::pictureCallback(const camera_application::Picture &msg)
{
	assert(msg.ID < 50);

	pictureCacheMutex.lock();

	bool idKnown = false;

	// Insert camera id, if not already there.
	for (int i = 0; i < camNoToNetId.size(); i++) {
		if (camNoToNetId[i] == msg.ID) {
			idKnown = true;
			break;
		}
	}

	if (!idKnown) {
		camNoToNetId.push_back(msg.ID);
	}

	if (isCalibrating)
	{
		// Will crash here, if more than 49 modules are used.
		if (pictureCache[msg.ID] != 0)
		{
			delete pictureCache[msg.ID];
			pictureCache[msg.ID] = 0;

		}

		cv::Mat* image = new cv::Mat(cv::Size(640, 480), CV_8UC3);

		for (int i = 0; i < 640 * 480 * 3; i++)
		{
			image->data[i] = msg.image[i];
		}

		pictureCache[msg.ID] = image;
		pictureTimes[msg.ID] = msg.timestamp;
	}

	pictureCacheMutex.unlock();
}

// Topic
void PositionModule::systemCallback(const api_application::System &msg)
{
	isRunning = msg.command == 1;

	if (isRunning) {
		ROS_INFO("Tracking started");
	} else {
		ROS_INFO("Tracking stopped");
	}

	if (!isRunning) {
		ros::shutdown();
	}
}

// Topic
void PositionModule::rawPositionCallback(const camera_application::RawPosition &msg)
{
 	// TODO: Calculate position in our coordinate system.
	// TODO: Is this coordinate change correct for amcctoolbox?
	Vector cameraVector(msg.xPosition, msg.yPosition, 1);
	ROS_DEBUG("msg.ID: %d netIdToCamNo[msg.ID]: %d msg.quadcopterId: %d", msg.ID, netIdToCamNo[msg.ID], msg.quadcopterId);
	Vector result = tracker.updatePosition(cameraVector, netIdToCamNo[msg.ID], msg.quadcopterId);

	std::vector<Vector> positions;
	std::vector<int> ids;
	std::vector<int> updates;
	positions.push_back(result);
	ids.push_back(msg.quadcopterId);
	updates.push_back(1);

	if (result.isValid()) {
		receiver->updatePositions(positions, ids, updates);
	} else {
		ROS_DEBUG("Not enough information to get position of quadcopter %d", msg.quadcopterId);
	}
}

void PositionModule::setPictureSendingActivated(bool activated)
{
	camera_application::PictureSendingActivation msg;
	msg.ID = 0;
	msg.active = activated;

	pictureSendingActivationPublisher.publish(msg);
}

void PositionModule::sendPing()
{
	api_application::Ping msg;
	msg.ID = rosId;
	pingPublisher.publish(msg);
}

bool PositionModule::isInitialized()
{
	return _isInitialized;
}<commit_msg>Only delete log if calibration was successful<commit_after>#include "PositionModule.hpp"

#include <assert.h>
#include <sys/stat.h>
#include <errno.h>
#include <stdlib.h>
#include <string>
#include <algorithm>
#include <iostream>

#include <ros/console.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "sensor_msgs/Image.h"
#include "api_application/Ping.h"
#include "api_application/Announce.h"

#include "../matlab/Vector.h"

// Use this to test if images are saved properly, when you only have one camera.
//#define SINGLE_CAMERA_CALIBRATION

PositionModule::PositionModule(IPositionReceiver* receiver) :
	pictureCache(50), // Assume we never have 50 or more modules running on the network.
	pictureTimes(50)
{
	assert(receiver != 0);
	this->receiver = receiver;

	ROS_DEBUG("Initializing PositionModule");

	_isInitialized = true;
	isCalibrating = false;
	isRunning = false;

	ros::NodeHandle n;

	this->pictureSendingActivationPublisher = n.advertise<camera_application::PictureSendingActivation>("PictureSendingActivation", 4);
	this->pingPublisher = n.advertise<api_application::Ping>("Ping", 4);

	this->pictureSubscriber = n.subscribe("Picture", 128, &PositionModule::pictureCallback, this);
	this->systemSubscriber = n.subscribe("System", 4, &PositionModule::systemCallback, this);
	this->rawPositionSubscriber = n.subscribe("RawPosition", 1024, &PositionModule::rawPositionCallback, this);

	this->startCalibrationService = n.advertiseService("StartCalibration", &PositionModule::startCalibrationCallback, this);
	this->takeCalibrationPictureService = n.advertiseService("TakeCalibrationPicture", &PositionModule::takeCalibrationPictureCallback, this);
	this->calculateCalibrationService = n.advertiseService("CalculateCalibration", &PositionModule::calculateCalibrationCallback, this);

	// Advertise myself to API
	ros::ServiceClient announceClient = n.serviceClient<api_application::Announce>("announce");

	api_application::Announce announce;
	announce.request.type = 3; // 3 means position module

	if (announceClient.call(announce))
	{
		rosId = announce.response.id;

		if (rosId == ~0 /* -1 */)
		{
			ROS_ERROR("Error! Got id -1");
			_isInitialized = false;
		}
		else
		{
			ROS_INFO("Position module successfully announced. Got id %d", rosId);
		}
	}
	else
	{
		ROS_ERROR("Error! Could not announce myself to API!");
		_isInitialized = false;
	}

	msg = new KitrokopterMessages(rosId);

	if (_isInitialized)
	{
		ROS_DEBUG("PositionModule initialized");
	} else {
		ROS_ERROR("Could not initialize PositionModule!");
	}
}

PositionModule::~PositionModule()
{
	msg->~KitrokopterMessages();

	// TODO: Free picture cache.

	ROS_DEBUG("PositionModule destroyed");
}

// Service
bool PositionModule::startCalibrationCallback(control_application::StartCalibration::Request &req, control_application::StartCalibration::Response &res)
{
	res.ok = !isCalibrating;

	if (!isCalibrating)
	{
		ROS_INFO("Starting multi camera calibration process");

		setPictureSendingActivated(true);
		calibrationPictureCount = 0;
		boardSize = cv::Size(req.chessboardWidth, req.chessboardHeight);
		realSize = cv::Size(req.chessboardRealWidth, req.chessboardRealHeight);
	}

	isCalibrating = true;
	return true;
}

// Service
bool PositionModule::takeCalibrationPictureCallback(control_application::TakeCalibrationPicture::Request &req, control_application::TakeCalibrationPicture::Response &res)
{
	ROS_DEBUG("Taking calibration picture. Have %ld cameras.", camNoToNetId.size());

	pictureCacheMutex.lock();

	// Build net id -> cam no map.
	if (camNoToNetId.size() != netIdToCamNo.size()) {
		// If already built, there are already images on the disk with wrong ids, so return an error.
		if (netIdToCamNo.size() != 0) {
			ROS_ERROR("Got new cameras after taking first calibration picture!");
			return false;
		}

		std::sort(camNoToNetId.begin(), camNoToNetId.end());
		netIdToCamNo.clear();

		for (int i = 0; i < camNoToNetId.size(); i++) {
			netIdToCamNo[camNoToNetId[i]] = i;
		}

		ROS_INFO("Got %ld cameras", netIdToCamNo.size());
	}

	int id = 0;
	std::map<int, cv::Mat*> goodPictures;

	for (std::vector<cv::Mat*>::iterator it = pictureCache.begin(); it != pictureCache.end(); it++, id++)
	{
		if (*it != 0)
		{
			std::vector<cv::Point2f> corners;
			bool foundAllCorners = cv::findChessboardCorners(**it, boardSize, corners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);

			if (!foundAllCorners)
			{
				ROS_INFO("Took bad picture (id %d)", id);
				delete *it;
				*it = 0;
				continue;
			}
			else
			{
				ROS_INFO("Took good picture (id %d)", id);
				goodPictures[id] = *it;

				// Remove image from image cache.
				*it = 0;
			}
		}
	}

	pictureCacheMutex.unlock();

	#ifdef SINGLE_CAMERA_CALIBRATION
	if (goodPictures.size() >= 1) {
	#else
	if (goodPictures.size() >= 2) {
	#endif
		// Create directory for images.
		int error = mkdir("/tmp/calibrationImages", 0777);

		if (error != 0 && errno != EEXIST)
		{
			ROS_ERROR("Could not create directory for calibration images (/tmp/calibrationImages): %d", errno);

			// Delete images.
			for (std::map<int, cv::Mat*>::iterator it = goodPictures.begin(); it != goodPictures.end(); it++) {
				delete it->second;
			}

			return false;
		}

		error = mkdir("/tmp/calibrationResult", 0777);

		if (error != 0 && errno != EEXIST)
		{
			ROS_ERROR("Could not create directory for calibration images (/tmp/calibrationResult): %d", errno);

			// Delete images.
			for (std::map<int, cv::Mat*>::iterator it = goodPictures.begin(); it != goodPictures.end(); it++) {
				delete it->second;
			}

			return false;
		}

		for (std::map<int, cv::Mat*>::iterator it = goodPictures.begin(); it != goodPictures.end(); it++) {
			std::stringstream ss;
			ss << "/tmp/calibrationImages/cam" << netIdToCamNo[it->first] << "_image" << calibrationPictureCount << ".png";

			// Save picture on disk for amcctoolbox.
			std::cout << "Saving picture: " << cv::imwrite(ss.str(), *(it->second)) << std::endl;

			sensor_msgs::Image img;
			img.width = 640;
			img.height = 480;
			img.step = 3 * 640;
			img.data.reserve(img.step * img.height);

			for (int i = 0; i < 640 * 480 * 3; i++)
			{
				img.data[i] = it->second->data[i];
			}

			res.images.push_back(img);
			res.ids.push_back(it->first);

			delete it->second;
		}

		calibrationPictureCount++;
	}

	return true;
}

// Service
bool PositionModule::calculateCalibrationCallback(control_application::CalculateCalibration::Request &req, control_application::CalculateCalibration::Response &res)
{
	if (!isCalibrating) {
		ROS_ERROR("Cannot calculate calibration! Start calibration first!");
		return false;
	}

	if (netIdToCamNo.size() < 2) {
		ROS_ERROR("Have not enough images for calibration (Have %ld)!", netIdToCamNo.size());
	}

	ROS_INFO("Calculating multi camera calibration. This could take up to 2 hours");
	ChessboardData data(boardSize.width, boardSize.height, realSize.width, realSize.height);

	pictureCacheMutex.lock();
	int camNumber = camNoToNetId.size();
	pictureCacheMutex.unlock();

	// Delete old calibration results.
	system("rm -rf /tmp/calibrationResult/*");
	bool ok = tracker.calibrate(&data, camNoToNetId.size());

	if (ok) {
		ROS_INFO("Finished multi camera calibration");

		// TODO make not uncommented
		system("rm -rf /tmp/calibrationImages/*");
	} else {
		ROS_ERROR("Calibration failed!");
	}

	isCalibrating = false;

	return ok;
}

// Topic
void PositionModule::pictureCallback(const camera_application::Picture &msg)
{
	assert(msg.ID < 50);

	pictureCacheMutex.lock();

	bool idKnown = false;

	// Insert camera id, if not already there.
	for (int i = 0; i < camNoToNetId.size(); i++) {
		if (camNoToNetId[i] == msg.ID) {
			idKnown = true;
			break;
		}
	}

	if (!idKnown) {
		camNoToNetId.push_back(msg.ID);
	}

	if (isCalibrating)
	{
		// Will crash here, if more than 49 modules are used.
		if (pictureCache[msg.ID] != 0)
		{
			delete pictureCache[msg.ID];
			pictureCache[msg.ID] = 0;

		}

		cv::Mat* image = new cv::Mat(cv::Size(640, 480), CV_8UC3);

		for (int i = 0; i < 640 * 480 * 3; i++)
		{
			image->data[i] = msg.image[i];
		}

		pictureCache[msg.ID] = image;
		pictureTimes[msg.ID] = msg.timestamp;
	}

	pictureCacheMutex.unlock();
}

// Topic
void PositionModule::systemCallback(const api_application::System &msg)
{
	isRunning = msg.command == 1;

	if (isRunning) {
		ROS_INFO("Tracking started");
	} else {
		ROS_INFO("Tracking stopped");
	}

	if (!isRunning) {
		ros::shutdown();
	}
}

// Topic
void PositionModule::rawPositionCallback(const camera_application::RawPosition &msg)
{
 	// TODO: Calculate position in our coordinate system.
	// TODO: Is this coordinate change correct for amcctoolbox?
	Vector cameraVector(msg.xPosition, msg.yPosition, 1);
	ROS_DEBUG("msg.ID: %d netIdToCamNo[msg.ID]: %d msg.quadcopterId: %d", msg.ID, netIdToCamNo[msg.ID], msg.quadcopterId);
	Vector result = tracker.updatePosition(cameraVector, netIdToCamNo[msg.ID], msg.quadcopterId);

	std::vector<Vector> positions;
	std::vector<int> ids;
	std::vector<int> updates;
	positions.push_back(result);
	ids.push_back(msg.quadcopterId);
	updates.push_back(1);

	if (result.isValid()) {
		receiver->updatePositions(positions, ids, updates);
	} else {
		ROS_DEBUG("Not enough information to get position of quadcopter %d", msg.quadcopterId);
	}
}

void PositionModule::setPictureSendingActivated(bool activated)
{
	camera_application::PictureSendingActivation msg;
	msg.ID = 0;
	msg.active = activated;

	pictureSendingActivationPublisher.publish(msg);
}

void PositionModule::sendPing()
{
	api_application::Ping msg;
	msg.ID = rosId;
	pingPublisher.publish(msg);
}

bool PositionModule::isInitialized()
{
	return _isInitialized;
}<|endoftext|>


<commit_before>//
// Created by jefferson on 6/09/16.
//

#include "AbstractHAEA.h"

int AbstractHAEA::operatorSelect(std::vector<double> rates) {
    size_t size = rates.size();

    std::vector<double> values(size + 1, 0.0);
    double sum = 0.0;
    values[size] = 1.0;
    for(size_t i = 1; i < size; ++i) {
        sum += rates[i - 1];
        values[i] = sum;
    }

    double num = this->ur.generate();
    for(size_t i = 0; i < size; ++i) {
        if(values[i] <= num && num < values[i + 1]) {
            return static_cast<int>(i);
        }
    }
}

void AbstractHAEA::ratesNormalize(std::vector<double> operatorRates) {
    double sum = 0.0;
    size_t size = operatorRates.size();
    for(size_t i = 0; i < size; ++i) {
        sum += operatorRates[i];
    }
    for(size_t i = 0; i < size; ++i) {
        operatorRates[i] /= sum;
    }
}

void AbstractHAEA::initPopulation() {
    for(int i = 0; i < this->populationSize; ++i) {
        this->population.push_back(this->space.getRandomIndividual());

        std::vector<double> operRates(this->operatorRates.size(), 0.0);
        for(size_t j = 0; j < this->operatorRates.size(); ++j) {
            operRates[j] = this->ur.generate();
        }

        this->ratesNormalize(operRates);

        this->operatorRates.push_back(operRates);
    }

}

template <class T>
AbstractHAEA::AbstractHAEA(std::vector<Operator<T> > operators, int populationSize, int maxIters) :
eng(rd()),
randomOperator(eng, 0, static_cast<int>(operators.size()) - 1),
randomIndividual(eng, 0, populationSize - 1),
ur(eng, 0.0, 1.0)
{
    this->operators = operators;
    this->maxIters = maxIters;
}

template <class T>
std::vector<T> AbstractHAEA::solve(Space<T> space, OptimizationFunction<T> goal) {
    this->space = space;
    this->optimizationFunction = goal;

    this->initPopulation();
    for(int i = 0; i < this->maxIters; ++i) {
        for(int j = 0; j < this->populationSize; ++j) {
            T individual = this->population[j];

            std::vector<double> rates = this->operatorRates[i];
            int operatorIndex = this->operatorSelect(rates);
            Operator<T> op = this->operators[operatorIndex];
            int args = op.getArguments();

            double parentFitness = this->optimizationFunction.apply(individual);
        }
    }
}
<commit_msg>final implementation without test.<commit_after>//
// Created by jefferson on 6/09/16.
//

#include "AbstractHAEA.h"

int AbstractHAEA::operatorSelect(std::vector<double> rates) {
    size_t size = rates.size();

    std::vector<double> values(size + 1, 0.0);
    double sum = 0.0;
    values[size] = 1.0;
    for(size_t i = 1; i < size; ++i) {
        sum += rates[i - 1];
        values[i] = sum;
    }

    double num = this->ur.generate();
    for(size_t i = 0; i < size; ++i) {
        if(values[i] <= num && num < values[i + 1]) {
            return static_cast<int>(i);
        }
    }
}

void AbstractHAEA::ratesNormalize(std::vector<double> operatorRates) {
    double sum = 0.0;
    size_t size = operatorRates.size();
    for(size_t i = 0; i < size; ++i) {
        sum += operatorRates[i];
    }
    for(size_t i = 0; i < size; ++i) {
        operatorRates[i] /= sum;
    }
}

void AbstractHAEA::initPopulation() {
    for(int i = 0; i < this->populationSize; ++i) {
        this->population.push_back(this->space.getRandomIndividual());

        std::vector<double> operRates(this->operatorRates.size(), 0.0);
        for(size_t j = 0; j < this->operatorRates.size(); ++j) {
            operRates[j] = this->ur.generate();
        }

        this->ratesNormalize(operRates);

        this->operatorRates.push_back(operRates);
    }

}

template <class T>
AbstractHAEA::AbstractHAEA(std::vector<Operator<T> > operators, int populationSize, int maxIters) :
eng(rd()),
randomOperator(eng, 0, static_cast<int>(operators.size()) - 1),
randomIndividual(eng, 0, populationSize - 1),
ur(eng, 0.0, 1.0)
{
    this->operators = operators;
    this->maxIters = maxIters;
}

template <class T>
std::vector<T> AbstractHAEA::solve(Space<T> space, OptimizationFunction<T> goal) {
    this->space = space;
    this->optimizationFunction = goal;

    this->initPopulation();
    for(int i = 0; i < this->maxIters; ++i) {
        std::vector<T> new_population;
        for(int j = 0; j < this->populationSize; ++j) {
            T parent = this->population[j];
            double delta = this->ur.generate();

            std::vector<double> rates = this->operatorRates[i];
            int operatorIndex = this->operatorSelect(rates);
            Operator<T> op = this->operators[operatorIndex];
            int args = op.getArguments();

            double parentFitness = this->optimizationFunction.apply(parent);
            std::vector<T> selectedIndividuals;
            selectedIndividuals.push_back(parent);
            for(int k = 1; k < args; ++k) {
                // TODO: selection of the other individuals
                // TODO: for now we select it randomly
                int index = -1;
                do {
                    index = static_cast<int>(this->randomIndividual.generate());
                } while(index != j);

                selectedIndividuals.push_back(this->population[index]);
            }

            std::vector<T> offspring = op.apply(selectedIndividuals);
            double childFitness = std::numeric_limits<double>::max();
            T child;
            if(offspring.size() > 1) {
                for(auto ind = offspring.begin(); ind != offspring.end(); ++ind){
                    double currentFitness = this->optimizationFunction.apply(*ind);
                    if(currentFitness < childFitness) {
                        childFitness = currentFitness;
                        child = *ind;
                    }
                }
            } else {
                child = offspring[0];
                childFitness = this->optimizationFunction.apply(child);
            }

            if(childFitness < parentFitness) {
                rates[operatorIndex] *= (1.0 + delta);
            } else {
                rates[operatorIndex] *= (1.0 - delta);
            }

            this->ratesNormalize(rates);

            new_population.push_back(child);
        }
        this->population = new_population;
    }

    return this->population;
}
<|endoftext|>


<commit_before>#include "number.h"

namespace rtl {

Number bitwise$and(Number x, Number y)
{
    return number_from_uint32(number_to_uint32(x) & number_to_uint32(y));
}

Number bitwise$extract(Number x, Number n, Number w)
{
    uint32_t b = number_to_uint32(n);
    if (b >= 32) {
        return number_from_uint32(0);
    }
    uint32_t v = number_to_uint32(w);
    if (b + v > 32) {
        v = 32 - b;
    }
    if (v == 32) {
        return x;
    }
    return number_from_uint32((number_to_uint32(x) >> b) & ((1 << v) - 1));
}

bool bitwise$get(Number x, Number n)
{
    uint32_t b = number_to_uint32(n);
    if (b >= 32) {
        return false;
    }
    return (number_to_uint32(x) & (1 << b)) != 0;
}

Number bitwise$not(Number x)
{
    return number_from_uint32(~number_to_uint32(x));
}

Number bitwise$or(Number x, Number y)
{
    return number_from_uint32(number_to_uint32(x) | number_to_uint32(y));
}

Number bitwise$replace(Number x, Number n, Number w, Number y)
{
    uint32_t b = number_to_uint32(n);
    if (b >= 32) {
        return number_from_uint32(0);
    }
    uint32_t v = number_to_uint32(w);
    if (b + v > 32) {
        v = 32 - b;
    }
    uint32_t z = number_to_uint32(y);
    uint32_t mask = v < 32 ? (1 << v) - 1 : ~0;
    return number_from_uint32((number_to_uint32(x) & ~(mask << b)) | (z << b));
}

Number bitwise$set(Number x, Number n, bool v)
{
    uint32_t b = number_to_uint32(n);
    if (b >= 32) {
        return x;
    }
    if (v) {
        return number_from_uint32(number_to_uint32(x) | (1 << b));
    } else {
        return number_from_uint32(number_to_uint32(x) & ~(1 << b));
    }
}

Number bitwise$shift_left(Number x, Number n)
{
    uint32_t b = number_to_uint32(n);
    if (b >= 32) {
        return number_from_uint32(0);
    }
    return number_from_uint32(number_to_uint32(x) << b);
}

Number bitwise$shift_right(Number x, Number n)
{
    uint32_t b = number_to_uint32(n);
    if (b >= 32) {
        return number_from_uint32(0);
    }
    return number_from_uint32(number_to_uint32(x) >> b);
}

Number bitwise$shift_right_signed(Number x, Number n)
{
    uint32_t b = number_to_uint32(n);
    if (b >= 32) {
        return number_from_uint32(0);
    }
    return number_from_sint32(number_to_sint32(x) >> b);
}

Number bitwise$xor(Number x, Number y)
{
    return number_from_uint32(number_to_uint32(x) ^ number_to_uint32(y));
}

} // namespace rtl
<commit_msg>Use template-based implementation of bitwise functions<commit_after>#include "number.h"

template <typename T> class traits {};

template <> class traits<uint32_t> {
public:
    static const unsigned int BITS = 32;
    static Number to_number(uint32_t x) { return number_from_uint32(x); }
    static uint32_t from_number(Number x) { return number_to_uint32(x); }
};

template <typename T> Number bitwise_and(Number x, Number y)
{
    return traits<T>::to_number(traits<T>::from_number(x) & traits<T>::from_number(y)); }

template <typename T> Number bitwise_extract(Number x, Number n, Number w)
{
    unsigned int b = number_to_uint32(n);
    if (b >= traits<T>::BITS) {
        return traits<T>::to_number(0);
    }
    unsigned int v = number_to_uint32(w);
    if (b + v > traits<T>::BITS) {
        v = traits<T>::BITS - b;
    }
    if (v == traits<T>::BITS) {
        return x;
    }
    return traits<T>::to_number((traits<T>::from_number(x) >> b) & ((1 << v) - 1));
}

template <typename T> bool bitwise_get(Number x, Number n)
{
    unsigned int b = number_to_uint32(n);
    if (b >= traits<T>::BITS) {
        return false;
    }
    return (traits<T>::from_number(x) & (1 << b)) != 0;
}

template <typename T> Number bitwise_not(Number x)
{
    return traits<T>::to_number(~traits<T>::from_number(x));
}

template <typename T> Number bitwise_or(Number x, Number y)
{
    return traits<T>::to_number(traits<T>::from_number(x) | traits<T>::from_number(y));
}

template <typename T> Number bitwise_replace(Number x, Number n, Number w, Number y)
{
    unsigned int b = number_to_uint32(n);
    if (b >= traits<T>::BITS) {
        return traits<T>::to_number(0);
    }
    unsigned int v = number_to_uint32(w);
    if (b + v > traits<T>::BITS) {
        v = traits<T>::BITS - b;
    }
    T z = traits<T>::from_number(y);
    T mask = v < traits<T>::BITS ? (1 << v) - 1 : ~0;
    return traits<T>::to_number((traits<T>::from_number(x) & ~(mask << b)) | (z << b));
}

template <typename T> Number bitwise_set(Number x, Number n, bool v)
{
    unsigned int b = number_to_uint32(n);
    if (b >= traits<T>::BITS) {
        return x;
    }
    if (v) {
        return traits<T>::to_number(traits<T>::from_number(x) | (1 << b));
    } else {
        return traits<T>::to_number(traits<T>::from_number(x) & ~(1 << b));
    }
}

template <typename T> Number bitwise_shift_left(Number x, Number n)
{
    unsigned int b = number_to_uint32(n);
    if (b >= traits<T>::BITS) {
        return traits<T>::to_number(0);
    }
    return traits<T>::to_number(traits<T>::from_number(x) << b);
}

template <typename T> Number bitwise_shift_right(Number x, Number n)
{
    unsigned int b = number_to_uint32(n);
    if (b >= traits<T>::BITS) {
        return traits<T>::to_number(0);
    }
    return traits<T>::to_number(traits<T>::from_number(x) >> b);
}

template <typename T> Number bitwise_shift_right_signed(Number x, Number n)
{
    unsigned int b = number_to_uint32(n);
    if (b >= traits<T>::BITS) {
        return traits<T>::to_number(0);
    }
    return number_from_sint32(number_to_sint32(x) >> b);
}

template <typename T> Number bitwise_xor(Number x, Number y)
{
    return traits<T>::to_number(traits<T>::from_number(x) ^ traits<T>::from_number(y));
}

namespace rtl {

Number bitwise$and(Number x, Number y)
{
    return bitwise_and<uint32_t>(x, y);
}

Number bitwise$extract(Number x, Number n, Number w)
{
    return bitwise_extract<uint32_t>(x, n, w);
}

bool bitwise$get(Number x, Number n)
{
    return bitwise_get<uint32_t>(x, n);
}

Number bitwise$not(Number x)
{
    return bitwise_not<uint32_t>(x);
}

Number bitwise$or(Number x, Number y)
{
    return bitwise_or<uint32_t>(x, y);
}

Number bitwise$replace(Number x, Number n, Number w, Number y)
{
    return bitwise_replace<uint32_t>(x, n, w, y);
}

Number bitwise$set(Number x, Number n, bool v)
{
    return bitwise_set<uint32_t>(x, n, v);
}

Number bitwise$shift_left(Number x, Number n)
{
    return bitwise_shift_left<uint32_t>(x, n);
}

Number bitwise$shift_right(Number x, Number n)
{
    return bitwise_shift_right<uint32_t>(x, n);
}

Number bitwise$shift_right_signed(Number x, Number n)
{
    return bitwise_shift_right_signed<uint32_t>(x, n);
}

Number bitwise$xor(Number x, Number y)
{
    return bitwise_xor<uint32_t>(x, y);
}

} // namespace rtl
<|endoftext|>


<commit_before>/***********************************************************************
 cmdline.cpp - Command line parsing stuff used by the example and
    utility programs.  Not intended for end-user use!

 Copyright (c) 2007-2009 by Educational Technology Resources,
 Inc.  getopt() and its associated globals are the public domain
 implementation made available at the 1985 UNIFORUM conference in
 Dallas, Texas; the code is untouched except for style tweaks.
 Others may also hold copyrights on code in this file.  See the
 CREDITS.txt file in the top directory of the distribution for details.

 This file is part of MySQL++.

 MySQL++ is free software; you can redistribute it and/or modify it
 under the terms of the GNU Lesser General Public License as published
 by the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 MySQL++ is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 License for more details.

 You should have received a copy of the GNU Lesser General Public
 License along with MySQL++; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
 USA
***********************************************************************/

#define MYSQLPP_NOT_HEADER
#include "cmdline.h"

#include <iostream>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

//// getopt ////////////////////////////////////////////////////////////
// Bring in system getopt(), or define our own.

#if defined(HAVE_POSIX_GETOPT)
#	include <unistd.h>
#elif defined(HAVE_LIBIBERTY_GETOPT)
#	include <libiberty.h>
#else
	// Need to define our own getopt() on this system
	extern "C" {
		// Using C linkage to avoid potential conflicts with system
		// getopt(), if autoconf test fails to find it.
		static const char* optarg;
		static int optind = 1;
		static int getopt(int argc, char* const argv[], const char* opts)
		{
			static int optopt;
			static int sp = 1;
			register int c;
			register const char *cp;

			if (sp == 1) {
				/* If all args are processed, finish */
				if (optind >= argc) {
					return EOF;
				}
				if (argv[optind][0] != '-' || argv[ag_optind][1] == '\0') {
					return EOF;
				}
			}
			else if (!strcmp(argv[optind], "--")) {
				/* No more goptions to be processed after this one */
				optind++;
				return EOF;
			}

			optopt = c = argv[optind][sp];

			/* Check for invalid goption */
			if (c == ':' || (cp = strchr(gopts, c)) == 0) {
				fprintf(stderr, "%s: illegal option -- %c\n", argv[0], c);
				if (argv[optind][++sp] == '\0') {
					optind++;
					sp = 1;
				}

				return '?';
			}

			/* Does this goption require an argument? */
			if (*++cp == ':') {
				/* If so, get argument; if none provided output error */
				if (argv[optind][sp + 1] != '\0') {
					optarg = &argv[ag_optind++][sp + 1];
				}
				else if (++optind >= argc) {
					fprintf(stderr,
							"%s: option requires an argument -- %c\n", argv[0], c);
					sp = 1;
					return '?';
				}
				else {
					optarg = argv[ag_optind++];
				}
				sp = 1;
			}
			else {
				if (argv[optind][++sp] == '\0') {
					sp = 1;
					optind++;
				}
				optarg = 0;
			}

			return c;
		}
	} // end extern "C"
#endif // didn't find either POSIX or libiberty getopt()

////////////////////////////////////////////////////////////////////////
// Generic MySQL++-specific command line parsing mechanism

namespace mysqlpp {

//// CommandLineBase::finish_parse /////////////////////////////////////

void
CommandLineBase::finish_parse()
{
	const int nextras = argc_ - option_index();
	if (nextras > 0) {
		extra_args_.resize(nextras);
		for (int i = 0; i < nextras; ++i) {
			extra_args_[i] = argv_[option_index() + i];
		}
	}

	successful_ = true;
}

//// CommandLineBase::option_argument //////////////////////////////////
// Accessor for optarg, so caller doesn't have to know about this
// getopt() style interface.  Particualy helpful on non-POSIX systems.

const char*
CommandLineBase::option_argument() const
{
	return optarg;
}

//// CommandLineBase::option_index /////////////////////////////////////
// Accessor for optind, so caller doesn't have to know about this
// getopt() style interface.  Particualy helpful on non-POSIX systems.

int
CommandLineBase::option_index() const
{
	return optind;
}

//// CommandLineBase::parse_next ///////////////////////////////////////
// Wrapper around getopt(), using the stuff passed to our ctor to
// construct its argument list.

int
CommandLineBase::parse_next() const
{
	return getopt(argc_, argv_, opts_);
}

////////////////////////////////////////////////////////////////////////
// Command line parser for MySQL++ examples.

namespace examples {

const char* db_name = "mysql_cpp_data";

//// examples::CommandLine ctor ////////////////////////////////////////

CommandLine::CommandLine(int argc, char* const argv[],
		const char* user, const char* pass, const char* usage_extra) :
CommandLineBase(argc, argv, "m:p:s:u:D"),
dtest_mode_(false),
run_mode_(0),
server_(0),
user_(user && *user ? user : 0),
pass_(pass && *pass ? pass : "")
{
	int ch;
	while ((ch = parse_next()) != EOF) {
		switch (ch) {
			case 'm': run_mode_ = atoi(option_argument()); break;
			case 'p': pass_ = option_argument();           break;
			case 's': server_ = option_argument();         break;
			case 'u': user_ = option_argument();           break;
			case 'D': dtest_mode_ = true;                  break;
			default:
				print_usage(usage_extra);
				return;
		}
	}

	finish_parse();
}

//// examples::CommandLine::print_usage ////////////////////////////////
// Show a generic usage message suitable for ../examples/*.cpp  The
// parameters specialize the message to a minor degree.

void
CommandLine::print_usage(const char* extra) const
{
	std::cout << "usage: " << program_name() <<
			" [-s server_addr] [-u user] [-p password] " <<
			extra << std::endl;
	std::cout << std::endl;
	std::cout << "    If no options are given, connects to database "
			"server on localhost" << std::endl;
	std::cout << "    using your user name and no password." << std::endl;
	if (strlen(extra) > 0) {
		std::cout << std::endl;
		std::cout << "    The extra parameter " << extra <<
				" is required, regardless of which" << std::endl;
		std::cout << "    other arguments you pass." << std::endl;
	}
	std::cout << std::endl;
}

} // end namespace mysqlpp::examples
} // end namespace mysqlpp
<commit_msg>att_getopt() stuff was broken in moving it into the library, then checked in because we're doing development on a system that supplies getopt, so it wasn't being compiled.  Fixed.<commit_after>/***********************************************************************
 cmdline.cpp - Command line parsing stuff used by the example and
    utility programs.  Not intended for end-user use!

 Copyright (c) 2007-2009 by Educational Technology Resources,
 Inc.  getopt() and its associated globals are the public domain
 implementation made available at the 1985 UNIFORUM conference in
 Dallas, Texas; the code is untouched except for style tweaks.
 Others may also hold copyrights on code in this file.  See the
 CREDITS.txt file in the top directory of the distribution for details.

 This file is part of MySQL++.

 MySQL++ is free software; you can redistribute it and/or modify it
 under the terms of the GNU Lesser General Public License as published
 by the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 MySQL++ is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public
 License for more details.

 You should have received a copy of the GNU Lesser General Public
 License along with MySQL++; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
 USA
***********************************************************************/

#define MYSQLPP_NOT_HEADER
#include "cmdline.h"

#include <iostream>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

//// getopt ////////////////////////////////////////////////////////////
// Bring in system getopt(), or define our own.

#if defined(HAVE_POSIX_GETOPT)
#	include <unistd.h>
#elif defined(HAVE_LIBIBERTY_GETOPT)
#	include <libiberty.h>
#else
	// Need to define our own getopt() on this system
	extern "C" {
		// Using C linkage to avoid potential conflicts with system
		// getopt(), if autoconf test fails to find it.
		static const char* optarg;
		static int optind = 1;
		static int getopt(int argc, char* const argv[], const char* opts)
		{
			static int optopt;
			static int sp = 1;
			register int c;
			register const char *cp;

			if (sp == 1) {
				/* If all args are processed, finish */
				if (optind >= argc) {
					return EOF;
				}
				if (argv[optind][0] != '-' || argv[optind][1] == '\0') {
					return EOF;
				}
			}
			else if (!strcmp(argv[optind], "--")) {
				/* No more goptions to be processed after this one */
				optind++;
				return EOF;
			}

			optopt = c = argv[optind][sp];

			/* Check for invalid goption */
			if (c == ':' || (cp = strchr(opts, c)) == 0) {
				fprintf(stderr, "%s: illegal option -- %c\n", argv[0], c);
				if (argv[optind][++sp] == '\0') {
					optind++;
					sp = 1;
				}

				return '?';
			}

			/* Does this goption require an argument? */
			if (*++cp == ':') {
				/* If so, get argument; if none provided output error */
				if (argv[optind][sp + 1] != '\0') {
					optarg = &argv[optind++][sp + 1];
				}
				else if (++optind >= argc) {
					fprintf(stderr,
							"%s: option requires an argument -- %c\n", argv[0], c);
					sp = 1;
					return '?';
				}
				else {
					optarg = argv[optind++];
				}
				sp = 1;
			}
			else {
				if (argv[optind][++sp] == '\0') {
					sp = 1;
					optind++;
				}
				optarg = 0;
			}

			return c;
		}
	} // end extern "C"
#endif // didn't find either POSIX or libiberty getopt()

////////////////////////////////////////////////////////////////////////
// Generic MySQL++-specific command line parsing mechanism

namespace mysqlpp {

//// CommandLineBase::finish_parse /////////////////////////////////////

void
CommandLineBase::finish_parse()
{
	const int nextras = argc_ - option_index();
	if (nextras > 0) {
		extra_args_.resize(nextras);
		for (int i = 0; i < nextras; ++i) {
			extra_args_[i] = argv_[option_index() + i];
		}
	}

	successful_ = true;
}

//// CommandLineBase::option_argument //////////////////////////////////
// Accessor for optarg, so caller doesn't have to know about this
// getopt() style interface.  Particualy helpful on non-POSIX systems.

const char*
CommandLineBase::option_argument() const
{
	return optarg;
}

//// CommandLineBase::option_index /////////////////////////////////////
// Accessor for optind, so caller doesn't have to know about this
// getopt() style interface.  Particualy helpful on non-POSIX systems.

int
CommandLineBase::option_index() const
{
	return optind;
}

//// CommandLineBase::parse_next ///////////////////////////////////////
// Wrapper around getopt(), using the stuff passed to our ctor to
// construct its argument list.

int
CommandLineBase::parse_next() const
{
	return getopt(argc_, argv_, opts_);
}

////////////////////////////////////////////////////////////////////////
// Command line parser for MySQL++ examples.

namespace examples {

const char* db_name = "mysql_cpp_data";

//// examples::CommandLine ctor ////////////////////////////////////////

CommandLine::CommandLine(int argc, char* const argv[],
		const char* user, const char* pass, const char* usage_extra) :
CommandLineBase(argc, argv, "m:p:s:u:D"),
dtest_mode_(false),
run_mode_(0),
server_(0),
user_(user && *user ? user : 0),
pass_(pass && *pass ? pass : "")
{
	int ch;
	while ((ch = parse_next()) != EOF) {
		switch (ch) {
			case 'm': run_mode_ = atoi(option_argument()); break;
			case 'p': pass_ = option_argument();           break;
			case 's': server_ = option_argument();         break;
			case 'u': user_ = option_argument();           break;
			case 'D': dtest_mode_ = true;                  break;
			default:
				print_usage(usage_extra);
				return;
		}
	}

	finish_parse();
}

//// examples::CommandLine::print_usage ////////////////////////////////
// Show a generic usage message suitable for ../examples/*.cpp  The
// parameters specialize the message to a minor degree.

void
CommandLine::print_usage(const char* extra) const
{
	std::cout << "usage: " << program_name() <<
			" [-s server_addr] [-u user] [-p password] " <<
			extra << std::endl;
	std::cout << std::endl;
	std::cout << "    If no options are given, connects to database "
			"server on localhost" << std::endl;
	std::cout << "    using your user name and no password." << std::endl;
	if (strlen(extra) > 0) {
		std::cout << std::endl;
		std::cout << "    The extra parameter " << extra <<
				" is required, regardless of which" << std::endl;
		std::cout << "    other arguments you pass." << std::endl;
	}
	std::cout << std::endl;
}

} // end namespace mysqlpp::examples
} // end namespace mysqlpp
<|endoftext|>