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

#include <stdlib.h>
#include <string.h>
#include <timer.h>
#include <heap.h>
#include <plat/rtc.h>
#include <plat/syscfg.h>
#include <hostIntf.h>
#include <nanohubPacket.h>
#include <floatRt.h>

#include <seos.h>

#include <nanohub_math.h>
#include <algos/fusion.h>
#include <sensors.h>
#include <variant/sensType.h>
#include <limits.h>
#include <slab.h>

#define ORIENTATION_APP_VERSION 1

#define MAX_NUM_COMMS_EVENT_SAMPLES 15 // at most 15 samples can fit in one comms_event
#define NUM_COMMS_EVENTS_IN_FIFO    2  // This controls how often the hub needs to wake up
                                       // in batching

// needs to be greater than max raw sensor rate ratio
#define FIFO_DEPTH                  (NUM_COMMS_EVENTS_IN_FIFO * MAX_NUM_COMMS_EVENT_SAMPLES)

/*
 * FIFO_MARGIN: max raw sensor rate ratio is 8:1.
 * If 2 batchs of high rate data comes before 1 low rate data, there can be at max 15 samples left
 * in the FIFO
 */
#define FIFO_MARGIN                 15
#define MAX_NUM_SAMPLES             (FIFO_MARGIN + FIFO_DEPTH) // actual input sample fifo depth
#define EVT_SENSOR_ACC_DATA_RDY     sensorGetMyEventType(SENS_TYPE_ACCEL)
#define EVT_SENSOR_GYR_DATA_RDY     sensorGetMyEventType(SENS_TYPE_GYRO)
#define EVT_SENSOR_MAG_DATA_RDY     sensorGetMyEventType(SENS_TYPE_MAG)
#define EVT_SENSOR_MAG_BIAS         sensorGetMyEventType(SENS_TYPE_MAG_BIAS)

#define kGravityEarth               9.80665f
#define kRad2deg                    (180.0f / M_PI)
#define MIN_GYRO_RATE_HZ            SENSOR_HZ(100.0f)
#define MAX_MAG_RATE_HZ             SENSOR_HZ(50.0f)

enum
{
    FUSION_FLAG_ENABLED             = 0x01,
    FUSION_FLAG_INITIALIZED         = 0x08,
    FUSION_FLAG_GAME_ENABLED        = 0x10,
    FUSION_FLAG_GAME_INITIALIZED    = 0x20
};

enum RawSensorType
{
    ACC,
    GYR,
    MAG,
    NUM_OF_RAW_SENSOR
};

enum FusionSensorType
{
    ORIENT,
    GRAVITY,
    GEOMAG,
    LINEAR,
    GAME,
    ROTAT,
    NUM_OF_FUSION_SENSOR
};


struct FusionSensorSample {
    uint64_t time;
    float x, y, z;
};

struct FusionSensor {
    uint32_t handle;
    struct TripleAxisDataEvent *ev;
    uint64_t prev_time;
    uint64_t latency;
    uint32_t rate;
    bool active;
    bool use_gyro_data;
    bool use_mag_data;
    uint8_t idx;
};

struct FusionTask {
    uint32_t tid;
    uint32_t accelHandle;
    uint32_t gyroHandle;
    uint32_t magHandle;

    struct Fusion fusion;
    struct Fusion game;

    struct FusionSensor sensors[NUM_OF_FUSION_SENSOR];
    struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES];
    size_t sample_indices[NUM_OF_RAW_SENSOR];
    size_t sample_counts[NUM_OF_RAW_SENSOR];
    uint32_t counters[NUM_OF_RAW_SENSOR];
    uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR];
    uint64_t last_time[NUM_OF_RAW_SENSOR];
    struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR];

    uint32_t flags;

    uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR];
    uint64_t raw_sensor_latency;

    uint8_t accel_client_cnt;
    uint8_t gyro_client_cnt;
    uint8_t mag_client_cnt;
};

static uint32_t FusionRates[] = {
    SENSOR_HZ(12.5f),
    SENSOR_HZ(25.0f),
    SENSOR_HZ(50.0f),
    SENSOR_HZ(100.0f),
    SENSOR_HZ(200.0f),
    0,
};

//should match "supported rates in length" and be the timer length for that rate in nanosecs
static const uint64_t rateTimerVals[] = {
    1000000000ULL / 12.5f,
    1000000000ULL / 25,
    1000000000ULL / 50,
    1000000000ULL / 100,
    1000000000ULL / 200,
};

static struct FusionTask mTask;

#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
    .sensorName = name, \
    .supportedRates = rates, \
    .sensorType = type, \
    .numAxis = axis, \
    .interrupt = inter, \
    .minSamples = samples

static const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] =
{
    { DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 20) },
    { DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 20) },
    { DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC,
            NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) },
    { DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 20) },
    { DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 300) },
    { DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 20) },
};

static struct SlabAllocator *mDataSlab;

static void dataEvtFree(void *ptr)
{
    slabAllocatorFree(mDataSlab, ptr);
}

static void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index)
{
    bool bad_timestamp;
    size_t i, w, n, num_samples;
    struct TripleAxisDataPoint *curr_sample, *next_sample;
    uint32_t counter;
    uint64_t ResamplePeriodNs, curr_time, next_time;
    uint64_t sample_spacing_ns;
    float weight_next;

    if (index == GYR && mTask.gyro_client_cnt == 0) {
        return;
    }
    if (index == MAG && mTask.mag_client_cnt == 0) {
        return;
    }

    n = mTask.sample_counts[index];
    i = mTask.sample_indices[index];
    counter = mTask.counters[index];
    ResamplePeriodNs = mTask.ResamplePeriodNs[index];
    w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES;

    // check if this sensor was used before
    if (mTask.last_time[index] == ULONG_LONG_MAX) {
        curr_sample = ev->samples;
        next_sample = curr_sample + 1;
        num_samples = ev->samples[0].firstSample.numSamples;
        curr_time = ev->referenceTime;
    } else {
        curr_sample = &mTask.last_sample[index];
        next_sample = ev->samples;
        num_samples = ev->samples[0].firstSample.numSamples + 1;
        curr_time = mTask.last_time[index];
    }

    while (num_samples > 1) {

        if (next_sample == ev->samples)
            next_time = ev->referenceTime;
        else
            next_time = curr_time + next_sample->deltaTime;

        // error handling for non-chronological accel timestamps
        sample_spacing_ns = (next_time > curr_time) ?  (next_time - curr_time) : 0;

        // This can happen during sensor config changes
        bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs);

        // Check to see if we need to move the interpolation window or
        // interpolate
        if ((counter >= sample_spacing_ns) || bad_timestamp) {
            num_samples--;
            counter -= (bad_timestamp ? counter : sample_spacing_ns);
            curr_sample = next_sample;
            next_sample++;

            curr_time = next_time;
        } else {
            weight_next = (float)counter / floatFromUint64(sample_spacing_ns);

            mTask.samples[index][w].x = curr_sample->x + weight_next *
                (next_sample->x - curr_sample->x);
            mTask.samples[index][w].y = curr_sample->y + weight_next *
                (next_sample->y - curr_sample->y);
            mTask.samples[index][w].z = curr_sample->z + weight_next *
                (next_sample->z - curr_sample->z);
            mTask.samples[index][w].time = curr_time + counter;

            // Move the read index when buffer is full
            if (++n > MAX_NUM_SAMPLES) {
                n = MAX_NUM_SAMPLES;

                if (++i == MAX_NUM_SAMPLES) {
                    i = 0;
                }
            }

            // Reset the write index
            if (++w == MAX_NUM_SAMPLES) {
                w = 0;
            }

            // Move to the next resample
            counter += ResamplePeriodNs;
        }
    }

    mTask.sample_counts[index] = n;
    mTask.sample_indices[index] = i;
    mTask.counters[index] = counter;
    mTask.last_sample[index] = *curr_sample;
    mTask.last_time[index] = curr_time;
}

static bool allocateDataEvt(struct FusionSensor *mSensor, uint64_t time)
{
    mSensor->ev = slabAllocatorAlloc(mDataSlab);
    if (mSensor->ev == NULL) {
        // slab allocation failed
        osLog(LOG_ERROR, "ORIENTATION: slabAllocatorAlloc() Failed\n");
        return false;
    }

    // delta time for the first sample is sample count
    memset(&mSensor->ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
    mSensor->ev->referenceTime = time;
    mSensor->prev_time = time;

    return true;
}

static void addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z)
{
    struct TripleAxisDataPoint *sample;

    if (mSensor->ev == NULL) {
        if (!allocateDataEvt(mSensor, time))
            return;
    }

    if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) {
        osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n");
        return;
    }

    sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++];

    if (mSensor->ev->samples[0].firstSample.numSamples > 1) {
        sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0;
        mSensor->prev_time = time;
    }

    sample->x = x;
    sample->y = y;
    sample->z = z;

    if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
        osEnqueueEvtOrFree(
                EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType),
                mSensor->ev, dataEvtFree);
        mSensor->ev = NULL;
    }
}

static void updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time)
{
    struct Vec4 attitude;
    struct Vec3 g, a;
    struct Mat33 R;  // direction-cosine/rotation matrix, inertial -> device
    bool   rInited;  // indicates if matrix R has been initialzed. for avoiding repeated computation

    if (fusionHasEstimate(&mTask.game)) {
        rInited = false;
        if (mTask.sensors[GAME].active) {
            fusionGetAttitude(&mTask.game, &attitude);
            addSample(&mTask.sensors[GAME],
                    last_sensor_time,
                    attitude.x,
                    attitude.y,
                    attitude.z);
        }

        if (mTask.sensors[GRAVITY].active) {
            fusionGetRotationMatrix(&mTask.game, &R);
            rInited = true;
            initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]);
            vec3ScalarMul(&g, kGravityEarth);
            addSample(&mTask.sensors[GRAVITY],
                    last_sensor_time,
                    g.x, g.y, g.z);
        }

        if (last_accel_sample_index >= 0
                && mTask.sensors[LINEAR].active) {
            if (!rInited) {
                fusionGetRotationMatrix(&mTask.game, &R);
            }
            initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]);
            vec3ScalarMul(&g, kGravityEarth);
            initVec3(&a,
                    mTask.samples[0][last_accel_sample_index].x,
                    mTask.samples[0][last_accel_sample_index].y,
                    mTask.samples[0][last_accel_sample_index].z);

            addSample(&mTask.sensors[LINEAR],
                    mTask.samples[0][last_accel_sample_index].time,
                    a.x - g.x,
                    a.y - g.y,
                    a.z - g.z);
        }
    }

    if (fusionHasEstimate(&mTask.fusion)) {
        fusionGetAttitude(&mTask.fusion, &attitude);

        if (mTask.sensors[ORIENT].active) {
            fusionGetRotationMatrix(&mTask.fusion, &R);
            // x, y, z = yaw, pitch, roll
            float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg;
            float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg;
            float z = asinf(R.elem[0][2]) * kRad2deg;

            if (x < 0.0f) {
                x += 360.0f;
            }

            addSample(&mTask.sensors[ORIENT],
                    last_sensor_time, x, y, z);
        }

        if (mTask.sensors[GEOMAG].active) {
            addSample(&mTask.sensors[GEOMAG],
                    last_sensor_time,
                    attitude.x,
                    attitude.y,
                    attitude.z);
        }

        if (mTask.sensors[ROTAT].active) {
            addSample(&mTask.sensors[ROTAT],
                    last_sensor_time,
                    attitude.x,
                    attitude.y,
                    attitude.z);
        }

    }
}

static void drainSamples()
{
    size_t i = mTask.sample_indices[ACC];
    size_t j = 0;
    size_t k = 0;
    size_t which;
    struct Vec3 a, w, m;
    float dT;
    uint64_t a_time, g_time, m_time;

    if (mTask.gyro_client_cnt > 0)
        j = mTask.sample_indices[GYR];

    if (mTask.mag_client_cnt > 0)
        k = mTask.sample_indices[MAG];

    while (mTask.sample_counts[ACC] > 0
            && (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0)
            && (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0)) {
        a_time = mTask.samples[ACC][i].time;
        g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time
                            : ULONG_LONG_MAX;
        m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time
                            : ULONG_LONG_MAX;

        // priority with same timestamp: gyro > acc > mag
        if (g_time <= a_time && g_time <= m_time) {
            which = GYR;
        } else if (a_time <= m_time) {
            which = ACC;
        } else {
            which = MAG;
        }

        dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f;
        switch (which) {
        case ACC:
            initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z);

            if (mTask.flags & FUSION_FLAG_ENABLED)
                fusionHandleAcc(&mTask.fusion, &a, dT);

            if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
                fusionHandleAcc(&mTask.game, &a, dT);

            updateOutput(i, mTask.samples[ACC][i].time);

            --mTask.sample_counts[ACC];
            if (++i == MAX_NUM_SAMPLES)
                i = 0;
            break;
        case GYR:
            initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z);

            if (mTask.flags & FUSION_FLAG_ENABLED)
                fusionHandleGyro(&mTask.fusion, &w, dT);

            if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
                fusionHandleGyro(&mTask.game, &w, dT);

            --mTask.sample_counts[GYR];
            if (++j == MAX_NUM_SAMPLES)
                j = 0;
            break;
        case MAG:
            initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z);

            fusionHandleMag(&mTask.fusion, &m, dT);

            --mTask.sample_counts[MAG];
            if (++k == MAX_NUM_SAMPLES)
                k = 0;
            break;
        }
    }

    mTask.sample_indices[ACC] = i;

    if (mTask.gyro_client_cnt > 0)
        mTask.sample_indices[GYR] = j;

    if (mTask.mag_client_cnt > 0)
        mTask.sample_indices[MAG] = k;

    for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
        if (mTask.sensors[i].ev != NULL) {
            osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType),
                               mTask.sensors[i].ev, dataEvtFree);
            mTask.sensors[i].ev = NULL;
        }
    }
}

static void configureFusion()
{
    if (mTask.sensors[ORIENT].active
            || mTask.sensors[ROTAT].active
            || mTask.sensors[GEOMAG].active) {
        mTask.flags |= FUSION_FLAG_ENABLED;
        initFusion(&mTask.fusion,
                (mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) |
                (mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) |
                ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE));
        mTask.flags |= FUSION_FLAG_INITIALIZED;
    } else {
        mTask.flags &= ~FUSION_FLAG_ENABLED;
        mTask.flags &= ~FUSION_FLAG_INITIALIZED;
    }
}

static void configureGame()
{
    if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active ||
            mTask.sensors[LINEAR].active) {
        mTask.flags |= FUSION_FLAG_GAME_ENABLED;
        initFusion(&mTask.game, FUSION_USE_GYRO |
                ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE));
        mTask.flags |= FUSION_FLAG_GAME_INITIALIZED;
    } else {
        mTask.flags &= ~FUSION_FLAG_GAME_ENABLED;
        mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
    }
}

static void fusionSetRateAcc(void)
{
    int i;
    if  (mTask.accelHandle == 0) {
        mTask.sample_counts[ACC] = 0;
        mTask.sample_indices[ACC] = 0;
        mTask.counters[ACC] = 0;
        mTask.last_time[ACC] = ULONG_LONG_MAX;
        for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) {
            if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC],
                        mTask.raw_sensor_latency)) {
                osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
                break;
            }
        }
    } else {
        sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC],
                mTask.raw_sensor_latency);
    }
}

static void fusionSetRateGyr(void)
{
    int i;
    if (mTask.gyroHandle == 0) {
        mTask.sample_counts[GYR] = 0;
        mTask.sample_indices[GYR] = 0;
        mTask.counters[GYR] = 0;
        mTask.last_time[GYR] = ULONG_LONG_MAX;
        for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) {
            if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR],
                        mTask.raw_sensor_latency)) {
                osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY);
                break;
            }
        }
    } else {
        sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR],
                mTask.raw_sensor_latency);
    }
}

static void fusionSetRateMag(void)
{
    int i;
    if (mTask.magHandle == 0) {
        mTask.sample_counts[MAG] = 0;
        mTask.sample_indices[MAG] = 0;
        mTask.counters[MAG] = 0;
        mTask.last_time[MAG] = ULONG_LONG_MAX;
        for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) {
            if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
                        mTask.raw_sensor_latency)) {
                osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
                osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS);
                break;
            }
        }
    } else {
        sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
                mTask.raw_sensor_latency);
    }
}

static bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
    struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
    int i;
    uint32_t max_rate = 0;
    uint32_t gyr_rate, mag_rate;
    uint64_t min_resample_period = ULONG_LONG_MAX;

    mSensor->rate = rate;
    mSensor->latency = latency;

    for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
        if (mTask.sensors[i].active) {
            max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate;
        }
    }

    if (mTask.accel_client_cnt > 0) {
        mTask.raw_sensor_rate[ACC] = max_rate;
        mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate);
        min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ?
            mTask.ResamplePeriodNs[ACC] : min_resample_period;
    }

    if (mTask.gyro_client_cnt > 0) {
        gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ;
        mTask.raw_sensor_rate[GYR] = gyr_rate;
        mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate);
        min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ?
            mTask.ResamplePeriodNs[GYR] : min_resample_period;
    }

    if (mTask.mag_client_cnt > 0) {
        mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ;
        mTask.raw_sensor_rate[MAG] = mag_rate;
        mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate);
        min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ?
            mTask.ResamplePeriodNs[MAG] : min_resample_period;
    }

    // This guarantees that local raw sensor FIFOs won't overflow.
    mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1);

    for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
        if (mTask.sensors[i].active) {
            mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ?
                mTask.sensors[i].latency : mTask.raw_sensor_latency;
        }
    }

    if (mTask.accel_client_cnt > 0)
        fusionSetRateAcc();
    if (mTask.gyro_client_cnt > 0)
        fusionSetRateGyr();
    if (mTask.mag_client_cnt > 0)
        fusionSetRateMag();
    if (mSensor->rate > 0)
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);

    return true;
}

static bool fusionPower(bool on, void *cookie)
{
    struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
    int idx;

    mSensor->active = on;
    if (on == false) {
        mTask.accel_client_cnt--;
        if (mSensor->use_gyro_data)
            mTask.gyro_client_cnt--;
        if (mSensor->use_mag_data)
            mTask.mag_client_cnt--;

        // if client_cnt == 0 and Handle == 0, nothing need to be done.
        // if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done.
        if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) {
            sensorRelease(mTask.tid, mTask.accelHandle);
            mTask.accelHandle = 0;
            osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
        }

        if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) {
            sensorRelease(mTask.tid, mTask.gyroHandle);
            mTask.gyroHandle = 0;
            osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY);
        }

        if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) {
            sensorRelease(mTask.tid, mTask.magHandle);
            mTask.magHandle = 0;
            osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
        }

        idx = mSensor->idx;
        (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx);
    } else {
        mTask.accel_client_cnt++;
        if (mSensor->use_gyro_data)
            mTask.gyro_client_cnt++;
        if (mSensor->use_mag_data)
            mTask.mag_client_cnt++;
    }

    configureFusion();
    configureGame();
    sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);

    return true;
}

static bool fusionFirmwareUpload(void *cookie)
{
    struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];

    sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    return true;
}

static bool fusionFlush(void *cookie)
{
    struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
    uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType);

    osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL);
    return true;
}

static void fusionHandleEvent(uint32_t evtType, const void* evtData)
{
    struct TripleAxisDataEvent *ev;
    int i;

    if (evtData == SENSOR_DATA_EVENT_FLUSH)
        return;

    switch (evtType) {
    case EVT_APP_START:
        // check for gyro and mag
        osEventUnsubscribe(mTask.tid, EVT_APP_START);
        if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) {
            for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++)
                mTask.sensors[i].use_gyro_data = false;
        }
        mTask.gyroHandle = 0;
        if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) {
            for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++)
                mTask.sensors[i].use_mag_data = false;
        }
        mTask.magHandle = 0;
        break;
    case EVT_SENSOR_ACC_DATA_RDY:
        ev = (struct TripleAxisDataEvent *)evtData;
        fillSamples(ev, ACC);
        drainSamples();
        break;
    case EVT_SENSOR_GYR_DATA_RDY:
        ev = (struct TripleAxisDataEvent *)evtData;
        fillSamples(ev, GYR);
        drainSamples();
        break;
    case EVT_SENSOR_MAG_BIAS:
        ev = (struct TripleAxisDataEvent *)evtData;
        if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) {
            //it is a user initiated mag cal event
            fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL);
        }
        break;
    case EVT_SENSOR_MAG_DATA_RDY:
        ev = (struct TripleAxisDataEvent *)evtData;
        fillSamples(ev, MAG);
        drainSamples();
        break;
    }
}

static const struct SensorOps mSops =
{
    .sensorPower = fusionPower,
    .sensorFirmwareUpload = fusionFirmwareUpload,
    .sensorSetRate = fusionSetRate,
    .sensorFlush = fusionFlush,
};

static bool fusionStart(uint32_t tid)
{
    size_t i, slabSize;

    mTask.tid = tid;
    mTask.flags = 0;

    for (i = 0; i < NUM_OF_RAW_SENSOR; i++) {
         mTask.sample_counts[i] = 0;
         mTask.sample_indices[i] = 0;
    }

    for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
        mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true);
        mTask.sensors[i].idx = i;
        mTask.sensors[i].use_gyro_data = true;
        mTask.sensors[i].use_mag_data = true;
    }

    mTask.sensors[GEOMAG].use_gyro_data = false;
    mTask.sensors[GAME].use_mag_data = false;
    mTask.sensors[GRAVITY].use_mag_data = false;
    mTask.sensors[LINEAR].use_mag_data = false;

    mTask.accel_client_cnt = 0;
    mTask.gyro_client_cnt = 0;
    mTask.mag_client_cnt = 0;

    slabSize = sizeof(struct TripleAxisDataEvent)
        + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);

    // worst case 6 output sensors * (N + 1) comms_events
    mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1));
    if (!mDataSlab) {
        osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n");
        return false;
    }

    osEventSubscribe(mTask.tid, EVT_APP_START);

    return true;
}

static void fusionEnd()
{
    mTask.flags &= ~FUSION_FLAG_INITIALIZED;
    mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
    slabAllocatorDestroy(mDataSlab);
}

INTERNAL_APP_INIT(
        APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 4),
        ORIENTATION_APP_VERSION,
        fusionStart,
        fusionEnd,
        fusionHandleEvent);