<commit_before>/** @file texture_3D.hpp
    @brief Implements three-dimensional textures.
*/

#pragma once
#ifndef TEXUTRE_3D_HPP
#define TEXTURE_3D_HPP

namespace oglwrap {

// -------======{[ 3D Textures' declaration ]}======-------
#ifdef GL_TEXTURE_3D
template<Tex3DType texture_t>
/// The base class describing functionality for all 2D textures.
/** You should rather use the typedefed versions than this template. */
class Texture3D_Base : public TextureBase<TexType(texture_t)> {
public:

    /// @brief Generates an empty texture.
    /// @see glGenTextures
    Texture3D_Base() {}

    template <TexType another_texture_t>
    /// @brief Copies a 3D texture or changes the type of a texture to Texture3D
    /// @param src - The texture to copy or cast to Texture3D
    Texture3D_Base(const TextureBase<another_texture_t>& src)
        :TextureBase<texture_t>(src) {}

    #ifdef glTexImage3D
    /// @brief Uploads the base image.
    /// @param internalFormat - Specifies the number, order, and size of the color components in the texture.
    /// @param width - Specifies the width of the texture image. All implementations support texture images that are at least 1024 texels wide.
    /// @param height - Specifies the height of the texture image, or the number of layers in a texture array. All implementations support 2D texture images that are at least 1024 texels high, and texture arrays that are at least 256 layers deep.
    /// @param depth - Specifies the depth of the texture image, or the number of layers in a texture array. All implementations support 3D texture images that are at least 256 texels deep, and texture arrays that are at least 256 layers deep.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexImage3D
    static void upload(
        PixelDataInternalFormat internalFormat,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexImage3D(
            texture_t, 0, internalFormat, width, height, depth, 0, format, type, data
        ));
    }
    #endif // glTexImage3D

    #ifdef glTexImage3D
    /// @brief Uploads a mipmap of the image.
    /// @param level - Specifies the level-of-detail number. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param internalFormat - Specifies the number, order, and size of the color components in the texture.
    /// @param width - Specifies the width of the texture image. All implementations support texture images that are at least 1024 texels wide.
    /// @param height - Specifies the height of the texture image, or the number of layers in a texture array. All implementations support 2D texture images that are at least 1024 texels high, and texture arrays that are at least 256 layers deep.
    /// @param depth - Specifies the depth of the texture image, or the number of layers in a texture array. All implementations support 3D texture images that are at least 256 texels deep, and texture arrays that are at least 256 layers deep.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexImage3D
    static void upload_Mipmap(
        GLint level,
        PixelDataInternalFormat internalFormat,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexImage3D(
            texture_t, level, internalFormat, width, height, depth, 0, format, type, data
        ));
    }
    #endif // glTexImage3D

    #ifdef glTexSubImage3D
    /// @brief Updates a part of the base image.
    /// @param xOffset/yOffset/zOffset - Specifies a texel offset in the x/y/z direction within the texture array.
    /// @param width/height/depth - Specifies the width/height/depth of the texture subimage.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexSubImage3D
    static void subUpload(
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexSubImage3D(
            texture_t, 0, xOffset, yOffset, zOffset, width, height, depth, format, type, data
        ));
    }
    #endif // glTexSubImage3D

    #ifdef glTexSubImage3D
    /// @brief Updates a part of a mipmap image.
    /// @param level - Specifies the level-of-detail number. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param xOffset/yOffset/zOffset - Specifies a texel offset in the x/y/z direction within the texture array.
    /// @param width/height/depth - Specifies the width/height/depth of the texture subimage.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexSubImage3D
    static void subUpload_Mipmap(
        GLint level,
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexSubImage3D(
            texture_t, level, xOffset, yOffset, zOffset, width, height, depth, format, type, data
        ));
    }
    #endif // glTexSubImage3D

    #ifdef glCopyTexSubImage3D
    /// @brief Copies pixels from the current GL_READ_BUFFER and updates part of the base mipmap of this texture with them.
    /// @param xOffset/yOffset/zOffset - Specifies the texel offset in the x/y/z direction within the destination texture array.
    /// @param x, y - Specify the window coordinates of the left corner of the row of pixels to be copied.
    /// @param width/height - Specifies the width/height of the texture to copy.
    /// @see glCopyTexSubImage3D
    static void copySub(
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLint x,
        GLint y,
        GLsizei width,
        GLsizei height
    ) {
        gl( CopyTexSubImage3D(texture_t, 0, xOffset, yOffset, zOffset, x, y, width, height) );
    }
    #endif // glCopyTexSubImage3D

    #ifdef glCopyTexSubImage3D
    /// @brief Copies pixels from the current GL_READ_BUFFER and updates part of a mipmap of this texture.
    /// @param level - Specifies the level-of-detail number. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param xOffset/yOffset/zOffset - Specifies the texel offset in the x/y/z direction within the destination texture array.
    /// @param x, y - Specify the window coordinates of the left corner of the row of pixels to be copied.
    /// @param width/height - Specifies the width/height of the texture to copy.
    /// @see glCopyTexSubImage3D
    static void copySub_Mipmap(
        GLint level,
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLint x,
        GLint y,
        GLsizei width,
        GLsizei height
    ) {
        gl( CopyTexSubImage3D(texture_t, level, xOffset, yOffset, zOffset, x, y, width, height) );
    }
    #endif // glCopyTexSubImage3D

    /// @brief Returns the width of a mipmap of the currently bound texture of this class.
    /// @param level - Specifies the mipmap whose size should be queried.
    /// @see glGetTexLevelParameteriv, GL_TEXTURE_WIDTH
    static GLsizei width(GLint level = 0) {
        GLsizei data;
        gl( GetTexLevelParameteriv(texture_t, level, GL_TEXTURE_WIDTH, &data) );
        return data;
    }

    /// @brief Returns the height of a mipmap of the currently bound texture of this class.
    /// @param level - Specifies the mipmap whose size should be queried.
    /// @see glGetTexLevelParameteriv, GL_TEXTURE_HEIGHT
    static GLsizei height(GLint level = 0) {
        GLsizei data;
        gl( GetTexLevelParameteriv(texture_t, level, GL_TEXTURE_HEIGHT, &data) );
        return data;
    }

    /// @brief Returns the depth of a mipmap of the currently bound texture of this class.
    /// @param level - Specifies the mipmap whose size should be queried.
    /// @see glGetTexLevelParameteriv, GL_TEXTURE_DEPTH
    static GLsizei depth(GLint level = 0) {
        GLsizei data;
        gl( GetTexLevelParameteriv(texture_t, level, GL_TEXTURE_DEPTH, &data) );
        return data;
    }

    #ifdef glGetCompressedTexImage
    /// @brief Return a compressed texture image
    /// @param level - Specifies the level-of-detail number of the desired image. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param img - Returns the compressed texture image.
    /// @see glGetCompressedTexImage
    static void getCompressedImage(GLint level, GLvoid* img) {
        gl( GetCompressedTexImage(TexType::Tex3D, level, img) );
    }
    #endif
};

#ifdef GL_TEXTURE_3D
/// @brief Three-dimensional texture.
/// @see GL_TEXTURE_3D
typedef Texture3D_Base<Tex3DType::Tex3D> Texture3D;
#endif // GL_TEXTURE_3D

#ifdef GL_TEXTURE_2D_ARRAY
/// An array of two dimensional textures
/// @see GL_TEXTURE_2D_ARRAY
typedef Texture3D_Base<Tex3DType::Tex2DArray> Texture2D_Array;
#endif // GL_TEXTURE_2D_ARRAY

#endif // GL_TEXTURE_3D

} // namespace oglwrap
<commit_msg>Removed an extra header guard.<commit_after>/** @file texture_3D.hpp
    @brief Implements three-dimensional textures.
*/

#pragma once

namespace oglwrap {

// -------======{[ 3D Textures' declaration ]}======-------
#ifdef GL_TEXTURE_3D
template<Tex3DType texture_t>
/// The base class describing functionality for all 2D textures.
/** You should rather use the typedefed versions than this template. */
class Texture3D_Base : public TextureBase<TexType(texture_t)> {
public:

    /// @brief Generates an empty texture.
    /// @see glGenTextures
    Texture3D_Base() {}

    template <TexType another_texture_t>
    /// @brief Copies a 3D texture or changes the type of a texture to Texture3D
    /// @param src - The texture to copy or cast to Texture3D
    Texture3D_Base(const TextureBase<another_texture_t>& src)
        :TextureBase<texture_t>(src) {}

    #ifdef glTexImage3D
    /// @brief Uploads the base image.
    /// @param internalFormat - Specifies the number, order, and size of the color components in the texture.
    /// @param width - Specifies the width of the texture image. All implementations support texture images that are at least 1024 texels wide.
    /// @param height - Specifies the height of the texture image, or the number of layers in a texture array. All implementations support 2D texture images that are at least 1024 texels high, and texture arrays that are at least 256 layers deep.
    /// @param depth - Specifies the depth of the texture image, or the number of layers in a texture array. All implementations support 3D texture images that are at least 256 texels deep, and texture arrays that are at least 256 layers deep.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexImage3D
    static void upload(
        PixelDataInternalFormat internalFormat,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexImage3D(
            texture_t, 0, internalFormat, width, height, depth, 0, format, type, data
        ));
    }
    #endif // glTexImage3D

    #ifdef glTexImage3D
    /// @brief Uploads a mipmap of the image.
    /// @param level - Specifies the level-of-detail number. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param internalFormat - Specifies the number, order, and size of the color components in the texture.
    /// @param width - Specifies the width of the texture image. All implementations support texture images that are at least 1024 texels wide.
    /// @param height - Specifies the height of the texture image, or the number of layers in a texture array. All implementations support 2D texture images that are at least 1024 texels high, and texture arrays that are at least 256 layers deep.
    /// @param depth - Specifies the depth of the texture image, or the number of layers in a texture array. All implementations support 3D texture images that are at least 256 texels deep, and texture arrays that are at least 256 layers deep.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexImage3D
    static void upload_Mipmap(
        GLint level,
        PixelDataInternalFormat internalFormat,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexImage3D(
            texture_t, level, internalFormat, width, height, depth, 0, format, type, data
        ));
    }
    #endif // glTexImage3D

    #ifdef glTexSubImage3D
    /// @brief Updates a part of the base image.
    /// @param xOffset/yOffset/zOffset - Specifies a texel offset in the x/y/z direction within the texture array.
    /// @param width/height/depth - Specifies the width/height/depth of the texture subimage.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexSubImage3D
    static void subUpload(
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexSubImage3D(
            texture_t, 0, xOffset, yOffset, zOffset, width, height, depth, format, type, data
        ));
    }
    #endif // glTexSubImage3D

    #ifdef glTexSubImage3D
    /// @brief Updates a part of a mipmap image.
    /// @param level - Specifies the level-of-detail number. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param xOffset/yOffset/zOffset - Specifies a texel offset in the x/y/z direction within the texture array.
    /// @param width/height/depth - Specifies the width/height/depth of the texture subimage.
    /// @param format - Specifies the format of the pixel data.
    /// @param type - Specifies the data type of the pixel data.
    /// @param data - Specifies a pointer to the image data in memory.
    /// @see glTexSubImage3D
    static void subUpload_Mipmap(
        GLint level,
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLsizei width,
        GLsizei height,
        GLsizei depth,
        PixelDataFormat format,
        PixelDataType type,
        const void *data
    ) {
        gl( TexSubImage3D(
            texture_t, level, xOffset, yOffset, zOffset, width, height, depth, format, type, data
        ));
    }
    #endif // glTexSubImage3D

    #ifdef glCopyTexSubImage3D
    /// @brief Copies pixels from the current GL_READ_BUFFER and updates part of the base mipmap of this texture with them.
    /// @param xOffset/yOffset/zOffset - Specifies the texel offset in the x/y/z direction within the destination texture array.
    /// @param x, y - Specify the window coordinates of the left corner of the row of pixels to be copied.
    /// @param width/height - Specifies the width/height of the texture to copy.
    /// @see glCopyTexSubImage3D
    static void copySub(
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLint x,
        GLint y,
        GLsizei width,
        GLsizei height
    ) {
        gl( CopyTexSubImage3D(texture_t, 0, xOffset, yOffset, zOffset, x, y, width, height) );
    }
    #endif // glCopyTexSubImage3D

    #ifdef glCopyTexSubImage3D
    /// @brief Copies pixels from the current GL_READ_BUFFER and updates part of a mipmap of this texture.
    /// @param level - Specifies the level-of-detail number. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param xOffset/yOffset/zOffset - Specifies the texel offset in the x/y/z direction within the destination texture array.
    /// @param x, y - Specify the window coordinates of the left corner of the row of pixels to be copied.
    /// @param width/height - Specifies the width/height of the texture to copy.
    /// @see glCopyTexSubImage3D
    static void copySub_Mipmap(
        GLint level,
        GLint xOffset,
        GLint yOffset,
        GLint zOffset,
        GLint x,
        GLint y,
        GLsizei width,
        GLsizei height
    ) {
        gl( CopyTexSubImage3D(texture_t, level, xOffset, yOffset, zOffset, x, y, width, height) );
    }
    #endif // glCopyTexSubImage3D

    /// @brief Returns the width of a mipmap of the currently bound texture of this class.
    /// @param level - Specifies the mipmap whose size should be queried.
    /// @see glGetTexLevelParameteriv, GL_TEXTURE_WIDTH
    static GLsizei width(GLint level = 0) {
        GLsizei data;
        gl( GetTexLevelParameteriv(texture_t, level, GL_TEXTURE_WIDTH, &data) );
        return data;
    }

    /// @brief Returns the height of a mipmap of the currently bound texture of this class.
    /// @param level - Specifies the mipmap whose size should be queried.
    /// @see glGetTexLevelParameteriv, GL_TEXTURE_HEIGHT
    static GLsizei height(GLint level = 0) {
        GLsizei data;
        gl( GetTexLevelParameteriv(texture_t, level, GL_TEXTURE_HEIGHT, &data) );
        return data;
    }

    /// @brief Returns the depth of a mipmap of the currently bound texture of this class.
    /// @param level - Specifies the mipmap whose size should be queried.
    /// @see glGetTexLevelParameteriv, GL_TEXTURE_DEPTH
    static GLsizei depth(GLint level = 0) {
        GLsizei data;
        gl( GetTexLevelParameteriv(texture_t, level, GL_TEXTURE_DEPTH, &data) );
        return data;
    }

    #ifdef glGetCompressedTexImage
    /// @brief Return a compressed texture image
    /// @param level - Specifies the level-of-detail number of the desired image. Level 0 is the base image level. Level n is the nth mipmap reduction image.
    /// @param img - Returns the compressed texture image.
    /// @see glGetCompressedTexImage
    static void getCompressedImage(GLint level, GLvoid* img) {
        gl( GetCompressedTexImage(TexType::Tex3D, level, img) );
    }
    #endif
};

#ifdef GL_TEXTURE_3D
/// @brief Three-dimensional texture.
/// @see GL_TEXTURE_3D
typedef Texture3D_Base<Tex3DType::Tex3D> Texture3D;
#endif // GL_TEXTURE_3D

#ifdef GL_TEXTURE_2D_ARRAY
/// An array of two dimensional textures
/// @see GL_TEXTURE_2D_ARRAY
typedef Texture3D_Base<Tex3DType::Tex2DArray> Texture2D_Array;
#endif // GL_TEXTURE_2D_ARRAY

#endif // GL_TEXTURE_3D

} // namespace oglwrap
<|endoftext|>


<commit_before>/* mbed Microcontroller Library
 * Copyright (c) 2017-2017 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "us_ticker_api.h"
#include "BLE.h"
#include "wsf_types.h"
#include "wsf_msg.h"
#include "wsf_os.h"
#include "wsf_buf.h"
#include "wsf_timer.h"
#include "hci_handler.h"
#include "dm_handler.h"
#include "l2c_handler.h"
#include "att_handler.h"
#include "smp_handler.h"
#include "l2c_api.h"
#include "att_api.h"
#include "smp_api.h"
#include "hci_drv.h"
#include "CordioBLE.h"
#include "mbed_assert.h"

#include "CordioPalAttClient.h"
#include "CordioPalSecurityManager.h"

/*! WSF handler ID */
wsfHandlerId_t stack_handler_id;

/**
 * Weak definition of ble_cordio_get_hci_driver.
 * A runtime error is generated if the user does not define any
 * ble_cordio_get_hci_driver.
 */
MBED_WEAK ble::vendor::cordio::CordioHCIDriver& ble_cordio_get_hci_driver()
{
    error("Please provide an implementation for the HCI driver");
    ble::vendor::cordio::CordioHCIDriver* bad_instance = NULL;
    return *bad_instance;
}

/**
 * Low level HCI interface between Cordio stack and the port.
 */
extern "C" uint16_t hci_mbed_os_drv_write(uint8_t type, uint16_t len, uint8_t *pData)
{
    return ble_cordio_get_hci_driver().write(type, len, pData);
}

extern "C" void hci_mbed_os_start_reset_sequence(void)
{
    ble_cordio_get_hci_driver().start_reset_sequence();
}

extern "C" void hci_mbed_os_handle_reset_sequence(uint8_t* msg)
{
     ble_cordio_get_hci_driver().handle_reset_sequence(msg);
}

/*
 * This function will signal to the user code by calling signalEventsToProcess.
 * It is registered and called into the Wsf Stack.
 */
extern "C" void wsf_mbed_ble_signal_event(void)
{
    ble::vendor::cordio::BLE::deviceInstance().signalEventsToProcess(::BLE::DEFAULT_INSTANCE);
}

/**
 * BLE-API requires an implementation of the following function in order to
 * obtain its transport handle.
 */
BLEInstanceBase *createBLEInstance()
{
    return (&(ble::vendor::cordio::BLE::deviceInstance()));
}

namespace ble {
namespace vendor {
namespace cordio {

BLE::BLE(CordioHCIDriver& hci_driver) :
    initialization_status(NOT_INITIALIZED),
    instanceID(::BLE::DEFAULT_INSTANCE),
    _event_queue()
{
    _hci_driver = &hci_driver;
    stack_setup();
}

BLE::~BLE() { }

/**
 * The singleton which represents the BLE transport for the BLE.
 */
BLE& BLE::deviceInstance()
{
    static BLE instance(
        ble_cordio_get_hci_driver()
    );
    return instance;
}

ble_error_t BLE::init(
    ::BLE::InstanceID_t instanceID,
    FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext *> initCallback)
{
    switch (initialization_status) {
        case NOT_INITIALIZED:
            _event_queue.initialize(this, instanceID);
            _init_callback = initCallback;
            start_stack_reset();
            return BLE_ERROR_NONE;

        case INITIALIZING:
            return BLE_ERROR_INITIALIZATION_INCOMPLETE;

        case INITIALIZED:
            return BLE_ERROR_NONE;

        default:
            return BLE_ERROR_UNSPECIFIED;
    }
}

bool BLE::hasInitialized() const
{
    return initialization_status == INITIALIZED;
}

ble_error_t BLE::shutdown()
{
    if (initialization_status != INITIALIZED) {
        return BLE_ERROR_INITIALIZATION_INCOMPLETE;
    }

    initialization_status = NOT_INITIALIZED;
    _hci_driver->terminate();

    getGattServer().reset();
    getGattClient().reset();
    getGap().reset();
    _event_queue.clear();

    return BLE_ERROR_NONE;
}

const char* BLE::getVersion()
{
    static const char version[] = "generic-cordio";
    return version;
}

generic::GenericGap& BLE::getGap()
{
    static pal::vendor::cordio::GenericAccessService cordio_gap_service;
    static ble::generic::GenericGap gap(
        _event_queue,
        pal::vendor::cordio::Gap::get_gap(),
        cordio_gap_service,
        pal::vendor::cordio::CordioSecurityManager::get_security_manager()
    );

    return gap;
}

const generic::GenericGap& BLE::getGap() const
{
    BLE &self = const_cast<BLE&>(*this);
    return const_cast<const generic::GenericGap&>(self.getGap());
};

GattServer& BLE::getGattServer()
{
    return cordio::GattServer::getInstance();
}

const GattServer& BLE::getGattServer() const
{
    return cordio::GattServer::getInstance();
}

generic::GenericGattClient& BLE::getGattClient()
{
    static pal::AttClientToGattClientAdapter pal_client(
        pal::vendor::cordio::CordioAttClient::get_client()
    );
    static generic::GenericGattClient client(&pal_client);

    return client;
}

SecurityManager& BLE::getSecurityManager()
{
    static SigningEventMonitorProxy signing_event_monitor(*this);
    static generic::GenericSecurityManager m_instance(
        pal::vendor::cordio::CordioSecurityManager::get_security_manager(),
        getGap(),
        signing_event_monitor
    );

    return m_instance;
}

const SecurityManager& BLE::getSecurityManager() const
{
    const BLE &self = const_cast<BLE&>(*this);
    return const_cast<const SecurityManager&>(self.getSecurityManager());
}

void BLE::waitForEvent()
{
    static Timeout nextTimeout;
    timestamp_t nextTimestamp;
    bool_t pTimerRunning;

    callDispatcher();

    if (wsfOsReadyToSleep()) {
        // setup an mbed timer for the next cordio timeout
        nextTimestamp = (timestamp_t)(WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

void BLE::processEvents()
{
    callDispatcher();
}

 void BLE::stack_handler(wsfEventMask_t event, wsfMsgHdr_t* msg)
 {
    if (msg == NULL) {
        return;
    }

    if (ble::pal::vendor::cordio::CordioSecurityManager::get_security_manager().sm_handler(msg)) {
        return;
    }

    switch(msg->event) {
        case DM_RESET_CMPL_IND: {
            ::BLE::InitializationCompleteCallbackContext context = {
                ::BLE::Instance(::BLE::DEFAULT_INSTANCE),
                BLE_ERROR_NONE
            };

            // initialize extended module if supported
            if (HciGetLeSupFeat() == HCI_LE_SUP_FEAT_LE_EXT_ADV) {
                DmExtAdvInit();
                DmExtScanInit();
                DmExtConnMasterInit();
                DmExtConnSlaveInit();
            }

            deviceInstance().getGattServer().initialize();
            deviceInstance().initialization_status = INITIALIZED;
            _init_callback.call(&context);
        }   break;

        default:
            ble::pal::vendor::cordio::Gap::gap_handler(msg);
            break;
    }
}

void BLE::device_manager_cb(dmEvt_t* dm_event)
{
    BLE::deviceInstance().stack_handler(0, &dm_event->hdr);
}

/*
 * AttServerInitDeInitCback callback is used to Initialize/Deinitialize
 * the CCC Table of the ATT Server when a remote peer requests to Open
 * or Close the connection.
 */
 void BLE::connection_handler(dmEvt_t* dm_event)
 {
    dmConnId_t connId = (dmConnId_t)dm_event->hdr.param;

    switch (dm_event->hdr.event) {
        case DM_CONN_OPEN_IND:
            /* set up CCC table with uninitialized (all zero) values */
            AttsCccInitTable(connId, NULL);
            break;
        case DM_CONN_CLOSE_IND:
            /* clear CCC table on connection close */
            AttsCccClearTable(connId);
            break;
        default:
            break;
    }
}

void BLE::timeoutCallback()
{
    wsf_mbed_ble_signal_event();
}

void BLE::stack_setup()
{
    MBED_ASSERT(_hci_driver != NULL);

    wsfHandlerId_t handlerId;

    buf_pool_desc_t buf_pool_desc = _hci_driver->get_buffer_pool_description();

    // Initialize buffers with the ones provided by the HCI driver
    uint16_t bytes_used = WsfBufInit(
        buf_pool_desc.buffer_size, buf_pool_desc.buffer_memory,
        buf_pool_desc.pool_count, buf_pool_desc.pool_description
    );

    MBED_ASSERT(bytes_used != 0);

    WsfTimerInit();
    SecInit();

    // Note: enable once security is supported
    SecRandInit();
    SecAesInit();
    SecCmacInit();
    SecEccInit();

    handlerId = WsfOsSetNextHandler(HciHandler);
    HciHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(DmHandler);
    DmAdvInit();
    DmScanInit();
    DmConnInit();
    DmConnMasterInit();
    DmConnSlaveInit();
    DmSecInit();
    DmPhyInit();

    // Note: enable once security is supported
    DmSecLescInit();
    DmPrivInit();
    DmHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(L2cSlaveHandler);
    L2cSlaveHandlerInit(handlerId);
    L2cInit();
    L2cSlaveInit();
    L2cMasterInit();

    handlerId = WsfOsSetNextHandler(AttHandler);
    AttHandlerInit(handlerId);
    AttsInit();
    AttsIndInit();
    AttsSignInit();
    AttsAuthorRegister(GattServer::atts_auth_cb);
    AttcInit();
    AttcSignInit();

    handlerId = WsfOsSetNextHandler(SmpHandler);
    SmpHandlerInit(handlerId);
    SmprInit();
    SmprScInit();
    SmpiInit();
    SmpiScInit();

    stack_handler_id = WsfOsSetNextHandler(&BLE::stack_handler);

    HciSetMaxRxAclLen(100);

    DmRegister(BLE::device_manager_cb);
    DmConnRegister(DM_CLIENT_ID_APP, BLE::device_manager_cb);
    AttConnRegister(BLE::connection_handler);
    AttRegister((attCback_t) ble::pal::vendor::cordio::CordioAttClient::att_client_handler);
}

void BLE::start_stack_reset()
{
    _hci_driver->initialize();
    DmDevReset();
}

void BLE::callDispatcher()
{
    // process the external event queue
    _event_queue.process();

    // follow by stack events
    static uint32_t lastTimeUs = us_ticker_read();
    uint32_t currTimeUs, deltaTimeMs;

    // Update the current cordio time
    currTimeUs = us_ticker_read();
    deltaTimeMs = (currTimeUs - lastTimeUs) / 1000;
    if (deltaTimeMs > 0) {
        WsfTimerUpdate(deltaTimeMs / WSF_MS_PER_TICK);
        lastTimeUs += deltaTimeMs * 1000;
    }

    wsfOsDispatcher();

    if (wsfOsReadyToSleep()) {
        static Timeout nextTimeout;
        // setup an mbed timer for the next Cordio timeout
        bool_t pTimerRunning;
        timestamp_t nextTimestamp = (timestamp_t) (WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

CordioHCIDriver* BLE::_hci_driver = NULL;

FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext*> BLE::_init_callback;

} // namespace cordio
} // namespace vendor
} // namespace ble
<commit_msg>BLE: Fix extended advertising check.<commit_after>/* mbed Microcontroller Library
 * Copyright (c) 2017-2017 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "mbed.h"
#include "us_ticker_api.h"
#include "BLE.h"
#include "wsf_types.h"
#include "wsf_msg.h"
#include "wsf_os.h"
#include "wsf_buf.h"
#include "wsf_timer.h"
#include "hci_handler.h"
#include "dm_handler.h"
#include "l2c_handler.h"
#include "att_handler.h"
#include "smp_handler.h"
#include "l2c_api.h"
#include "att_api.h"
#include "smp_api.h"
#include "hci_drv.h"
#include "CordioBLE.h"
#include "mbed_assert.h"

#include "CordioPalAttClient.h"
#include "CordioPalSecurityManager.h"

/*! WSF handler ID */
wsfHandlerId_t stack_handler_id;

/**
 * Weak definition of ble_cordio_get_hci_driver.
 * A runtime error is generated if the user does not define any
 * ble_cordio_get_hci_driver.
 */
MBED_WEAK ble::vendor::cordio::CordioHCIDriver& ble_cordio_get_hci_driver()
{
    error("Please provide an implementation for the HCI driver");
    ble::vendor::cordio::CordioHCIDriver* bad_instance = NULL;
    return *bad_instance;
}

/**
 * Low level HCI interface between Cordio stack and the port.
 */
extern "C" uint16_t hci_mbed_os_drv_write(uint8_t type, uint16_t len, uint8_t *pData)
{
    return ble_cordio_get_hci_driver().write(type, len, pData);
}

extern "C" void hci_mbed_os_start_reset_sequence(void)
{
    ble_cordio_get_hci_driver().start_reset_sequence();
}

extern "C" void hci_mbed_os_handle_reset_sequence(uint8_t* msg)
{
     ble_cordio_get_hci_driver().handle_reset_sequence(msg);
}

/*
 * This function will signal to the user code by calling signalEventsToProcess.
 * It is registered and called into the Wsf Stack.
 */
extern "C" void wsf_mbed_ble_signal_event(void)
{
    ble::vendor::cordio::BLE::deviceInstance().signalEventsToProcess(::BLE::DEFAULT_INSTANCE);
}

/**
 * BLE-API requires an implementation of the following function in order to
 * obtain its transport handle.
 */
BLEInstanceBase *createBLEInstance()
{
    return (&(ble::vendor::cordio::BLE::deviceInstance()));
}

namespace ble {
namespace vendor {
namespace cordio {

BLE::BLE(CordioHCIDriver& hci_driver) :
    initialization_status(NOT_INITIALIZED),
    instanceID(::BLE::DEFAULT_INSTANCE),
    _event_queue()
{
    _hci_driver = &hci_driver;
    stack_setup();
}

BLE::~BLE() { }

/**
 * The singleton which represents the BLE transport for the BLE.
 */
BLE& BLE::deviceInstance()
{
    static BLE instance(
        ble_cordio_get_hci_driver()
    );
    return instance;
}

ble_error_t BLE::init(
    ::BLE::InstanceID_t instanceID,
    FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext *> initCallback)
{
    switch (initialization_status) {
        case NOT_INITIALIZED:
            _event_queue.initialize(this, instanceID);
            _init_callback = initCallback;
            start_stack_reset();
            return BLE_ERROR_NONE;

        case INITIALIZING:
            return BLE_ERROR_INITIALIZATION_INCOMPLETE;

        case INITIALIZED:
            return BLE_ERROR_NONE;

        default:
            return BLE_ERROR_UNSPECIFIED;
    }
}

bool BLE::hasInitialized() const
{
    return initialization_status == INITIALIZED;
}

ble_error_t BLE::shutdown()
{
    if (initialization_status != INITIALIZED) {
        return BLE_ERROR_INITIALIZATION_INCOMPLETE;
    }

    initialization_status = NOT_INITIALIZED;
    _hci_driver->terminate();

    getGattServer().reset();
    getGattClient().reset();
    getGap().reset();
    _event_queue.clear();

    return BLE_ERROR_NONE;
}

const char* BLE::getVersion()
{
    static const char version[] = "generic-cordio";
    return version;
}

generic::GenericGap& BLE::getGap()
{
    static pal::vendor::cordio::GenericAccessService cordio_gap_service;
    static ble::generic::GenericGap gap(
        _event_queue,
        pal::vendor::cordio::Gap::get_gap(),
        cordio_gap_service,
        pal::vendor::cordio::CordioSecurityManager::get_security_manager()
    );

    return gap;
}

const generic::GenericGap& BLE::getGap() const
{
    BLE &self = const_cast<BLE&>(*this);
    return const_cast<const generic::GenericGap&>(self.getGap());
};

GattServer& BLE::getGattServer()
{
    return cordio::GattServer::getInstance();
}

const GattServer& BLE::getGattServer() const
{
    return cordio::GattServer::getInstance();
}

generic::GenericGattClient& BLE::getGattClient()
{
    static pal::AttClientToGattClientAdapter pal_client(
        pal::vendor::cordio::CordioAttClient::get_client()
    );
    static generic::GenericGattClient client(&pal_client);

    return client;
}

SecurityManager& BLE::getSecurityManager()
{
    static SigningEventMonitorProxy signing_event_monitor(*this);
    static generic::GenericSecurityManager m_instance(
        pal::vendor::cordio::CordioSecurityManager::get_security_manager(),
        getGap(),
        signing_event_monitor
    );

    return m_instance;
}

const SecurityManager& BLE::getSecurityManager() const
{
    const BLE &self = const_cast<BLE&>(*this);
    return const_cast<const SecurityManager&>(self.getSecurityManager());
}

void BLE::waitForEvent()
{
    static Timeout nextTimeout;
    timestamp_t nextTimestamp;
    bool_t pTimerRunning;

    callDispatcher();

    if (wsfOsReadyToSleep()) {
        // setup an mbed timer for the next cordio timeout
        nextTimestamp = (timestamp_t)(WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

void BLE::processEvents()
{
    callDispatcher();
}

 void BLE::stack_handler(wsfEventMask_t event, wsfMsgHdr_t* msg)
 {
    if (msg == NULL) {
        return;
    }

    if (ble::pal::vendor::cordio::CordioSecurityManager::get_security_manager().sm_handler(msg)) {
        return;
    }

    switch(msg->event) {
        case DM_RESET_CMPL_IND: {
            ::BLE::InitializationCompleteCallbackContext context = {
                ::BLE::Instance(::BLE::DEFAULT_INSTANCE),
                BLE_ERROR_NONE
            };

            // initialize extended module if supported
            if (HciGetLeSupFeat() & HCI_LE_SUP_FEAT_LE_EXT_ADV) {
                DmExtAdvInit();
                DmExtScanInit();
                DmExtConnMasterInit();
                DmExtConnSlaveInit();
            }

            deviceInstance().getGattServer().initialize();
            deviceInstance().initialization_status = INITIALIZED;
            _init_callback.call(&context);
        }   break;

        default:
            ble::pal::vendor::cordio::Gap::gap_handler(msg);
            break;
    }
}

void BLE::device_manager_cb(dmEvt_t* dm_event)
{
    BLE::deviceInstance().stack_handler(0, &dm_event->hdr);
}

/*
 * AttServerInitDeInitCback callback is used to Initialize/Deinitialize
 * the CCC Table of the ATT Server when a remote peer requests to Open
 * or Close the connection.
 */
 void BLE::connection_handler(dmEvt_t* dm_event)
 {
    dmConnId_t connId = (dmConnId_t)dm_event->hdr.param;

    switch (dm_event->hdr.event) {
        case DM_CONN_OPEN_IND:
            /* set up CCC table with uninitialized (all zero) values */
            AttsCccInitTable(connId, NULL);
            break;
        case DM_CONN_CLOSE_IND:
            /* clear CCC table on connection close */
            AttsCccClearTable(connId);
            break;
        default:
            break;
    }
}

void BLE::timeoutCallback()
{
    wsf_mbed_ble_signal_event();
}

void BLE::stack_setup()
{
    MBED_ASSERT(_hci_driver != NULL);

    wsfHandlerId_t handlerId;

    buf_pool_desc_t buf_pool_desc = _hci_driver->get_buffer_pool_description();

    // Initialize buffers with the ones provided by the HCI driver
    uint16_t bytes_used = WsfBufInit(
        buf_pool_desc.buffer_size, buf_pool_desc.buffer_memory,
        buf_pool_desc.pool_count, buf_pool_desc.pool_description
    );

    MBED_ASSERT(bytes_used != 0);

    WsfTimerInit();
    SecInit();

    // Note: enable once security is supported
    SecRandInit();
    SecAesInit();
    SecCmacInit();
    SecEccInit();

    handlerId = WsfOsSetNextHandler(HciHandler);
    HciHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(DmHandler);
    DmAdvInit();
    DmScanInit();
    DmConnInit();
    DmConnMasterInit();
    DmConnSlaveInit();
    DmSecInit();
    DmPhyInit();

    // Note: enable once security is supported
    DmSecLescInit();
    DmPrivInit();
    DmHandlerInit(handlerId);

    handlerId = WsfOsSetNextHandler(L2cSlaveHandler);
    L2cSlaveHandlerInit(handlerId);
    L2cInit();
    L2cSlaveInit();
    L2cMasterInit();

    handlerId = WsfOsSetNextHandler(AttHandler);
    AttHandlerInit(handlerId);
    AttsInit();
    AttsIndInit();
    AttsSignInit();
    AttsAuthorRegister(GattServer::atts_auth_cb);
    AttcInit();
    AttcSignInit();

    handlerId = WsfOsSetNextHandler(SmpHandler);
    SmpHandlerInit(handlerId);
    SmprInit();
    SmprScInit();
    SmpiInit();
    SmpiScInit();

    stack_handler_id = WsfOsSetNextHandler(&BLE::stack_handler);

    HciSetMaxRxAclLen(100);

    DmRegister(BLE::device_manager_cb);
    DmConnRegister(DM_CLIENT_ID_APP, BLE::device_manager_cb);
    AttConnRegister(BLE::connection_handler);
    AttRegister((attCback_t) ble::pal::vendor::cordio::CordioAttClient::att_client_handler);
}

void BLE::start_stack_reset()
{
    _hci_driver->initialize();
    DmDevReset();
}

void BLE::callDispatcher()
{
    // process the external event queue
    _event_queue.process();

    // follow by stack events
    static uint32_t lastTimeUs = us_ticker_read();
    uint32_t currTimeUs, deltaTimeMs;

    // Update the current cordio time
    currTimeUs = us_ticker_read();
    deltaTimeMs = (currTimeUs - lastTimeUs) / 1000;
    if (deltaTimeMs > 0) {
        WsfTimerUpdate(deltaTimeMs / WSF_MS_PER_TICK);
        lastTimeUs += deltaTimeMs * 1000;
    }

    wsfOsDispatcher();

    if (wsfOsReadyToSleep()) {
        static Timeout nextTimeout;
        // setup an mbed timer for the next Cordio timeout
        bool_t pTimerRunning;
        timestamp_t nextTimestamp = (timestamp_t) (WsfTimerNextExpiration(&pTimerRunning) * WSF_MS_PER_TICK) * 1000;
        if (pTimerRunning) {
            nextTimeout.attach_us(timeoutCallback, nextTimestamp);
        }
    }
}

CordioHCIDriver* BLE::_hci_driver = NULL;

FunctionPointerWithContext< ::BLE::InitializationCompleteCallbackContext*> BLE::_init_callback;

} // namespace cordio
} // namespace vendor
} // namespace ble
<|endoftext|>


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

#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
#define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_

#include <pcl/features/organized_edge_detection.h>

/**
 *  Directions: 1 2 3
 *              0 x 4
 *              7 6 5
 * e.g. direction y means we came from pixel with label y to the center pixel x
 */
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointLT> void
pcl::OrganizedEdgeDetection<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
{
  const unsigned invalid_label = std::numeric_limits<unsigned>::max ();
  pcl::Label invalid_pt;
  invalid_pt.label = std::numeric_limits<unsigned>::max ();
  labels.points.resize (input_->points.size (), invalid_pt);
  labels.width = input_->width;
  labels.height = input_->height;
  unsigned int clust_id = 0;

  std::cout << "width: " << labels.width << std::endl;
  std::cout << "height: " << labels.height << std::endl;
  std::cout << "[done] OrganizedEdgeDetection::compute ()" << std::endl;

  // fill lookup table for next points to visit
  const int num_of_ngbr = 8;
  Neighbor directions [num_of_ngbr] = {Neighbor(-1,  0,                -1),
                                       Neighbor(-1, -1, -labels.width - 1),
                                       Neighbor( 0, -1, -labels.width    ),
                                       Neighbor( 1, -1, -labels.width + 1),
                                       Neighbor( 1,  0,                 1),
                                       Neighbor( 1,  1,  labels.width + 1),
                                       Neighbor( 0,  1,  labels.width    ),
                                       Neighbor(-1,  1,  labels.width - 1)};

  for (int row = 1; row < int(input_->height) - 1; row++)
  {
    for (int col = 1; col < int(input_->width) - 1; col++)
    {
      int curr_idx = row*int(input_->width) + col;
      if (!pcl_isfinite (input_->points[curr_idx].z))
        continue;

      float curr_depth = abs (input_->points[curr_idx].z);

      // Calculate depth distances between current point and neighboring points
      std::vector<float> nghr_dist;
      nghr_dist.resize (8);
      bool found_invalid_neighbor = false;
      for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
      {
        int nghr_idx = curr_idx + directions[d_idx].d_index;
        assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
        if (!pcl_isfinite (input_->points[nghr_idx].z))
        {
          found_invalid_neighbor = true;
          break;
        }
        nghr_dist[d_idx] = curr_depth - abs (input_->points[nghr_idx].z);
      }

      if (!found_invalid_neighbor)
      {
        // Every neighboring points are valid
        std::pair<std::vector<float>::iterator, std::vector<float>::iterator> minmax;
        //minmax = boost::minmax_element (nghr_dist.begin (), nghr_dist.end ());
        std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());//*(minmax.first);
        std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());//*(minmax.first);
        float nghr_dist_min = *min_itr;
        float nghr_dist_max = *max_itr;
        float dist_dominant = abs (nghr_dist_min) > abs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
        if (abs (dist_dominant) > th_depth_discon_*abs (curr_depth))
        {
          // Found a depth discontinuity
          if (dist_dominant > 0.f)
            labels[curr_idx].label = EDGELABEL_OCCLUDED;
          else
            labels[curr_idx].label = EDGELABEL_OCCLUDING;
        }
      }
      else
      {
        // Some neighboring points are not valid (nan points)
        // Search for corresponding point across invalid points
        // Search direction is determined by nan point locations with respect to current point
        int dx = 0;
        int dy = 0;
        int num_of_invalid_pt = 0;
        for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
        {
          int nghr_idx = curr_idx + directions[d_idx].d_index;
          assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
          if (!pcl_isfinite (input_->points[nghr_idx].z))
          {
            dx += directions[d_idx].d_x;
            dy += directions[d_idx].d_y;
            num_of_invalid_pt++;
          }
        }

        // Search directions
        assert (num_of_invalid_pt > 0);
        float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
        float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);

        // Search for corresponding point across invalid points
        float corr_depth = std::numeric_limits<float>::quiet_NaN ();
        for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
        {
          int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
          int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));

          if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
            break;

          if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z))
          {
            corr_depth = abs (input_->points[s_row*int(input_->width)+s_col].z);
            break;
          }
        }

        if (!pcl_isnan (corr_depth))
        {
          // Found a corresponding point
          float dist = curr_depth - corr_depth;
          if (abs (dist) > th_depth_discon_*abs (curr_depth))
          {
            // Found a depth discontinuity
            if (dist > 0.f)
              labels[curr_idx].label = EDGELABEL_OCCLUDED;
            else
              labels[curr_idx].label = EDGELABEL_OCCLUDING;
          }
        }
        else
        {
          // Not found a corresponding point, just nan boundary edge
          labels[curr_idx].label = EDGELABEL_NAN_BOUNDARY;
        }
      }
    }
  }

  // Assign label indices
  label_indices.resize (3);
  for (unsigned idx = 0; idx < input_->points.size (); idx++)
  {
    if (labels[idx].label != invalid_label)
    {
      label_indices[labels[idx].label].indices.push_back (idx);
    }
  }
}

#define PCL_INSTANTIATE_OrganizedEdgeDetection(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeDetection<T,LT>;

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

#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
#define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_

#include <pcl/features/organized_edge_detection.h>

/**
 *  Directions: 1 2 3
 *              0 x 4
 *              7 6 5
 * e.g. direction y means we came from pixel with label y to the center pixel x
 */
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT, typename PointLT> void
pcl::OrganizedEdgeDetection<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
{
  const unsigned invalid_label = std::numeric_limits<unsigned>::max ();
  pcl::Label invalid_pt;
  invalid_pt.label = std::numeric_limits<unsigned>::max ();
  labels.points.resize (input_->points.size (), invalid_pt);
  labels.width = input_->width;
  labels.height = input_->height;
  unsigned int clust_id = 0;

  std::cout << "width: " << labels.width << std::endl;
  std::cout << "height: " << labels.height << std::endl;
  std::cout << "[done] OrganizedEdgeDetection::compute ()" << std::endl;

  // fill lookup table for next points to visit
  const int num_of_ngbr = 8;
  Neighbor directions [num_of_ngbr] = {Neighbor(-1,  0,                -1),
                                       Neighbor(-1, -1, -labels.width - 1),
                                       Neighbor( 0, -1, -labels.width    ),
                                       Neighbor( 1, -1, -labels.width + 1),
                                       Neighbor( 1,  0,                 1),
                                       Neighbor( 1,  1,  labels.width + 1),
                                       Neighbor( 0,  1,  labels.width    ),
                                       Neighbor(-1,  1,  labels.width - 1)};

  for (int row = 1; row < int(input_->height) - 1; row++)
  {
    for (int col = 1; col < int(input_->width) - 1; col++)
    {
      int curr_idx = row*int(input_->width) + col;
      if (!pcl_isfinite (input_->points[curr_idx].z))
        continue;

      float curr_depth = fabs (input_->points[curr_idx].z);

      // Calculate depth distances between current point and neighboring points
      std::vector<float> nghr_dist;
      nghr_dist.resize (8);
      bool found_invalid_neighbor = false;
      for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
      {
        int nghr_idx = curr_idx + directions[d_idx].d_index;
        assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
        if (!pcl_isfinite (input_->points[nghr_idx].z))
        {
          found_invalid_neighbor = true;
          break;
        }
        nghr_dist[d_idx] = curr_depth - fabs (input_->points[nghr_idx].z);
      }

      if (!found_invalid_neighbor)
      {
        // Every neighboring points are valid
        std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
        std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
        float nghr_dist_min = *min_itr;
        float nghr_dist_max = *max_itr;
        float dist_dominant = fabs (nghr_dist_min) > fabs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
        if (fabs (dist_dominant) > th_depth_discon_*fabs (curr_depth))
        {
          // Found a depth discontinuity
          if (dist_dominant > 0.f)
            labels[curr_idx].label = EDGELABEL_OCCLUDED;
          else
            labels[curr_idx].label = EDGELABEL_OCCLUDING;
        }
      }
      else
      {
        // Some neighboring points are not valid (nan points)
        // Search for corresponding point across invalid points
        // Search direction is determined by nan point locations with respect to current point
        int dx = 0;
        int dy = 0;
        int num_of_invalid_pt = 0;
        for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
        {
          int nghr_idx = curr_idx + directions[d_idx].d_index;
          assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
          if (!pcl_isfinite (input_->points[nghr_idx].z))
          {
            dx += directions[d_idx].d_x;
            dy += directions[d_idx].d_y;
            num_of_invalid_pt++;
          }
        }

        // Search directions
        assert (num_of_invalid_pt > 0);
        float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
        float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);

        // Search for corresponding point across invalid points
        float corr_depth = std::numeric_limits<float>::quiet_NaN ();
        for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
        {
          int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
          int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));

          if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
            break;

          if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z))
          {
            corr_depth = fabs (input_->points[s_row*int(input_->width)+s_col].z);
            break;
          }
        }

        if (!pcl_isnan (corr_depth))
        {
          // Found a corresponding point
          float dist = curr_depth - corr_depth;
          if (fabs (dist) > th_depth_discon_*fabs (curr_depth))
          {
            // Found a depth discontinuity
            if (dist > 0.f)
              labels[curr_idx].label = EDGELABEL_OCCLUDED;
            else
              labels[curr_idx].label = EDGELABEL_OCCLUDING;
          }
        }
        else
        {
          // Not found a corresponding point, just nan boundary edge
          labels[curr_idx].label = EDGELABEL_NAN_BOUNDARY;
        }
      }
    }
  }

  // Assign label indices
  label_indices.resize (3);
  for (unsigned idx = 0; idx < input_->points.size (); idx++)
  {
    if (labels[idx].label != invalid_label)
    {
      label_indices[labels[idx].label].indices.push_back (idx);
    }
  }
}

#define PCL_INSTANTIATE_OrganizedEdgeDetection(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeDetection<T,LT>;

#endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: LResultSet.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 05:46:32 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library 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 this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _COM_SUN_STAR_SDBCX_COMPAREBOOKMARK_HPP_
#include <com/sun/star/sdbcx/CompareBookmark.hpp>
#endif
#ifndef _COM_SUN_STAR_SDBCX_XDELETEROWS_HPP_
#include <com/sun/star/sdbcx/XDeleteRows.hpp>
#endif
#ifndef _CONNECTIVITY_EVOAB_LRESULTSET_HXX_
#include "LResultSet.hxx"
#endif
#ifndef _COM_SUN_STAR_LANG_DISPOSEDEXCEPTION_HPP_
#include <com/sun/star/lang/DisposedException.hpp>
#endif
#ifndef _COMPHELPER_SEQUENCE_HXX_
#include <comphelper/sequence.hxx>
#endif
#ifndef _COMPHELPER_TYPES_HXX_
#include <comphelper/types.hxx>
#endif

using namespace ::comphelper;

using namespace connectivity::evoab;
using namespace connectivity::file;
using namespace ::cppu;
using namespace com::sun::star::uno;
using namespace com::sun::star::lang;
using namespace com::sun::star::beans;
using namespace com::sun::star::sdbc;
using namespace com::sun::star::sdbcx;
//  using namespace com::sun::star::container;
//  using namespace com::sun::star::util;
//------------------------------------------------------------------------------
OEvoabResultSet::OEvoabResultSet( OStatement_Base* pStmt,connectivity::OSQLParseTreeIterator&   _aSQLIterator)
                : file::OResultSet(pStmt,_aSQLIterator)
                ,m_bBookmarkable(sal_True)
{
    registerProperty(OMetaConnection::getPropMap().getNameByIndex(PROPERTY_ID_ISBOOKMARKABLE),         PROPERTY_ID_ISBOOKMARKABLE,       PropertyAttribute::READONLY,&m_bBookmarkable,                ::getBooleanCppuType());
}
// -------------------------------------------------------------------------
::rtl::OUString SAL_CALL OEvoabResultSet::getImplementationName(  ) throw ( RuntimeException)
{
    return ::rtl::OUString::createFromAscii("com.sun.star.sdbcx.evoab.ResultSet");
}
// -------------------------------------------------------------------------
Sequence< ::rtl::OUString > SAL_CALL OEvoabResultSet::getSupportedServiceNames(  ) throw( RuntimeException)
{
     Sequence< ::rtl::OUString > aSupported(2);
    aSupported[0] = ::rtl::OUString::createFromAscii("com.sun.star.sdbc.ResultSet");
    aSupported[1] = ::rtl::OUString::createFromAscii("com.sun.star.sdbcx.ResultSet");
    return aSupported;
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::supportsService( const ::rtl::OUString& _rServiceName ) throw( RuntimeException)
{
    Sequence< ::rtl::OUString > aSupported(getSupportedServiceNames());
    const ::rtl::OUString* pSupported = aSupported.getConstArray();
    const ::rtl::OUString* pEnd = pSupported + aSupported.getLength();
    for (;pSupported != pEnd && !pSupported->equals(_rServiceName); ++pSupported)
        ;

    return pSupported != pEnd;
}
// -------------------------------------------------------------------------
Any SAL_CALL OEvoabResultSet::queryInterface( const Type & rType ) throw(RuntimeException)
{
    if(rType == ::getCppuType((const Reference<XDeleteRows>*)0) || rType == ::getCppuType((const Reference<XResultSetUpdate>*)0)
        || rType == ::getCppuType((const Reference<XRowUpdate>*)0))
        return Any();

    Any aRet = OResultSet::queryInterface(rType);
    return aRet.hasValue() ? aRet : OEvoabResultSet_BASE::queryInterface(rType);
}
// -------------------------------------------------------------------------
Sequence<  Type > SAL_CALL OEvoabResultSet::getTypes(  ) throw( RuntimeException)
{
    Sequence< Type > aTypes = OResultSet::getTypes();
    ::std::vector<Type> aOwnTypes;
    aOwnTypes.reserve(aTypes.getLength());
    const Type* pBegin = aTypes.getConstArray();
    const Type* pEnd = pBegin + aTypes.getLength();
    for(;pBegin != pEnd;++pBegin)
    {
        if(!(*pBegin == ::getCppuType((const Reference<XDeleteRows>*)0) ||
            *pBegin == ::getCppuType((const Reference<XResultSetUpdate>*)0) ||
            *pBegin == ::getCppuType((const Reference<XRowUpdate>*)0)))
        {
            aOwnTypes.push_back(*pBegin);
        }
    }
    Type *pTypes = aOwnTypes.empty() ? 0 : &aOwnTypes[0];
    Sequence< Type > aRet(pTypes, aOwnTypes.size());
    return ::comphelper::concatSequences(aRet,OEvoabResultSet_BASE::getTypes());
}

// -------------------------------------------------------------------------
// XRowLocate
Any SAL_CALL OEvoabResultSet::getBookmark(  ) throw( SQLException,  RuntimeException)
{
     ::osl::MutexGuard aGuard( m_aMutex );
        checkDisposed(OResultSet_BASE::rBHelper.bDisposed);

    return makeAny((sal_Int32)(*m_aRow)[0]->getValue());
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::moveToBookmark( const  Any& bookmark ) throw( SQLException,  RuntimeException)
{
    ::osl::MutexGuard aGuard( m_aMutex );
        checkDisposed(OResultSet_BASE::rBHelper.bDisposed);

    m_bRowDeleted = m_bRowInserted = m_bRowUpdated = sal_False;

    return Move(IResultSetHelper::BOOKMARK,comphelper::getINT32(bookmark),sal_True);
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::moveRelativeToBookmark( const  Any& bookmark, sal_Int32 rows ) throw( SQLException,  RuntimeException)
{
    ::osl::MutexGuard aGuard( m_aMutex );
        checkDisposed(OResultSet_BASE::rBHelper.bDisposed);

    m_bRowDeleted = m_bRowInserted = m_bRowUpdated = sal_False;

    Move(IResultSetHelper::BOOKMARK,comphelper::getINT32(bookmark),sal_False);

    return relative(rows);
}

// -------------------------------------------------------------------------
sal_Int32 SAL_CALL OEvoabResultSet::compareBookmarks( const  Any& first, const  Any& second ) throw( SQLException,  RuntimeException)
{
    return (first == second) ? 0 : 2;
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::hasOrderedBookmarks(  ) throw( SQLException,  RuntimeException)
{
    return sal_True;
}
// -------------------------------------------------------------------------
sal_Int32 SAL_CALL OEvoabResultSet::hashBookmark( const  Any& bookmark ) throw( SQLException,  RuntimeException)
{
    return comphelper::getINT32(bookmark);
}
// -------------------------------------------------------------------------
IPropertyArrayHelper* OEvoabResultSet::createArrayHelper( ) const
{
    Sequence< Property > aProps;
    describeProperties(aProps);
    return new ::cppu::OPropertyArrayHelper(aProps);
}
// -------------------------------------------------------------------------
IPropertyArrayHelper & OEvoabResultSet::getInfoHelper()
{
    return *OEvoabResultSet_BASE3::getArrayHelper();
}
// -----------------------------------------------------------------------------
void SAL_CALL OEvoabResultSet::acquire() throw()
{
    OEvoabResultSet_BASE2::acquire();
}
// -----------------------------------------------------------------------------
void SAL_CALL OEvoabResultSet::release() throw()
{
    OEvoabResultSet_BASE2::release();
}
// -----------------------------------------------------------------------------
::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySetInfo > SAL_CALL OEvoabResultSet::getPropertySetInfo(  ) throw(::com::sun::star::uno::RuntimeException)
{
    return ::cppu::OPropertySetHelper::createPropertySetInfo(getInfoHelper());
}
// -----------------------------------------------------------------------------

<commit_msg>INTEGRATION: CWS warnings01 (1.5.30); FILE MERGED 2005/11/16 12:58:58 fs 1.5.30.1: #i57457# warning free code<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: LResultSet.cxx,v $
 *
 *  $Revision: 1.6 $
 *
 *  last change: $Author: hr $ $Date: 2006-06-20 01:23:32 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library 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 this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _COM_SUN_STAR_SDBCX_COMPAREBOOKMARK_HPP_
#include <com/sun/star/sdbcx/CompareBookmark.hpp>
#endif
#ifndef _COM_SUN_STAR_SDBCX_XDELETEROWS_HPP_
#include <com/sun/star/sdbcx/XDeleteRows.hpp>
#endif
#ifndef _CONNECTIVITY_EVOAB_LRESULTSET_HXX_
#include "LResultSet.hxx"
#endif
#ifndef _COM_SUN_STAR_LANG_DISPOSEDEXCEPTION_HPP_
#include <com/sun/star/lang/DisposedException.hpp>
#endif
#ifndef _COMPHELPER_SEQUENCE_HXX_
#include <comphelper/sequence.hxx>
#endif
#ifndef _COMPHELPER_TYPES_HXX_
#include <comphelper/types.hxx>
#endif

using namespace ::comphelper;

using namespace connectivity::evoab;
using namespace connectivity::file;
using namespace ::cppu;
using namespace com::sun::star::uno;
using namespace com::sun::star::lang;
using namespace com::sun::star::beans;
using namespace com::sun::star::sdbc;
using namespace com::sun::star::sdbcx;
//  using namespace com::sun::star::container;
//  using namespace com::sun::star::util;
//------------------------------------------------------------------------------
OEvoabResultSet::OEvoabResultSet( OStatement_Base* pStmt,connectivity::OSQLParseTreeIterator&   _aSQLIterator)
                : file::OResultSet(pStmt,_aSQLIterator)
                ,m_bBookmarkable(sal_True)
{
    registerProperty(OMetaConnection::getPropMap().getNameByIndex(PROPERTY_ID_ISBOOKMARKABLE),         PROPERTY_ID_ISBOOKMARKABLE,       PropertyAttribute::READONLY,&m_bBookmarkable,                ::getBooleanCppuType());
}
// -------------------------------------------------------------------------
::rtl::OUString SAL_CALL OEvoabResultSet::getImplementationName(  ) throw ( RuntimeException)
{
    return ::rtl::OUString::createFromAscii("com.sun.star.sdbcx.evoab.ResultSet");
}
// -------------------------------------------------------------------------
Sequence< ::rtl::OUString > SAL_CALL OEvoabResultSet::getSupportedServiceNames(  ) throw( RuntimeException)
{
     Sequence< ::rtl::OUString > aSupported(2);
    aSupported[0] = ::rtl::OUString::createFromAscii("com.sun.star.sdbc.ResultSet");
    aSupported[1] = ::rtl::OUString::createFromAscii("com.sun.star.sdbcx.ResultSet");
    return aSupported;
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::supportsService( const ::rtl::OUString& _rServiceName ) throw( RuntimeException)
{
    Sequence< ::rtl::OUString > aSupported(getSupportedServiceNames());
    const ::rtl::OUString* pSupported = aSupported.getConstArray();
    const ::rtl::OUString* pEnd = pSupported + aSupported.getLength();
    for (;pSupported != pEnd && !pSupported->equals(_rServiceName); ++pSupported)
        ;

    return pSupported != pEnd;
}
// -------------------------------------------------------------------------
Any SAL_CALL OEvoabResultSet::queryInterface( const Type & rType ) throw(RuntimeException)
{
    if(rType == ::getCppuType((const Reference<XDeleteRows>*)0) || rType == ::getCppuType((const Reference<XResultSetUpdate>*)0)
        || rType == ::getCppuType((const Reference<XRowUpdate>*)0))
        return Any();

    Any aRet = OResultSet::queryInterface(rType);
    return aRet.hasValue() ? aRet : OEvoabResultSet_BASE::queryInterface(rType);
}
// -------------------------------------------------------------------------
Sequence<  Type > SAL_CALL OEvoabResultSet::getTypes(  ) throw( RuntimeException)
{
    Sequence< Type > aTypes = OResultSet::getTypes();
    ::std::vector<Type> aOwnTypes;
    aOwnTypes.reserve(aTypes.getLength());
    const Type* pBegin = aTypes.getConstArray();
    const Type* pEnd = pBegin + aTypes.getLength();
    for(;pBegin != pEnd;++pBegin)
    {
        if(!(*pBegin == ::getCppuType((const Reference<XDeleteRows>*)0) ||
            *pBegin == ::getCppuType((const Reference<XResultSetUpdate>*)0) ||
            *pBegin == ::getCppuType((const Reference<XRowUpdate>*)0)))
        {
            aOwnTypes.push_back(*pBegin);
        }
    }
    Type *pTypes = aOwnTypes.empty() ? 0 : &aOwnTypes[0];
    Sequence< Type > aRet(pTypes, aOwnTypes.size());
    return ::comphelper::concatSequences(aRet,OEvoabResultSet_BASE::getTypes());
}

// -------------------------------------------------------------------------
// XRowLocate
Any SAL_CALL OEvoabResultSet::getBookmark(  ) throw( SQLException,  RuntimeException)
{
     ::osl::MutexGuard aGuard( m_aMutex );
        checkDisposed(OResultSet_BASE::rBHelper.bDisposed);

    return makeAny((sal_Int32)(*m_aRow)[0]->getValue());
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::moveToBookmark( const  Any& bookmark ) throw( SQLException,  RuntimeException)
{
    ::osl::MutexGuard aGuard( m_aMutex );
        checkDisposed(OResultSet_BASE::rBHelper.bDisposed);

    m_bRowDeleted = m_bRowInserted = m_bRowUpdated = sal_False;

    return Move(IResultSetHelper::BOOKMARK,comphelper::getINT32(bookmark),sal_True);
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::moveRelativeToBookmark( const  Any& bookmark, sal_Int32 rows ) throw( SQLException,  RuntimeException)
{
    ::osl::MutexGuard aGuard( m_aMutex );
        checkDisposed(OResultSet_BASE::rBHelper.bDisposed);

    m_bRowDeleted = m_bRowInserted = m_bRowUpdated = sal_False;

    Move(IResultSetHelper::BOOKMARK,comphelper::getINT32(bookmark),sal_False);

    return relative(rows);
}

// -------------------------------------------------------------------------
sal_Int32 SAL_CALL OEvoabResultSet::compareBookmarks( const Any& lhs, const Any& rhs ) throw( SQLException,  RuntimeException)
{
    return (lhs == rhs) ? 0 : 2;
}
// -------------------------------------------------------------------------
sal_Bool SAL_CALL OEvoabResultSet::hasOrderedBookmarks(  ) throw( SQLException,  RuntimeException)
{
    return sal_True;
}
// -------------------------------------------------------------------------
sal_Int32 SAL_CALL OEvoabResultSet::hashBookmark( const  Any& bookmark ) throw( SQLException,  RuntimeException)
{
    return comphelper::getINT32(bookmark);
}
// -------------------------------------------------------------------------
IPropertyArrayHelper* OEvoabResultSet::createArrayHelper( ) const
{
    Sequence< Property > aProps;
    describeProperties(aProps);
    return new ::cppu::OPropertyArrayHelper(aProps);
}
// -------------------------------------------------------------------------
IPropertyArrayHelper & OEvoabResultSet::getInfoHelper()
{
    return *OEvoabResultSet_BASE3::getArrayHelper();
}
// -----------------------------------------------------------------------------
void SAL_CALL OEvoabResultSet::acquire() throw()
{
    OEvoabResultSet_BASE2::acquire();
}
// -----------------------------------------------------------------------------
void SAL_CALL OEvoabResultSet::release() throw()
{
    OEvoabResultSet_BASE2::release();
}
// -----------------------------------------------------------------------------
::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySetInfo > SAL_CALL OEvoabResultSet::getPropertySetInfo(  ) throw(::com::sun::star::uno::RuntimeException)
{
    return ::cppu::OPropertySetHelper::createPropertySetInfo(getInfoHelper());
}
// -----------------------------------------------------------------------------

<|endoftext|>


<commit_before>// UjoImro, 2013
// Experimental code for the CARP Project
// Copyright (c) RealEyes, 2013
// This version tests the responseMap calculation with input dumps

/*
extern int EF_ALIGNMENT = 0;
extern int EF_PROTECT_BELOW = 0;
extern int EF_PROTECT_FREE = 0;
extern int EF_ALLOW_MALLOC_0 = 1;
extern int EF_FILL = 1922;
*/

#include "bench_mlp.hpp"

int main()
{
    conductor_t conductor;
    int fail = 0;
    long int elapsed_time = 0;

    for ( conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.id);
          ((conductor.id != -1) and (conductor.id != 25));
          // conductor.id != -1;
          conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.id)
        )
    {
        PRINT(conductor.id);
        conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.hack);

        // here comes the function call
        {
            // preparing the inputs
            MatChar alignedImage = convertCVToMatChar(conductor.hack.alignedImage);
            MatFloat shape = convertCVToMatFloat(conductor.hack.shape);
            mlp * m_classifiers = convertHackToMlp(conductor.hack);
            MatFloat * responseMaps;
            allocateResponseMaps( conductor.hack.m_mapSize, conductor.hack.m_visibleLandmarks_size, &responseMaps );

            auto start = std::chrono::high_resolution_clock::now();
            calculateMaps(
                conductor.hack.m_visibleLandmarks_size,
                conductor.hack.m_mapSize,
                alignedImage,
                shape,
                m_classifiers,
                &responseMaps
                );
            auto end = std::chrono::high_resolution_clock::now();
            elapsed_time = microseconds(end - start);

            // releasing the inputs
            freeMatChar(&alignedImage);
            freeMatFloat(&shape);
            freeClassifiers(&m_classifiers, conductor.hack.m_classifiers.size());

            // converting the outputs
            std::vector< cv::Mat_<double> > calculatedResults;
            for (int q=0; q<conductor.hack.m_visibleLandmarks_size; q++)
            {
                cv::Mat_<double> nextResult;
                nextResult = convertMatFloatToCV( responseMaps[q] );
                calculatedResults.push_back(nextResult);
            }

            // testing the output
            for (int q=0; q<conductor.hack.m_visibleLandmarks_size; q++)
            {
              // std::cout << "cv::norm( conductor.hack.responseMaps[" << q << "] - calculatedResults[" << q << "] ) = "
              //             << cv::norm( conductor.hack.responseMaps[q] - calculatedResults[q] ) << std::endl;
              assert(cv::norm( conductor.hack.responseMaps[q] - calculatedResults[q] ) < 0.00001);
            }

            // releasing the outputs
            freeResponseMaps( &responseMaps, conductor.hack.m_visibleLandmarks_size );

        }
        // here comes the test
        // PRINT(cv::norm( ));
    }

    std::cout << "total elapsed time = " << elapsed_time / 1000000. << " s." << std::endl;
    //conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.hack);

    return EXIT_SUCCESS;
}

// LuM end of file
<commit_msg>comment change<commit_after>// UjoImro, 2013
// Experimental code for the CARP Project
// Copyright (c) RealEyes, 2013
// This version tests the responseMap calculation with input dumps

/*
extern int EF_ALIGNMENT = 0;
extern int EF_PROTECT_BELOW = 0;
extern int EF_PROTECT_FREE = 0;
extern int EF_ALLOW_MALLOC_0 = 1;
extern int EF_FILL = 1922;
*/

#include "bench_mlp.hpp"

int main()
{
    conductor_t conductor;
    int fail = 0;
    long int elapsed_time = 0;

    for ( conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.id);
          ((conductor.id != -1) and (conductor.id != 25));
          // conductor.id != -1;
          conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.id)
        )
    {
        PRINT(conductor.id);
        conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.hack);

        // here comes the function call
        {
            // preparing the inputs
            MatChar alignedImage = convertCVToMatChar(conductor.hack.alignedImage);
            MatFloat shape = convertCVToMatFloat(conductor.hack.shape);
            mlp * m_classifiers = convertHackToMlp(conductor.hack);
            MatFloat * responseMaps;
            allocateResponseMaps( conductor.hack.m_mapSize, conductor.hack.m_visibleLandmarks_size, &responseMaps );

            auto start = std::chrono::high_resolution_clock::now();
            calculateMaps(
                conductor.hack.m_visibleLandmarks_size,
                conductor.hack.m_mapSize,
                alignedImage,
                shape,
                m_classifiers,
                &responseMaps
                );
            auto end = std::chrono::high_resolution_clock::now();
            elapsed_time = microseconds(end - start);

            // releasing the inputs
            freeMatChar(&alignedImage);
            freeMatFloat(&shape);
            freeClassifiers(&m_classifiers, conductor.hack.m_classifiers.size());

            // converting the outputs
            std::vector< cv::Mat_<double> > calculatedResults;
            for (int q=0; q<conductor.hack.m_visibleLandmarks_size; q++)
            {
                cv::Mat_<double> nextResult;
                nextResult = convertMatFloatToCV( responseMaps[q] );
                calculatedResults.push_back(nextResult);
            }

            // testing the output
            for (int q=0; q<conductor.hack.m_visibleLandmarks_size; q++)
            {
              // std::cout << "cv::norm( conductor.hack.responseMaps[" << q << "] - calculatedResults[" << q << "] ) = "
              //             << cv::norm( conductor.hack.responseMaps[q] - calculatedResults[q] ) << std::endl;
              assert(cv::norm( conductor.hack.responseMaps[q] - calculatedResults[q] ) < 0.00001);
            }

            // releasing the outputs
            freeResponseMaps( &responseMaps, conductor.hack.m_visibleLandmarks_size );

        }
        // here comes the test
        // PRINT(cv::norm( ));
    }

    std::cout << "total elapsed time = " << elapsed_time / 1000000. << " s." << std::endl;
    //conductor.importer >> BOOST_SERIALIZATION_NVP(conductor.hack);

    return EXIT_SUCCESS;
} // int main

// LuM end of file
<|endoftext|>


<commit_before>/**
 * \file
 * \brief MessageQueueBase class header
 *
 * \author Copyright (C) 2015 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef INCLUDE_DISTORTOS_INTERNAL_SYNCHRONIZATION_MESSAGEQUEUEBASE_HPP_
#define INCLUDE_DISTORTOS_INTERNAL_SYNCHRONIZATION_MESSAGEQUEUEBASE_HPP_

#include "distortos/Semaphore.hpp"

#include "distortos/internal/synchronization/QueueFunctor.hpp"
#include "distortos/internal/synchronization/SemaphoreFunctor.hpp"

#include "estd/SortedIntrusiveForwardList.hpp"

#include <memory>

namespace distortos
{

namespace internal
{

/// MessageQueueBase class implements basic functionality of MessageQueue template class
class MessageQueueBase
{
public:

	/// entry in the MessageQueueBase
	struct Entry
	{
		/**
		 * \brief Entry's constructor
		 *
		 * \param [in] priorityy is the priority of the entry
		 * \param [in] storagee is the storage for the entry
		 */

		constexpr Entry(const uint8_t priorityy, void* const storagee) :
				node{},
				priority{priorityy},
				storage{storagee}
		{

		}

		/// node for intrusive forward list
		estd::IntrusiveForwardListNode node;

		/// priority of the entry
		uint8_t priority;

		/// storage for the entry
		void* storage;
	};

	/// type of uninitialized storage for Entry
	using EntryStorage = typename std::aligned_storage<sizeof(Entry), alignof(Entry)>::type;

	/// unique_ptr (with deleter) to EntryStorage[]
	using EntryStorageUniquePointer = std::unique_ptr<EntryStorage[], void(&)(EntryStorage*)>;

	/**
	 * type of uninitialized storage for value
	 *
	 * \tparam T is the type of data in queue
	 */

	template<typename T>
	using ValueStorage = typename std::aligned_storage<sizeof(T), alignof(T)>::type;

	/// unique_ptr (with deleter) to storage
	using ValueStorageUniquePointer = std::unique_ptr<void, void(&)(void*)>;

	/// functor which gives descending priority order of elements on the list
	struct DescendingPriority
	{
		/**
		 * \brief DescendingPriority's function call operator
		 *
		 * \param [in] left is the object on the left side of comparison
		 * \param [in] right is the object on the right side of comparison
		 *
		 * \return true if left's priority is less than right's priority
		 */

		bool operator()(const Entry& left, const Entry& right) const
		{
			return left.priority < right.priority;
		}
	};

	/// type of entry list
	using EntryList = estd::SortedIntrusiveForwardList<DescendingPriority, Entry, &Entry::node>;

	/// type of free entry list
	using FreeEntryList = EntryList::UnsortedIntrusiveForwardList;

	/**
	 * \brief InternalFunctor is a type-erased interface for functors which execute common code of pop() and push()
	 * operations.
	 *
	 * The functor will be called by MessageQueueBase internals with references to \a entryList_ and \a freeEntryList_.
	 * It should perform common actions and execute the QueueFunctor passed from callers.
	 */

	class InternalFunctor : public estd::TypeErasedFunctor<void(EntryList&, FreeEntryList&)>
	{

	};

	/**
	 * \brief MessageQueueBase's constructor
	 *
	 * \param [in] entryStorageUniquePointer is a rvalue reference to EntryStorageUniquePointer with storage for queue
	 * entries (sufficiently large for \a maxElements EntryStorage objects) and appropriate deleter
	 * \param [in] valueStorageUniquePointer is a rvalue reference to ValueStorageUniquePointer with storage for queue
	 * elements (sufficiently large for \a maxElements, each \a elementSize bytes long) and appropriate deleter
	 * \param [in] elementSize is the size of single queue element, bytes
	 * \param [in] maxElements is the number of elements in \a entryStorage array and valueStorage memory block
	 */

	MessageQueueBase(EntryStorageUniquePointer&& entryStorageUniquePointer,
			ValueStorageUniquePointer&& valueStorageUniquePointer, size_t elementSize, size_t maxElements);

	/**
	 * \brief MessageQueueBase's destructor
	 */

	~MessageQueueBase();

	/**
	 * \brief Implementation of pop() using type-erased functor
	 *
	 * \param [in] waitSemaphoreFunctor is a reference to SemaphoreFunctor which will be executed with \a popSemaphore_
	 * \param [out] priority is a reference to variable that will be used to return priority of popped value
	 * \param [in] functor is a reference to QueueFunctor which will execute actions related to popping - it will get a
	 * pointer to storage with element
	 *
	 * \return zero if element was popped successfully, error code otherwise:
	 * - error codes returned by \a waitSemaphoreFunctor's operator() call;
	 * - error codes returned by Semaphore::post();
	 */

	int pop(const SemaphoreFunctor& waitSemaphoreFunctor, uint8_t& priority, const QueueFunctor& functor);

	/**
	 * \brief Implementation of push() using type-erased functor
	 *
	 * \param [in] waitSemaphoreFunctor is a reference to SemaphoreFunctor which will be executed with \a pushSemaphore_
	 * \param [in] priority is the priority of new element
	 * \param [in] functor is a reference to QueueFunctor which will execute actions related to pushing - it will get a
	 * pointer to storage for element
	 *
	 * \return zero if element was pushed successfully, error code otherwise:
	 * - error codes returned by \a waitSemaphoreFunctor's operator() call;
	 * - error codes returned by Semaphore::post();
	 */

	int push(const SemaphoreFunctor& waitSemaphoreFunctor, uint8_t priority, const QueueFunctor& functor);

private:

	/**
	 * \brief Implementation of pop() and push() using type-erased internal functor
	 *
	 * \param [in] waitSemaphoreFunctor is a reference to SemaphoreFunctor which will be executed with \a waitSemaphore
	 * \param [in] internalFunctor is a reference to InternalFunctor which will execute actions related to
	 * popping/pushing
	 * \param [in] waitSemaphore is a reference to semaphore that will be waited for, \a popSemaphore_ for pop(), \a
	 * pushSemaphore_ for push()
	 * \param [in] postSemaphore is a reference to semaphore that will be posted after the operation, \a pushSemaphore_
	 * for pop(), \a popSemaphore_ for push()
	 *
	 * \return zero if operation was successful, error code otherwise:
	 * - error codes returned by \a waitSemaphoreFunctor's operator() call;
	 * - error codes returned by Semaphore::post();
	 */

	int popPush(const SemaphoreFunctor& waitSemaphoreFunctor, const InternalFunctor& internalFunctor,
			Semaphore& waitSemaphore, Semaphore& postSemaphore);

	/// semaphore guarding access to "pop" functions - its value is equal to the number of available elements
	Semaphore popSemaphore_;

	/// semaphore guarding access to "push" functions - its value is equal to the number of free slots
	Semaphore pushSemaphore_;

	/// storage for queue entries
	const EntryStorageUniquePointer entryStorageUniquePointer_;

	/// storage for queue elements
	const ValueStorageUniquePointer valueStorageUniquePointer_;

	/// list of available entries, sorted in descending order of priority
	EntryList entryList_;

	/// list of "free" entries
	FreeEntryList freeEntryList_;
};

}	// namespace internal

}	// namespace distortos

#endif	// INCLUDE_DISTORTOS_INTERNAL_SYNCHRONIZATION_MESSAGEQUEUEBASE_HPP_
<commit_msg>MessageQueueBase: add constructor for DescendingPriority struct<commit_after>/**
 * \file
 * \brief MessageQueueBase class header
 *
 * \author Copyright (C) 2015 Kamil Szczygiel http://www.distortec.com http://www.freddiechopin.info
 *
 * \par License
 * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not
 * distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */

#ifndef INCLUDE_DISTORTOS_INTERNAL_SYNCHRONIZATION_MESSAGEQUEUEBASE_HPP_
#define INCLUDE_DISTORTOS_INTERNAL_SYNCHRONIZATION_MESSAGEQUEUEBASE_HPP_

#include "distortos/Semaphore.hpp"

#include "distortos/internal/synchronization/QueueFunctor.hpp"
#include "distortos/internal/synchronization/SemaphoreFunctor.hpp"

#include "estd/SortedIntrusiveForwardList.hpp"

#include <memory>

namespace distortos
{

namespace internal
{

/// MessageQueueBase class implements basic functionality of MessageQueue template class
class MessageQueueBase
{
public:

	/// entry in the MessageQueueBase
	struct Entry
	{
		/**
		 * \brief Entry's constructor
		 *
		 * \param [in] priorityy is the priority of the entry
		 * \param [in] storagee is the storage for the entry
		 */

		constexpr Entry(const uint8_t priorityy, void* const storagee) :
				node{},
				priority{priorityy},
				storage{storagee}
		{

		}

		/// node for intrusive forward list
		estd::IntrusiveForwardListNode node;

		/// priority of the entry
		uint8_t priority;

		/// storage for the entry
		void* storage;
	};

	/// type of uninitialized storage for Entry
	using EntryStorage = typename std::aligned_storage<sizeof(Entry), alignof(Entry)>::type;

	/// unique_ptr (with deleter) to EntryStorage[]
	using EntryStorageUniquePointer = std::unique_ptr<EntryStorage[], void(&)(EntryStorage*)>;

	/**
	 * type of uninitialized storage for value
	 *
	 * \tparam T is the type of data in queue
	 */

	template<typename T>
	using ValueStorage = typename std::aligned_storage<sizeof(T), alignof(T)>::type;

	/// unique_ptr (with deleter) to storage
	using ValueStorageUniquePointer = std::unique_ptr<void, void(&)(void*)>;

	/// functor which gives descending priority order of elements on the list
	struct DescendingPriority
	{
		/**
		 * \brief DescendingPriority's constructor
		 */

		constexpr DescendingPriority()
		{

		}

		/**
		 * \brief DescendingPriority's function call operator
		 *
		 * \param [in] left is the object on the left side of comparison
		 * \param [in] right is the object on the right side of comparison
		 *
		 * \return true if left's priority is less than right's priority
		 */

		bool operator()(const Entry& left, const Entry& right) const
		{
			return left.priority < right.priority;
		}
	};

	/// type of entry list
	using EntryList = estd::SortedIntrusiveForwardList<DescendingPriority, Entry, &Entry::node>;

	/// type of free entry list
	using FreeEntryList = EntryList::UnsortedIntrusiveForwardList;

	/**
	 * \brief InternalFunctor is a type-erased interface for functors which execute common code of pop() and push()
	 * operations.
	 *
	 * The functor will be called by MessageQueueBase internals with references to \a entryList_ and \a freeEntryList_.
	 * It should perform common actions and execute the QueueFunctor passed from callers.
	 */

	class InternalFunctor : public estd::TypeErasedFunctor<void(EntryList&, FreeEntryList&)>
	{

	};

	/**
	 * \brief MessageQueueBase's constructor
	 *
	 * \param [in] entryStorageUniquePointer is a rvalue reference to EntryStorageUniquePointer with storage for queue
	 * entries (sufficiently large for \a maxElements EntryStorage objects) and appropriate deleter
	 * \param [in] valueStorageUniquePointer is a rvalue reference to ValueStorageUniquePointer with storage for queue
	 * elements (sufficiently large for \a maxElements, each \a elementSize bytes long) and appropriate deleter
	 * \param [in] elementSize is the size of single queue element, bytes
	 * \param [in] maxElements is the number of elements in \a entryStorage array and valueStorage memory block
	 */

	MessageQueueBase(EntryStorageUniquePointer&& entryStorageUniquePointer,
			ValueStorageUniquePointer&& valueStorageUniquePointer, size_t elementSize, size_t maxElements);

	/**
	 * \brief MessageQueueBase's destructor
	 */

	~MessageQueueBase();

	/**
	 * \brief Implementation of pop() using type-erased functor
	 *
	 * \param [in] waitSemaphoreFunctor is a reference to SemaphoreFunctor which will be executed with \a popSemaphore_
	 * \param [out] priority is a reference to variable that will be used to return priority of popped value
	 * \param [in] functor is a reference to QueueFunctor which will execute actions related to popping - it will get a
	 * pointer to storage with element
	 *
	 * \return zero if element was popped successfully, error code otherwise:
	 * - error codes returned by \a waitSemaphoreFunctor's operator() call;
	 * - error codes returned by Semaphore::post();
	 */

	int pop(const SemaphoreFunctor& waitSemaphoreFunctor, uint8_t& priority, const QueueFunctor& functor);

	/**
	 * \brief Implementation of push() using type-erased functor
	 *
	 * \param [in] waitSemaphoreFunctor is a reference to SemaphoreFunctor which will be executed with \a pushSemaphore_
	 * \param [in] priority is the priority of new element
	 * \param [in] functor is a reference to QueueFunctor which will execute actions related to pushing - it will get a
	 * pointer to storage for element
	 *
	 * \return zero if element was pushed successfully, error code otherwise:
	 * - error codes returned by \a waitSemaphoreFunctor's operator() call;
	 * - error codes returned by Semaphore::post();
	 */

	int push(const SemaphoreFunctor& waitSemaphoreFunctor, uint8_t priority, const QueueFunctor& functor);

private:

	/**
	 * \brief Implementation of pop() and push() using type-erased internal functor
	 *
	 * \param [in] waitSemaphoreFunctor is a reference to SemaphoreFunctor which will be executed with \a waitSemaphore
	 * \param [in] internalFunctor is a reference to InternalFunctor which will execute actions related to
	 * popping/pushing
	 * \param [in] waitSemaphore is a reference to semaphore that will be waited for, \a popSemaphore_ for pop(), \a
	 * pushSemaphore_ for push()
	 * \param [in] postSemaphore is a reference to semaphore that will be posted after the operation, \a pushSemaphore_
	 * for pop(), \a popSemaphore_ for push()
	 *
	 * \return zero if operation was successful, error code otherwise:
	 * - error codes returned by \a waitSemaphoreFunctor's operator() call;
	 * - error codes returned by Semaphore::post();
	 */

	int popPush(const SemaphoreFunctor& waitSemaphoreFunctor, const InternalFunctor& internalFunctor,
			Semaphore& waitSemaphore, Semaphore& postSemaphore);

	/// semaphore guarding access to "pop" functions - its value is equal to the number of available elements
	Semaphore popSemaphore_;

	/// semaphore guarding access to "push" functions - its value is equal to the number of free slots
	Semaphore pushSemaphore_;

	/// storage for queue entries
	const EntryStorageUniquePointer entryStorageUniquePointer_;

	/// storage for queue elements
	const ValueStorageUniquePointer valueStorageUniquePointer_;

	/// list of available entries, sorted in descending order of priority
	EntryList entryList_;

	/// list of "free" entries
	FreeEntryList freeEntryList_;
};

}	// namespace internal

}	// namespace distortos

#endif	// INCLUDE_DISTORTOS_INTERNAL_SYNCHRONIZATION_MESSAGEQUEUEBASE_HPP_
<|endoftext|>