motion.h

/* -*- mode: C; c-basic-offset: 4; intent-tabs-mode: nil -*-
 *
 * Sifteo SDK
 *
 * Copyright <c> 2012 Sifteo, Inc.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#pragma once
#ifdef NOT_USERSPACE
#   error This is a userspace-only header, not allowed by the current build.
#endif

#include <sifteo/abi.h>
#include <sifteo/cube.h>
#include <sifteo/math.h>

namespace Sifteo {

template < unsigned tSize = 32 >
struct MotionBuffer {
    struct SysType {
        _SYSMotionBufferHeader header;
        _SYSByte4 samples[tSize];
    } sys;

    static const unsigned TICK_NS = _SYS_MOTION_TIMESTAMP_NS;

    static const unsigned TICK_US = _SYS_MOTION_TIMESTAMP_NS / 1000;

    static const unsigned TICK_HZ = _SYS_MOTION_TIMESTAMP_HZ;

    // Implicit conversions
    operator _SYSMotionBuffer* () { return reinterpret_cast<_SYSMotionBuffer*>(&sys); }
    operator const _SYSMotionBuffer* () const { return reinterpret_cast<const _SYSMotionBuffer*>(&sys); }

    void attach(_SYSCubeID id, unsigned hz=100)
    {
        STATIC_ASSERT(id < _SYS_NUM_CUBE_SLOTS);
        STATIC_ASSERT(tSize <= _SYS_MOTION_MAX_ENTRIES);
        bzero(sys);
        sys.header.last = tSize - 1;
        sys.header.rate = TICK_HZ / hz;

        /*
         * If this ASSERT fails, the buffer is too close to the top of RAM. This
         * usually means you've allocated the MotionBuffer on the stack, which isn't
         * recommended. See the warning in the MotionBuffer class comments.
         */
        ASSERT(reinterpret_cast<uintptr_t>(this) + sizeof(_SYSMotionBuffer) <= 0x18000);

        _SYS_setMotionBuffer(id, *this);
    }

    Int3 integrate(unsigned duration)
    {
        _SYSInt3 result;
        _SYS_motion_integrate(*this, duration, &result);
        return vec(result.x, result.y, result.z);
    }
};

class MotionIterator {
private:
    _SYSMotionBuffer *buffer;
    uint32_t tickCounter;
    Byte3 lastAccel;
    uint8_t head;

public:
    static const unsigned TICK_NS = MotionBuffer<>::TICK_NS;

    static const unsigned TICK_US = MotionBuffer<>::TICK_US;

    static const unsigned TICK_HZ = MotionBuffer<>::TICK_HZ;

    MotionIterator(_SYSMotionBuffer *buffer)
        : buffer(buffer), tickCounter(0), lastAccel(vec(0,0,0)),
          head(buffer->header.tail) {}

    bool next()
    {
        unsigned tail = buffer->header.tail;
        unsigned last = buffer->header.last;
        unsigned head = this->head;

        if (head > last)
            head = 0;

        if (head == tail)
            return false;

        _SYSByte4 sample = buffer->samples[head];
        this->head = head + 1;

        lastAccel = vec(sample.x, sample.y, sample.z);
        tickCounter += unsigned(uint8_t(sample.w)) + 1;

        return true;
    }

    Byte3 accel() const {
        return lastAccel;
    }

    uint32_t ticks() const {
        return tickCounter;
    }

    float seconds() const {
        return ticks() * (1.0f / TICK_HZ);
    }

    void setTicks(uint32_t value) {
        tickCounter = value;
    }

    void adjustTicks(int32_t value) {
        tickCounter += value;
    }
};

class MotionMedian {
public:
    _SYSMotionMedian sys;

    // Implicit conversions
    operator _SYSMotionMedian* () { return &sys; }
    operator const _SYSMotionMedian* () const { return &sys; }

    void calculate(_SYSMotionBuffer *mbuf, unsigned duration) {
        _SYS_motion_median(mbuf, duration, *this);
    }

    MotionMedian() {}

    MotionMedian(_SYSMotionBuffer *mbuf, unsigned duration) {
        calculate(mbuf, duration);
    }

    Byte3 median() const {
        return vec(sys.axes[0].median, sys.axes[1].median, sys.axes[2].median);
    }

    Byte3 minimum() const {
        return vec(sys.axes[0].minimum, sys.axes[1].minimum, sys.axes[2].minimum);
    }

    Byte3 maximum() const {
        return vec(sys.axes[0].maximum, sys.axes[1].maximum, sys.axes[2].maximum);
    }

    Int3 range() const {
        return Int3(maximum()) - Int3(minimum());
    }
};


class TiltShakeRecognizer {
public:
    static const int kFilterLatency = MotionBuffer<>::TICK_HZ / 30;
    static const int kTiltThresholdMin = 15;
    static const int kTiltThresholdMax = 26;
    static const int kShakeThresholdMin = 1000;
    static const int kShakeThresholdMax = 50000;

    MotionBuffer<> buffer;

    MotionMedian median;

    Byte3 tilt;

    bool shake;

    void attach(_SYSCubeID id)
    {
        buffer.attach(id);
        bzero(median);
        bzero(tilt);
        shake = false;
    }

    Byte3 physicalTilt() const {
        return tilt;
    }

    Byte3 virtualTilt(Side orientation) const {
        return tilt.zRotateI(orientation);
    }

    enum ChangeFlags {
        Shake_Begin      = 1 << 0,                                          
        Shake_End        = 1 << 1,                                          
        Shake_Change     = Shake_Begin | Shake_End,                         

        Tilt_XNeg        = 1 << 2,                                          
        Tilt_XZero       = 1 << 3,                                          
        Tilt_XPos        = 1 << 4,                                          
        Tilt_XChange     = Tilt_XNeg | Tilt_XZero | Tilt_XPos,              

        Tilt_YNeg        = 1 << 5,                                          
        Tilt_YZero       = 1 << 6,                                          
        Tilt_YPos        = 1 << 7,                                          
        Tilt_YChange     = Tilt_YNeg | Tilt_YZero | Tilt_YPos,              

        Tilt_ZNeg        = 1 << 8,                                          
        Tilt_ZZero       = 1 << 9,                                          
        Tilt_ZPos        = 1 << 10,                                         
        Tilt_ZChange     = Tilt_ZNeg | Tilt_ZZero | Tilt_ZPos,              

        Tilt_Change      = Tilt_XChange | Tilt_YChange | Tilt_ZChange,      
    };

    unsigned update(int latency = kFilterLatency)
    {
        unsigned changed = 0;

        median.calculate(buffer, latency);
        auto m = median.median();
        int wobble = median.range().len2();

        // Shake hysteresis
        if (wobble >= kShakeThresholdMax) {
            if (!shake) changed |= Shake_Begin;
            shake = true;

        } else if (wobble < kShakeThresholdMin) {
            if (shake) changed |= Shake_End;
            shake = false;

            // Only update tilt state when wobble is low.
            // Each tilt axis has hysteresis.

            if (m.x <= -kTiltThresholdMax) {
                if (tilt.x != -1) changed |= Tilt_XNeg;
                tilt.x = -1;
            } else if (m.x >= kTiltThresholdMax) {
                if (tilt.x != 1) changed |= Tilt_XPos;
                tilt.x = 1;
            } else if (abs(m.x) < kTiltThresholdMin) {
                if (tilt.x != 0) changed |= Tilt_XZero;
                tilt.x = 0;
            }

            if (m.y <= -kTiltThresholdMax) {
                if (tilt.y != -1) changed |= Tilt_YNeg;
                tilt.y = -1;
            } else if (m.y >= kTiltThresholdMax) {
                if (tilt.y != 1) changed |= Tilt_YPos;
                tilt.y = 1;
            } else if (abs(m.y) < kTiltThresholdMin) {
                if (tilt.y != 0) changed |= Tilt_YZero;
                tilt.y = 0;
            }

            if (m.z <= -kTiltThresholdMax) {
                if (tilt.z != -1) changed |= Tilt_ZNeg;
                tilt.z = -1;
            } else if (m.z >= kTiltThresholdMax) {
                if (tilt.z != 1) changed |= Tilt_ZPos;
                tilt.z = 1;
            } else if (abs(m.z) < kTiltThresholdMin) {
                if (tilt.z != 0) changed |= Tilt_ZZero;
                tilt.z = 0;
            }
        }

        return changed;
    }
};


};  // namespace Sifteo