/*
 * Copyright 2015-16 Hillcrest Laboratories, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License and 
 * any applicable agreements you may have with Hillcrest Laboratories, Inc.
 * 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.
 */

/*
 * BNO080 Sensor Event decoding
 */

#include "sh2_SensorValue.h"
#include "sh2_err.h"
#include "sh2_util.h"

#define SCALE_Q(n) (1.0f / (1 << n))

const float scaleRadToDeg = 180.0 / 3.14159265358;

// ------------------------------------------------------------------------
// Forward declarations

static int decodeRawAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeLinearAcceleration(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeGravity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeRawGyroscope(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeGyroscopeCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeGyroscopeUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeRawMagnetometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeMagneticFieldCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeMagneticFieldUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeGameRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeGeomagneticRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodePressure(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeAmbientLight(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeHumidity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeProximity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeTemperature(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeReserved(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeTapDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeStepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeStepCounter(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeSignificantMotion(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeStabilityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeShakeDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeFlipDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodePickupDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeStabilityDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodePersonalActivityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeSleepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeTiltDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodePocketDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeCircleDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeHeartRateMonitor(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeArvrStabilizedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeArvrStabilizedGRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeGyroIntegratedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);
static int decodeIZroRequest(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event);

// ------------------------------------------------------------------------
// Public API

int sh2_decodeSensorEvent(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    // Fill out fields of *value based on *event, converting data from message representation
    // to natural representation.

    int rc = SH2_OK;

    value->sensorId = event->reportId;
    value->timestamp = event->timestamp_uS;

    if (value->sensorId != SH2_GYRO_INTEGRATED_RV) {
        value->sequence = event->report[1];
        value->status = event->report[2] & 0x03;
    }
    else {
        value->sequence = 0;
        value->status = 0;
    }

    // extract delay field (100uS units)
    
    
    switch (value->sensorId) {
        case SH2_RAW_ACCELEROMETER:
            rc = decodeRawAccelerometer(value, event);
            break;
        case SH2_ACCELEROMETER:
            rc = decodeAccelerometer(value, event);
            break;
        case SH2_LINEAR_ACCELERATION:
            rc = decodeLinearAcceleration(value, event);
            break;
        case SH2_GRAVITY:
            rc = decodeGravity(value, event);
            break;
        case SH2_RAW_GYROSCOPE:
            rc = decodeRawGyroscope(value, event);
            break;
        case SH2_GYROSCOPE_CALIBRATED:
            rc = decodeGyroscopeCalibrated(value, event);
            break;
        case SH2_GYROSCOPE_UNCALIBRATED:
            rc = decodeGyroscopeUncal(value, event);
            break;
        case SH2_RAW_MAGNETOMETER:
            rc = decodeRawMagnetometer(value, event);
            break;
        case SH2_MAGNETIC_FIELD_CALIBRATED:
            rc = decodeMagneticFieldCalibrated(value, event);
            break;
        case SH2_MAGNETIC_FIELD_UNCALIBRATED:
            rc = decodeMagneticFieldUncal(value, event);
            break;
        case SH2_ROTATION_VECTOR:
            rc = decodeRotationVector(value, event);
            break;
        case SH2_GAME_ROTATION_VECTOR:
            rc = decodeGameRotationVector(value, event);
            break;
        case SH2_GEOMAGNETIC_ROTATION_VECTOR:
            rc = decodeGeomagneticRotationVector(value, event);
            break;
        case SH2_PRESSURE:
            rc = decodePressure(value, event);
            break;
        case SH2_AMBIENT_LIGHT:
            rc = decodeAmbientLight(value, event);
            break;
        case SH2_HUMIDITY:
            rc = decodeHumidity(value, event);
            break;
        case SH2_PROXIMITY:
            rc = decodeProximity(value, event);
            break;
        case SH2_TEMPERATURE:
            rc = decodeTemperature(value, event);
            break;
        case SH2_RESERVED:
            rc = decodeReserved(value, event);
            break;
        case SH2_TAP_DETECTOR:
            rc = decodeTapDetector(value, event);
            break;
        case SH2_STEP_DETECTOR:
            rc = decodeStepDetector(value, event);
            break;
        case SH2_STEP_COUNTER:
            rc = decodeStepCounter(value, event);
            break;
        case SH2_SIGNIFICANT_MOTION:
            rc = decodeSignificantMotion(value, event);
            break;
        case SH2_STABILITY_CLASSIFIER:
            rc = decodeStabilityClassifier(value, event);
            break;
        case SH2_SHAKE_DETECTOR:
            rc = decodeShakeDetector(value, event);
            break;
        case SH2_FLIP_DETECTOR:
            rc = decodeFlipDetector(value, event);
            break;
        case SH2_PICKUP_DETECTOR:
            rc = decodePickupDetector(value, event);
            break;
        case SH2_STABILITY_DETECTOR:
            rc = decodeStabilityDetector(value, event);
            break;
        case SH2_PERSONAL_ACTIVITY_CLASSIFIER:
            rc = decodePersonalActivityClassifier(value, event);
            break;
        case SH2_SLEEP_DETECTOR:
            rc = decodeSleepDetector(value, event);
            break;
        case SH2_TILT_DETECTOR:
            rc = decodeTiltDetector(value, event);
            break;
        case SH2_POCKET_DETECTOR:
            rc = decodePocketDetector(value, event);
            break;
        case SH2_CIRCLE_DETECTOR:
            rc = decodeCircleDetector(value, event);
            break;
        case SH2_HEART_RATE_MONITOR:
            rc = decodeHeartRateMonitor(value, event);
            break;
        case SH2_ARVR_STABILIZED_RV:
            rc = decodeArvrStabilizedRV(value, event);
            break;
        case SH2_ARVR_STABILIZED_GRV:
            rc = decodeArvrStabilizedGRV(value, event);
            break;
        case SH2_GYRO_INTEGRATED_RV:
            rc = decodeGyroIntegratedRV(value, event);
            break;
        case SH2_IZRO_MOTION_REQUEST:
            rc = decodeIZroRequest(value, event);
            break;
        default:
            // Unknown report id
            rc = SH2_ERR;
            break;
    }

    return rc;
}

// ------------------------------------------------------------------------
// Private utility functions

static int decodeRawAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.rawAccelerometer.x = read16(&event->report[4]);
    value->un.rawAccelerometer.y = read16(&event->report[6]);
    value->un.rawAccelerometer.z = read16(&event->report[8]);
    value->un.rawAccelerometer.timestamp = read32(&event->report[12]);

    return SH2_OK;
}

static int decodeAccelerometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.accelerometer.x = read16(&event->report[4]) * SCALE_Q(8);
    value->un.accelerometer.y = read16(&event->report[6]) * SCALE_Q(8);
    value->un.accelerometer.z = read16(&event->report[8]) * SCALE_Q(8);

    return SH2_OK;
}

static int decodeLinearAcceleration(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.linearAcceleration.x = read16(&event->report[4]) * SCALE_Q(8);
    value->un.linearAcceleration.y = read16(&event->report[6]) * SCALE_Q(8);
    value->un.linearAcceleration.z = read16(&event->report[8]) * SCALE_Q(8);

    return SH2_OK;
}

static int decodeGravity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.gravity.x = read16(&event->report[4]) * SCALE_Q(8);
    value->un.gravity.y = read16(&event->report[6]) * SCALE_Q(8);
    value->un.gravity.z = read16(&event->report[8]) * SCALE_Q(8);

    return SH2_OK;
}

static int decodeRawGyroscope(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.rawGyroscope.x = read16(&event->report[4]);
    value->un.rawGyroscope.y = read16(&event->report[6]);
    value->un.rawGyroscope.z = read16(&event->report[8]);
    value->un.rawGyroscope.temperature = read16(&event->report[10]);
    value->un.rawGyroscope.timestamp = read32(&event->report[12]);

    return SH2_OK;
}

static int decodeGyroscopeCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.gyroscope.x = read16(&event->report[4]) * SCALE_Q(9);
    value->un.gyroscope.y = read16(&event->report[6]) * SCALE_Q(9);
    value->un.gyroscope.z = read16(&event->report[8]) * SCALE_Q(9);

    return SH2_OK;
}

static int decodeGyroscopeUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.gyroscopeUncal.x = read16(&event->report[4]) * SCALE_Q(9);
    value->un.gyroscopeUncal.y = read16(&event->report[6]) * SCALE_Q(9);
    value->un.gyroscopeUncal.z = read16(&event->report[8]) * SCALE_Q(9);

    value->un.gyroscopeUncal.biasX = read16(&event->report[10]) * SCALE_Q(9);
    value->un.gyroscopeUncal.biasY = read16(&event->report[12]) * SCALE_Q(9);
    value->un.gyroscopeUncal.biasZ = read16(&event->report[14]) * SCALE_Q(9);

    return SH2_OK;
}

static int decodeRawMagnetometer(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.rawMagnetometer.x = read16(&event->report[4]);
    value->un.rawMagnetometer.y = read16(&event->report[6]);
    value->un.rawMagnetometer.z = read16(&event->report[8]);
    value->un.rawMagnetometer.timestamp = read32(&event->report[12]);

    return SH2_OK;
}

static int decodeMagneticFieldCalibrated(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.magneticField.x = read16(&event->report[4]) * SCALE_Q(4);
    value->un.magneticField.y = read16(&event->report[6]) * SCALE_Q(4);
    value->un.magneticField.z = read16(&event->report[8]) * SCALE_Q(4);

    return SH2_OK;
}

static int decodeMagneticFieldUncal(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.magneticFieldUncal.x = read16(&event->report[4]) * SCALE_Q(4);
    value->un.magneticFieldUncal.y = read16(&event->report[6]) * SCALE_Q(4);
    value->un.magneticFieldUncal.z = read16(&event->report[8]) * SCALE_Q(4);

    value->un.magneticFieldUncal.biasX = read16(&event->report[10]) * SCALE_Q(4);
    value->un.magneticFieldUncal.biasY = read16(&event->report[12]) * SCALE_Q(4);
    value->un.magneticFieldUncal.biasZ = read16(&event->report[14]) * SCALE_Q(4);

    return SH2_OK;
}

static int decodeRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.rotationVector.i = read16(&event->report[4]) * SCALE_Q(14);
    value->un.rotationVector.j = read16(&event->report[6]) * SCALE_Q(14);
    value->un.rotationVector.k = read16(&event->report[8]) * SCALE_Q(14);
    value->un.rotationVector.real = read16(&event->report[10]) * SCALE_Q(14);
    value->un.rotationVector.accuracy = read16(&event->report[12]) * SCALE_Q(12);

    return SH2_OK;
}

static int decodeGameRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.gameRotationVector.i = read16(&event->report[4]) * SCALE_Q(14);
    value->un.gameRotationVector.j = read16(&event->report[6]) * SCALE_Q(14);
    value->un.gameRotationVector.k = read16(&event->report[8]) * SCALE_Q(14);
    value->un.gameRotationVector.real = read16(&event->report[10]) * SCALE_Q(14);

    return SH2_OK;
}

static int decodeGeomagneticRotationVector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.geoMagRotationVector.i = read16(&event->report[4]) * SCALE_Q(14);
    value->un.geoMagRotationVector.j = read16(&event->report[6]) * SCALE_Q(14);
    value->un.geoMagRotationVector.k = read16(&event->report[8]) * SCALE_Q(14);
    value->un.geoMagRotationVector.real = read16(&event->report[10]) * SCALE_Q(14);
    value->un.geoMagRotationVector.accuracy = read16(&event->report[12]) * SCALE_Q(12);

    return SH2_OK;
}

static int decodePressure(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.pressure.value = read32(&event->report[4]) * SCALE_Q(20);

    return SH2_OK;
}

static int decodeAmbientLight(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.ambientLight.value = read32(&event->report[4]) * SCALE_Q(8);

    return SH2_OK;
}

static int decodeHumidity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.humidity.value = read16(&event->report[4]) * SCALE_Q(8);

    return SH2_OK;
}

static int decodeProximity(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.proximity.value = read16(&event->report[4]) * SCALE_Q(4);

    return SH2_OK;
}

static int decodeTemperature(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.temperature.value = read16(&event->report[4]) * SCALE_Q(7);

    return SH2_OK;
}

static int decodeReserved(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.reserved.tbd = read16(&event->report[4]) * SCALE_Q(7);

    return SH2_OK;
}

static int decodeTapDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.tapDetector.flags = event->report[4];

    return SH2_OK;
}

static int decodeStepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.stepDetector.latency = readu32(&event->report[4]);

    return SH2_OK;
}

static int decodeStepCounter(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.stepCounter.latency = readu32(&event->report[4]);
    value->un.stepCounter.steps = readu32(&event->report[8]);

    return SH2_OK;
}

static int decodeSignificantMotion(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.sigMotion.motion = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodeStabilityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.stabilityClassifier.classification = event->report[4];

    return SH2_OK;
}

static int decodeShakeDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.shakeDetector.shake = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodeFlipDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.flipDetector.flip = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodePickupDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.pickupDetector.pickup = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodeStabilityDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.stabilityDetector.stability = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodePersonalActivityClassifier(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.personalActivityClassifier.page = event->report[4] & 0x7F;
    value->un.personalActivityClassifier.lastPage = ((event->report[4] & 0x80) != 0);
    value->un.personalActivityClassifier.mostLikelyState = event->report[5];
    for (int n = 0; n < 10; n++) {
        value->un.personalActivityClassifier.confidence[n] = event->report[6+n];
    }
    
    return SH2_OK;
}

static int decodeSleepDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.sleepDetector.sleepState = event->report[4];

    return SH2_OK;
}

static int decodeTiltDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.tiltDetector.tilt = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodePocketDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.pocketDetector.pocket = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodeCircleDetector(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.circleDetector.circle = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodeHeartRateMonitor(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.heartRateMonitor.heartRate = readu16(&event->report[4]);

    return SH2_OK;
}

static int decodeArvrStabilizedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.arvrStabilizedRV.i = read16(&event->report[4]) * SCALE_Q(14);
    value->un.arvrStabilizedRV.j = read16(&event->report[6]) * SCALE_Q(14);
    value->un.arvrStabilizedRV.k = read16(&event->report[8]) * SCALE_Q(14);
    value->un.arvrStabilizedRV.real = read16(&event->report[10]) * SCALE_Q(14);
    value->un.arvrStabilizedRV.accuracy = read16(&event->report[12]) * SCALE_Q(12);

    return SH2_OK;
}

static int decodeArvrStabilizedGRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.arvrStabilizedGRV.i = read16(&event->report[4]) * SCALE_Q(14);
    value->un.arvrStabilizedGRV.j = read16(&event->report[6]) * SCALE_Q(14);
    value->un.arvrStabilizedGRV.k = read16(&event->report[8]) * SCALE_Q(14);
    value->un.arvrStabilizedGRV.real = read16(&event->report[10]) * SCALE_Q(14);

    return SH2_OK;
}

static int decodeGyroIntegratedRV(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.gyroIntegratedRV.i = read16(&event->report[0]) * SCALE_Q(14);
    value->un.gyroIntegratedRV.j = read16(&event->report[2]) * SCALE_Q(14);
    value->un.gyroIntegratedRV.k = read16(&event->report[4]) * SCALE_Q(14);
    value->un.gyroIntegratedRV.real = read16(&event->report[6]) * SCALE_Q(14);
    value->un.gyroIntegratedRV.angVelX = read16(&event->report[8]) * SCALE_Q(10);
    value->un.gyroIntegratedRV.angVelY = read16(&event->report[10]) * SCALE_Q(10);
    value->un.gyroIntegratedRV.angVelZ = read16(&event->report[12]) * SCALE_Q(10);

    return SH2_OK;
}

static int decodeIZroRequest(sh2_SensorValue_t *value, const sh2_SensorEvent_t *event)
{
    value->un.izroRequest.intent = (sh2_IZroMotionIntent_t)event->report[4];
    value->un.izroRequest.request = (sh2_IZroMotionRequest_t)event->report[5];

    return SH2_OK;
}