<commit_before>/*******************************************************************************
 * Copyright (c) 2015 Matthijs Kooijman
 * Copyright (c) 2018 MCCI Corporation
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * This the HAL to run LMIC on top of the Arduino environment.
 *******************************************************************************/

#include <Arduino.h>
#include <SPI.h>
// include all the lmic header files, including ../lmic/hal.h
#include "../lmic.h"
// include the C++ hal.h
#include "hal.h"
// we may need some things from stdio.
#include <stdio.h>

// -----------------------------------------------------------------------------
// I/O

static const Arduino_LMIC::HalPinmap_t *plmic_pins;
static Arduino_LMIC::HalConfiguration_t *pHalConfig;
static Arduino_LMIC::HalConfiguration_t nullHalConig;

static void hal_interrupt_init(); // Fwd declaration

static void hal_io_init () {
    // NSS and DIO0 are required, DIO1 is required for LoRa, DIO2 for FSK
    ASSERT(plmic_pins->nss != LMIC_UNUSED_PIN);
    ASSERT(plmic_pins->dio[0] != LMIC_UNUSED_PIN);
    ASSERT(plmic_pins->dio[1] != LMIC_UNUSED_PIN || plmic_pins->dio[2] != LMIC_UNUSED_PIN);

//    Serial.print("nss: "); Serial.println(plmic_pins->nss);
//    Serial.print("rst: "); Serial.println(plmic_pins->rst);
//    Serial.print("dio[0]: "); Serial.println(plmic_pins->dio[0]);
//    Serial.print("dio[1]: "); Serial.println(plmic_pins->dio[1]);
//    Serial.print("dio[2]: "); Serial.println(plmic_pins->dio[2]);

    // initialize SPI chip select to high (it's active low)
    digitalWrite(plmic_pins->nss, HIGH);
    pinMode(plmic_pins->nss, OUTPUT);

    if (plmic_pins->rxtx != LMIC_UNUSED_PIN) {
        // initialize to RX
        digitalWrite(plmic_pins->rxtx, LOW != plmic_pins->rxtx_rx_active);
        pinMode(plmic_pins->rxtx, OUTPUT);
    }
    if (plmic_pins->rst != LMIC_UNUSED_PIN) {
        // initialize RST to floating
        pinMode(plmic_pins->rst, INPUT);
    }

    hal_interrupt_init();
}

// val == 1  => tx
void hal_pin_rxtx (u1_t val) {
    if (plmic_pins->rxtx != LMIC_UNUSED_PIN)
        digitalWrite(plmic_pins->rxtx, val != plmic_pins->rxtx_rx_active);
}

// set radio RST pin to given value (or keep floating!)
void hal_pin_rst (u1_t val) {
    if (plmic_pins->rst == LMIC_UNUSED_PIN)
        return;

    if(val == 0 || val == 1) { // drive pin
        digitalWrite(plmic_pins->rst, val);
        pinMode(plmic_pins->rst, OUTPUT);
    } else { // keep pin floating
        pinMode(plmic_pins->rst, INPUT);
    }
}

s1_t hal_getRssiCal (void) {
    return plmic_pins->rssi_cal;
}

#if !defined(LMIC_USE_INTERRUPTS)
static void hal_interrupt_init() {
    pinMode(plmic_pins->dio[0], INPUT);
    if (plmic_pins->dio[1] != LMIC_UNUSED_PIN)
        pinMode(plmic_pins->dio[1], INPUT);
    if (plmic_pins->dio[2] != LMIC_UNUSED_PIN)
        pinMode(plmic_pins->dio[2], INPUT);
}

static bool dio_states[NUM_DIO] = {0};
static void hal_io_check() {
    uint8_t i;
    for (i = 0; i < NUM_DIO; ++i) {
        if (plmic_pins->dio[i] == LMIC_UNUSED_PIN)
            continue;

        if (dio_states[i] != digitalRead(plmic_pins->dio[i])) {
            dio_states[i] = !dio_states[i];
            if (dio_states[i])
                radio_irq_handler(i);
        }
    }
}

#else
// Interrupt handlers
static ostime_t interrupt_time[NUM_DIO] = {0};

static void hal_isrPin0() {
    ostime_t now = os_getTime();
    interrupt_time[0] = now ? now : 1;
}
static void hal_isrPin1() {
    ostime_t now = os_getTime();
    interrupt_time[1] = now ? now : 1;
}
static void hal_isrPin2() {
    ostime_t now = os_getTime();
    interrupt_time[2] = now ? now : 1;
}

typedef void (*isr_t)();
static isr_t interrupt_fns[NUM_DIO] = {hal_isrPin0, hal_isrPin1, hal_isrPin2};

static void hal_interrupt_init() {
  for (uint8_t i = 0; i < NUM_DIO; ++i) {
      if (plmic_pins->dio[i] == LMIC_UNUSED_PIN)
          continue;

      attachInterrupt(digitalPinToInterrupt(plmic_pins->dio[i]), interrupt_fns[i], RISING);
  }
}

static void hal_io_check() {
    uint8_t i;
    for (i = 0; i < NUM_DIO; ++i) {
        ostime_t iTime;
        if (plmic_pins->dio[i] == LMIC_UNUSED_PIN)
            continue;

        iTime = interrupt_time[i];
        if (iTime) {
            interrupt_time[i] = 0;
            radio_irq_handler_v2(i, iTime);
        }
    }
}
#endif // LMIC_USE_INTERRUPTS

// -----------------------------------------------------------------------------
// SPI

static void hal_spi_init () {
    SPI.begin();
}

static void hal_spi_trx(u1_t cmd, u1_t* buf, size_t len, bit_t is_read) {
    uint32_t spi_freq;
    u1_t nss = plmic_pins->nss;

    if ((spi_freq = plmic_pins->spi_freq) == 0)
        spi_freq = LMIC_SPI_FREQ;

    SPISettings settings(spi_freq, MSBFIRST, SPI_MODE0);
    SPI.beginTransaction(settings);
    digitalWrite(nss, 0);

    SPI.transfer(cmd);

    for (; len > 0; --len, ++buf) {
        u1_t data = is_read ? 0x00 : *buf;
        data = SPI.transfer(data);
        if (is_read)
            *buf = data;
    }

    digitalWrite(nss, 1);
    SPI.endTransaction();
}

void hal_spi_write(u1_t cmd, const u1_t* buf, size_t len) {
    hal_spi_trx(cmd, (u1_t*)buf, len, 0);
}

void hal_spi_read(u1_t cmd, u1_t* buf, size_t len) {
    hal_spi_trx(cmd, buf, len, 1);
}

// -----------------------------------------------------------------------------
// TIME

static void hal_time_init () {
    // Nothing to do
}

u4_t hal_ticks () {
    // Because micros() is scaled down in this function, micros() will
    // overflow before the tick timer should, causing the tick timer to
    // miss a significant part of its values if not corrected. To fix
    // this, the "overflow" serves as an overflow area for the micros()
    // counter. It consists of three parts:
    //  - The US_PER_OSTICK upper bits are effectively an extension for
    //    the micros() counter and are added to the result of this
    //    function.
    //  - The next bit overlaps with the most significant bit of
    //    micros(). This is used to detect micros() overflows.
    //  - The remaining bits are always zero.
    //
    // By comparing the overlapping bit with the corresponding bit in
    // the micros() return value, overflows can be detected and the
    // upper bits are incremented. This is done using some clever
    // bitwise operations, to remove the need for comparisons and a
    // jumps, which should result in efficient code. By avoiding shifts
    // other than by multiples of 8 as much as possible, this is also
    // efficient on AVR (which only has 1-bit shifts).
    static uint8_t overflow = 0;

    // Scaled down timestamp. The top US_PER_OSTICK_EXPONENT bits are 0,
    // the others will be the lower bits of our return value.
    uint32_t scaled = micros() >> US_PER_OSTICK_EXPONENT;
    // Most significant byte of scaled
    uint8_t msb = scaled >> 24;
    // Mask pointing to the overlapping bit in msb and overflow.
    const uint8_t mask = (1 << (7 - US_PER_OSTICK_EXPONENT));
    // Update overflow. If the overlapping bit is different
    // between overflow and msb, it is added to the stored value,
    // so the overlapping bit becomes equal again and, if it changed
    // from 1 to 0, the upper bits are incremented.
    overflow += (msb ^ overflow) & mask;

    // Return the scaled value with the upper bits of stored added. The
    // overlapping bit will be equal and the lower bits will be 0, so
    // bitwise or is a no-op for them.
    return scaled | ((uint32_t)overflow << 24);

    // 0 leads to correct, but overly complex code (it could just return
    // micros() unmodified), 8 leaves no room for the overlapping bit.
    static_assert(US_PER_OSTICK_EXPONENT > 0 && US_PER_OSTICK_EXPONENT < 8, "Invalid US_PER_OSTICK_EXPONENT value");
}

// Returns the number of ticks until time. Negative values indicate that
// time has already passed.
static s4_t delta_time(u4_t time) {
    return (s4_t)(time - hal_ticks());
}

void hal_waitUntil (u4_t time) {
    s4_t delta = delta_time(time);
    // From delayMicroseconds docs: Currently, the largest value that
    // will produce an accurate delay is 16383.
    while (delta > (16000 / US_PER_OSTICK)) {
        delay(16);
        delta -= (16000 / US_PER_OSTICK);
    }
    if (delta > 0)
        delayMicroseconds(delta * US_PER_OSTICK);
}

// check and rewind for target time
u1_t hal_checkTimer (u4_t time) {
    // No need to schedule wakeup, since we're not sleeping
    return delta_time(time) <= 0;
}

static uint8_t irqlevel = 0;

void hal_disableIRQs () {
    noInterrupts();
    irqlevel++;
}

void hal_enableIRQs () {
    if(--irqlevel == 0) {
        interrupts();

        // Instead of using proper interrupts (which are a bit tricky
        // and/or not available on all pins on AVR), just poll the pin
        // values. Since os_runloop disables and re-enables interrupts,
        // putting this here makes sure we check at least once every
        // loop.
        //
        // As an additional bonus, this prevents the can of worms that
        // we would otherwise get for running SPI transfers inside ISRs
        hal_io_check();
    }
}

void hal_sleep () {
    // Not implemented
}

// -----------------------------------------------------------------------------

#if defined(LMIC_PRINTF_TO)
#if !defined(__AVR)
static ssize_t uart_putchar (void *, const char *buf, size_t len) {
    return LMIC_PRINTF_TO.write((const uint8_t *)buf, len);
}

static cookie_io_functions_t functions =
 {
     .read = NULL,
     .write = uart_putchar,
     .seek = NULL,
     .close = NULL
 };

void hal_printf_init() {
    stdout = fopencookie(NULL, "w", functions);
    if (stdout != nullptr) {
        setvbuf(stdout, NULL, _IONBF, 0);
    }
}
#else // defined(__AVR)
static int uart_putchar (char c, FILE *)
{
    LMIC_PRINTF_TO.write(c) ;
    return 0 ;
}

void hal_printf_init() {
    // create a FILE structure to reference our UART output function
    static FILE uartout;
    memset(&uartout, 0, sizeof(uartout));

    // fill in the UART file descriptor with pointer to writer.
    fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);

    // The uart is the standard output device STDOUT.
    stdout = &uartout ;
}

#endif // !defined(ESP8266) || defined(ESP31B) || defined(ESP32)
#endif // defined(LMIC_PRINTF_TO)

void hal_init (void) {
    // use the global constant
    Arduino_LMIC::hal_init_with_pinmap(&lmic_pins);
}

// hal_init_ex is a C API routine, written in C++, and it's called
// with a pointer to an lmic_pinmap.
void hal_init_ex (const void *pContext) {
    const lmic_pinmap * const pHalPinmap = (const lmic_pinmap *) pContext;
    if (! Arduino_LMIC::hal_init_with_pinmap(pHalPinmap)) {
        hal_failed(__FILE__, __LINE__);
    }
}

// C++ API: initialize the HAL properly with a configuration object
namespace Arduino_LMIC {
bool hal_init_with_pinmap(const HalPinmap_t *pPinmap)
    {
    if (pPinmap == nullptr)
        return false;

    // set the static pinmap pointer.
    plmic_pins = pPinmap;

    // set the static HalConfiguration pointer.
    HalConfiguration_t * const pThisHalConfig = pPinmap->pConfig;

    if (pThisHalConfig != nullptr)
        pHalConfig = pThisHalConfig;
    else
        pHalConfig = &nullHalConig;

    pHalConfig->begin();

    // configure radio I/O and interrupt handler
    hal_io_init();
    // configure radio SPI
    hal_spi_init();
    // configure timer and interrupt handler
    hal_time_init();
#if defined(LMIC_PRINTF_TO)
    // printf support
    hal_printf_init();
#endif
    // declare success
    return true;
    }
}; // namespace Arduino_LMIC

static hal_failure_handler_t* custom_hal_failure_handler = NULL;

void hal_failed (const char *file, u2_t line) {
    if (custom_hal_failure_handler != NULL) {
        (*custom_hal_failure_handler)(file, line);
    } else {
#if defined(LMIC_FAILURE_TO)
        LMIC_FAILURE_TO.println("FAILURE ");
        LMIC_FAILURE_TO.print(file);
        LMIC_FAILURE_TO.print(':');
        LMIC_FAILURE_TO.println(line);
        LMIC_FAILURE_TO.flush();
#endif
        hal_disableIRQs();
        while(1);
    }
}

void hal_set_failure_handler(const hal_failure_handler_t* const handler) {
    custom_hal_failure_handler = handler;
}

ostime_t hal_setModuleActive (bit_t val) {
    // setModuleActive() takes a c++ bool, so
    // it effectively says "val != 0". We
    // don't have to.
    return pHalConfig->setModuleActive(val);
}

bit_t hal_queryUsingTcxo(void) {
    return pHalConfig->queryUsingTcxo();
}<commit_msg>Loop infinitely if the custom failure handler returns<commit_after>/*******************************************************************************
 * Copyright (c) 2015 Matthijs Kooijman
 * Copyright (c) 2018 MCCI Corporation
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * This the HAL to run LMIC on top of the Arduino environment.
 *******************************************************************************/

#include <Arduino.h>
#include <SPI.h>
// include all the lmic header files, including ../lmic/hal.h
#include "../lmic.h"
// include the C++ hal.h
#include "hal.h"
// we may need some things from stdio.
#include <stdio.h>

// -----------------------------------------------------------------------------
// I/O

static const Arduino_LMIC::HalPinmap_t *plmic_pins;
static Arduino_LMIC::HalConfiguration_t *pHalConfig;
static Arduino_LMIC::HalConfiguration_t nullHalConig;
static hal_failure_handler_t* custom_hal_failure_handler = NULL;

static void hal_interrupt_init(); // Fwd declaration

static void hal_io_init () {
    // NSS and DIO0 are required, DIO1 is required for LoRa, DIO2 for FSK
    ASSERT(plmic_pins->nss != LMIC_UNUSED_PIN);
    ASSERT(plmic_pins->dio[0] != LMIC_UNUSED_PIN);
    ASSERT(plmic_pins->dio[1] != LMIC_UNUSED_PIN || plmic_pins->dio[2] != LMIC_UNUSED_PIN);

//    Serial.print("nss: "); Serial.println(plmic_pins->nss);
//    Serial.print("rst: "); Serial.println(plmic_pins->rst);
//    Serial.print("dio[0]: "); Serial.println(plmic_pins->dio[0]);
//    Serial.print("dio[1]: "); Serial.println(plmic_pins->dio[1]);
//    Serial.print("dio[2]: "); Serial.println(plmic_pins->dio[2]);

    // initialize SPI chip select to high (it's active low)
    digitalWrite(plmic_pins->nss, HIGH);
    pinMode(plmic_pins->nss, OUTPUT);

    if (plmic_pins->rxtx != LMIC_UNUSED_PIN) {
        // initialize to RX
        digitalWrite(plmic_pins->rxtx, LOW != plmic_pins->rxtx_rx_active);
        pinMode(plmic_pins->rxtx, OUTPUT);
    }
    if (plmic_pins->rst != LMIC_UNUSED_PIN) {
        // initialize RST to floating
        pinMode(plmic_pins->rst, INPUT);
    }

    hal_interrupt_init();
}

// val == 1  => tx
void hal_pin_rxtx (u1_t val) {
    if (plmic_pins->rxtx != LMIC_UNUSED_PIN)
        digitalWrite(plmic_pins->rxtx, val != plmic_pins->rxtx_rx_active);
}

// set radio RST pin to given value (or keep floating!)
void hal_pin_rst (u1_t val) {
    if (plmic_pins->rst == LMIC_UNUSED_PIN)
        return;

    if(val == 0 || val == 1) { // drive pin
        digitalWrite(plmic_pins->rst, val);
        pinMode(plmic_pins->rst, OUTPUT);
    } else { // keep pin floating
        pinMode(plmic_pins->rst, INPUT);
    }
}

s1_t hal_getRssiCal (void) {
    return plmic_pins->rssi_cal;
}

#if !defined(LMIC_USE_INTERRUPTS)
static void hal_interrupt_init() {
    pinMode(plmic_pins->dio[0], INPUT);
    if (plmic_pins->dio[1] != LMIC_UNUSED_PIN)
        pinMode(plmic_pins->dio[1], INPUT);
    if (plmic_pins->dio[2] != LMIC_UNUSED_PIN)
        pinMode(plmic_pins->dio[2], INPUT);
}

static bool dio_states[NUM_DIO] = {0};
static void hal_io_check() {
    uint8_t i;
    for (i = 0; i < NUM_DIO; ++i) {
        if (plmic_pins->dio[i] == LMIC_UNUSED_PIN)
            continue;

        if (dio_states[i] != digitalRead(plmic_pins->dio[i])) {
            dio_states[i] = !dio_states[i];
            if (dio_states[i])
                radio_irq_handler(i);
        }
    }
}

#else
// Interrupt handlers
static ostime_t interrupt_time[NUM_DIO] = {0};

static void hal_isrPin0() {
    ostime_t now = os_getTime();
    interrupt_time[0] = now ? now : 1;
}
static void hal_isrPin1() {
    ostime_t now = os_getTime();
    interrupt_time[1] = now ? now : 1;
}
static void hal_isrPin2() {
    ostime_t now = os_getTime();
    interrupt_time[2] = now ? now : 1;
}

typedef void (*isr_t)();
static isr_t interrupt_fns[NUM_DIO] = {hal_isrPin0, hal_isrPin1, hal_isrPin2};

static void hal_interrupt_init() {
  for (uint8_t i = 0; i < NUM_DIO; ++i) {
      if (plmic_pins->dio[i] == LMIC_UNUSED_PIN)
          continue;

      attachInterrupt(digitalPinToInterrupt(plmic_pins->dio[i]), interrupt_fns[i], RISING);
  }
}

static void hal_io_check() {
    uint8_t i;
    for (i = 0; i < NUM_DIO; ++i) {
        ostime_t iTime;
        if (plmic_pins->dio[i] == LMIC_UNUSED_PIN)
            continue;

        iTime = interrupt_time[i];
        if (iTime) {
            interrupt_time[i] = 0;
            radio_irq_handler_v2(i, iTime);
        }
    }
}
#endif // LMIC_USE_INTERRUPTS

// -----------------------------------------------------------------------------
// SPI

static void hal_spi_init () {
    SPI.begin();
}

static void hal_spi_trx(u1_t cmd, u1_t* buf, size_t len, bit_t is_read) {
    uint32_t spi_freq;
    u1_t nss = plmic_pins->nss;

    if ((spi_freq = plmic_pins->spi_freq) == 0)
        spi_freq = LMIC_SPI_FREQ;

    SPISettings settings(spi_freq, MSBFIRST, SPI_MODE0);
    SPI.beginTransaction(settings);
    digitalWrite(nss, 0);

    SPI.transfer(cmd);

    for (; len > 0; --len, ++buf) {
        u1_t data = is_read ? 0x00 : *buf;
        data = SPI.transfer(data);
        if (is_read)
            *buf = data;
    }

    digitalWrite(nss, 1);
    SPI.endTransaction();
}

void hal_spi_write(u1_t cmd, const u1_t* buf, size_t len) {
    hal_spi_trx(cmd, (u1_t*)buf, len, 0);
}

void hal_spi_read(u1_t cmd, u1_t* buf, size_t len) {
    hal_spi_trx(cmd, buf, len, 1);
}

// -----------------------------------------------------------------------------
// TIME

static void hal_time_init () {
    // Nothing to do
}

u4_t hal_ticks () {
    // Because micros() is scaled down in this function, micros() will
    // overflow before the tick timer should, causing the tick timer to
    // miss a significant part of its values if not corrected. To fix
    // this, the "overflow" serves as an overflow area for the micros()
    // counter. It consists of three parts:
    //  - The US_PER_OSTICK upper bits are effectively an extension for
    //    the micros() counter and are added to the result of this
    //    function.
    //  - The next bit overlaps with the most significant bit of
    //    micros(). This is used to detect micros() overflows.
    //  - The remaining bits are always zero.
    //
    // By comparing the overlapping bit with the corresponding bit in
    // the micros() return value, overflows can be detected and the
    // upper bits are incremented. This is done using some clever
    // bitwise operations, to remove the need for comparisons and a
    // jumps, which should result in efficient code. By avoiding shifts
    // other than by multiples of 8 as much as possible, this is also
    // efficient on AVR (which only has 1-bit shifts).
    static uint8_t overflow = 0;

    // Scaled down timestamp. The top US_PER_OSTICK_EXPONENT bits are 0,
    // the others will be the lower bits of our return value.
    uint32_t scaled = micros() >> US_PER_OSTICK_EXPONENT;
    // Most significant byte of scaled
    uint8_t msb = scaled >> 24;
    // Mask pointing to the overlapping bit in msb and overflow.
    const uint8_t mask = (1 << (7 - US_PER_OSTICK_EXPONENT));
    // Update overflow. If the overlapping bit is different
    // between overflow and msb, it is added to the stored value,
    // so the overlapping bit becomes equal again and, if it changed
    // from 1 to 0, the upper bits are incremented.
    overflow += (msb ^ overflow) & mask;

    // Return the scaled value with the upper bits of stored added. The
    // overlapping bit will be equal and the lower bits will be 0, so
    // bitwise or is a no-op for them.
    return scaled | ((uint32_t)overflow << 24);

    // 0 leads to correct, but overly complex code (it could just return
    // micros() unmodified), 8 leaves no room for the overlapping bit.
    static_assert(US_PER_OSTICK_EXPONENT > 0 && US_PER_OSTICK_EXPONENT < 8, "Invalid US_PER_OSTICK_EXPONENT value");
}

// Returns the number of ticks until time. Negative values indicate that
// time has already passed.
static s4_t delta_time(u4_t time) {
    return (s4_t)(time - hal_ticks());
}

void hal_waitUntil (u4_t time) {
    s4_t delta = delta_time(time);
    // From delayMicroseconds docs: Currently, the largest value that
    // will produce an accurate delay is 16383.
    while (delta > (16000 / US_PER_OSTICK)) {
        delay(16);
        delta -= (16000 / US_PER_OSTICK);
    }
    if (delta > 0)
        delayMicroseconds(delta * US_PER_OSTICK);
}

// check and rewind for target time
u1_t hal_checkTimer (u4_t time) {
    // No need to schedule wakeup, since we're not sleeping
    return delta_time(time) <= 0;
}

static uint8_t irqlevel = 0;

void hal_disableIRQs () {
    noInterrupts();
    irqlevel++;
}

void hal_enableIRQs () {
    if(--irqlevel == 0) {
        interrupts();

        // Instead of using proper interrupts (which are a bit tricky
        // and/or not available on all pins on AVR), just poll the pin
        // values. Since os_runloop disables and re-enables interrupts,
        // putting this here makes sure we check at least once every
        // loop.
        //
        // As an additional bonus, this prevents the can of worms that
        // we would otherwise get for running SPI transfers inside ISRs
        hal_io_check();
    }
}

void hal_sleep () {
    // Not implemented
}

// -----------------------------------------------------------------------------

#if defined(LMIC_PRINTF_TO)
#if !defined(__AVR)
static ssize_t uart_putchar (void *, const char *buf, size_t len) {
    return LMIC_PRINTF_TO.write((const uint8_t *)buf, len);
}

static cookie_io_functions_t functions =
 {
     .read = NULL,
     .write = uart_putchar,
     .seek = NULL,
     .close = NULL
 };

void hal_printf_init() {
    stdout = fopencookie(NULL, "w", functions);
    if (stdout != nullptr) {
        setvbuf(stdout, NULL, _IONBF, 0);
    }
}
#else // defined(__AVR)
static int uart_putchar (char c, FILE *)
{
    LMIC_PRINTF_TO.write(c) ;
    return 0 ;
}

void hal_printf_init() {
    // create a FILE structure to reference our UART output function
    static FILE uartout;
    memset(&uartout, 0, sizeof(uartout));

    // fill in the UART file descriptor with pointer to writer.
    fdev_setup_stream (&uartout, uart_putchar, NULL, _FDEV_SETUP_WRITE);

    // The uart is the standard output device STDOUT.
    stdout = &uartout ;
}

#endif // !defined(ESP8266) || defined(ESP31B) || defined(ESP32)
#endif // defined(LMIC_PRINTF_TO)

void hal_init (void) {
    // use the global constant
    Arduino_LMIC::hal_init_with_pinmap(&lmic_pins);
}

// hal_init_ex is a C API routine, written in C++, and it's called
// with a pointer to an lmic_pinmap.
void hal_init_ex (const void *pContext) {
    const lmic_pinmap * const pHalPinmap = (const lmic_pinmap *) pContext;
    if (! Arduino_LMIC::hal_init_with_pinmap(pHalPinmap)) {
        hal_failed(__FILE__, __LINE__);
    }
}

// C++ API: initialize the HAL properly with a configuration object
namespace Arduino_LMIC {
bool hal_init_with_pinmap(const HalPinmap_t *pPinmap)
    {
    if (pPinmap == nullptr)
        return false;

    // set the static pinmap pointer.
    plmic_pins = pPinmap;

    // set the static HalConfiguration pointer.
    HalConfiguration_t * const pThisHalConfig = pPinmap->pConfig;

    if (pThisHalConfig != nullptr)
        pHalConfig = pThisHalConfig;
    else
        pHalConfig = &nullHalConig;

    pHalConfig->begin();

    // configure radio I/O and interrupt handler
    hal_io_init();
    // configure radio SPI
    hal_spi_init();
    // configure timer and interrupt handler
    hal_time_init();
#if defined(LMIC_PRINTF_TO)
    // printf support
    hal_printf_init();
#endif
    // declare success
    return true;
    }
}; // namespace Arduino_LMIC

void hal_failed (const char *file, u2_t line) {
    if (custom_hal_failure_handler != NULL) {
        (*custom_hal_failure_handler)(file, line);
    }

#if defined(LMIC_FAILURE_TO)
    LMIC_FAILURE_TO.println("FAILURE ");
    LMIC_FAILURE_TO.print(file);
    LMIC_FAILURE_TO.print(':');
    LMIC_FAILURE_TO.println(line);
    LMIC_FAILURE_TO.flush();
#endif

    hal_disableIRQs();

    // Infinite loop
    while (1) {
        ;
    }
}

void hal_set_failure_handler(const hal_failure_handler_t* const handler) {
    custom_hal_failure_handler = handler;
}

ostime_t hal_setModuleActive (bit_t val) {
    // setModuleActive() takes a c++ bool, so
    // it effectively says "val != 0". We
    // don't have to.
    return pHalConfig->setModuleActive(val);
}

bit_t hal_queryUsingTcxo(void) {
    return pHalConfig->queryUsingTcxo();
}<|endoftext|>


<commit_before>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkConnectPointsInteractor.h"
#include <mitkLineOperation.h>
#include <mitkPositionEvent.h>
#include "mitkMesh.h"
#include <mitkDataTreeNode.h>
#include <mitkInteractionConst.h>
#include "mitkAction.h"

//how precise must the user pick the point
//default value
const int PRECISION = 5;

mitk::ConnectPointsInteractor::ConnectPointsInteractor(const char * type, DataTreeNode* dataTreeNode, int n)
:Interactor(type, dataTreeNode), m_N(n), m_CurrentCellId(0), m_Precision(PRECISION)
{
  m_LastPoint.Fill(0);
  m_SumVec.Fill(0);
}

mitk::ConnectPointsInteractor::~ConnectPointsInteractor()
{
}

void mitk::ConnectPointsInteractor::SetPrecision(unsigned int precision)
{
  m_Precision = precision;
}

//##Documentation
//## overwritten cause this class can handle it better!
float mitk::ConnectPointsInteractor::CalculateJurisdiction(StateEvent const* stateEvent) const
{
  float returnvalue = 0.0;
  //if it is a key event that can be handled in the current state, then return 0.5
  mitk::DisplayPositionEvent const  *disPosEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());

  //Key event handling:
  if (disPosEvent == NULL)
  {
    //check, if the current state has a transition waiting for that key event.
    if (this->GetCurrentState()->GetTransition(stateEvent->GetId())!=NULL)
    {
      return 0.5;
    }
    else
    {
      return 0;
    }
  }

  //on MouseMove do nothing!
  if (stateEvent->GetEvent()->GetType() == mitk::Type_MouseMove)
  {
    return 0;
  }

  //if we don't have a Point in our PointSet, then return with 0.5
  mitk::PointSet* pointSet = dynamic_cast<mitk::PointSet*>(m_DataTreeNode->GetData());
  if (pointSet != NULL)
  {
    if (pointSet->GetSize()<1)
      returnvalue = 0.5;
  }

  return returnvalue;
}

bool mitk::ConnectPointsInteractor::ExecuteAction( Action* action, mitk::StateEvent const* stateEvent )
{
  bool ok = false;//for return type bool

  //checking corresponding Data; has to be a Mesh or a subclass
  mitk::Mesh* mesh = dynamic_cast<mitk::Mesh*>(m_DataTreeNode->GetData());
  if (mesh == NULL)
    return false;

  //for reading on the points, Id's etc
  //mitk::PointSet::DataType *itkpointSet = mesh->GetPointSet();
  //mitk::PointSet::PointsContainer *points = itkpointSet->GetPoints();//Warning Fix: not used!

  /*Each case must watch the type of the event!*/
  switch (action->GetActionId())
  {
  case AcDONOTHING:
    ok = true;
    break;
  case AcADDPOINT:
    {
      mitk::DisplayPositionEvent const  *posEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
      if (posEvent == NULL)
        return false;

      mitk::Point3D worldPoint;
      worldPoint = posEvent->GetWorldPosition();

      int position = mesh->SearchPoint(worldPoint, m_Precision);
      if (position >= 0)//found a point near enough to the given point
      {
        // if the point is the last in current cell, remove it (this has to be moved in a separate action)
        bool deleteLine=false;
        if(mesh->GetMesh()->GetCells()->Size() > 0)
        {
          Mesh::CellAutoPointer cellAutoPointer;
          ok = mesh->GetMesh()->GetCell(m_CurrentCellId, cellAutoPointer);
          if(ok)
          {
            Mesh::PointIdIterator last = cellAutoPointer->PointIdsEnd();
            --last;
            int foundCell = mesh->SearchFirstCell( (unsigned long) position );
            deleteLine = ( ((unsigned int)(foundCell)) == m_CurrentCellId ) && ( *last == position );
          }
        }
        if(deleteLine)
        {
          LineOperation* doOp = new mitk::LineOperation(OpDELETELINE, m_CurrentCellId, position);
          if (m_UndoEnabled)
          {
            LineOperation* undoOp = new mitk::LineOperation(OpADDLINE, m_CurrentCellId, position);
            OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
            m_UndoController->SetOperationEvent(operationEvent);
          }
          //execute the Operation
          mesh->ExecuteOperation(doOp );
        }
        else
        {
          // add new cell if necessary
          if(mesh->GetNewCellId() == 0) //allow single line only
            //allow multiple lines:          if((mesh->SearchFirstCell(position) >= 0) || ((m_CurrentCellId == 0) && (mesh->GetNewCellId() == 0)))
          {
            //get the next cellId and set m_CurrentCellId
            m_CurrentCellId = mesh->GetNewCellId();

            //now reserv a new cell in m_ItkData
            LineOperation* doOp = new mitk::LineOperation(OpNEWCELL, m_CurrentCellId);
            if (m_UndoEnabled)
            {
              LineOperation* undoOp = new mitk::LineOperation(OpDELETECELL, m_CurrentCellId);
              OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
              m_UndoController->SetOperationEvent(operationEvent);
            }
            mesh->ExecuteOperation(doOp);
          }
          // add line if point is not yet included in current cell
          if(mesh->SearchFirstCell(position) < 0)
          {
            LineOperation* doOp = new mitk::LineOperation(OpADDLINE, m_CurrentCellId, position);
            if (m_UndoEnabled)
            {
              LineOperation* undoOp = new mitk::LineOperation(OpDELETELINE, m_CurrentCellId, position);
              OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
              m_UndoController->SetOperationEvent(operationEvent);
            }
            //execute the Operation
            mesh->ExecuteOperation(doOp );
          }
        }
      }
      ok = true;
      break;
    }
  case AcREMOVEPOINT:
    {
      //mitk::DisplayPositionEvent const  *posEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
      //if (posEvent == NULL)
      //  return false;

      //mitk::Point3D worldPoint;
      //worldPoint = posEvent->GetWorldPosition();

      //int position = mesh->SearchPoint(worldPoint, m_Precision);
      //if (position>=0)//found a point near enough to the given point
      //{
      //  // if the point is in the current cell, remove it (this has to be moved in a separate action)
      //  if(mesh->SearchFirstCell(position) == m_CurrentCellId)
      //  {
      //    LineOperation* doOp = new mitk::LineOperation(OpDELETELINE, m_CurrentCellId, position);
      //  if (m_UndoEnabled)
      //  {
      //   LineOperation* undoOp = new mitk::LineOperation(OpADDLINE, m_CurrentCellId, position);
      //   OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
      //   m_UndoController->SetOperationEvent(operationEvent);
      //  }
      //  //execute the Operation
      //    mesh->ExecuteOperation(doOp );
      //  }
      //}
      ok = true;
      break;
    }
  case AcCHECKELEMENT:
    /*checking if the Point transmitted is close enough to one point. Then generate a new event with the point and let this statemaschine handle the event.*/
    {
      mitk::DisplayPositionEvent const  *posEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
      if (posEvent != NULL)
      {
        mitk::Point3D worldPoint = posEvent->GetWorldPosition();

        int position = mesh->SearchPoint(worldPoint, m_Precision);
        if (position>=0)//found a point near enough to the given point
        {
          PointSet::PointType pt = mesh->GetPoint(position);//get that point, the one meant by the user!
          mitk::Point2D displPoint;
          displPoint[0] = worldPoint[0]; displPoint[1] = worldPoint[1];
          //new Event with information YES and with the correct point
          mitk::PositionEvent const* newPosEvent = new mitk::PositionEvent(posEvent->GetSender(), Type_None, BS_NoButton, BS_NoButton, Key_none, displPoint, pt);
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDYES, newPosEvent);
          //call HandleEvent to leave the guard-state
          this->HandleEvent( newStateEvent );
          ok = true;
        }
        else
        {
          //new Event with information NO
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDNO, posEvent);
          this->HandleEvent(newStateEvent );
          ok = true;
        }
      }
      else
      {
        mitk::DisplayPositionEvent const  *disPosEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
        if (disPosEvent != NULL)
        {//2d Koordinates for 3D Interaction; return false to redo the last statechange
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDNO, posEvent);
          this->HandleEvent(newStateEvent );
          ok = true;
        }
      }
      break;
    }
  case AcCHECKNMINUS1://generate Events if the set will be full after the addition of the point or not.
    {
      if (m_N<0)//number of points not limited->pass on "Amount of points in Set is smaller then N-1"
      {
        mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDSTSMALERNMINUS1, stateEvent->GetEvent());
        this->HandleEvent( newStateEvent );
        ok = true;
      }
      else
      {
        if (mesh->GetSize()<(m_N-1))
          //pointset after addition won't be full
        {
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDSTSMALERNMINUS1, stateEvent->GetEvent());
          this->HandleEvent( newStateEvent );
          ok = true;
        }
        else //(mesh->GetSize()>=(m_N-1))
          //after the addition of a point, the container will be full
        {
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDSTLARGERNMINUS1, stateEvent->GetEvent());
          this->HandleEvent(newStateEvent );
          ok = true;
        }//else
      }//else
    }
    break;
  case AcCHECKEQUALS1:
    {
      if (mesh->GetSize()<=1)//the number of points in the list is 1 (or smaler)
      {
        mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDYES, stateEvent->GetEvent());
        this->HandleEvent( newStateEvent );
        ok = true;
      }
      else //more than 1 points in list, so stay in the state!
      {
        mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDNO, stateEvent->GetEvent());
        this->HandleEvent(newStateEvent );
        ok = true;
      }
    }
    break;
  default:
    return Superclass::ExecuteAction( action, stateEvent );
    //mitk::StatusBar::DisplayText("Message from mitkConnectPointsInteractor: I do not understand the Action!", 10000);
    //ok = false;
    //a false here causes the statemachine to undo its last statechange.
    //otherwise it will end up in a different state, but without done Action.
    //if a transition really has no Action, than call donothing
  }

  return ok;
}
<commit_msg>FIX: warning unsigned-signed mismatch<commit_after>/*=========================================================================

Program:   Medical Imaging & Interaction Toolkit
Module:    $RCSfile$
Language:  C++
Date:      $Date$
Version:   $Revision$

Copyright (c) German Cancer Research Center, Division of Medical and
Biological Informatics. All rights reserved.
See MITKCopyright.txt or http://www.mitk.org/copyright.html for details.

This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

#include "mitkConnectPointsInteractor.h"
#include <mitkLineOperation.h>
#include <mitkPositionEvent.h>
#include "mitkMesh.h"
#include <mitkDataTreeNode.h>
#include <mitkInteractionConst.h>
#include "mitkAction.h"

//how precise must the user pick the point
//default value
const int PRECISION = 5;

mitk::ConnectPointsInteractor::ConnectPointsInteractor(const char * type, DataTreeNode* dataTreeNode, int n)
:Interactor(type, dataTreeNode), m_N(n), m_CurrentCellId(0), m_Precision(PRECISION)
{
  m_LastPoint.Fill(0);
  m_SumVec.Fill(0);
}

mitk::ConnectPointsInteractor::~ConnectPointsInteractor()
{
}

void mitk::ConnectPointsInteractor::SetPrecision(unsigned int precision)
{
  m_Precision = precision;
}

//##Documentation
//## overwritten cause this class can handle it better!
float mitk::ConnectPointsInteractor::CalculateJurisdiction(StateEvent const* stateEvent) const
{
  float returnvalue = 0.0;
  //if it is a key event that can be handled in the current state, then return 0.5
  mitk::DisplayPositionEvent const  *disPosEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());

  //Key event handling:
  if (disPosEvent == NULL)
  {
    //check, if the current state has a transition waiting for that key event.
    if (this->GetCurrentState()->GetTransition(stateEvent->GetId())!=NULL)
    {
      return 0.5;
    }
    else
    {
      return 0;
    }
  }

  //on MouseMove do nothing!
  if (stateEvent->GetEvent()->GetType() == mitk::Type_MouseMove)
  {
    return 0;
  }

  //if we don't have a Point in our PointSet, then return with 0.5
  mitk::PointSet* pointSet = dynamic_cast<mitk::PointSet*>(m_DataTreeNode->GetData());
  if (pointSet != NULL)
  {
    if (pointSet->GetSize()<1)
      returnvalue = 0.5;
  }

  return returnvalue;
}

bool mitk::ConnectPointsInteractor::ExecuteAction( Action* action, mitk::StateEvent const* stateEvent )
{
  bool ok = false;//for return type bool

  //checking corresponding Data; has to be a Mesh or a subclass
  mitk::Mesh* mesh = dynamic_cast<mitk::Mesh*>(m_DataTreeNode->GetData());
  if (mesh == NULL)
    return false;

  //for reading on the points, Id's etc
  //mitk::PointSet::DataType *itkpointSet = mesh->GetPointSet();
  //mitk::PointSet::PointsContainer *points = itkpointSet->GetPoints();//Warning Fix: not used!

  /*Each case must watch the type of the event!*/
  switch (action->GetActionId())
  {
  case AcDONOTHING:
    ok = true;
    break;
  case AcADDPOINT:
    {
      mitk::DisplayPositionEvent const  *posEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
      if (posEvent == NULL)
        return false;

      mitk::Point3D worldPoint;
      worldPoint = posEvent->GetWorldPosition();

      int position = mesh->SearchPoint(worldPoint, m_Precision);
      if (position >= 0)//found a point near enough to the given point
      {
        // if the point is the last in current cell, remove it (this has to be moved in a separate action)
        bool deleteLine=false;
        if(mesh->GetMesh()->GetCells()->Size() > 0)
        {
          Mesh::CellAutoPointer cellAutoPointer;
          ok = mesh->GetMesh()->GetCell(m_CurrentCellId, cellAutoPointer);
          if(ok)
          {
            Mesh::PointIdIterator last = cellAutoPointer->PointIdsEnd();
            --last;
            int foundCell = mesh->SearchFirstCell( (unsigned long) position );
            if (foundCell != -1)//not found
              deleteLine = ((unsigned int)foundCell == m_CurrentCellId) && (*last == (unsigned int)position);
          }
        }
        if(deleteLine)
        {
          LineOperation* doOp = new mitk::LineOperation(OpDELETELINE, m_CurrentCellId, position);
          if (m_UndoEnabled)
          {
            LineOperation* undoOp = new mitk::LineOperation(OpADDLINE, m_CurrentCellId, position);
            OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
            m_UndoController->SetOperationEvent(operationEvent);
          }
          //execute the Operation
          mesh->ExecuteOperation(doOp );
        }
        else
        {
          // add new cell if necessary
          if(mesh->GetNewCellId() == 0) //allow single line only
            //allow multiple lines:          if((mesh->SearchFirstCell(position) >= 0) || ((m_CurrentCellId == 0) && (mesh->GetNewCellId() == 0)))
          {
            //get the next cellId and set m_CurrentCellId
            m_CurrentCellId = mesh->GetNewCellId();

            //now reserv a new cell in m_ItkData
            LineOperation* doOp = new mitk::LineOperation(OpNEWCELL, m_CurrentCellId);
            if (m_UndoEnabled)
            {
              LineOperation* undoOp = new mitk::LineOperation(OpDELETECELL, m_CurrentCellId);
              OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
              m_UndoController->SetOperationEvent(operationEvent);
            }
            mesh->ExecuteOperation(doOp);
          }
          // add line if point is not yet included in current cell
          if(mesh->SearchFirstCell(position) < 0)
          {
            LineOperation* doOp = new mitk::LineOperation(OpADDLINE, m_CurrentCellId, position);
            if (m_UndoEnabled)
            {
              LineOperation* undoOp = new mitk::LineOperation(OpDELETELINE, m_CurrentCellId, position);
              OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
              m_UndoController->SetOperationEvent(operationEvent);
            }
            //execute the Operation
            mesh->ExecuteOperation(doOp );
          }
        }
      }
      ok = true;
      break;
    }
  case AcREMOVEPOINT:
    {
      //mitk::DisplayPositionEvent const  *posEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
      //if (posEvent == NULL)
      //  return false;

      //mitk::Point3D worldPoint;
      //worldPoint = posEvent->GetWorldPosition();

      //int position = mesh->SearchPoint(worldPoint, m_Precision);
      //if (position>=0)//found a point near enough to the given point
      //{
      //  // if the point is in the current cell, remove it (this has to be moved in a separate action)
      //  if(mesh->SearchFirstCell(position) == m_CurrentCellId)
      //  {
      //    LineOperation* doOp = new mitk::LineOperation(OpDELETELINE, m_CurrentCellId, position);
      //  if (m_UndoEnabled)
      //  {
      //   LineOperation* undoOp = new mitk::LineOperation(OpADDLINE, m_CurrentCellId, position);
      //   OperationEvent *operationEvent = new OperationEvent(mesh, doOp, undoOp);
      //   m_UndoController->SetOperationEvent(operationEvent);
      //  }
      //  //execute the Operation
      //    mesh->ExecuteOperation(doOp );
      //  }
      //}
      ok = true;
      break;
    }
  case AcCHECKELEMENT:
    /*checking if the Point transmitted is close enough to one point. Then generate a new event with the point and let this statemaschine handle the event.*/
    {
      mitk::DisplayPositionEvent const  *posEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
      if (posEvent != NULL)
      {
        mitk::Point3D worldPoint = posEvent->GetWorldPosition();

        int position = mesh->SearchPoint(worldPoint, m_Precision);
        if (position>=0)//found a point near enough to the given point
        {
          PointSet::PointType pt = mesh->GetPoint(position);//get that point, the one meant by the user!
          mitk::Point2D displPoint;
          displPoint[0] = worldPoint[0]; displPoint[1] = worldPoint[1];
          //new Event with information YES and with the correct point
          mitk::PositionEvent const* newPosEvent = new mitk::PositionEvent(posEvent->GetSender(), Type_None, BS_NoButton, BS_NoButton, Key_none, displPoint, pt);
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDYES, newPosEvent);
          //call HandleEvent to leave the guard-state
          this->HandleEvent( newStateEvent );
          ok = true;
        }
        else
        {
          //new Event with information NO
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDNO, posEvent);
          this->HandleEvent(newStateEvent );
          ok = true;
        }
      }
      else
      {
        mitk::DisplayPositionEvent const  *disPosEvent = dynamic_cast <const mitk::DisplayPositionEvent *> (stateEvent->GetEvent());
        if (disPosEvent != NULL)
        {//2d Koordinates for 3D Interaction; return false to redo the last statechange
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDNO, posEvent);
          this->HandleEvent(newStateEvent );
          ok = true;
        }
      }
      break;
    }
  case AcCHECKNMINUS1://generate Events if the set will be full after the addition of the point or not.
    {
      if (m_N<0)//number of points not limited->pass on "Amount of points in Set is smaller then N-1"
      {
        mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDSTSMALERNMINUS1, stateEvent->GetEvent());
        this->HandleEvent( newStateEvent );
        ok = true;
      }
      else
      {
        if (mesh->GetSize()<(m_N-1))
          //pointset after addition won't be full
        {
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDSTSMALERNMINUS1, stateEvent->GetEvent());
          this->HandleEvent( newStateEvent );
          ok = true;
        }
        else //(mesh->GetSize()>=(m_N-1))
          //after the addition of a point, the container will be full
        {
          mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDSTLARGERNMINUS1, stateEvent->GetEvent());
          this->HandleEvent(newStateEvent );
          ok = true;
        }//else
      }//else
    }
    break;
  case AcCHECKEQUALS1:
    {
      if (mesh->GetSize()<=1)//the number of points in the list is 1 (or smaler)
      {
        mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDYES, stateEvent->GetEvent());
        this->HandleEvent( newStateEvent );
        ok = true;
      }
      else //more than 1 points in list, so stay in the state!
      {
        mitk::StateEvent* newStateEvent = new mitk::StateEvent(EIDNO, stateEvent->GetEvent());
        this->HandleEvent(newStateEvent );
        ok = true;
      }
    }
    break;
  default:
    return Superclass::ExecuteAction( action, stateEvent );
    //mitk::StatusBar::DisplayText("Message from mitkConnectPointsInteractor: I do not understand the Action!", 10000);
    //ok = false;
    //a false here causes the statemachine to undo its last statechange.
    //otherwise it will end up in a different state, but without done Action.
    //if a transition really has no Action, than call donothing
  }

  return ok;
}
<|endoftext|>


<commit_before>/*--------------------------------------------------------------------------
 *--------------------------------------------------------------------------
 *
 * Copyright (C) 2008,2009 The PECOS Development Team
 *
 * Please see http://pecos.ices.utexas.edu for more information.
 *
 * This file is part of the QUESO Library (Quantification of Uncertainty
 * for Estimation, Simulation and Optimization).
 *
 * QUESO is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * QUESO 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with QUESO. If not, see <http://www.gnu.org/licenses/>.
 *
 *--------------------------------------------------------------------------
 *
 * $Id$
 *
 * Basic API: Class member functions.
 *
 *--------------------------------------------------------------------------
 *-------------------------------------------------------------------------- */

using namespace std;

#include <basic_classes.h>
#include <basic_int.h>
#include <hpct.h>

namespace QUESO_Basic_API {

  void   QUESO_fatal        (const char *message);
  double Likelihood_Wrapper (const basicV &,const basicV *,const void *,basicV *,basicM *,basicV *);

  //------------------
  // Member Functions
  //------------------

  QUESO_Basic_Class::QUESO_Basic_Class()
  {
    m_initialized           = 0;
    m_silent                = 0;
    m_paramSpace            = NULL;
    m_paramDomain           = NULL;
    m_user_likelihood_func  = NULL;
  }

  void QUESO_Basic_Class::Initialize(const char *inputfile)
  {

    // Define new QUESO environment

    m_env         = new uqFullEnvironmentClass(MPI_COMM_WORLD,inputfile,"");
    m_initialized = 1;

    m_inputfile = new string(inputfile);

    //    memcpy(m_inputfile,inputfile,strlen()
}

  void QUESO_Basic_Class:: DefineParameterSpace()
  {
    int     num_params;		// # of UQ parameters
    double *param_min;		// min value of each parameter
    double *param_max;		// max value of each parameter
    double *param_ini;		// initial value of each parameter

    int     ierr = 1;

    VerifyInit();

    // Verify presence of required input file

    ierr *= hpct_input_fopen(m_inputfile->c_str());

    if(ierr == 0)
      QUESO_fatal("Unable to access QUESO input file");

    // Derive the UQ parameters from input file and create QUESO parameter vector

    ierr *= hpct_input_fread_int("queso/parameters/num_params",&m_num_params);

    if(ierr == 0)
      QUESO_fatal("Unable to read num_params from input file.");

    printf("--> Setup parameter space variables/ranges...\n");
    m_paramSpace  = new uqVectorSpaceClass <basicV,basicM> (*m_env,"queso_basic_",m_num_params,NULL);

    param_min = (double *) calloc(m_num_params,sizeof(double));
    param_max = (double *) calloc(m_num_params,sizeof(double));
    param_ini = (double *) calloc(m_num_params,sizeof(double));

    if(param_min == NULL || param_max == NULL || param_ini == NULL)
      QUESO_fatal("Unable to allocate memory for desired parameter space");

    ierr *= hpct_input_fread_double_vec("queso/parameters/param_mins",  param_min, m_num_params);
    ierr *= hpct_input_fread_double_vec("queso/parameters/param_maxs",  param_max, m_num_params);
    ierr *= hpct_input_fread_double_vec("queso/parameters/param_inits", param_ini, m_num_params);

    if(ierr == 0)
      QUESO_fatal("Unable to read parameter min/max/init values");

    m_queso_var_min = new basicV ( m_paramSpace->zeroVector() );
    m_queso_var_max = new basicV ( m_paramSpace->zeroVector() );
    m_queso_var_ini = new basicV ( m_paramSpace->zeroVector() );

    for(int i = 0;i<m_num_params;i++)
      {
	(*m_queso_var_min)[i] = param_min[i];
	(*m_queso_var_max)[i] = param_max[i];
	(*m_queso_var_ini)[i] = param_ini[i];
      }

    printf("--> Setup parameter search box...\n");
    m_paramDomain = new uqBoxSubsetClass<basicV, basicM> ("queso_basic_",*m_paramSpace,
							  *m_queso_var_min,*m_queso_var_max);
    // Clean up.

    free(param_min);
    free(param_max);
    free(param_ini);

    hpct_input_fclose();

  }

  void QUESO_Basic_Class:: VerifyInit()
  {
    if(!m_initialized)
      QUESO_fatal("QUESO not initialized prior to use");
  }

  void QUESO_Basic_Class::Likelihood_Register(double (*fp)(double *) )
  {

    VerifyInit();

    printf("--> Setting prior and post vectors...\n");

    m_priorRV = new uqUniformVectorRVClass <basicV, basicM> ("prior_",*m_paramDomain);
    m_postRV  = new uqGenericVectorRVClass <basicV, basicM> ("post_" ,*m_paramSpace );

    m_likelihoodObj = new uqGenericScalarFunctionClass
      <basicV,basicM> ("like_",*m_paramDomain,Likelihood_Wrapper,NULL,true);

    m_user_likelihood_func = fp;

    // define the inverse (calibration) problem

    printf("--> Defining inverse problem...\n");

    m_ip = new uqStatisticalInverseProblemClass <basicV,basicM> ("",*m_priorRV,*m_likelihoodObj,*m_postRV);

    // Default covariance matrix for now - default assumption assumes
    // 6-sigma distribution range falls over 1/3 of the max parameter range.

    double cov_param = 1/(3.*6.);	// 6-sigma over 1/3 of the range
    double param_range = 0.0;

    printf("--> Defining default covariance matrix...\n");

    m_CovMatrix = m_postRV->imageSet().vectorSpace().newGaussianMatrix(m_priorRV->pdf().domainVarVector(),
								       *m_queso_var_ini);

    for(int i=0;i<m_num_params;i++)
      {
	param_range = (*m_queso_var_max)[i] - (*m_queso_var_min)[i];
	(*m_CovMatrix)(i,i) = (cov_param*param_range)*(cov_param*param_range);
      }

  }

  void QUESO_Basic_Class::SolveInverseProblem()
  {
    VerifyInit();

    // Launch the Markov Chain

    m_ip->solveWithBayesMarkovChain(*m_queso_var_ini,m_CovMatrix);
  }

  //----------------------------------------------
  // Fatal error(): example only - to be replaced.
  //----------------------------------------------

  void QUESO_fatal(const char *message)
  {
    printf("%s\n",message);
    exit(1);

    // koomie note: likely need mpi_abort and better exception handling.
  }

  //---------------------------------------------------------------------
  // Wrapper for user likelihood routine: culls data from QUESO uqVector
  // and passes to the user routine as an array of doubles.
  //---------------------------------------------------------------------

  double Likelihood_Wrapper(const basicV &paramValue,
			    const basicV *paramDirection,
			    const void *Data,basicV *gradV,basicM *hesianM,
			    basicV *hessianE)
  {
    static int first_entry = 1;
    double user_func_return;
    double likelihood;
    double *uqParams;
    int num_params;

    num_params = paramValue.size();

    if(first_entry)
      {
	if(num_params < 1)
	  QUESO_fatal("Invalid number of parameters");

	if(_QUESO_Basic->m_user_likelihood_func  == NULL )
	  QUESO_fatal("Invalid user-supplied likelihood function");

	uqParams = (double *)calloc(num_params,sizeof(double));
	if(uqParams == NULL)
	  QUESO_fatal("Unable to allocate emmory for uqParams");
      }

    for(int i=0;i<num_params;i++)
      uqParams[i] = paramValue[i];

      return( _QUESO_Basic->m_user_likelihood_func(uqParams) );

  }

}   //  QUESO_Basic_API namespace
<commit_msg>queso [branch]: member function changed for the size of a uqGslVector; update basic interface to use global size<commit_after>/*--------------------------------------------------------------------------
 *--------------------------------------------------------------------------
 *
 * Copyright (C) 2008,2009 The PECOS Development Team
 *
 * Please see http://pecos.ices.utexas.edu for more information.
 *
 * This file is part of the QUESO Library (Quantification of Uncertainty
 * for Estimation, Simulation and Optimization).
 *
 * QUESO is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * QUESO 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with QUESO. If not, see <http://www.gnu.org/licenses/>.
 *
 *--------------------------------------------------------------------------
 *
 * $Id$
 *
 * Basic API: Class member functions.
 *
 *--------------------------------------------------------------------------
 *-------------------------------------------------------------------------- */

using namespace std;

#include <basic_classes.h>
#include <basic_int.h>
#include <hpct.h>

namespace QUESO_Basic_API {

  void   QUESO_fatal        (const char *message);
  double Likelihood_Wrapper (const basicV &,const basicV *,const void *,basicV *,basicM *,basicV *);

  //------------------
  // Member Functions
  //------------------

  QUESO_Basic_Class::QUESO_Basic_Class()
  {
    m_initialized           = 0;
    m_silent                = 0;
    m_paramSpace            = NULL;
    m_paramDomain           = NULL;
    m_user_likelihood_func  = NULL;
  }

  void QUESO_Basic_Class::Initialize(const char *inputfile)
  {

    // Define new QUESO environment

    m_env         = new uqFullEnvironmentClass(MPI_COMM_WORLD,inputfile,"");
    m_initialized = 1;

    m_inputfile = new string(inputfile);

    //    memcpy(m_inputfile,inputfile,strlen()
}

  void QUESO_Basic_Class:: DefineParameterSpace()
  {
    int     num_params;		// # of UQ parameters
    double *param_min;		// min value of each parameter
    double *param_max;		// max value of each parameter
    double *param_ini;		// initial value of each parameter

    int     ierr = 1;

    VerifyInit();

    // Verify presence of required input file

    ierr *= hpct_input_fopen(m_inputfile->c_str());

    if(ierr == 0)
      QUESO_fatal("Unable to access QUESO input file");

    // Derive the UQ parameters from input file and create QUESO parameter vector

    ierr *= hpct_input_fread_int("queso/parameters/num_params",&m_num_params);

    if(ierr == 0)
      QUESO_fatal("Unable to read num_params from input file.");

    printf("--> Setup parameter space variables/ranges...\n");
    m_paramSpace  = new uqVectorSpaceClass <basicV,basicM> (*m_env,"queso_basic_",m_num_params,NULL);

    param_min = (double *) calloc(m_num_params,sizeof(double));
    param_max = (double *) calloc(m_num_params,sizeof(double));
    param_ini = (double *) calloc(m_num_params,sizeof(double));

    if(param_min == NULL || param_max == NULL || param_ini == NULL)
      QUESO_fatal("Unable to allocate memory for desired parameter space");

    ierr *= hpct_input_fread_double_vec("queso/parameters/param_mins",  param_min, m_num_params);
    ierr *= hpct_input_fread_double_vec("queso/parameters/param_maxs",  param_max, m_num_params);
    ierr *= hpct_input_fread_double_vec("queso/parameters/param_inits", param_ini, m_num_params);

    if(ierr == 0)
      QUESO_fatal("Unable to read parameter min/max/init values");

    m_queso_var_min = new basicV ( m_paramSpace->zeroVector() );
    m_queso_var_max = new basicV ( m_paramSpace->zeroVector() );
    m_queso_var_ini = new basicV ( m_paramSpace->zeroVector() );

    for(int i = 0;i<m_num_params;i++)
      {
	(*m_queso_var_min)[i] = param_min[i];
	(*m_queso_var_max)[i] = param_max[i];
	(*m_queso_var_ini)[i] = param_ini[i];
      }

    printf("--> Setup parameter search box...\n");
    m_paramDomain = new uqBoxSubsetClass<basicV, basicM> ("queso_basic_",*m_paramSpace,
							  *m_queso_var_min,*m_queso_var_max);
    // Clean up.

    free(param_min);
    free(param_max);
    free(param_ini);

    hpct_input_fclose();

  }

  void QUESO_Basic_Class:: VerifyInit()
  {
    if(!m_initialized)
      QUESO_fatal("QUESO not initialized prior to use");
  }

  void QUESO_Basic_Class::Likelihood_Register(double (*fp)(double *) )
  {

    VerifyInit();

    printf("--> Setting prior and post vectors...\n");

    m_priorRV = new uqUniformVectorRVClass <basicV, basicM> ("prior_",*m_paramDomain);
    m_postRV  = new uqGenericVectorRVClass <basicV, basicM> ("post_" ,*m_paramSpace );

    m_likelihoodObj = new uqGenericScalarFunctionClass
      <basicV,basicM> ("like_",*m_paramDomain,Likelihood_Wrapper,NULL,true);

    m_user_likelihood_func = fp;

    // define the inverse (calibration) problem

    printf("--> Defining inverse problem...\n");

    m_ip = new uqStatisticalInverseProblemClass <basicV,basicM> ("",*m_priorRV,*m_likelihoodObj,*m_postRV);

    // Default covariance matrix for now - default assumption assumes
    // 6-sigma distribution range falls over 1/3 of the max parameter range.

    double cov_param = 1/(3.*6.);	// 6-sigma over 1/3 of the range
    double param_range = 0.0;

    printf("--> Defining default covariance matrix...\n");

    m_CovMatrix = m_postRV->imageSet().vectorSpace().newGaussianMatrix(m_priorRV->pdf().domainVarVector(),
								       *m_queso_var_ini);

    for(int i=0;i<m_num_params;i++)
      {
	param_range = (*m_queso_var_max)[i] - (*m_queso_var_min)[i];
	(*m_CovMatrix)(i,i) = (cov_param*param_range)*(cov_param*param_range);
      }

  }

  void QUESO_Basic_Class::SolveInverseProblem()
  {
    VerifyInit();

    // Launch the Markov Chain

    m_ip->solveWithBayesMarkovChain(*m_queso_var_ini,m_CovMatrix);
  }

  //----------------------------------------------
  // Fatal error(): example only - to be replaced.
  //----------------------------------------------

  void QUESO_fatal(const char *message)
  {
    printf("%s\n",message);
    exit(1);

    // koomie note: likely need mpi_abort and better exception handling.
  }

  //---------------------------------------------------------------------
  // Wrapper for user likelihood routine: culls data from QUESO uqVector
  // and passes to the user routine as an array of doubles.
  //---------------------------------------------------------------------

  double Likelihood_Wrapper(const basicV &paramValue,
			    const basicV *paramDirection,
			    const void *Data,basicV *gradV,basicM *hesianM,
			    basicV *hessianE)
  {
    static int first_entry = 1;
    double user_func_return;
    double likelihood;
    double *uqParams;
    int num_params;

    num_params = paramValue.sizeGlobal();

    if(first_entry)
      {
	if(num_params < 1)
	  QUESO_fatal("Invalid number of parameters");

	if(_QUESO_Basic->m_user_likelihood_func  == NULL )
	  QUESO_fatal("Invalid user-supplied likelihood function");

	uqParams = (double *)calloc(num_params,sizeof(double));
	if(uqParams == NULL)
	  QUESO_fatal("Unable to allocate emmory for uqParams");
      }

    for(int i=0;i<num_params;i++)
      uqParams[i] = paramValue[i];

      return( _QUESO_Basic->m_user_likelihood_func(uqParams) );

  }

}   //  QUESO_Basic_API namespace
<|endoftext|>


<commit_before>#include <vector>
#include <string>
#include <sstream>

#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>

#include "Types.h"
#include "TetraMesh.h"
#include "ObjState.h"

namespace{
    using namespace BallonFEM;
    const Vec3 v[3] = {Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1)};
    /* coeff of dr[i] in
     * d(2An) = (dr1 - dr3) x (r2 -r3) + (r1 - r3) x (dr2 - dr3)
     */
    const double coeff_v[3][2] = {
        { 1, 0},
        { 0, 1},
        { -1, -1},
    };

}

namespace BallonFEM
{
    void ObjState::volumeGradientDiffMat(SpMat &M)
    {
        /* clear Matrix */
        M.setZero();
        M.resize( 3 * m_size, 3 * m_size);

        std::vector<T> coefficients;
        coefficients.clear();
        /* to be completed */
        //coefficients.reserve( 9 * 9 * m_tetra->holes[0].holeface.size());

        /**/
        Vvec3 &pos = world_space_pos;
        for(size_t i = 0; i < m_h_size; i++)
        {
            Hole &h = m_tetra->holes[i];
            for(FCIter f = h.holeface.begin(); f != h.holeface.end(); f++)
            {
                iVec3 v_id = f->v_id;
                /* j is index of vertex, k is index of dimention */
                for (size_t j = 0; j < 3; j++)
                for (size_t k = 0; k < 3; k++)
                {
                    /* d(2An) = (dr1 - dr3) x (r2 -r3) + (r1 - r3) x (dr2 - dr3) */
                    Vec3 dn = coeff_v[j][0] * glm::cross(v[k], pos[v_id[1]] - pos[v_id[2]])
                        + coeff_v[j][1] * glm::cross(pos[v_id[0]] - pos[v_id[2]], v[k]);

                    for(size_t w = 0; w < 3; w++)
                        for(size_t l = 0; l < 3; l++)
                            coefficients.push_back( T( 3*v_id[w] + l, 3*v_id[j] + k, dn[l]));
                }
            }
        }

        /* since the hole surface normal towards inner, the V is negtive*/
        M.setFromTriplets(coefficients.begin(), coefficients.end());
        M /= -6.0;
    }

    SpMat DeltaState::projectMat()
    {
        /* present only consider fixed points */
        SpMat W(3 * m_size, 3 * m_size + 6 * m_r_size);

        std::vector<T> coefficients;
        coefficients.clear();
        coefficients.reserve(3 * m_size);

        /* vertices, fixed and rigid elements get 0 influence */
        for(VIter vi = m_tetra->vertices.begin(); vi != m_tetra->vertices.end(); vi++)
        {
            if (!vi->m_fixed  &&  vi->rigid == -1)
            {
                coefficients.push_back(T( 3 * vi->id    , 3 * vi->id    , 1));
                coefficients.push_back(T( 3 * vi->id + 1, 3 * vi->id + 1, 1));
                coefficients.push_back(T( 3 * vi->id + 2, 3 * vi->id + 2, 1));
            }
        }

        /* rigid bodies, begin at 3 * m_size */
        // to be completed

        W.setFromTriplets(coefficients.begin(), coefficients.end());

        return W;
    }

    SpMat DeltaState::restrictedMat()
    {
        SpMat I(3 * m_size + 6 * m_r_size, 3 * m_size + 6 * m_r_size);

        std::vector<T> coefficients;
        coefficients.clear();
        coefficients.reserve(3 * m_size);

        for(VIter vi = m_tetra->vertices.begin(); vi != m_tetra->vertices.end(); vi++)
        {
            if (vi->m_fixed  ||  vi->rigid != -1)
            {
                coefficients.push_back(T( 3 * vi->id    , 3 * vi->id    , 1));
                coefficients.push_back(T( 3 * vi->id + 1, 3 * vi->id + 1, 1));
                coefficients.push_back(T( 3 * vi->id + 2, 3 * vi->id + 2, 1));
            }
        }
        I.setFromTriplets(coefficients.begin(), coefficients.end());

        return I;
    }

    SpVec DeltaState::toSpVec()
    {
        SpVec stateVec( 3 * m_size + 6 * m_r_size );

		size_t count = 0;
        /* output vertices state, fixed and rigid part should be zero */
		for (size_t i = 0; i < m_size; i++)
		{
			stateVec(count    ) = dm_pos[i].x;
			stateVec(count + 1) = dm_pos[i].y;
			stateVec(count + 2) = dm_pos[i].z;
			count += 3;
		}

        /* output rigid body state */
        for (size_t i = 0; i < m_r_size; i++)
        {
			stateVec(count	  ) = dm_r_pos[i].x;
			stateVec(count + 1) = dm_r_pos[i].y;
			stateVec(count + 2) = dm_r_pos[i].z;
			stateVec(count + 3) = dm_r_rot[i].x;
			stateVec(count + 4) = dm_r_rot[i].y;
			stateVec(count + 5) = dm_r_rot[i].z;
			count += 6;
        }

        return stateVec;
    }

    void DeltaState::readSpVec(SpVec& stateVec)
    {
		size_t count = 0;
		/* output vertices state, fixed and rigid part should be zero */
		for (size_t i = 0; i < m_size; i++)
		{
			dm_pos[i].x = stateVec(count);
			dm_pos[i].y = stateVec(count + 1);
			dm_pos[i].z = stateVec(count + 2);
			count += 3;
		}

		/* output rigid body state */
		for (size_t i = 0; i < m_r_size; i++)
		{
			dm_r_pos[i].x = stateVec(count);
			dm_r_pos[i].y = stateVec(count + 1);
			dm_r_pos[i].z = stateVec(count + 2);
			dm_r_rot[i].x = stateVec(count + 3);
			dm_r_rot[i].y = stateVec(count + 4);
			dm_r_rot[i].z = stateVec(count + 5);
			count += 6;
		}
    }
}
<commit_msg>add support for rigid body in matrix solver<commit_after>#include <vector>
#include <string>
#include <sstream>

#include <glm/glm.hpp>
#include <glm/gtx/quaternion.hpp>

#include "Types.h"
#include "TetraMesh.h"
#include "ObjState.h"

namespace{
    using namespace BallonFEM;
    const Vec3 v[3] = {Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1)};
    /* coeff of dr[i] in
     * d(2An) = (dr1 - dr3) x (r2 -r3) + (r1 - r3) x (dr2 - dr3)
     */
    const double coeff_v[3][2] = {
        { 1, 0},
        { 0, 1},
        { -1, -1},
    };

}

namespace BallonFEM
{
    void ObjState::volumeGradientDiffMat(SpMat &M)
    {
        /* clear Matrix */
        M.setZero();
        M.resize( 3 * m_size, 3 * m_size);

        std::vector<T> coefficients;
        coefficients.clear();
        size_t count_holeface = 0;
        for (size_t i = 0; i < m_h_size; i++)
            count_holeface += m_tetra->holes[i].holeface.size();
        coefficients.reserve( 9 * 9 * count_holeface);

        /* compute contributions triangle by triangle */
        Vvec3 &pos = world_space_pos;
        for(size_t i = 0; i < m_h_size; i++)
        {
            Hole &h = m_tetra->holes[i];
            for(FCIter f = h.holeface.begin(); f != h.holeface.end(); f++)
            {
                iVec3 v_id = f->v_id;
                /* j is index of vertex, k is index of dimention */
                for (size_t j = 0; j < 3; j++)
                for (size_t k = 0; k < 3; k++)
                {
                    /* d(2An) = (dr1 - dr3) x (r2 -r3) + (r1 - r3) x (dr2 - dr3) */
                    Vec3 dn = coeff_v[j][0] * glm::cross(v[k], pos[v_id[1]] - pos[v_id[2]])
                        + coeff_v[j][1] * glm::cross(pos[v_id[0]] - pos[v_id[2]], v[k]);

                    for(size_t w = 0; w < 3; w++)
                        for(size_t l = 0; l < 3; l++)
                            coefficients.push_back( T( 3*v_id[w] + l, 3*v_id[j] + k, dn[l]));
                }
            }
        }

        /* since the hole surface normal towards inner, the V is negtive*/
        M.setFromTriplets(coefficients.begin(), coefficients.end());
        M /= -6.0;
    }

    SpMat DeltaState::projectMat()
    {
        /* present only consider fixed points */
        SpMat W(3 * m_size, 3 * m_size + 6 * m_r_size);

        std::vector<T> coefficients;
        coefficients.clear();
        size_t count_rigid = 0;
        for (size_t i = 0; i < m_r_size; i++)
            count_rigid += m_tetra->rigid_bodies[i].elements.size();
        /* each vertex contribute 3 and rigid element contribute 9 */
        coefficients.reserve(3 * m_size + 9 * count_rigid);

        /* vertices, fixed and rigid elements get 0 influence */
        for(VIter vi = m_tetra->vertices.begin(); vi != m_tetra->vertices.end(); vi++)
        {
            if (!vi->m_fixed  &&  vi->rigid == -1)
            {
                coefficients.push_back(T( 3 * vi->id    , 3 * vi->id    , 1));
                coefficients.push_back(T( 3 * vi->id + 1, 3 * vi->id + 1, 1));
                coefficients.push_back(T( 3 * vi->id + 2, 3 * vi->id + 2, 1));
            }
        }

        /* rigid bodies, begin at 3 * m_size */
        size_t count = 3 * m_size;
        for(size_t i = 0; i < m_r_size; i++)
        {
            Rigid &R = m_tetra->rigid_bodies[i];
            for(size_t j = 0; j < R.elements.size(); j++)
            {
                size_t id = R.elements[j];
                /* dr = drc + dw x (r - rc) */
                coefficients.push_back(T( 3 * id    , count    , 1));
                coefficients.push_back(T( 3 * id + 1, count + 1, 1));
                coefficients.push_back(T( 3 * id + 2, count + 2, 1));

                Vec3 r = m_state->m_r_pos[i] - m_state->world_space_pos[id];
                coefficients.push_back(T( 3 * id + 1, count + 3, r.z ));
                coefficients.push_back(T( 3 * id + 2, count + 3, -r.y));
                coefficients.push_back(T( 3 * id    , count + 4, -r.z));
                coefficients.push_back(T( 3 * id + 2, count + 4, r.x ));
                coefficients.push_back(T( 3 * id    , count + 5, r.y ));
                coefficients.push_back(T( 3 * id + 1, count + 5, -r.x));
            }
        }

        W.setFromTriplets(coefficients.begin(), coefficients.end());

        return W;
    }

    SpMat DeltaState::restrictedMat()
    {
        SpMat I(3 * m_size + 6 * m_r_size, 3 * m_size + 6 * m_r_size);

        std::vector<T> coefficients;
        coefficients.clear();
        coefficients.reserve(3 * m_size);

        for(VIter vi = m_tetra->vertices.begin(); vi != m_tetra->vertices.end(); vi++)
        {
            if (vi->m_fixed  ||  vi->rigid != -1)
            {
                coefficients.push_back(T( 3 * vi->id    , 3 * vi->id    , 1));
                coefficients.push_back(T( 3 * vi->id + 1, 3 * vi->id + 1, 1));
                coefficients.push_back(T( 3 * vi->id + 2, 3 * vi->id + 2, 1));
            }
        }
        I.setFromTriplets(coefficients.begin(), coefficients.end());

        return I;
    }

    SpVec DeltaState::toSpVec()
    {
        SpVec stateVec( 3 * m_size + 6 * m_r_size );

		size_t count = 0;
        /* output vertices state, fixed and rigid part should be zero */
		for (size_t i = 0; i < m_size; i++)
		{
			stateVec(count    ) = dm_pos[i].x;
			stateVec(count + 1) = dm_pos[i].y;
			stateVec(count + 2) = dm_pos[i].z;
			count += 3;
		}

        /* output rigid body state */
        for (size_t i = 0; i < m_r_size; i++)
        {
			stateVec(count	  ) = dm_r_pos[i].x;
			stateVec(count + 1) = dm_r_pos[i].y;
			stateVec(count + 2) = dm_r_pos[i].z;
			stateVec(count + 3) = dm_r_rot[i].x;
			stateVec(count + 4) = dm_r_rot[i].y;
			stateVec(count + 5) = dm_r_rot[i].z;
			count += 6;
        }

        return stateVec;
    }

    void DeltaState::readSpVec(SpVec& stateVec)
    {
		size_t count = 0;
		/* output vertices state, fixed and rigid part should be zero */
		for (size_t i = 0; i < m_size; i++)
		{
			dm_pos[i].x = stateVec(count);
			dm_pos[i].y = stateVec(count + 1);
			dm_pos[i].z = stateVec(count + 2);
			count += 3;
		}

		/* output rigid body state */
		for (size_t i = 0; i < m_r_size; i++)
		{
			dm_r_pos[i].x = stateVec(count);
			dm_r_pos[i].y = stateVec(count + 1);
			dm_r_pos[i].z = stateVec(count + 2);
			dm_r_rot[i].x = stateVec(count + 3);
			dm_r_rot[i].y = stateVec(count + 4);
			dm_r_rot[i].z = stateVec(count + 5);
			count += 6;
		}
    }
}
<|endoftext|>


<commit_before>#ifndef _PARSER_HH_
#define _PARSER_HH_ I_CAN_HAS

#include <vector>

typedef struct _ansi_sequence {
	char             mode;
	std::vector<int> values; // XXX: Technically wrong
} ansi_sequence;

void ansi_validate( char * sequence );

#endif
<commit_msg>adding in the parsing routines and such.<commit_after>#ifndef _PARSER_HH_
#define _PARSER_HH_ I_CAN_HAS

#include <vector>

typedef struct _ansi_sequence {
	char             mode;
	std::vector<int> values; // XXX: Technically wrong
} ansi_sequence;

void ansi_validate(       char * sequence );
ansi_sequence ansi_parse( char * sequence );

#endif
<|endoftext|>


<commit_before>/**
 * @file   ReuseQueue.hpp
 * @brief  ReuseQueue class prototype.
 * @author zer0
 * @date   2016-08-03
 */

#ifndef __INCLUDE_LIBTBAG__LIBTBAG_CONTAINER_REUSEQUEUE_HPP__
#define __INCLUDE_LIBTBAG__LIBTBAG_CONTAINER_REUSEQUEUE_HPP__

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <libtbag/config.h>
#include <libtbag/predef.hpp>
#include <libtbag/Err.hpp>
#include <libtbag/Type.hpp>

#include <deque>
#include <memory>
#include <utility>
#include <type_traits>

// -------------------
NAMESPACE_LIBTBAG_OPEN
// -------------------

namespace container {

/**
 * ReuseQueue class prototype.
 *
 * @author zer0
 * @date   2016-08-03
 */
template <typename ValueType>
class ReuseQueue
{
public:
    using Value = ValueType;
    using Queue = std::deque<Value>;

private:
    Queue _active;
    Queue _ready;

public:
    ReuseQueue() : _active(), _ready()
    {
        // EMPTY.
    }

    ReuseQueue(ReuseQueue const & obj) : ReuseQueue()
    {
        (*this) = obj;
    }

    ReuseQueue(ReuseQueue && obj) : ReuseQueue()
    {
        (*this) = std::move(obj);
    }

    ~ReuseQueue()
    {
        // EMPTY.
    }

public:
    ReuseQueue & operator =(ReuseQueue const & obj)
    {
        if (this != &obj) {
            _active = obj._active;
            _ready = obj._ready;
        }
        return *this;
    }

    ReuseQueue & operator =(ReuseQueue && obj)
    {
        if (this != &obj) {
            swap(obj);
        }
        return *this;
    }

public:
    void swap(ReuseQueue & obj)
    {
        _active.swap(obj._active);
        _ready.swap(obj._ready);
    }

    friend void swap(ReuseQueue & lh, ReuseQueue & rh)
    {
        lh.swap(rh);
    }

public:
    inline std::size_t size() const TBAG_NOEXCEPT_SP_OP(_active.empty())
    { return _active.size(); }
    inline bool empty() const TBAG_NOEXCEPT_SP_OP(_active.empty())
    { return _active.empty(); }

public:
    inline std::size_t sizeOfReadyQueue() const TBAG_NOEXCEPT_SP_OP(_ready.size())
    { return _ready.size(); }
    inline bool emptyOfReadyQueue() const TBAG_NOEXCEPT_SP_OP(_ready.empty())
    { return _ready.empty(); }

public:
    void clear()
    {
        _active.clear();
        _ready.clear();
    }

public:
    inline Value       & frontRef()       TBAG_NOEXCEPT_SP_OP(_active.front()) { return _active.front(); }
    inline Value const & frontRef() const TBAG_NOEXCEPT_SP_OP(_active.front()) { return _active.front(); }

    Err front(Value & result)
    {
        if (_active.empty()) {
            return Err::E_EQUEUE;
        }
        result = _active.front();
        return Err::E_SUCCESS;
    }

public:
    template <typename ... Args>
    Value & push(Args && ... args)
    {
        if (_ready.empty()) {
            _active.emplace_back(std::forward<Args>(args) ...);
        } else {
            _active.push_back(_ready.front());
            _ready.pop_front();
        }
        return _active.back();
    }

    Err pop()
    {
        if (_active.empty()) {
            return Err::E_EQUEUE;
        }
        _ready.push_back(_active.front());
        _active.pop_front();
        return Err::E_SUCCESS;
    }

public:
    Err frontAndPop(Value & result)
    {
        if (_active.empty()) {
            return Err::E_EQUEUE;
        }
        result = _active.front();
        _ready.push_back(_active.front());
        _active.pop_front();
        return Err::E_SUCCESS;
    }
};

namespace tr {

/**
 * ReuseQueue class prototype.
 *
 * @author zer0
 * @date   2016-08-03
 * @date   2017-11-14 (Use the std::shared_ptr)
 */
template <typename ValueType>
class ReuseQueue
{
public:
    using Value       = ValueType;
    using SharedValue = std::shared_ptr<Value>;
    using Queue       = std::deque<SharedValue>;

    using value_type             = typename Queue::value_type;
    using allocator_type         = typename Queue::allocator_type;
    using reference              = typename Queue::reference;
    using const_reference        = typename Queue::const_reference;
    using iterator               = typename Queue::iterator;
    using const_iterator         = typename Queue::const_iterator;
    using size_type              = typename Queue::size_type;
    using difference_type        = typename Queue::difference_type;
    using pointer                = typename Queue::pointer;
    using const_pointer          = typename Queue::const_pointer;
    using reverse_iterator       = typename Queue::reverse_iterator;
    using const_reverse_iterator = typename Queue::const_reverse_iterator;

    static_assert(is_shared_ptr<Value>::value == false,
                  "Value should not be std::shared_ptr type.");
    static_assert(std::is_same<SharedValue, value_type>::value,
                  "SharedValue must be the same type as value_type");

private:
    Queue _active;
    Queue _ready;

public:
    ReuseQueue() : _active(), _ready()
    { /* EMPTY. */ }

    ReuseQueue(ReuseQueue const & obj) : ReuseQueue()
    { (*this) = obj; }

    ReuseQueue(ReuseQueue && obj) : ReuseQueue()
    { (*this) = std::move(obj); }

    ~ReuseQueue()
    { /* EMPTY. */ }

public:
    ReuseQueue & operator =(ReuseQueue const & obj)
    {
        if (this != &obj) {
            _active = obj._active;
            _ready = obj._ready;
        }
        return *this;
    }

    ReuseQueue & operator =(ReuseQueue && obj)
    {
        swap(obj);
        return *this;
    }

public:
    void swap(ReuseQueue & obj)
    {
        if (this != &obj) {
            _active.swap(obj._active);
            _ready.swap(obj._ready);
        }
    }

    friend void swap(ReuseQueue & lh, ReuseQueue & rh)
    {
        lh.swap(rh);
    }

public:
    inline std::size_t size() const TBAG_NOEXCEPT_SP_OP(_active.size()) { return _active.size(); }
    inline bool empty() const TBAG_NOEXCEPT_SP_OP(_active.empty()) { return _active.empty(); }

public:
    inline std::size_t sizeOfReady() const TBAG_NOEXCEPT_SP_OP(_ready.size()) { return _ready.size(); }
    inline bool emptyOfReady() const TBAG_NOEXCEPT_SP_OP(_ready.empty()) { return _ready.empty(); }

public:
    inline Value       & front()       { return *(_active.front()); }
    inline Value const & front() const { return *(_active.front()); }

    inline Value       & back()       { return *(_active.back()); }
    inline Value const & back() const { return *(_active.back()); }

public:
    Value       * frontPointer()       { if (_active.empty()) { return nullptr; } return _active.front().get(); }
    Value const * frontPointer() const { if (_active.empty()) { return nullptr; } return _active.front().get(); }

    Value       * backPointer()       { if (_active.empty()) { return nullptr; } return _active.back().get(); }
    Value const * backPointer() const { if (_active.empty()) { return nullptr; } return _active.back().get(); }

public:
    // @formatter:off
          iterator begin()       TBAG_NOEXCEPT_SP_OP(_active.begin()) { return _active.begin(); }
    const_iterator begin() const TBAG_NOEXCEPT_SP_OP(_active.begin()) { return _active.begin(); }
          iterator   end()       TBAG_NOEXCEPT_SP_OP(_active.  end()) { return _active.  end(); }
    const_iterator   end() const TBAG_NOEXCEPT_SP_OP(_active.  end()) { return _active.  end(); }

          reverse_iterator rbegin()       TBAG_NOEXCEPT_SP_OP(_active.rbegin()) { return _active.rbegin(); }
    const_reverse_iterator rbegin() const TBAG_NOEXCEPT_SP_OP(_active.rbegin()) { return _active.rbegin(); }
          reverse_iterator   rend()       TBAG_NOEXCEPT_SP_OP(_active.  rend()) { return _active.  rend(); }
    const_reverse_iterator   rend() const TBAG_NOEXCEPT_SP_OP(_active.  rend()) { return _active.  rend(); }

            const_iterator  cbegin() const TBAG_NOEXCEPT_SP_OP(_active. cbegin()) { return _active. cbegin(); }
            const_iterator    cend() const TBAG_NOEXCEPT_SP_OP(_active.   cend()) { return _active.   cend(); }
    const_reverse_iterator crbegin() const TBAG_NOEXCEPT_SP_OP(_active.crbegin()) { return _active.crbegin(); }
    const_reverse_iterator   crend() const TBAG_NOEXCEPT_SP_OP(_active.  crend()) { return _active.  crend(); }
    // @formatter:on

public:
    template <typename ... Args>
    void push(Args && ... args)
    {
        if (_ready.empty()) {
            _active.emplace_back(new Value(std::forward<Args>(args) ...));
        } else {
            SharedValue temp = _ready.front();
            _ready.pop_front();
            *temp = Value(std::forward<Args>(args) ...);
            _active.push_back(temp);
        }
    }

    bool pop()
    {
        if (_active.empty()) {
            return false;
        }
        _ready.push_back(_active.front());
        _active.pop_front();
        return true;
    }

    void clear()
    {
        while (isSuccess(pop())) {
            // EMPTY.
        }
    }

    // ------------------
    // Extension methods.
    // ------------------

    template <typename Predicated>
    void pushWithNewCallback(Predicated if_new_callback)
    {
        SharedValue value;
        if (_ready.empty()) {
            value = SharedValue(if_new_callback());
        } else {
            value = _ready.front();
            _ready.pop_front();
        }
        _active.push_back(value);
    }

    bool frontAndPop(Value & result)
    {
        if (_active.empty()) {
            return false;
        }
        result = _active.front();
        _ready.push_back(_active.front());
        _active.pop_front();
        return true;
    }
};

} // namespace tr

} // namespace container

// --------------------
NAMESPACE_LIBTBAG_CLOSE
// --------------------

#endif // __INCLUDE_LIBTBAG__LIBTBAG_CONTAINER_REUSEQUEUE_HPP__

<commit_msg>Fixed bug in ReuseQueue::frontAndPop() method.<commit_after>/**
 * @file   ReuseQueue.hpp
 * @brief  ReuseQueue class prototype.
 * @author zer0
 * @date   2016-08-03
 */

#ifndef __INCLUDE_LIBTBAG__LIBTBAG_CONTAINER_REUSEQUEUE_HPP__
#define __INCLUDE_LIBTBAG__LIBTBAG_CONTAINER_REUSEQUEUE_HPP__

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <libtbag/config.h>
#include <libtbag/predef.hpp>
#include <libtbag/Err.hpp>
#include <libtbag/Type.hpp>

#include <deque>
#include <memory>
#include <utility>
#include <type_traits>

// -------------------
NAMESPACE_LIBTBAG_OPEN
// -------------------

namespace container {

/**
 * ReuseQueue class prototype.
 *
 * @author zer0
 * @date   2016-08-03
 */
template <typename ValueType>
class ReuseQueue
{
public:
    using Value = ValueType;
    using Queue = std::deque<Value>;

private:
    Queue _active;
    Queue _ready;

public:
    ReuseQueue() : _active(), _ready()
    {
        // EMPTY.
    }

    ReuseQueue(ReuseQueue const & obj) : ReuseQueue()
    {
        (*this) = obj;
    }

    ReuseQueue(ReuseQueue && obj) : ReuseQueue()
    {
        (*this) = std::move(obj);
    }

    ~ReuseQueue()
    {
        // EMPTY.
    }

public:
    ReuseQueue & operator =(ReuseQueue const & obj)
    {
        if (this != &obj) {
            _active = obj._active;
            _ready = obj._ready;
        }
        return *this;
    }

    ReuseQueue & operator =(ReuseQueue && obj)
    {
        if (this != &obj) {
            swap(obj);
        }
        return *this;
    }

public:
    void swap(ReuseQueue & obj)
    {
        _active.swap(obj._active);
        _ready.swap(obj._ready);
    }

    friend void swap(ReuseQueue & lh, ReuseQueue & rh)
    {
        lh.swap(rh);
    }

public:
    inline std::size_t size() const TBAG_NOEXCEPT_SP_OP(_active.empty())
    { return _active.size(); }
    inline bool empty() const TBAG_NOEXCEPT_SP_OP(_active.empty())
    { return _active.empty(); }

public:
    inline std::size_t sizeOfReadyQueue() const TBAG_NOEXCEPT_SP_OP(_ready.size())
    { return _ready.size(); }
    inline bool emptyOfReadyQueue() const TBAG_NOEXCEPT_SP_OP(_ready.empty())
    { return _ready.empty(); }

public:
    void clear()
    {
        _active.clear();
        _ready.clear();
    }

public:
    inline Value       & frontRef()       TBAG_NOEXCEPT_SP_OP(_active.front()) { return _active.front(); }
    inline Value const & frontRef() const TBAG_NOEXCEPT_SP_OP(_active.front()) { return _active.front(); }

    Err front(Value & result)
    {
        if (_active.empty()) {
            return Err::E_EQUEUE;
        }
        result = _active.front();
        return Err::E_SUCCESS;
    }

public:
    template <typename ... Args>
    Value & push(Args && ... args)
    {
        if (_ready.empty()) {
            _active.emplace_back(std::forward<Args>(args) ...);
        } else {
            _active.push_back(_ready.front());
            _ready.pop_front();
        }
        return _active.back();
    }

    Err pop()
    {
        if (_active.empty()) {
            return Err::E_EQUEUE;
        }
        _ready.push_back(_active.front());
        _active.pop_front();
        return Err::E_SUCCESS;
    }

public:
    Err frontAndPop(Value & result)
    {
        if (_active.empty()) {
            return Err::E_EQUEUE;
        }
        result = _active.front();
        _ready.push_back(_active.front());
        _active.pop_front();
        return Err::E_SUCCESS;
    }
};

namespace tr {

/**
 * ReuseQueue class prototype.
 *
 * @author zer0
 * @date   2016-08-03
 * @date   2017-11-14 (Use the std::shared_ptr)
 */
template <typename ValueType>
class ReuseQueue
{
public:
    using Value       = ValueType;
    using SharedValue = std::shared_ptr<Value>;
    using Queue       = std::deque<SharedValue>;

    using value_type             = typename Queue::value_type;
    using allocator_type         = typename Queue::allocator_type;
    using reference              = typename Queue::reference;
    using const_reference        = typename Queue::const_reference;
    using iterator               = typename Queue::iterator;
    using const_iterator         = typename Queue::const_iterator;
    using size_type              = typename Queue::size_type;
    using difference_type        = typename Queue::difference_type;
    using pointer                = typename Queue::pointer;
    using const_pointer          = typename Queue::const_pointer;
    using reverse_iterator       = typename Queue::reverse_iterator;
    using const_reverse_iterator = typename Queue::const_reverse_iterator;

    static_assert(is_shared_ptr<Value>::value == false,
                  "Value should not be std::shared_ptr type.");
    static_assert(std::is_same<SharedValue, value_type>::value,
                  "SharedValue must be the same type as value_type");

private:
    Queue _active;
    Queue _ready;

public:
    ReuseQueue() : _active(), _ready()
    { /* EMPTY. */ }

    ReuseQueue(ReuseQueue const & obj) : ReuseQueue()
    { (*this) = obj; }

    ReuseQueue(ReuseQueue && obj) : ReuseQueue()
    { (*this) = std::move(obj); }

    ~ReuseQueue()
    { /* EMPTY. */ }

public:
    ReuseQueue & operator =(ReuseQueue const & obj)
    {
        if (this != &obj) {
            _active = obj._active;
            _ready = obj._ready;
        }
        return *this;
    }

    ReuseQueue & operator =(ReuseQueue && obj)
    {
        swap(obj);
        return *this;
    }

public:
    void swap(ReuseQueue & obj)
    {
        if (this != &obj) {
            _active.swap(obj._active);
            _ready.swap(obj._ready);
        }
    }

    friend void swap(ReuseQueue & lh, ReuseQueue & rh)
    {
        lh.swap(rh);
    }

public:
    inline std::size_t size() const TBAG_NOEXCEPT_SP_OP(_active.size()) { return _active.size(); }
    inline bool empty() const TBAG_NOEXCEPT_SP_OP(_active.empty()) { return _active.empty(); }

public:
    inline std::size_t sizeOfReady() const TBAG_NOEXCEPT_SP_OP(_ready.size()) { return _ready.size(); }
    inline bool emptyOfReady() const TBAG_NOEXCEPT_SP_OP(_ready.empty()) { return _ready.empty(); }

public:
    inline Value       & front()       { return *(_active.front()); }
    inline Value const & front() const { return *(_active.front()); }

    inline Value       & back()       { return *(_active.back()); }
    inline Value const & back() const { return *(_active.back()); }

public:
    Value       * frontPointer()       { if (_active.empty()) { return nullptr; } return _active.front().get(); }
    Value const * frontPointer() const { if (_active.empty()) { return nullptr; } return _active.front().get(); }

    Value       * backPointer()       { if (_active.empty()) { return nullptr; } return _active.back().get(); }
    Value const * backPointer() const { if (_active.empty()) { return nullptr; } return _active.back().get(); }

public:
    // @formatter:off
          iterator begin()       TBAG_NOEXCEPT_SP_OP(_active.begin()) { return _active.begin(); }
    const_iterator begin() const TBAG_NOEXCEPT_SP_OP(_active.begin()) { return _active.begin(); }
          iterator   end()       TBAG_NOEXCEPT_SP_OP(_active.  end()) { return _active.  end(); }
    const_iterator   end() const TBAG_NOEXCEPT_SP_OP(_active.  end()) { return _active.  end(); }

          reverse_iterator rbegin()       TBAG_NOEXCEPT_SP_OP(_active.rbegin()) { return _active.rbegin(); }
    const_reverse_iterator rbegin() const TBAG_NOEXCEPT_SP_OP(_active.rbegin()) { return _active.rbegin(); }
          reverse_iterator   rend()       TBAG_NOEXCEPT_SP_OP(_active.  rend()) { return _active.  rend(); }
    const_reverse_iterator   rend() const TBAG_NOEXCEPT_SP_OP(_active.  rend()) { return _active.  rend(); }

            const_iterator  cbegin() const TBAG_NOEXCEPT_SP_OP(_active. cbegin()) { return _active. cbegin(); }
            const_iterator    cend() const TBAG_NOEXCEPT_SP_OP(_active.   cend()) { return _active.   cend(); }
    const_reverse_iterator crbegin() const TBAG_NOEXCEPT_SP_OP(_active.crbegin()) { return _active.crbegin(); }
    const_reverse_iterator   crend() const TBAG_NOEXCEPT_SP_OP(_active.  crend()) { return _active.  crend(); }
    // @formatter:on

public:
    template <typename ... Args>
    void push(Args && ... args)
    {
        if (_ready.empty()) {
            _active.emplace_back(new Value(std::forward<Args>(args) ...));
        } else {
            SharedValue temp = _ready.front();
            _ready.pop_front();
            *temp = Value(std::forward<Args>(args) ...);
            _active.push_back(temp);
        }
    }

    bool pop()
    {
        if (_active.empty()) {
            return false;
        }
        _ready.push_back(_active.front());
        _active.pop_front();
        return true;
    }

    void clear()
    {
        while (isSuccess(pop())) {
            // EMPTY.
        }
    }

    // ------------------
    // Extension methods.
    // ------------------

    template <typename Predicated>
    void pushWithNewCallback(Predicated if_new_callback)
    {
        SharedValue value;
        if (_ready.empty()) {
            value = SharedValue(if_new_callback());
        } else {
            value = _ready.front();
            _ready.pop_front();
        }
        _active.push_back(value);
    }

    bool frontAndPop(Value & result)
    {
        if (_active.empty()) {
            return false;
        }
        result = *_active.front();
        _ready.push_back(_active.front());
        _active.pop_front();
        return true;
    }
};

} // namespace tr

} // namespace container

// --------------------
NAMESPACE_LIBTBAG_CLOSE
// --------------------

#endif // __INCLUDE_LIBTBAG__LIBTBAG_CONTAINER_REUSEQUEUE_HPP__

<|endoftext|>