JeeLabs Café

//>>> The latest version of this code can be found at https://github.com/jcw/ !!

// Ports library definitions

// 2009-02-13 <jcw@equi4.com> http://opensource.org/licenses/mit-license.php

// $Id: Ports.cpp 7763 2011-12-11 01:28:16Z jcw $

#include "Ports.h"

#include <avr/sleep.h>

#include <util/atomic.h>

// flag bits sent to the receiver

#define MODE_CHANGE 0x80    // a pin mode was changed

#define DIG_CHANGE  0x40    // a digital output was changed

#define PWM_CHANGE  0x30    // an analog (pwm) value was changed on port 2..3

#define ANA_MASK    0x0F    // an analog read was requested on port 1..4

uint16_t Port::shiftRead(uint8_t bitOrder, uint8_t count) const {

    uint16_t value = 0, mask = bit(LSBFIRST ? 0 : count - 1);

    for (uint8_t i = 0; i < count; ++i) {

        digiWrite2(1);

        delayMicroseconds(5);

        if (digiRead())

            value |= mask;

        if (bitOrder == LSBFIRST)

            mask <<= 1;

        else

            mask >>= 1;

        digiWrite2(0);

        delayMicroseconds(5);

    }

    return value;

}

void Port::shiftWrite(uint8_t bitOrder, uint16_t value, uint8_t count) const {

    uint16_t mask = bit(LSBFIRST ? 0 : count - 1);

    for (uint8_t i = 0; i < count; ++i) {

        digiWrite((value & mask) != 0);

        if (bitOrder == LSBFIRST)

            mask <<= 1;

        else

            mask >>= 1;

        digiWrite2(1);

        digiWrite2(0);

    }

}

RemoteNode::RemoteNode (char id, uint8_t band, uint8_t group) 

    : nid (id & 0x1F)

{

    memset(&data, 0, sizeof data);

    RemoteHandler::setup(nid, band, group);

}

void RemoteNode::poll(uint16_t msecs) {

    uint8_t pending = millis() >= lastPoll + msecs;

    if (RemoteHandler::poll(*this, pending))

        lastPoll = millis();

}

void RemotePort::mode(uint8_t value) const {

    node.data.flags |= MODE_CHANGE;

    bitWrite(node.data.modes, pinBit(), value);

}

uint8_t RemotePort::digiRead() const {

    return bitRead(node.data.digiIO, pinBit());

}

void RemotePort::digiWrite(uint8_t value) const {

    node.data.flags |= DIG_CHANGE;

    bitWrite(node.data.digiIO, pinBit(), value);

}

void RemotePort::anaWrite(uint8_t val) const {

    if (portNum == 2 || portNum == 3) {

        bitSet(node.data.flags, portNum + 2);

        node.data.anaOut[portNum - 2] = val;

    } else

        digiWrite2(val >= 128);

}

void RemotePort::mode2(uint8_t value) const {

    node.data.flags |= MODE_CHANGE;

    bitWrite(node.data.modes, pinBit2(), value);

}

uint16_t RemotePort::anaRead() const {

    bitSet(node.data.flags, pinBit());

    return node.data.anaIn[pinBit()];

}

uint8_t RemotePort::digiRead2() const {

    return bitRead(node.data.digiIO, pinBit2());

}

void RemotePort::digiWrite2(uint8_t value) const {

    node.data.flags |= DIG_CHANGE;

    bitWrite(node.data.digiIO, pinBit2(), value);

}

PortI2C::PortI2C (uint8_t num, uint8_t rate)

    : Port (num), uswait (rate)

{

    sdaOut(1);

    mode2(OUTPUT);

    sclHi();

}

uint8_t PortI2C::start(uint8_t addr) const {

    sclLo();

    sclHi();

    sdaOut(0);

    return write(addr);

}

void PortI2C::stop() const {

    sdaOut(0);

    sclHi();

    sdaOut(1);

}

uint8_t PortI2C::write(uint8_t data) const {

    sclLo();

    for (uint8_t mask = 0x80; mask != 0; mask >>= 1) {

        sdaOut(data & mask);

        sclHi();

        sclLo();

    }

    sdaOut(1);

    sclHi();

    uint8_t ack = ! sdaIn();

    sclLo();

    return ack;

}

uint8_t PortI2C::read(uint8_t last) const {

    uint8_t data = 0;

    for (uint8_t mask = 0x80; mask != 0; mask >>= 1) {

        sclHi();

        if (sdaIn())

            data |= mask;

        sclLo();

    }

    sdaOut(last);

    sclHi();

    sclLo();

    if (last)

        stop();

    sdaOut(1);

    return data;

}

bool DeviceI2C::isPresent () const {

    byte ok = send();

    stop();

    return ok;

}

byte MilliTimer::poll(word ms) {

    byte ready = 0;

    if (armed) {

        word remain = next - millis();

        // since remain is unsigned, it will overflow to large values when

        // the timeout is reached, so this test works as long as poll() is

        // called no later than 5535 millisecs after the timer has expired

        if (remain <= 60000)

            return 0;

        // return a value between 1 and 255, being msecs+1 past expiration

        // note: the actual return value is only reliable if poll() is

        // called no later than 255 millisecs after the timer has expired

        ready = -remain;

    }

    set(ms);

    return ready;

}

word MilliTimer::remaining() const {

    word remain = armed ? next - millis() : 0;

    return remain <= 60000 ? remain : 0;

}

void MilliTimer::set(word ms) {

    armed = ms != 0;

    if (armed)

        next = millis() + ms - 1;

}

void BlinkPlug::ledOn (byte mask) {

    if (mask & 1) {

        digiWrite(0);

        mode(OUTPUT);

    }

    if (mask & 2) {

        digiWrite2(0);

        mode2(OUTPUT);

    }

    leds |= mask; //TODO could be read back from pins, i.s.o. saving here

}

void BlinkPlug::ledOff (byte mask) {

    if (mask & 1) {

        mode(INPUT);

        digiWrite(1);

    }

    if (mask & 2) {

        mode2(INPUT);

        digiWrite2(1);

    }

    leds &= ~ mask; //TODO could be read back from pins, i.s.o. saving here

}

byte BlinkPlug::state () {

    byte saved = leds;

    ledOff(1+2);

    byte result = !digiRead() | (!digiRead2() << 1);

    ledOn(saved);

    return result;

}

//TODO deprecated, use buttonCheck() !

byte BlinkPlug::pushed () {

    if (debounce.idle() || debounce.poll()) {

        byte newState = state();

        if (newState != lastState) {

            debounce.set(100); // don't check again for at least 100 ms

            byte nowOn = (lastState ^ newState) & newState;

            lastState = newState;

            return nowOn;

        }

    }

    return 0;

}

byte BlinkPlug::buttonCheck () {

    // collect button changes in the checkFlags bits, with proper debouncing

    if (debounce.idle() || debounce.poll()) {

        byte newState = state();

        if (newState != lastState) {

            debounce.set(100); // don't check again for at least 100 ms

            if ((lastState ^ newState) & 1)

                bitSet(checkFlags, newState & 1 ? ON1 : OFF1);

            if ((lastState ^ newState) & 2)

                bitSet(checkFlags, newState & 2 ? ON2 : OFF2);

            lastState = newState;

        }

    }

    // note that simultaneous button events will be returned in successive calls

    if (checkFlags)

        for (byte i = ON1; i <= OFF2; ++i) {

            if (bitRead(checkFlags, i)) {

                bitClear(checkFlags, i);

                return i;

            }

        }

    // if there are no button events, return the overall current button state

    return lastState == 3 ? ALL_ON : lastState ? SOME_ON : ALL_OFF;

}

void MemoryPlug::load (word page, void* buf, byte offset, int count) {

    setAddress(0x50 + (page >> 8));

    send();

    write((byte) page);

    write(offset);

    receive();

    byte* p = (byte*) buf;

    while (--count >= 0)

        *p++ = read(count == 0);

    stop();

}

void MemoryPlug::save (word page, const void* buf, byte offset, int count) {

    // don't do back-to-back saves, last one must have had time to finish!

    while (millis() < nextSave)

        ;

    setAddress(0x50 + (page >> 8));

    send();

    write((byte) page);

    write(offset);

    const byte* p = (const byte*) buf;

    while (--count >= 0)

        write(*p++);

    stop();

    nextSave = millis() + 6;

    // delay(5);

}

long MemoryStream::position (byte writing) const {

    long v = (curr - start) * step;

    if (pos > 0 && !writing)

        --v; // get() advances differently than put()

    return (v << 8) | pos;

}

byte MemoryStream::get () {

    if (pos == 0) {

        dev.load(curr, buffer);

        curr += step;

    }

    return buffer[pos++];

}

void MemoryStream::put (byte data) {

    buffer[pos++] = data;

    if (pos == 0) {

        dev.save(curr, buffer);

        curr += step;

    }

}

word MemoryStream::flush () {

    if (pos != 0) {

        memset(buffer + pos, 0xFF, 256 - pos);

        dev.save(curr, buffer);

    }

    return curr;

}

void MemoryStream::reset () {

    curr = start;

    pos = 0;

}

// uart register definitions

#define RHR     (0 << 3)

#define THR     (0 << 3)

#define DLL     (0 << 3)

#define DLH     (1 << 3)

#define FCR     (2 << 3)

#define LCR     (3 << 3)

#define RXLVL   (9 << 3)

void UartPlug::regSet (byte reg, byte value) {

  dev.send();

  dev.write(reg);

  dev.write(value);

}

void UartPlug::regRead (byte reg) {

  dev.send();

  dev.write(reg);

  dev.receive();

}

void UartPlug::begin (long baud) {

    word divisor = 230400 / baud;

    regSet(LCR, 0x80);          // divisor latch enable

    regSet(DLL, divisor);       // low byte

    regSet(DLH, divisor >> 8);  // high byte

    regSet(LCR, 0x03);          // 8 bits, no parity

    regSet(FCR, 0x07);          // fifo enable (and flush)

    dev.stop();

}

byte UartPlug::available () {

    if (in != out)

        return 1;

    out = 0;

    regRead(RXLVL);

    in = dev.read(1);

    if (in == 0)

        return 0;

    if (in > sizeof rxbuf)

        in = sizeof rxbuf;

    regRead(RHR);

    for (byte i = 0; i < in; ++i)

        rxbuf[i] = dev.read(i == in - 1);

    return 1;

}

int UartPlug::read () {

    return available() ? rxbuf[out++] : -1;

}

void UartPlug::flush () {

    regSet(FCR, 0x07); // flush both RX and TX queues

    dev.stop();

    in = out;

}

void UartPlug::write (byte data) {

    regSet(THR, data);

    dev.stop();

}

void DimmerPlug::begin () {

    setReg(MODE1, 0x00);     // normal

    setReg(MODE2, 0x14);     // inverted, totem-pole

    setReg(GRPPWM, 0xFF);    // set group dim to max brightness

    setMulti(LEDOUT0, 0xFF, 0xFF, 0xFF, 0xFF, -1); // all LEDs group-dimmable

}

byte DimmerPlug::getReg(byte reg) const {

    send();

    write(reg);

    receive();

    byte result = read(1);

    stop();

    return result;

}

void DimmerPlug::setReg(byte reg, byte value) const {

    send();

    write(reg);

    write(value);

    stop();

}

void DimmerPlug::setMulti(byte reg, ...) const {

    va_list ap;

    va_start(ap, reg);

    send();

    write(0xE0 | reg); // auto-increment

    for (;;) {

        int v = va_arg(ap, int);

        if (v < 0) break;

        write(v);

    }

    stop();

}

void LuxPlug::setGain(byte high) {

    send();

    write(0x81); // write to Timing regiser

    write(high ? 0x12 : 0x02);

    stop();

}

const word* LuxPlug::getData() {

    send();

    write(0xA0 | DATA0LOW);

    receive();

    data.b[0] = read(0);

    data.b[1] = read(0);

    data.b[2] = read(0);

    data.b[3] = read(1);

    stop();

    return data.w;

}

#define LUX_SCALE        14        // scale by 2^14 

#define RATIO_SCALE 9        // scale ratio by 2^9

#define CH_SCALE    10        // scale channel values by 2^10 

word LuxPlug::calcLux(byte iGain, byte tInt) const

{

    unsigned long chScale; 

    switch (tInt) {

        case 0:  chScale = 0x7517; break; 

        case 1:  chScale = 0x0fe7; break; 

        default: chScale = (1 << CH_SCALE); break;

    }

    if (!iGain)

        chScale <<= 4;

    unsigned long channel0 = (data.w[0] * chScale) >> CH_SCALE; 

    unsigned long channel1 = (data.w[1] * chScale) >> CH_SCALE; 

    unsigned long ratio1 = 0; 

    if (channel0 != 0)

        ratio1 = (channel1 << (RATIO_SCALE+1)) / channel0;

    unsigned long ratio = (ratio1 + 1) >> 1;

    word b, m;

         if (ratio <= 0x0040) { b = 0x01F2; m = 0x01BE; } 

    else if (ratio <= 0x0080) { b = 0x0214; m = 0x02D1; } 

    else if (ratio <= 0x00C0) { b = 0x023F; m = 0x037B; } 

    else if (ratio <= 0x0100) { b = 0x0270; m = 0x03FE; } 

    else if (ratio <= 0x0138) { b = 0x016F; m = 0x01FC; }

    else if (ratio <= 0x019A) { b = 0x00D2; m = 0x00FB; }

    else if (ratio <= 0x029A) { b = 0x0018; m = 0x0012; }

    else                      { b = 0x0000; m = 0x0000; }

    unsigned long temp = channel0 * b - channel1 * m;

    temp += 1 << (LUX_SCALE-1);

    return temp >> LUX_SCALE;

}

const int* GravityPlug::getAxes() {

    send();

    write(0x02);

    receive();

    for (byte i = 0; i < 5; ++i)

        data.b[i] = read(0);

    data.b[5] = read(1);

    stop();

    data.w[0] = (data.b[0] >> 6) | (data.b[1] << 2);

    data.w[1] = (data.b[2] >> 6) | (data.b[3] << 2);

    data.w[2] = (data.b[4] >> 6) | (data.b[5] << 2);

    for (byte i = 0; i < 3; ++i)

        data.w[i] = (data.w[i] ^ 0x200) - 0x200; // sign extends bit 9

    return data.w;

}

void InputPlug::select(uint8_t channel) {

    digiWrite(0);

    mode(OUTPUT);

    delayMicroseconds(slow ? 400 : 50);

    byte data = 0x10 | (channel & 0x0F);

    byte mask = 1 << (portNum + 3); // digitalWrite is too slow

    ATOMIC_BLOCK(ATOMIC_FORCEON) {

        for (byte i = 0; i < 5; ++i) {

            byte us = bitRead(data, 4 - i) ? 9 : 3;

            if (slow)

                us <<= 3;

#ifdef PORTD

            PORTD |= mask;

            delayMicroseconds(us);

            PORTD &= ~ mask;

#else

            //XXX TINY!

#endif

            delayMicroseconds(slow ? 32 : 4);

        }

    }

}

byte HeadingBoard::eepromByte(byte reg) const {

    eeprom.send();

    eeprom.write(reg);

    eeprom.receive();

    byte result = eeprom.read(1);

    eeprom.stop();

    return result;

}

void HeadingBoard::getConstants() {

    for (byte i = 0; i < 18; ++i)

        ((byte*) &C1)[i < 14 ? i^1 : i] = eepromByte(16 + i);

    // Serial.println(C1);

    // Serial.println(C2);

    // Serial.println(C3);

    // Serial.println(C4);

    // Serial.println(C5);

    // Serial.println(C6);

    // Serial.println(C7);

    // Serial.println(A, DEC);

    // Serial.println(B, DEC);

    // Serial.println(C, DEC);

    // Serial.println(D, DEC);

}

word HeadingBoard::adcValue(byte press) const {

    aux.digiWrite(1);

    adc.send();

    adc.write(0xFF);

    adc.write(0xE0 | (press << 4));

    adc.stop();

    delay(40);

    adc.send();

    adc.write(0xFD);

    adc.receive();

    byte msb = adc.read(0);

    int result = (msb << 8) | adc.read(1);

    adc.stop();

    aux.digiWrite(0);

    return result;

}

void HeadingBoard::begin() {

    // prepare ADC

    aux.mode(OUTPUT);

    aux.digiWrite(0);

    // generate 32768 Hz on IRQ pin (OC2B)

#ifdef TCCR2A

    TCCR2A = bit(COM2B0) | bit(WGM21);

    TCCR2B = bit(CS20);

    OCR2A = 243;

#else

    //XXX TINY!

#endif

    aux.mode3(OUTPUT);

    getConstants();

}

void HeadingBoard::pressure(int& temp, int& pres) const {

    word D2 = adcValue(0);

    // Serial.print("D2 = ");

    // Serial.println(D2);

    int corr = (D2 - C5) >> 7;        

    // Serial.print("corr = ");

    // Serial.println(corr);

    int dUT = (D2 - C5) - (corr * (long) corr * (D2 >= C5 ? A : B) >> C);

    // Serial.print("dUT = ");

    // Serial.println(dUT);

    temp = 250 + (dUT * C6 >> 16) - (dUT >> D); 

    word D1 = adcValue(1);

    // Serial.print("D1 = ");

    // Serial.println(D1);

    word OFF = (C2 + ((C4 - 1024) * dUT >> 14)) << 2;

    // Serial.print("OFF = ");

    // Serial.println(OFF);

    word SENS = C1 + (C3 * dUT >> 10);

    // Serial.print("SENS = ");

    // Serial.println(SENS);

    word X = (SENS * (D1 - 7168L) >> 14) - OFF;

    // Serial.print("X = ");

    // Serial.println(X);

    pres = (X * 10L >> 5) + C7;

}

void HeadingBoard::heading(int& xaxis, int& yaxis) {

    // set or reset the magnetometer coil

    compass.send();

    compass.write(0x00);

    compass.write(setReset);

    compass.stop();

    delayMicroseconds(50);

    setReset = 6 - setReset;

    // perform measurement

    compass.send();

    compass.write(0x00);

    compass.write(0x01);

    compass.stop();

    delay(5);

    compass.send();

    compass.write(0x00);

    compass.receive();

    byte tmp, reg = compass.read(0);

    tmp = compass.read(0);

    xaxis = ((tmp << 8) | compass.read(0)) - 2048;

    tmp = compass.read(0);

    yaxis = ((tmp << 8) | compass.read(1)) - 2048;

    compass.stop();

}

InfraredPlug::InfraredPlug (uint8_t num)

        : Port (num), slot (140), gap (80), fill (-1), prev (0) {

    digiWrite(0);

    mode(OUTPUT);

    mode2(INPUT);

    digiWrite2(1); // pull-up        

}

void InfraredPlug::configure(uint8_t slot4, uint8_t gap256) {

    slot = slot4;

    gap = gap256;

    fill = -1;

}

void InfraredPlug::poll() {

    byte bit = digiRead2(); // 0 is interpreted as pulse ON

    if (fill < 0) {

        if (fill < -1 || bit == 1)

            return;

        fill = 0;

        prev = micros();

        memset(buf, 0, sizeof buf);

    }

    // act only if the bit changed, using the low bit of the nibble fill count

    if (bit != (fill & 1) && fill < 2 * sizeof buf) {

        uint32_t curr = micros(), diff = (curr - prev + 2) >> 2;

        if (diff > 65000)

            diff = 65000; // * 4 us, i.e. 260 ms

        // convert to a slot number, with rounding halfway between each slot

        word ticks = ((word) diff + slot / 2) / slot;

        if (ticks > 20)

            ticks = 20;

        // condense upper values to fit in the range 0..15

        byte nibble = ticks;

        if (nibble > 10)

            nibble -= (nibble - 10) / 2;

        buf[fill>>1] |= nibble << ((fill & 1) << 2);

        ++fill;

        prev = curr;

    }

}

uint8_t InfraredPlug::done() {

    byte result = 0;

    if (fill > 0)

        ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {

            if (((micros() - prev) >> 8) >= gap) {

                result = fill;

                fill = -2; // prevent new pulses from clobbering buf

            }

        }

    else if (fill < -1)

        fill = -1; // second call to done() release buffer again for capture

    return result;

}

uint8_t InfraredPlug::decoder(uint8_t nibbles) {

    switch (nibbles) {

        case 67: // 2 + 64 + 1 nibbles could be a NEC packet

            if (buf[0] == 0x8D && buf[33] == 0x01) {

                // check that all nibbles are either 1 or 3

                for (byte i = 1; i < 33; ++i)

                    if ((buf[i] & ~0x20) != 0x11)

                        return UNKNOWN;

                // valid packet, convert in-place

                for (byte i = 0; i < 4; ++i) {

                    byte v;

                    for (byte j = 0; j < 8; ++j)

                        v = (v << 1) | (buf[1+j+8*i] >> 5);

                    buf[i] = v;

                }

                return NEC;

            }

            break;

        case 3: // 2 + 1 nibbles could be a NEC repeat packet

            if (buf[0] == 0x4D && buf[1] == 0x01)

                return NEC_REP;

            break;

    }

    return UNKNOWN;

}

void InfraredPlug::send(const uint8_t* data, uint16_t bits) {

    // TODO: switch to an interrupt-driven design

    for (byte i = 0; i < bits; ++i) {

        digiWrite(bitRead(data[i/8], i%8));

        delayMicroseconds(4 * slot);

    }

    digiWrite(0);

}

void ProximityPlug::begin() {

    delay(100);

    setReg(CONFIG, 0x04);   // reset, STOP1

    delay(100);

    // setReg(TPCONFIG, 0xB5); // TPSE, BKA, ACE, TPTBE, TPE

    setReg(TPCONFIG, 0xB1); // TPSE, BKA, ACE, TPE

    setReg(CONFIG, 0x15);   // RUN1

    delay(100);

}

void ProximityPlug::setReg(byte reg, byte value) const {

    send();

    write(reg);

    write(value);

    stop();

}

byte ProximityPlug::getReg(byte reg) const {

    send();

    write(reg);

    receive();

    byte result = read(1);

    stop();

    return result;

}

// ISR(WDT_vect) { Sleepy::watchdogEvent(); }

static volatile byte watchdogCounter;

void Sleepy::watchdogInterrupts (char mode) {

    // correct for the fact that WDP3 is *not* in bit position 3!

    if (mode & bit(3))

        mode ^= bit(3) | bit(WDP3);

    // pre-calculate the WDTCSR value, can't do it inside the timed sequence

    // we only generate interrupts, no reset

    byte wdtcsr = mode >= 0 ? bit(WDIE) | mode : 0;

    MCUSR &= ~(1<<WDRF);

    ATOMIC_BLOCK(ATOMIC_FORCEON) {

        WDTCSR |= (1<<WDCE) | (1<<WDE); // timed sequence

        WDTCSR = wdtcsr;

    }

}

void Sleepy::powerDown (byte prrOff) {

    byte adcsraSave = ADCSRA;

    ADCSRA &= ~ bit(ADEN); // disable the ADC

#ifdef PRR

    byte prrSave = PRR;

    PRR = prrOff;

#endif

    // see http://www.nongnu.org/avr-libc/user-manual/group__avr__sleep.html

    set_sleep_mode(SLEEP_MODE_PWR_DOWN);

    ATOMIC_BLOCK(ATOMIC_FORCEON) {

    sleep_enable();

    // sleep_bod_disable(); // can't use this - not in my avr-libc version!

#ifdef BODSE

    MCUCR = MCUCR | bit(BODSE) | bit(BODS); // timed sequence

    MCUCR = MCUCR & ~ bit(BODSE) | bit(BODS);

#endif

    }

    sleep_cpu();

    sleep_disable();

    // re-enable what we disabled

#ifdef PRR

    PRR = prrSave;

#endif

    ADCSRA = adcsraSave;

}

byte Sleepy::loseSomeTime (word msecs) {

    // only slow down for periods longer than the watchdog granularity

    while (msecs >= 16) {

        char wdp = 0; // wdp 0..9 corresponds to roughly 16..8192 ms

        while (msecs >= (32 << wdp) && wdp < 9)

            ++wdp;

        watchdogCounter = 0;

        watchdogInterrupts(wdp);

        powerDown();

        watchdogInterrupts(-1); // off

        if (watchdogCounter == 0)

            return 0; // lost some time, but got interrupted

        // adjust the milli ticks, since we will have missed several

        extern volatile unsigned long timer0_millis;

        timer0_millis += 16 << wdp;

        msecs -= 16 << wdp;

    }

    return 1; // lost some time as planned

}

void Sleepy::watchdogEvent() {

    ++watchdogCounter;

}

Scheduler::Scheduler (byte size) : maxTasks (size) {

    byte bytes = size * sizeof *tasks;

    tasks = (word*) malloc(bytes);

    memset(tasks, 0xFF, bytes);

}

Scheduler::Scheduler (word* buf, byte size) : tasks (buf), maxTasks (size) {

    byte bytes = size * sizeof *tasks;

    memset(tasks, 0xFF, bytes);

}

char Scheduler::poll() {

    // all times in the tasks array are relative to the "remaining" value

    // i.e. only remaining counts down while waiting for the next timeout

    if (remaining == 0) {

        word lowest = ~0;

        for (byte i = 0; i < maxTasks; ++i) {

            if (tasks[i] == 0) {

                tasks[i] = ~0;

                return i;

            }

            if (tasks[i] < lowest)

                lowest = tasks[i];

        }

        if (lowest != ~0)

            for (byte i = 0; i < maxTasks; ++i)

                tasks[i] -= lowest;

        remaining = lowest;

    } else if (ms100.poll(100))

        --remaining;

    return -1;

}

char Scheduler::pollWaiting() {

    // first wait until the remaining time we need to wait is less than 0.1s

    while (remaining > 0) {

        if (!Sleepy::loseSomeTime(100)) // approximate, actually waits 96 ms

            return -1;

        --remaining;

    }

    // now lose some more time until that 0.1s mark

    if (!Sleepy::loseSomeTime(ms100.remaining()))

        return -1;

    // lastly, just ignore the 0..15 ms still left to go until the 0.1s mark

    return poll();

}

void Scheduler::timer(byte task, word tenths) {

    // if new timer will go off sooner than the rest, then adjust all entries

    if (tenths < remaining) {

        word diff = remaining - tenths;

        for (byte i = 0; i < maxTasks; ++i)

            if (tasks[i] != ~0)

                tasks[i] += diff;

        remaining = tenths;

    }

    tasks[task] = tenths - remaining;

}

void Scheduler::cancel(byte task) {

    tasks[task] = ~0;

}

#ifdef Stream_h // only available in recent Arduino IDE versions

InputParser::InputParser (byte* buf, byte size, Commands* ctab, Stream& stream)

        : buffer (buf), limit (size), cmds (ctab), io (stream) {

    reset();

}

InputParser::InputParser (byte size, Commands* ctab, Stream& stream)

        : limit (size), cmds (ctab), io (stream) {

    buffer = (byte*) malloc(size);

    reset();

}

void InputParser::reset() {

    fill = next = 0;

    instring = hexmode = hasvalue = 0;

    top = limit;

}

void InputParser::poll() {

    if (!io.available())

        return;

    char ch = io.read();

    if (ch < ' ' || fill >= top) {

        reset();

        return;

    }

    if (instring) {

        if (ch == '"') {

            buffer[fill++] = 0;

            do

                buffer[--top] = buffer[--fill];

            while (fill > value);

            ch = top;

            instring = 0;

        }

        buffer[fill++] = ch;

        return;

    }

    if (hexmode && ('0' <= ch && ch <= '9' ||

                    'A' <= ch && ch <= 'F' ||

                    'a' <= ch && ch <= 'f')) {

        if (!hasvalue)

            value = 0;

        if (ch > '9')

            ch += 9;

        value <<= 4;

        value |= (byte) (ch & 0x0F);

        hasvalue = 1;

        return;

    }

    if ('0' <= ch && ch <= '9') {

        if (!hasvalue)

            value = 0;

        value = 10 * value + (ch - '0');

        hasvalue = 1;

        return;

    }

    hexmode = 0;

    switch (ch) {

        case '$':   hexmode = 1;

                    return;

        case '"':   instring = 1;

                    value = fill;

                    return;

        case ':':   (word&) buffer[fill] = value;

                    fill += 2;

                    value >>= 16;

                    // fall through

        case '.':   (word&) buffer[fill] = value;

                    fill += 2;

                    hasvalue = 0;

                    return;

        case '-':   value = - value;

                    hasvalue = 0;

                    return;

        case ' ':   if (!hasvalue)

                        return;

                    // fall through

        case ',':   buffer[fill++] = value;

                    hasvalue = 0;

                    return;

    }

    if (hasvalue) {

        io.print("Unrecognized character: ");

        io.print(ch);

        io.println();

        reset();

        return;

    }

    for (Commands* p = cmds; ; ++p) {

        char code = pgm_read_byte(&p->code);

        if (code == 0)

            break;

        if (ch == code) {

            byte bytes = pgm_read_byte(&p->bytes);

            if (fill < bytes) {

                io.print("Not enough data, need ");

                io.print((int) bytes);

                io.println(" bytes");

            } else {

                memset(buffer + fill, 0, top - fill);

                ((void (*)()) pgm_read_word(&p->fun))();

            }

            reset();

            return;

        }

    }

    io.print("Known commands:");

    for (Commands* p = cmds; ; ++p) {

        char code = pgm_read_byte(&p->code);

        if (code == 0)

            break;

        io.print(' ');

        io.print(code);

    }

    io.println();

}

InputParser& InputParser::get(void* ptr, byte len) {

    memcpy(ptr, buffer + next, len);

    next += len;

    return *this;

}

InputParser& InputParser::operator >> (const char*& v) {

    byte offset = buffer[next++];

    v = top <= offset && offset < limit ? (char*) buffer + offset : "";

    return *this;

}

#endif // Stream_h