/*
* Copyright (C) 2012 Invensense, Inc.
*
* 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.
*/

#define LOG_NDEBUG 0
//see also the EXTRA_VERBOSE define in the MPLSensor.h header file

#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <float.h>
#include <poll.h>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <sys/select.h>
#include <sys/syscall.h>
#include <dlfcn.h>
#include <pthread.h>
#include <cutils/log.h>
#include <utils/KeyedVector.h>
#include <utils/String8.h>
#include <string.h>
#include <linux/input.h>

#include "MPLSensor.h"
#include "MPLSupport.h"
#include "sensor_params.h"

#include "invensense.h"
#include "invensense_adv.h"
#include "ml_stored_data.h"
#include "ml_load_dmp.h"
#include "ml_sysfs_helper.h"

// #define TESTING
// #define ENABLE_LP_QUAT_FEAT /* Uncomment to enable Low Power Quaternion */

#ifdef THIRD_PARTY_ACCEL
#   warning "Third party accel"
#   define USE_THIRD_PARTY_ACCEL        (1)
#else
#   define USE_THIRD_PARTY_ACCEL        (0)
#endif

#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*))

/******************************************************************************/
/*  MPL interface misc.                                                       */
/******************************************************************************/
static int hertz_request = 200;

#define DEFAULT_MPL_GYRO_RATE           (20000L)     //us
#define DEFAULT_MPL_COMPASS_RATE        (20000L)     //us

#define DEFAULT_HW_GYRO_RATE            (100)        //Hz
#define DEFAULT_HW_ACCEL_RATE           (20)         //ms
#define DEFAULT_HW_COMPASS_RATE         (20000000L)  //ns
#define DEFAULT_HW_AKMD_COMPASS_RATE    (200000000L) //ns

/* convert ns to hardware units */
#define HW_GYRO_RATE_NS                 (1000000000LL / rate_request) // to Hz
#define HW_ACCEL_RATE_NS                (rate_request / (1000000L))   // to ms
#define HW_COMPASS_RATE_NS              (rate_request)                // to ns

/* convert Hz to hardware units */
#define HW_GYRO_RATE_HZ                 (hertz_request)
#define HW_ACCEL_RATE_HZ                (1000 / hertz_request)
#define HW_COMPASS_RATE_HZ              (1000000000LL / hertz_request)

static struct sensor_t sSensorList[] =
{
    {"MPL Gyroscope", "Invensense", 1,
     SENSORS_GYROSCOPE_HANDLE,
     SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, {}},
    {"MPL Accelerometer", "Invensense", 1,
     SENSORS_ACCELERATION_HANDLE,
     SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, {}},
    {"MPL Magnetic Field", "Invensense", 1,
     SENSORS_MAGNETIC_FIELD_HANDLE,
     SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, {}},
    {"MPL Orientation", "Invensense", 1,
     SENSORS_ORIENTATION_HANDLE,
     SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, {}},
    {"MPL Rotation Vector", "Invensense", 1,
     SENSORS_ROTATION_VECTOR_HANDLE,
     SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, {}},
    {"MPL Linear Acceleration", "Invensense", 1,
     SENSORS_LINEAR_ACCEL_HANDLE,
     SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, {}},
    {"MPL Gravity", "Invensense", 1,
     SENSORS_GRAVITY_HANDLE,
     SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, {}},
};

MPLSensor *MPLSensor::gMPLSensor = NULL;

extern "C" {
void procData_cb_wrapper()
{
    if(MPLSensor::gMPLSensor) {
        MPLSensor::gMPLSensor->cbProcData();
    }
}

void setCallbackObject(MPLSensor* gbpt)
{
    MPLSensor::gMPLSensor = gbpt;
}

MPLSensor* getCallbackObject() {
    return MPLSensor::gMPLSensor;
}

} // end of extern C

#ifdef INV_PLAYBACK_DBG
static FILE *logfile = NULL;
#endif

/*******************************************************************************
 * MPLSensor class implementation
 ******************************************************************************/

MPLSensor::MPLSensor(CompassSensor *compass)
                       : SensorBase(NULL, NULL),
                         mMplSensorInitialized(false),
                         mNewData(0),
                         mMasterSensorMask(INV_ALL_SENSORS),
                         mLocalSensorMask(ALL_MPL_SENSORS_NP),
                         mPollTime(-1),
                         mHaveGoodMpuCal(0),
                         mGyroAccuracy(0),
                         mAccelAccuracy(0),
                         mSampleCount(0),
                         mEnabled(0),
                         mOldEnabledMask(0),
                         mAccelInputReader(4),
                         mGyroInputReader(32),
                         mTempScale(0),
                         mTempOffset(0),
                         mTempCurrentTime(0),
                         mAccelScale(2),
                         mPendingMask(0),
                         mSensorMask(0),
                         mGyroOrientation{0},
                         mAccelOrientation{0},
                         mFeatureActiveMask(0) {
    VFUNC_LOG;

    inv_error_t rv;
    int i, fd;
    char *port = NULL;
    char *ver_str;
    unsigned long mSensorMask;
    int res;
    FILE *fptr;

    mCompassSensor = compass;

    LOGV_IF(EXTRA_VERBOSE,
            "HAL:MPLSensor constructor : numSensors = %d", numSensors);

    pthread_mutex_init(&mMplMutex, NULL);
    pthread_mutex_init(&mHALMutex, NULL);

#ifdef INV_PLAYBACK_DBG
    LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging");
    logfile = fopen("/data/playback.bin", "wb");
    if (logfile)
        inv_turn_on_data_logging(logfile);
#endif

    /* setup sysfs paths */
    if(inv_init_sysfs_attributes()) {
        ALOGE("MPLSensor failed to init sysfs attributes");
        return;
    }

    /* get chip name */
    if (inv_get_chip_name(chip_ID) != INV_SUCCESS) {
        LOGE("HAL:ERR- Failed to get chip ID\n");
    } else {
        LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID);
    }

    enable_iio_sysfs();

    /* turn on power state */
    onPower(1);

    /* reset driver master enable */
    masterEnable(0);

    /* Load DMP image if capable, ie. MPU6xxx/9xxx */
    // TODO: disabled for GED tablet
#ifdef ENABLE_LP_QUAT_FEAT
    loadDMP();
#endif

    /* open temperature fd for temp comp */
    LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature);
    gyro_temperature_fd = open(mpu.temperature, O_RDONLY);
    if (gyro_temperature_fd == -1) {
        LOGE("HAL:could not open temperature node");
    } else {
        LOGV_IF(EXTRA_VERBOSE,
                "HAL:temperature_fd opened: %s", mpu.temperature);
    }

    /* read accel FSR to calcuate accel scale later */
    if (USE_THIRD_PARTY_ACCEL == 0) {
        char buf[3];
        int count = 0;
        LOGV_IF(SYSFS_VERBOSE,
                "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp());

        fd = open(mpu.accel_fsr, O_RDONLY);
        if(fd < 0) {
            LOGE("HAL:Error opening accel FSR");
        } else {
           memset(buf, 0, sizeof(buf));
           count = read_attribute_sensor(fd, buf, sizeof(buf));
           if(count < 1) {
               LOGE("HAL:Error reading accel FSR");
           } else {
               count = sscanf(buf, "%d", &mAccelScale);
               if(count)
                   LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale);
           }
           close(fd);
        }
    }

    /* initialize sensor data */
    memset(mPendingEvents, 0, sizeof(mPendingEvents));

    mPendingEvents[RotationVector].version = sizeof(sensors_event_t);
    mPendingEvents[RotationVector].sensor = ID_RV;
    mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR;
    mPendingEvents[RotationVector].acceleration.status
            = SENSOR_STATUS_ACCURACY_HIGH;

    mPendingEvents[LinearAccel].version = sizeof(sensors_event_t);
    mPendingEvents[LinearAccel].sensor = ID_LA;
    mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION;
    mPendingEvents[LinearAccel].acceleration.status
            = SENSOR_STATUS_ACCURACY_HIGH;

    mPendingEvents[Gravity].version = sizeof(sensors_event_t);
    mPendingEvents[Gravity].sensor = ID_GR;
    mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY;
    mPendingEvents[Gravity].acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;

    mPendingEvents[Gyro].version = sizeof(sensors_event_t);
    mPendingEvents[Gyro].sensor = ID_GY;
    mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE;
    mPendingEvents[Gyro].gyro.status = SENSOR_STATUS_ACCURACY_HIGH;

    mPendingEvents[Accelerometer].version = sizeof(sensors_event_t);
    mPendingEvents[Accelerometer].sensor = ID_A;
    mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER;
    mPendingEvents[Accelerometer].acceleration.status
            = SENSOR_STATUS_ACCURACY_HIGH;

    /* Invensense compass calibration */
    mPendingEvents[MagneticField].version = sizeof(sensors_event_t);
    mPendingEvents[MagneticField].sensor = ID_M;
    mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD;
    mPendingEvents[MagneticField].magnetic.status =
        SENSOR_STATUS_ACCURACY_HIGH;

    mPendingEvents[Orientation].version = sizeof(sensors_event_t);
    mPendingEvents[Orientation].sensor = ID_O;
    mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION;
    mPendingEvents[Orientation].orientation.status
            = SENSOR_STATUS_ACCURACY_HIGH;

    mHandlers[RotationVector] = &MPLSensor::rvHandler;
    mHandlers[LinearAccel] = &MPLSensor::laHandler;
    mHandlers[Gravity] = &MPLSensor::gravHandler;
    mHandlers[Gyro] = &MPLSensor::gyroHandler;
    mHandlers[Accelerometer] = &MPLSensor::accelHandler;
    mHandlers[MagneticField] = &MPLSensor::compassHandler;
    mHandlers[Orientation] = &MPLSensor::orienHandler;

    for (int i = 0; i < numSensors; i++) {
        mDelays[i] = 0;
    }

    (void)inv_get_version(&ver_str);
    LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str);

    /* setup MPL */
    inv_constructor_init();

    /* load calibration file from /data/cal.bin */
    rv = inv_load_calibration();
    if(rv == INV_SUCCESS)
        LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded");
    else
        LOGE("HAL:Could not open or load MPL calibration file (%d)", rv);

    inv_set_device_properties();

    /* disable driver master enable the first sensor goes on */
    masterEnable(0);
    enableGyro(0);
    enableAccel(0);
    enableCompass(0);
    onPower(0);

#ifdef INV_PLAYBACK_DBG
    logfile = fopen("/data/playback.bin", "wb");
    if (logfile)
        inv_turn_on_data_logging(logfile);
#endif

    mMplSensorInitialized = true;
}


void MPLSensor::enable_iio_sysfs() {
    VFUNC_LOG;

    int tempFd = 0;
    char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN];
    FILE *tempFp = NULL;

    LOGV_IF(SYSFS_VERBOSE, 
            "HAL:sysfs:echo 1 > %s (%lld)", 
            mpu.in_timestamp_en, getTimestamp());
    tempFd = open(mpu.in_timestamp_en, O_RDWR);
    if(tempFd < 0) {
        LOGE("HAL:could not open timestamp enable");
    } else if(enable_sysfs_sensor(tempFd, 1) < 0) {
        LOGE("HAL:could not enable timestamp");
    }

    LOGV_IF(SYSFS_VERBOSE, 
            "HAL:sysfs:cat %s (%lld)", 
            mpu.trigger_name, getTimestamp());
    tempFp = fopen(mpu.trigger_name, "r");
    if (tempFp == NULL) {
        LOGE("HAL:could not open trigger name");
    } else if (fscanf(tempFp, "%s", iio_trigger_name) < 0) {
        LOGE("HAL:could not read trigger name");
    }
    fclose(tempFp);

    LOGV_IF(SYSFS_VERBOSE, 
            "HAL:sysfs:echo %s > %s (%lld)", 
            iio_trigger_name, mpu.current_trigger, getTimestamp());
    tempFp = fopen(mpu.current_trigger, "w");
    if (tempFp == NULL) {
        LOGE("HAL:could not open current trigger");
    } else if (fprintf(tempFp, "%s", iio_trigger_name) < 0) {
        LOGE("HAL:could not write current trigger");
    }
    fclose(tempFp);

    LOGV_IF(SYSFS_VERBOSE, 
            "HAL:sysfs:echo %d > %s (%lld)", 
            IIO_BUFFER_LENGTH, mpu.buffer_length, getTimestamp());
    tempFp = fopen(mpu.buffer_length, "w");
    if (tempFp == NULL) {
        LOGE("HAL:could not open buffer length");
    } else if (fprintf(tempFp, "%d", IIO_BUFFER_LENGTH) < 0) {
        LOGE("HAL:could not write buffer length");
    }
    fclose(tempFp);

    inv_get_iio_device_node(iio_device_node);
    iio_fd = open(iio_device_node, O_RDONLY);
    if (iio_fd < 0) {
        LOGE("HAL:could not open iio device node");
    } else {
        LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd opened : %d", iio_fd);
    }
}

int MPLSensor::inv_constructor_init()
{
    VFUNC_LOG;

    inv_error_t result = inv_init_mpl();
    if (result) {
        LOGE("HAL:inv_init_mpl() failed");
        return result;
    }
    result = inv_constructor_default_enable();
    result = inv_start_mpl();
    if (result) {
        LOGE("HAL:inv_start_mpl() failed");
        LOG_RESULT_LOCATION(result);
        return result;
    }

    return result;
}

int MPLSensor::inv_constructor_default_enable()
{
    VFUNC_LOG;

    inv_error_t result;

    result = inv_enable_quaternion();
    if (result) {
        LOGE("HAL:Cannot enable quaternion\n");
        return result;
    }

    result = inv_enable_in_use_auto_calibration();
    if (result) {
        return result;
    }

    // TODO: double-check for GED tablet
    // result = inv_enable_motion_no_motion();
    result = inv_enable_fast_nomot();
    if (result) {
        return result;
    }

    result = inv_enable_gyro_tc();
    if (result) {
        return result;
    }

    result = inv_enable_hal_outputs();
    if (result) {
        return result;
    }

    if (!mCompassSensor->providesCalibration()) {
        /* Invensense compass calibration */
        LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled");
        result = inv_enable_vector_compass_cal();
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        result = inv_enable_compass_bias_w_gyro();
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        result = inv_enable_heading_from_gyro();
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }

        result = inv_enable_magnetic_disturbance();
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
    }

    result = inv_enable_9x_sensor_fusion();
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    result = inv_enable_no_gyro_fusion();
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    // TODO: double-check for GED tablet
    result = inv_enable_quat_accuracy_monitor();
    if (result) {
        LOG_RESULT_LOCATION(result);
        return result;
    }

    return result;
}

/* TODO: create function pointers to calculate scale */
void MPLSensor::inv_set_device_properties()
{
    VFUNC_LOG;

    unsigned short orient;

    inv_get_sensors_orientation();

    inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE);
    inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE);

    /* gyro setup */
    orient = inv_orientation_matrix_to_scalar(mGyroOrientation);
    inv_set_gyro_orientation_and_scale(orient, 2000L << 15);

    /* accel setup */
    orient = inv_orientation_matrix_to_scalar(mAccelOrientation);
    // BMA250
    //inv_set_accel_orientation_and_scale(orient, 1LL << 22);
    // MPU6050
    inv_set_accel_orientation_and_scale(orient, mAccelScale << 15);

    /* compass setup */
    signed char orientMtx[9];
    mCompassSensor->getOrientationMatrix(orientMtx);
    orient = 
        inv_orientation_matrix_to_scalar(orientMtx);
    long sensitivity;
    sensitivity = mCompassSensor->getSensitivity();
    inv_set_compass_orientation_and_scale(orient, sensitivity);
}

void MPLSensor::loadDMP()
{
    int res, fd;
    FILE *fptr;

    if (!strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050")) {
        //DMP support only for MPU6xxx/9xxx currently
        return;
    }

    /* load DMP firmware */
    LOGV_IF(SYSFS_VERBOSE, 
            "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp());
    fd = open(mpu.firmware_loaded, O_RDONLY);
    if(fd < 0) {
        LOGE("HAL:could not open dmp state");
    } else {
        if(inv_read_dmp_state(fd) == 0) {
            LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware);
            fptr = fopen(mpu.dmp_firmware, "w");
            if(!fptr) {
                LOGE("HAL:could not write to dmp");
            } else {
                int res = inv_load_dmp(fptr);
                if(res < 0) {
                    LOGE("HAL:load DMP failed");
                } else {
                    LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded");
                }
                fclose(fptr);
            }
        } else {
            LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded");
        }
    }

    // onDMP(1);		//Can't enable here. See note onDMP()
}

void MPLSensor::inv_get_sensors_orientation()
{
    FILE *fptr;

    // get gyro orientation
    LOGV_IF(SYSFS_VERBOSE, 
            "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp());
    fptr = fopen(mpu.gyro_orient, "r");
    if (fptr != NULL) {
        int om[9];
        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d",
               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], 
               &om[6], &om[7], &om[8]);
        fclose(fptr);

        LOGV_IF(EXTRA_VERBOSE,
                "HAL:gyro mounting matrix: "
                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);

        mGyroOrientation[0] = om[0];
        mGyroOrientation[1] = om[1];
        mGyroOrientation[2] = om[2];
        mGyroOrientation[3] = om[3];
        mGyroOrientation[4] = om[4];
        mGyroOrientation[5] = om[5];
        mGyroOrientation[6] = om[6];
        mGyroOrientation[7] = om[7];
        mGyroOrientation[8] = om[8];
    } else {
        LOGE("HAL:Couldn't read gyro mounting matrix");
    }

    // get accel orientation
    LOGV_IF(SYSFS_VERBOSE, 
            "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp());
    fptr = fopen(mpu.accel_orient, "r");
    if (fptr != NULL) {
        int om[9];
        fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", 
               &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], 
               &om[6], &om[7], &om[8]);
        fclose(fptr);

        LOGV_IF(EXTRA_VERBOSE,
                "HAL:accel mounting matrix: "
                "%+d %+d %+d %+d %+d %+d %+d %+d %+d",
                om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]);

        mAccelOrientation[0] = om[0];
        mAccelOrientation[1] = om[1];
        mAccelOrientation[2] = om[2];
        mAccelOrientation[3] = om[3];
        mAccelOrientation[4] = om[4];
        mAccelOrientation[5] = om[5];
        mAccelOrientation[6] = om[6];
        mAccelOrientation[7] = om[7];
        mAccelOrientation[8] = om[8];
    } else {
        LOGE("HAL:Couldn't read accel mounting matrix");
    }
}

MPLSensor::~MPLSensor()
{
    VFUNC_LOG;

    /* Close open fds */
    if (iio_fd > 0)
        close(iio_fd);

    if( accel_fd > 0 )
        close(accel_fd );
    if (gyro_temperature_fd > 0)
        close(gyro_temperature_fd);
    if (sysfs_names_ptr)
        free(sysfs_names_ptr);

    /* Turn off Gyro master enable          */
    /* A workaround until driver handles it */
    /* TODO: Turn off and close all sensors */
    LOGV_IF(SYSFS_VERBOSE,
            "HAL:sysfs:echo 0 > %s (%lld)", mpu.chip_enable, getTimestamp());
    int fd = open(mpu.chip_enable, O_RDWR);
    if(fd < 0) {
        LOGE("HAL:could not open gyro chip enable");
    } else {
        if(enable_sysfs_sensor(fd, 0) < 0) {
            LOGE("HAL:could not disable gyro master enable");
        }
    }

#ifdef INV_PLAYBACK_DBG
    inv_turn_off_data_logging();
    fclose(logfile);
#endif
}

#define GY_ENABLED ((1 << ID_GY) & enabled_sensors) // ID_GY = 0
#define A_ENABLED  ((1 << ID_A)  & enabled_sensors) // ID_A  = 1
#ifdef INVENSENSE_COMPASS_CAL                       // ID_M  = 2
#define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
#else
// TODO: ID_M = 2 even for 3rd-party solution
#define M_ENABLED  ((1 << ID_M)  & enabled_sensors)
#endif
#define O_ENABLED  ((1 << ID_O)  & enabled_sensors) // ID_O  = 3
#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) // ID_RV = 4
#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) // ID_LA = 5
#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) // ID_GR = 6

/* TODO: this step is optional, remove?  */
int MPLSensor::setGyroInitialState()
{
    VFUNC_LOG;

    int res = 0;

    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
            HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp());
    int fd = open(mpu.gyro_fifo_rate, O_RDWR);
    res = errno;
    if(fd < 0) {
        LOGE("HAL:open of %s failed with '%s' (%d)",
             mpu.gyro_fifo_rate, strerror(res), res);
        return res;
    }
    res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ);
    if(res < 0) {
        LOGE("HAL:write_attribute_sensor : error writing %s with %d",
             mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ);
        return res;
    }

    // Setting LPF is deprecated

    return 0;
}

/* this applies to BMA250 only */
int MPLSensor::setAccelInitialState()
{
    VFUNC_LOG;

    struct input_absinfo absinfo_x;
    struct input_absinfo absinfo_y;
    struct input_absinfo absinfo_z;
    float value;
    if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) &&
        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) &&
        !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) {
        value = absinfo_x.value;
        mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X;
        value = absinfo_y.value;
        mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y;
        value = absinfo_z.value;
        mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z;
        //mHasPendingEvent = true;
    }
    return 0;
}

int MPLSensor::onPower(int en)
{
    VFUNC_LOG;

    int res = 0;
    char buf[sizeof(int)+1];
    int count, curr_power_state;

    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
            en, mpu.power_state, getTimestamp());
    int tempFd = open(mpu.power_state, O_RDWR);
    res = errno;
    if(tempFd < 0){
        LOGE("HAL:Open of %s failed with '%s' (%d)",
             mpu.power_state, strerror(res), res);
    } else {
        // check and set new power state
        count = read_attribute_sensor(tempFd, buf, sizeof(buf));
        if(count < 1) {
            LOGE("HAL:Error reading power state");
            // will set power_state anyway
            curr_power_state= -1;
        } else {
            sscanf(buf, "%d", &curr_power_state);
        }
       
        if (en!=curr_power_state) {    
            if((res=enable_sysfs_sensor(tempFd, en)) < 0) {
                LOGE("HAL:Couldn't write power state");
            }
        } else {
            LOGV_IF(EXTRA_VERBOSE,
                    "HAL:Power state already enable/disable curr=%d new=%d", 
                    curr_power_state, en);
            close(tempFd);   
        }
    }
    return res;
}

int MPLSensor::onDMP(int en)
{
    VFUNC_LOG;

    int res= -1;
    int status;

    //Sequence to enable DMP
    //1. Turn On power if not already on
    //2. Load DMP image if not already loaded
    //3. Either Gyro or Accel must be enabled/configured before next step
    //4. Enable DMP

    if(read_sysfs_int(mpu.firmware_loaded, &status) < 0){
        LOGE("HAL:ERR can't get firmware_loaded status");
    } else if (status == 1) {
        //Write only if curr DMP state <> request
        if (read_sysfs_int(mpu.dmp_on, &status) < 0) {
            LOGE("HAL:ERR can't read DMP state");
        } else if (status != en) {
            if (write_sysfs_int(mpu.dmp_on, en) < 0) {
                LOGE("HAL:ERR can't write dmp_on");  
            } else {
                res= 0;	//Indicate write successful
            }
            //Enable DMP interrupt
            if (write_sysfs_int(mpu.dmp_int_on, en) < 0) {
                LOGE("HAL:ERR can't en/dis DMP interrupt");  
            }
        } else {
            res= 0;  	//DMP already set as requested
        }
    } else {
        LOGE("HAL:ERR No DMP image");
    }
    return res;
}

int MPLSensor::checkLPQuaternion(void)
{
    VFUNC_LOG;

    return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0);
}

int MPLSensor::enableLPQuaternion(int en)
{
    VFUNC_LOG;

    if (!en) {
        enableQuaternionData(0);
        onDMP(0);
        mFeatureActiveMask &= ~INV_DMP_QUATERNION;
        LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled");
    } else {
        if (enableQuaternionData(1) < 0 || onDMP(1) < 0) {    
            LOGE("HAL:ERR can't enable LP Quaternion");
        } else {
            mFeatureActiveMask |= INV_DMP_QUATERNION;
            LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled");
        }
    }
    return 0;
}

int MPLSensor::enableQuaternionData(int en)
{
    int res= 0;
    VFUNC_LOG;

    //Enable DMP quaternion
    if (write_sysfs_int(mpu.quaternion_on, en) < 0) {
        LOGE("HAL:ERR can't write DMP quaternion_on");
        res= -1;	//Indicate an err
    } 

    if (!en) {
        LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off");
        inv_quaternion_sensor_was_turned_off();
    } else {
        LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems");
        if (write_sysfs_int(mpu.in_quat_r_en, 1) < 0) {
            LOGE("HAL:ERR write in_quat_r_en");
        }
        if (write_sysfs_int(mpu.in_quat_x_en, 1) < 0) {
            LOGE("HAL:ERR write in_quat_x_en");
        }
        if (write_sysfs_int(mpu.in_quat_y_en, 1) < 0) {
            LOGE("HAL:ERR write in_quat_y_en");
        }
        if (write_sysfs_int(mpu.in_quat_z_en, 1) < 0) {
            LOGE("HAL:ERR write in_quat_z_en"); 
        }
    }

    return res;

}

int MPLSensor::enableTap(int en)
{
    VFUNC_LOG;

    return 0;
}

int MPLSensor::enableFlick(int en)
{
    VFUNC_LOG;

    return 0;
}

int MPLSensor::enablePedometer(int en)
{
    VFUNC_LOG;

    return 0;
}

int MPLSensor::masterEnable(int en)
{
    VFUNC_LOG;

    int res = 0;

    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
            en, mpu.chip_enable, getTimestamp());
    int tempFd = open(mpu.chip_enable, O_RDWR);
    res = errno;
    if(tempFd < 0){
        LOGE("HAL:open of %s failed with '%s' (%d)",
             mpu.chip_enable, strerror(res), res);
        return res;
    }
    res = enable_sysfs_sensor(tempFd, en);
    return res;
}

int MPLSensor::enableGyro(int en)
{
    VFUNC_LOG;

    int res = 0;

    /* need to also turn on/off the master enable */
    LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
            en, mpu.gyro_enable, getTimestamp());
    int tempFd = open(mpu.gyro_enable, O_RDWR);
    res = errno;
    if (tempFd > 0) {
        res = enable_sysfs_sensor(tempFd, en);
    } else {
        LOGE("HAL:open of %s failed with '%s' (%d)",
             mpu.gyro_enable, strerror(res), res);
    }

    if (!en) {
        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off");
        inv_gyro_was_turned_off();
    } else {
        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
                en, mpu.gyro_x_fifo_enable, getTimestamp());
        tempFd = open(mpu.gyro_x_fifo_enable, O_RDWR);
        res = errno;
        if (tempFd > 0) {
            res = enable_sysfs_sensor(tempFd, en);
        } else {
            LOGE("HAL:open of %s failed with '%s' (%d)",
                 mpu.gyro_x_fifo_enable, strerror(res), res);
        }

        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
                en, mpu.gyro_y_fifo_enable, getTimestamp());
        tempFd = open(mpu.gyro_y_fifo_enable, O_RDWR);
        res = errno;
        if (tempFd > 0) {
            res = enable_sysfs_sensor(tempFd, en);
        } else {
            LOGE("HAL:open of %s failed with '%s' (%d)",
                 mpu.gyro_y_fifo_enable, strerror(res), res);
        }

        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
                en, mpu.gyro_z_fifo_enable, getTimestamp());
        tempFd = open(mpu.gyro_z_fifo_enable, O_RDWR);
        res = errno;
        if (tempFd > 0) {
            res = enable_sysfs_sensor(tempFd, en);
        } else {
            LOGE("HAL:open of %s failed with '%s' (%d)",
                 mpu.gyro_z_fifo_enable, strerror(res), res);
        }
    }

    return res;
}

int MPLSensor::enableAccel(int en)
{
    VFUNC_LOG;

    int res;

   /* need to also turn on/off the master enable */
   LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
           en, mpu.accel_enable, getTimestamp());
   int tempFd = open(mpu.accel_enable, O_RDWR);
   res = errno;
   if (tempFd > 0) {
       res = enable_sysfs_sensor(tempFd, en);
   } else {
       LOGE("HAL:open of %s failed with '%s' (%d)",
            mpu.accel_enable, strerror(res), res);
   }

    if (!en) {
        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off");
        inv_accel_was_turned_off();
    } else {
        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
                en, mpu.accel_x_fifo_enable, getTimestamp());
        tempFd = open(mpu.accel_x_fifo_enable, O_RDWR);
        res = errno;
        if (tempFd > 0) {
            res = enable_sysfs_sensor(tempFd, en);
        } else {
            LOGE("HAL:open of %s failed with '%s' (%d)",
                 mpu.accel_x_fifo_enable, strerror(res), res);
        }

        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
                en, mpu.accel_y_fifo_enable, getTimestamp());
        tempFd = open(mpu.accel_y_fifo_enable, O_RDWR);
        res = errno;
        if (tempFd > 0) {
            res = enable_sysfs_sensor(tempFd, en);
        } else {
            LOGE("HAL:open of %s failed with '%s' (%d)",
                 mpu.accel_y_fifo_enable, strerror(res), res);
        }

        LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", 
                en, mpu.accel_z_fifo_enable, getTimestamp());
        tempFd = open(mpu.accel_z_fifo_enable, O_RDWR);
        res = errno;
        if (tempFd > 0) {
            res = enable_sysfs_sensor(tempFd, en);
        } else {
            LOGE("HAL:open of %s failed with '%s' (%d)",
                 mpu.accel_z_fifo_enable, strerror(res), res);
        }
    }

    if (!en && USE_THIRD_PARTY_ACCEL == 0) {
    } 

    if(USE_THIRD_PARTY_ACCEL == 1 && en) {
        setAccelInitialState(); // BMA250
    }
    return res;
}

int MPLSensor::enableCompass(int en)
{
    VFUNC_LOG;

    int res = mCompassSensor->enable(ID_M, en);
    if (en == 0) {
        LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off");
        inv_compass_was_turned_off();
    } 
    return res;
}

void MPLSensor::computeLocalSensorMask(int enabled_sensors)
{
    VFUNC_LOG;

    do {
        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
            LOGV_IF(ENG_VERBOSE, "FUSION ENABLED");
            mLocalSensorMask = ALL_MPL_SENSORS_NP;
            break;
        }

        if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
            /* Invensense compass cal */
            LOGV_IF(ENG_VERBOSE, "ALL DISABLED");
            mLocalSensorMask = 0;
            break;
        }

        if (GY_ENABLED) {
            LOGV_IF(ENG_VERBOSE, "G ENABLED");
            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
        } else {
            LOGV_IF(ENG_VERBOSE, "G DISABLED");
            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
        }

        if (A_ENABLED) {
            LOGV_IF(ENG_VERBOSE, "A ENABLED");
            mLocalSensorMask |= INV_THREE_AXIS_ACCEL;
        } else {
            LOGV_IF(ENG_VERBOSE, "A DISABLED");
            mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL;
        }

        /* Invensense compass calibration */
        if (M_ENABLED) {
            LOGV_IF(ENG_VERBOSE, "M ENABLED");
            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
        } else {
            LOGV_IF(ENG_VERBOSE, "M DISABLED");
            mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS;
        }
    } while (0);
}

int MPLSensor::enableSensors(unsigned long sensors, int en, uint32_t changed) {
    VFUNC_LOG;

    inv_error_t res = -1;
    int on = 1;
    int off = 0;

    // Sequence to enable or disable a sensor
    // 1. enable Power state 
    // 2. reset master enable (=0)
    // 3. enable or disable a sensor
    // 4. set master enable (=1)

    if (changed & ((1 << Gyro) | (1 << Accelerometer) |
            (mCompassSensor->isIntegrated() << MagneticField))) {
        /* ensure power state is on */
        onPower(1);
      
        /* reset master enable */
        res = masterEnable(0);
        if(res < 0) {
            return res;
        }
    }

    LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors);

    if (changed & (1 << Gyro)) {
        if(sensors & INV_THREE_AXIS_GYRO) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable gyro");
            res = enableGyro(on);
            if(res < 0) {
                return res;
            }
        } else if((sensors & INV_THREE_AXIS_GYRO) == 0) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable gyro");
            res = enableGyro(off);
            if(res < 0) {
                return res;
            }
        }
    }

    if (changed & (1 << Accelerometer)) {
        if(sensors & INV_THREE_AXIS_ACCEL) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable accel");
            res = enableAccel(on);
            if(res < 0) {
                return res;
            }
        } else if((sensors & INV_THREE_AXIS_ACCEL) == 0) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable accel");
            res = enableAccel(off);
            if(res < 0) {
                return res;
            }
        }
    }

    if (changed & (1 << MagneticField)) {
        /* Invensense compass calibration */
        if (sensors & INV_THREE_AXIS_COMPASS) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - enable compass");
            res = enableCompass(on);
            if(res < 0) {
                return res;
            }
        } else if ((sensors & INV_THREE_AXIS_COMPASS) == 0) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - disable compass");
            res = enableCompass(off);
            if(res < 0) {
                return res;
            }
        }
    }

// TODO: disabled for GED tablet
#ifdef ENABLE_LP_QUAT_FEAT
    // Enable LP Quat
    if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) |
            (1 << LinearAccel) | (1 << Gravity)))) {
        if (!checkLPQuaternion()) {
            enableLPQuaternion(1);
        } else {
            LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled");
        }
    } else if (checkLPQuaternion()) {
        enableLPQuaternion(0);
    }
#endif

    /*
        if sensor & THREE_AXIS_GYRO
            enable = 1
        if sensor & THREE_AXIS_ACCEL
            enable = 1
        if compass_on_secondary
            if sensor & THREE_AXIS_COMPASS
                enable = 1
        else
            enable = 0
    */
    if (changed & ((1 << Gyro) | (1 << Accelerometer) |
            (mCompassSensor->isIntegrated() << MagneticField))) {
        if (sensors & 
            (INV_THREE_AXIS_GYRO 
                | INV_THREE_AXIS_ACCEL 
                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
            res = masterEnable(1);
            if(res < 0) {
                return res;
            }
        } else { // all sensors idle -> reduce power
            res = onPower(0);
            if(res < 0) {
                return res;
            }
            storeCalibration();
        }
    }

    return res;
}

/* Store calibration file */
void MPLSensor::storeCalibration()
{
    if(mHaveGoodMpuCal == true || mAccelAccuracy >= 2) {
       int res = inv_store_calibration();
       if (res) {
           LOGE("HAL:Cannot store calibration on file");
       } else {
           LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated");
       }
    }
}

void MPLSensor::cbProcData()
{
    mNewData = 1;
    mSampleCount++;
    LOGV_IF(EXTRA_VERBOSE, "HAL:new data");
}

/*  these handlers transform mpl data into one of the Android sensor types */
int MPLSensor::gyroHandler(sensors_event_t* s)
{
    VHANDLER_LOG;
    int update;
    update = inv_get_sensor_type_gyroscope(s->gyro.v, &s->gyro.status, &s->timestamp);
    LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d",
            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
    return update;
}

int MPLSensor::accelHandler(sensors_event_t* s)
{
    VHANDLER_LOG;
    int update;
    update = inv_get_sensor_type_accelerometer(
        s->acceleration.v, &s->acceleration.status, &s->timestamp);
    LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d",
            s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2], 
            s->timestamp, update);
    mAccelAccuracy = s->acceleration.status;
    return update;
}

int MPLSensor::compassHandler(sensors_event_t* s)
{
    VHANDLER_LOG;
    int update;
    update = inv_get_sensor_type_magnetic_field(
        s->magnetic.v, &s->magnetic.status, &s->timestamp);
    LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d",
            s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update);
    return update;
}

int MPLSensor::rvHandler(sensors_event_t* s)
{
    // rotation vector does not have an accuracy or status
    VHANDLER_LOG;
    int8_t status;
    int update;
    update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp);
    LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d",
            s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update);
    return update;
}

int MPLSensor::laHandler(sensors_event_t* s)
{
    VHANDLER_LOG;
    int update;
    update = inv_get_sensor_type_linear_acceleration(
            s->gyro.v, &s->gyro.status, &s->timestamp);
    LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d",
            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
    return update;
}

int MPLSensor::gravHandler(sensors_event_t* s)
{
    VHANDLER_LOG;
    int update;
    update = inv_get_sensor_type_gravity(s->gyro.v, &s->gyro.status, &s->timestamp);
    LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d",
            s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update);
    return update;
}

int MPLSensor::orienHandler(sensors_event_t* s)
{
    VHANDLER_LOG;
    int update;
    update = inv_get_sensor_type_orientation(
            s->orientation.v, &s->orientation.status, &s->timestamp);
    LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d",
            s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update);
    return update;
}

int MPLSensor::enable(int32_t handle, int en)
{
    VFUNC_LOG;

    android::String8 sname;
    int what = -1;

    switch (handle) {
    case ID_A:
        what = Accelerometer;
        sname = "Accelerometer";
        break;
    case ID_M:
        what = MagneticField;
        sname = "MagneticField";
        break;
    case ID_O:
        what = Orientation;
        sname = "Orientation";
        break;
    case ID_GY:
        what = Gyro;
        sname = "Gyro";
        break;
    case ID_GR:
        what = Gravity;
        sname = "Gravity";
        break;
    case ID_RV:
        what = RotationVector;
        sname = "RotationVector";
        break;
    case ID_LA:
        what = LinearAccel;
        sname = "LinearAccel";
        break;
    default: //this takes care of all the gestures
        what = handle;
        sname = "Others";
        break;
    }

    if (uint32_t(what) >= numSensors)
        return -EINVAL;

    int newState = en ? 1 : 0;
    int err = 0;
    unsigned long sen_mask;

    LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle,
            ((mEnabled & (1 << what)) ? "en" : "dis"),
            ((uint32_t(newState) << what) ? "en" : "dis"));
    LOGV_IF(PROCESS_VERBOSE,
            "HAL:%s sensor state change what=%d", sname.string(), what);

    // TODO: disabled for GED tablet
    // pthread_mutex_lock(&mMplMutex);
    // pthread_mutex_lock(&mHALMutex);

    if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) {
        uint32_t sensor_type;
        short flags = newState;
        uint32_t lastEnabled = mEnabled, changed = 0;

        mEnabled &= ~(1 << what);
        mEnabled |= (uint32_t(flags) << what);

        LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle);
        LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags);
        computeLocalSensorMask(mEnabled);
        LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled);
        sen_mask = mLocalSensorMask & mMasterSensorMask;
        mSensorMask = sen_mask;
        LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask);

        switch (what) {
            case Gyro:
            case Accelerometer:
            case MagneticField:
                if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
                        (1 << LinearAccel) | (1 << Gravity))) &&
                        ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) {
                    changed |= (1 << what);
                }
                break;

            case Orientation:
            case RotationVector:
            case LinearAccel:
            case Gravity:
                if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) |
                        (1 << LinearAccel) | (1 << Gravity)))) ||
                        (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) |
                        (1 << LinearAccel) | (1 << Gravity))))) {
                    for (int i = Gyro; i <= MagneticField; i++) {
                        if (!(mEnabled & (1 << i))) {
                            changed |= (1 << i);
                        }
                    }
                }
                break;
        }
        LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed);
        enableSensors(sen_mask, flags, changed);
    }

    // pthread_mutex_unlock(&mMplMutex);
    // pthread_mutex_unlock(&mHALMutex);

#ifdef INV_PLAYBACK_DBG
    /* apparently the logging needs to be go through this sequence 
       to properly flush the log file */
    inv_turn_off_data_logging();
    fclose(logfile);
    logfile = fopen("/data/playback.bin", "ab");
    if (logfile)
        inv_turn_on_data_logging(logfile);
#endif

    return err;
}

int MPLSensor::setDelay(int32_t handle, int64_t ns)
{
    VFUNC_LOG;

    android::String8 sname;
    int what = -1;

    switch (handle) {
    case ID_A:
        what = Accelerometer;
        sname = "Accelerometer";
        break;
    case ID_M:
        what = MagneticField;
        sname = "MagneticField";
        break;
    case ID_O:
        what = Orientation;
        sname = "Orientation";
        break;
    case ID_GY:
        what = Gyro;
        sname = "Gyro";
        break;
    case ID_GR:
        what = Gravity;
        sname = "Gravity";
        break;
    case ID_RV:
        what = RotationVector;
        sname = "RotationVector";
        break;
    case ID_LA:
        what = LinearAccel;
        sname = "LinearAccel";
        break;
    default: // this takes care of all the gestures
        what = handle;
        sname = "Others";
        break;
    }

// TODO: disabled for GED tablet
#if 0
    // skip the 1st call for enalbing sensors called by ICS/JB sensor service
    static int counter_delay = 0;
    if (!(mEnabled & (1 << what))) {
        counter_delay = 0;
    } else {
        if (++counter_delay == 1) {
            return 0;
        }
        else {
            counter_delay = 0;
        }
    }
#endif

    if (uint32_t(what) >= numSensors)
        return -EINVAL;

    if (ns < 0)
        return -EINVAL;

    LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns);

    // TODO: for GED tablet
    // limit all rates to reasonable ones */
/*
    if (ns < 10000000LL) {
        ns = 10000000LL;
    }
*/
    if (ns < 5000000LL) {
        ns = 5000000LL;
    }

    /* store request rate to mDelays arrary for each sensor */
    mDelays[what] = ns;

    switch (what) {
        case Gyro:
        case Accelerometer:
            for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated();
                    i++) {
                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
                    LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
                    return 0;
                }
            }
            break;

        case MagneticField:
            if (mCompassSensor->isIntegrated() &&
                    (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) ||
                    ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) {
                 LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel");
                 return 0;
            }
            break;

        case Orientation:
        case RotationVector:
        case LinearAccel:
        case Gravity:
            for (int i = 0; i < numSensors; i++) {
                if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) {
                    LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i);
                    return 0;
                }
            }
            break;
    }

    // TODO: disabled for GED tablet
    // pthread_mutex_lock(&mHALMutex);
    int res = update_delay();
    // pthread_mutex_unlock(&mHALMutex);
    return res;
}

int MPLSensor::update_delay() {
    VHANDLER_LOG;

    int res = 0;
    int64_t got;

    if (mEnabled) {
        uint64_t wanted = -1LLU;
        uint64_t wanted_ec = -1LLU;

        // Sequence to change sensor's FIFO rate 
        // 1. enable Power state 
        // 2. reset master enable
        // 3. Update delay
        // 4. set master enable

        // TODO: unnecessary for IIO
        // ensure power on
        // onPower(1);

        // TODO: unnecessary for IIO
        // reset master enable
        // masterEnable(0);

        /* search the minimum delay requested across all enabled sensors */
        for (int i = 0; i < numSensors; i++) {
            if (mEnabled & (1 << i)) {
                uint64_t ns = mDelays[i];
                wanted = wanted < ns ? wanted : ns;
            }
        }

	      // same delay for ext compass
        wanted_ec = wanted;

        /* mpl rate in us in future maybe different for
           gyro vs compass vs accel */
        int rateInus = (int)wanted / 1000LL;
        int mplGyroRate = rateInus;
        int mplAccelRate = rateInus;
        int mplCompassRate = rateInus;

        LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : "
             "%llu ns, mpl rate: %d us, (%.2f Hz)",
             wanted, rateInus, 1000000000.f / wanted);

        /* set rate in MPL */
        /* compass can only do 100Hz max */
        inv_set_gyro_sample_rate(mplGyroRate);
        inv_set_accel_sample_rate(mplAccelRate);
        inv_set_compass_sample_rate(mplCompassRate);

// TODO: disabled for GED tablet
#ifdef ENABLE_LP_QUAT_FEAT
        // Set LP Quaternion sample rate if enabled
        if (checkLPQuaternion()) {
            // TODO: need to verify this part for LPQ
            if (wanted < 5000000LL) {
                enableLPQuaternion(0);
            } else {
                inv_set_quat_sample_rate(rateInus);
                // set DMP output rate to FIFO 
                write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / wanted);

                //DMP running rate must be @ 200Hz 
                wanted= 5000000LL;	
                LOGV("HAL:DMP rate= %.2f Hz Fifo Rate= %d us", 1000000000.f / wanted, rateInus);
            }
        }
#endif

        /* TODO: Test 200Hz */
        // inv_set_gyro_sample_rate(5000);
        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate);
        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate);
        LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate);

        int enabled_sensors = mEnabled;
        int tempFd = -1;
        if(LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
            uint64_t tempRate = wanted;
            LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion");
            //nsToHz
            LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
                    1000000000.f / tempRate, mpu.gyro_fifo_rate, 
                    getTimestamp());
            tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
            res = write_attribute_sensor(tempFd, 1000000000.f / tempRate);
            if(res < 0) {
                LOGE("HAL:GYRO update delay error");
            }

            //nsToHz (BMA250)
            if(USE_THIRD_PARTY_ACCEL == 1) {
                LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)",
                        wanted / 1000000L, mpu.accel_fifo_rate, 
                        getTimestamp());
                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
                res = write_attribute_sensor(tempFd, wanted / 1000000L);
                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
            }

            if (!mCompassSensor->isIntegrated()) {
                mCompassSensor->setDelay(ID_M, wanted_ec);
                got = mCompassSensor->getDelay(ID_M);
                inv_set_compass_sample_rate(got / 1000);
            }

            /*
            //nsTons - nothing to be done
            strcpy(&compass_sensor_sysfs_path[compass_sensor_sysfs_path_len],
                   COMPASS_SENSOR_DELAY);
            tempFd = open(compass_sensor_sysfs_path, O_RDWR);
            LOGV_IF(PROCESS_VERBOSE, "setDelay - open path: %s", compass_sensor_sysfs_path);
            wanted = 20000000LLU;
            res = write_attribute_sensor(tempFd, wanted);
            if(res < 0) {
                LOGE("Compass update delay error");
            }
            */

        } else {

            if (GY_ENABLED) {
                wanted = mDelays[Gyro];
                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
                        1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp());
                tempFd = open(mpu.gyro_fifo_rate, O_RDWR);
                res = write_attribute_sensor(tempFd, 1000000000.f / wanted);
                LOGE_IF(res < 0, "HAL:GYRO update delay error");
            }

            if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */
                if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) {
                    wanted = mDelays[Gyro];
                } else {
                    wanted = mDelays[Accelerometer];
                }
                /* TODO: use function pointers to calculate delay value specific to vendor */
                LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)",
                        1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp());
                tempFd = open(mpu.accel_fifo_rate, O_RDWR);
                //BMA250 in ms
                //res = write_attribute_sensor(tempFd, wanted / 1000000L);
                //MPU6050 in hz
                res = write_attribute_sensor(tempFd, 1000000000.f/wanted);
                LOGE_IF(res < 0, "HAL:ACCEL update delay error");
            }

            /* Invensense compass calibration */
            if (M_ENABLED) {
                if (!mCompassSensor->isIntegrated()) {
                    wanted = mDelays[MagneticField];
                } else {
                    if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) {
                        wanted = mDelays[Gyro];
                    } else if (GY_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) {
                        wanted = mDelays[Accelerometer];
                    } else {
                        wanted = mDelays[MagneticField];
                    }
                }
                mCompassSensor->setDelay(ID_M, wanted);
                got = mCompassSensor->getDelay(ID_M);
                inv_set_compass_sample_rate(got / 1000);
            }

        }

        /*
            if sensor & THREE_AXIS_GYRO
                enable = 1
            if sensor & THREE_AXIS_ACCEL
                enable = 1
            if compass_on_secondary
                if sensor & THREE_AXIS_COMPASS
                    enable = 1
            else
                enable = 0
        */
        unsigned long sensors = mLocalSensorMask & mMasterSensorMask;
        if (sensors & 
            (INV_THREE_AXIS_GYRO 
                | INV_THREE_AXIS_ACCEL 
                | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) {
// TODO: unnecessary for IIO
#if 0
            res = masterEnable(1);
            if(res < 0) {
                return res;
            }
#endif
        } else { // all sensors idle -> reduce power
// TODO: unnecessary for IIO
#if 0
            res = onPower(0);
            if(res < 0) {
                return res;
            }
#endif
        }
    }

    return res;
}

/* use for third party accel */
int MPLSensor::readAccelEvents(sensors_event_t* data, int count)
{
    VHANDLER_LOG;

    if (count < 1)
        return -EINVAL;

    ssize_t n = mAccelInputReader.fill(accel_fd);
    if (n < 0) {
        LOGE("HAL:missed accel events, exit");
        return n;
    }

    int numEventReceived = 0;
    input_event const* event;
    int nb, done = 0;

    while (done == 0 && count && mAccelInputReader.readEvent(&event)) {
        int type = event->type;
        if (type == EV_ABS) {
            if (event->code == EVENT_TYPE_ACCEL_X) {
                mPendingMask |= 1 << Accelerometer;
                mCachedAccelData[0] = event->value;
            } else if (event->code == EVENT_TYPE_ACCEL_Y) {
                mPendingMask |= 1 << Accelerometer;
                mCachedAccelData[1] = event->value;
            } else if (event->code == EVENT_TYPE_ACCEL_Z) {
                mPendingMask |= 1 << Accelerometer;
                mCachedAccelData[2] =event-> value;
            }
        } else if (type == EV_SYN) {
            done = 1;
            if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
                inv_build_accel(mCachedAccelData, 0, getTimestamp());
            }
        } else {
            LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)",
                    type, event->code);
        }
        mAccelInputReader.next();
    }

    LOGV_IF(ENG_VERBOSE, "HAL:readAccelEvents - events read=%d", numEventReceived);

    return numEventReceived;
}

/** 
 *  Should be called after reading at least one of gyro
 *  compass or accel data. You should only read 1 sample of
 *  data and call this.
 *  @returns 0, if successful, error number if not.
 */
int MPLSensor::executeOnData(sensors_event_t* data, int count)
{
    VFUNC_LOG;

    inv_execute_on_data();

    int numEventReceived = 0;

    long msg;
    msg = inv_get_message_level_0(1);
    if (msg) {
        if (msg & INV_MSG_MOTION_EVENT) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n");
        }
        if (msg & INV_MSG_NO_MOTION_EVENT) {
            LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n");
            /* after the first no motion, the gyro should be
               calibrated well */
            mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH;
            /* if gyros are on and we got a no motion, set a flag
               indicating that the cal file can be written. */
            mHaveGoodMpuCal = true;
        }
    }

    // load up virtual sensors
    for (int i = 0; i < numSensors; i++) {
        int update;
        if (mEnabled & (1 << i)) {
            update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i);
            mPendingMask |= (1 << i);

            if (update && (count > 0)) {
                *data++ = mPendingEvents[i];
                count--;
                numEventReceived++;
            }
        }
    }

    return numEventReceived;
}

int MPLSensor::readEvents(sensors_event_t *data, int count) {


    int lp_quaternion_on, nbyte;
    int i, nb, mask = 0, numEventReceived = 0,
        sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1: 0) +
            (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1: 0);
    char *rdata = mIIOBuffer;

    nbyte= (8 * sensors + 8) * 1;

// TODO: disabled for GED tablet
#ifdef ENABLE_LP_QUAT_FEAT
    lp_quaternion_on = checkLPQuaternion();
    if (lp_quaternion_on == 1) {
        nbyte += sizeof(mCachedQuaternionData);		//currently 16 bytes for Q data
    }
#endif

    // TODO: disabled for GED tablet
    // pthread_mutex_lock(&mMplMutex);
    // pthread_mutex_lock(&mHALMutex);

    size_t rsize = read(iio_fd, rdata, nbyte);
    if (sensors == 0) {
        // read(iio_fd, rdata, nbyte);
        read(iio_fd, rdata, (16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH);
    }
/*
    LOGI("get one sample of IIO data with size: %d", rsize);
    LOGI("sensors: %d", sensors);

    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d",
        *((short *) (rdata + 0)), *((short *) (rdata + 2)),
        *((short *) (rdata + 4)));
    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d",
        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))),
        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));

    LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS &
        mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d",
        *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
        *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))),
        *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) +
            ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0)));
*/

    // TODO: need to verify this for LPQ
    if (rsize < (nbyte - 8)) {
        LOGE("HAL:ERR Full data packet was not read");
        // return -1;
    }

// TODO: disabled for GED tablet
#ifdef ENABLE_LP_QUAT_FEAT
    if (lp_quaternion_on == 1) {
        for (i=0; i< 4; i++) {
            mCachedQuaternionData[i]= *(long*)rdata;
            rdata += sizeof(long);
        }
    }
/*
    LOGV("HAL:rdata= %x sensors= %d  lp_q_on= %d nbyte= %d rsize= %d",
        rdata, sensors, lp_quaternion_on, nbyte, rsize);  //tbd
*/
#endif

    for (i = 0; i < 3; i++) {
        if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
            mCachedGyroData[i] = *((short *) (rdata + i * 2));
        }
        if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
            mCachedAccelData[i] = *((short *) (rdata + i * 2 +
                ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0)));
        }
        if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) {
            mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1)));
        }
    }

    mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1: 0) +
        ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 2: 0));
    if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() &&
            (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) {
        mask |= 4;
    }

    mSensorTimestamp = *((long long *) (rdata + 8 * sensors));
    if (mCompassSensor->isIntegrated()) {
        mCompassTimestamp = mSensorTimestamp;
    }

    // send down temperature every 0.5 seconds
    if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) {
        mTempCurrentTime = mSensorTimestamp;
        long long temperature[2];
        if(inv_read_temperature(temperature) == 0) {
            LOGV_IF(INPUT_DATA,
                    "HAL:inv_read_temperature = %lld, timestamp= %lld",
                    temperature[0], temperature[1]);
            inv_build_temp(temperature[0], temperature[1]);
        }
#ifdef TESTING
        long bias[3], temp, temp_slope[3];
        inv_get_gyro_bias(bias, &temp);
        inv_get_gyro_ts(temp_slope);
        
        LOGI("T: %.3f "
             "GB: %+13f %+13f %+13f "
             "TS: %+13f %+13f %+13f "
             "\n", 
             (float)temperature[0] / 65536.f,
             (float)bias[0] / 65536.f / 16.384f,
             (float)bias[1] / 65536.f / 16.384f,
             (float)bias[2] / 65536.f / 16.384f,
             temp_slope[0] / 65536.f,
             temp_slope[1] / 65536.f,
             temp_slope[2] / 65536.f);
#endif
    }

    if (mask & 1) {
        mPendingMask |= 1 << Gyro;
        if (mLocalSensorMask & INV_THREE_AXIS_GYRO) {
            inv_build_gyro(mCachedGyroData, mSensorTimestamp);
            LOGV_IF(INPUT_DATA,
                    "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld",
                    mCachedGyroData[0], mCachedGyroData[1], 
                    mCachedGyroData[2], mSensorTimestamp);
        }
    }

    if (mask & 2) {
        mPendingMask |= 1 << Accelerometer;
        if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) {
            inv_build_accel(mCachedAccelData, 0, mSensorTimestamp);
             LOGV_IF(INPUT_DATA,
                    "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld",
                    mCachedAccelData[0], mCachedAccelData[1], 
                    mCachedAccelData[2], mSensorTimestamp);
        }
    }

    if ((mask & 4) && mCompassSensor->isIntegrated()) {
        int status = 0;
        if (mCompassSensor->providesCalibration()) {
            status = mCompassSensor->getAccuracy();
            status |= INV_CALIBRATED;
        }
        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
            inv_build_compass(mCachedCompassData, status,
                              mCompassTimestamp);
            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", 
                    mCachedCompassData[0], mCachedCompassData[1], 
                    mCachedCompassData[2], mCompassTimestamp);
        }
    }

// TODO: disabled for GED tablet
#ifdef ENABLE_LP_QUAT_FEAT
    if (lp_quaternion_on == 1) {
        inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp);
        LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld - %d",
                    mCachedQuaternionData[0], mCachedQuaternionData[1], 
                    mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp);
    }
#endif

    // pthread_mutex_unlock(&mMplMutex);
    // pthread_mutex_unlock(&mHALMutex);

    return numEventReceived;
}

/* use for both MPUxxxx and third party compass */
int MPLSensor::readCompassEvents(sensors_event_t *data, int count)
{
    VHANDLER_LOG;

    if (count < 1)
        return -EINVAL;

    int numEventReceived = 0;
    int done = 0;
    int nb;

    // TODO: disabled for GED tablet
    // TODO: for AMI306
    // pthread_mutex_lock(&mMplMutex);
    // pthread_mutex_lock(&mHALMutex);

    done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp);
    if (done > 0) {
        int status = 0;
        if (mCompassSensor->providesCalibration()) {
            status = mCompassSensor->getAccuracy();
            status |= INV_CALIBRATED;
        }
        if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) {
            inv_build_compass(mCachedCompassData, status,
                              mCompassTimestamp);
            LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", 
                    mCachedCompassData[0], mCachedCompassData[1], 
                    mCachedCompassData[2], mCompassTimestamp);
        }
    }

    // pthread_mutex_unlock(&mMplMutex);
    // pthread_mutex_unlock(&mHALMutex);

    return numEventReceived;
}

int MPLSensor::getFd() const
{
    VFUNC_LOG;
    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd);
    return iio_fd;
}

int MPLSensor::getAccelFd() const
{
    VFUNC_LOG;
    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd);
    return accel_fd;
}

int MPLSensor::getCompassFd() const
{
    VFUNC_LOG;
    int fd = mCompassSensor->getFd();
    LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd);
    return fd;
}

int MPLSensor::getPollTime()
{
    VHANDLER_LOG;
    return mPollTime;
}

bool MPLSensor::hasPendingEvents() const
{
    VHANDLER_LOG;
    // if we are using the polling workaround, force the main
    // loop to check for data every time
    return (mPollTime != -1);
}

/* TODO: support resume suspend when we gain more info about them*/
void MPLSensor::sleepEvent()
{
    VFUNC_LOG;
}

void MPLSensor::wakeEvent()
{
    VFUNC_LOG;
}

int MPLSensor::inv_float_to_q16(float *fdata, long *ldata)
{
    VHANDLER_LOG;

    if (!fdata || !ldata)
        return -1;
    ldata[0] = (long)(fdata[0] * 65536.f);
    ldata[1] = (long)(fdata[1] * 65536.f);
    ldata[2] = (long)(fdata[2] * 65536.f);
    return 0;
}

int MPLSensor::inv_long_to_q16(long *fdata, long *ldata)
{
    VHANDLER_LOG;

    if (!fdata || !ldata)
        return -1;
    ldata[0] = (fdata[1] * 65536.f);
    ldata[1] = (fdata[2] * 65536.f);
    ldata[2] = (fdata[3] * 65536.f);
    return 0;
}

int MPLSensor::inv_float_to_round(float *fdata, long *ldata)
{
    VHANDLER_LOG;

    if (!fdata || !ldata)
            return -1;
    ldata[0] = (long)fdata[0];
    ldata[1] = (long)fdata[1];
    ldata[2] = (long)fdata[2];
    return 0;
}

int MPLSensor::inv_float_to_round2(float *fdata, short *ldata)
{
    VHANDLER_LOG;

    if (!fdata || !ldata)
        return -1;
    ldata[0] = (short)fdata[0];
    ldata[1] = (short)fdata[1];
    ldata[2] = (short)fdata[2];
    return 0;
}

int MPLSensor::inv_read_temperature(long long *data)
{
    VHANDLER_LOG;

    int count = 0;
    char raw_buf[40];
    long raw = 0;

    long long timestamp = 0;

    memset(raw_buf, 0, sizeof(raw_buf));
    count = read_attribute_sensor(gyro_temperature_fd, raw_buf,
                                  sizeof(raw_buf));
    if(count < 1) {
        LOGE("HAL:error reading gyro temperature");
        return -1;
    }

    count = sscanf(raw_buf, "%ld%lld", &raw, &timestamp);

    if(count < 0) {
        return -1;
    }

    LOGV_IF(ENG_VERBOSE, 
            "HAL:temperature raw = %ld, timestamp = %lld, count = %d",
            raw, timestamp, count);
    data[0] = raw;
    data[1] = timestamp;

    return 0;
}

int MPLSensor::inv_read_dmp_state(int fd)
{
    VFUNC_LOG;

    if(fd < 0)
        return -1;

    int count = 0;
    char raw_buf[10];
    short raw = 0;

    memset(raw_buf, 0, sizeof(raw_buf));
    count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf));
    if(count < 1) {
        LOGE("HAL:error reading dmp state");
        close(fd);
        return -1;
    }
    count = sscanf(raw_buf, "%hd", &raw);
    if(count < 0) {
        LOGE("HAL:dmp state data is invalid");
        close(fd);
        return -1;
    }
    LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count);
    close(fd);
    return (int)raw;
}

int MPLSensor::inv_read_sensor_bias(int fd, long *data)
{
    VFUNC_LOG;

    if(fd == -1) {
        return -1;
    }

    char buf[50];
    char x[15], y[15], z[15];

    memset(buf, 0, sizeof(buf));
    int count = read_attribute_sensor(fd, buf, sizeof(buf));
    if(count < 1) {
        LOGE("HAL:Error reading gyro bias");
        return -1;
    }
    count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z);
    if(count) {
        /* scale appropriately for MPL */
        LOGV_IF(ENG_VERBOSE, 
                "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)", 
                atol(x), atol(y), atol(z));

        data[0] = (long)(atol(x) / 10000 * (1L << 16));
        data[1] = (long)(atol(y) / 10000 * (1L << 16));
        data[2] = (long)(atol(z) / 10000 * (1L << 16));

        LOGV_IF(ENG_VERBOSE, 
                "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)", 
                data[0], data[1], data[2]);
    }
    return 0;
}


/** fill in the sensor list based on which sensors are configured.
 *  return the number of configured sensors.
 *  parameter list must point to a memory region of at least 7*sizeof(sensor_t)
 *  parameter len gives the length of the buffer pointed to by list
 */

int MPLSensor::populateSensorList(struct sensor_t *list, int len)
{
    VFUNC_LOG;

    int numsensors;

    if(len < (int)(7 * sizeof(sensor_t))) {
        LOGE("HAL:sensor list too small, not populating.");
        return 0;
    }

    /* fill in the base values */
    memcpy(list, sSensorList, sizeof (struct sensor_t) * 7);

    /* first add gyro, accel and compass to the list */

    /* fill in gyro/accel values */
    if(chip_ID == NULL) {
        LOGE("HAL:Can not get gyro/accel id");
    }
    fillGyro(chip_ID, list);
    fillAccel(chip_ID, list);

    // TODO: need fixes for unified HAL and 3rd-party solution
    mCompassSensor->fillList(&list[MagneticField]);

    if(1) {
        numsensors = 7;
        /* all sensors will be added to the list
           fill in orientation values */
        fillOrientation(list);
        /* fill in rotation vector values */
        fillRV(list);
        /* fill in gravity values */
        fillGravity(list);
        /* fill in Linear accel values */
        fillLinearAccel(list);
    } else {
        /* no 9-axis sensors, zero fill that part of the list */
        numsensors = 3;
        memset(list + 3, 0, 4 * sizeof(struct sensor_t));
    }

    return numsensors;
}

void MPLSensor::fillAccel(const char* accel, struct sensor_t *list)
{
    VFUNC_LOG;

    if (accel) {
        if(accel != NULL && strcmp(accel, "BMA250") == 0) {
            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
            list[Accelerometer].power = ACCEL_BMA250_POWER;
            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
            return;
        } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) {
            list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE;
            list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION;
            list[Accelerometer].power = ACCEL_MPU6050_POWER;

            // TODO: for GED tablet
            // list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY;
            list[Accelerometer].minDelay = 5000;

            return;
        } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) {
            list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE;
            list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION;
            list[Accelerometer].power = ACCEL_MPU9150_POWER;
            list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY;
            return;
        } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) { 
            list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
            list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
            list[Accelerometer].power = ACCEL_BMA250_POWER;
            list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
            return;
        }
    }

    LOGE("HAL:unknown accel id %s -- "
         "params default to bma250 and might be wrong.",
         accel);
    list[Accelerometer].maxRange = ACCEL_BMA250_RANGE;
    list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION;
    list[Accelerometer].power = ACCEL_BMA250_POWER;
    list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY;
}

void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list)
{
    VFUNC_LOG;

    if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) {
        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
        list[Gyro].power = GYRO_MPU3050_POWER;
        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
    } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) {
        list[Gyro].maxRange = GYRO_MPU6050_RANGE;
        list[Gyro].resolution = GYRO_MPU6050_RESOLUTION;
        list[Gyro].power = GYRO_MPU6050_POWER;

        // TODO: for GED tablet
        // list[Gyro].minDelay = GYRO_MPU6050_MINDELAY;
        list[Gyro].minDelay = 5000;
    } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) {
        list[Gyro].maxRange = GYRO_MPU9150_RANGE;
        list[Gyro].resolution = GYRO_MPU9150_RESOLUTION;
        list[Gyro].power = GYRO_MPU9150_POWER;
        list[Gyro].minDelay = GYRO_MPU9150_MINDELAY;
    } else {
        LOGE("HAL:unknown gyro id -- gyro params will be wrong.");
        LOGE("HAL:default to use mpu3050 params");
        list[Gyro].maxRange = GYRO_MPU3050_RANGE;
        list[Gyro].resolution = GYRO_MPU3050_RESOLUTION;
        list[Gyro].power = GYRO_MPU3050_POWER;
        list[Gyro].minDelay = GYRO_MPU3050_MINDELAY;
    }
    return;
}

/* fillRV depends on values of accel and compass in the list */
void MPLSensor::fillRV(struct sensor_t *list)
{
    VFUNC_LOG;

    /* compute power on the fly */
    list[RotationVector].power = list[Gyro].power + 
                                 list[Accelerometer].power +
                                 list[MagneticField].power;
    list[RotationVector].resolution = .00001;
    list[RotationVector].maxRange = 1.0;

    // TODO: for GED tablet
    // list[RotationVector].minDelay = 10000;
    list[RotationVector].minDelay = 5000;

    return;
}

void MPLSensor::fillOrientation(struct sensor_t *list)
{
    VFUNC_LOG;

    list[Orientation].power = list[Gyro].power +
                              list[Accelerometer].power +
                              list[MagneticField].power;
    list[Orientation].resolution = .00001;
    list[Orientation].maxRange = 360.0;

    // TODO: for GED tablet
    // list[Orientation].minDelay = 10000;
    list[Orientation].minDelay = 5000;

    return;
}

void MPLSensor::fillGravity( struct sensor_t *list)
{
    VFUNC_LOG;

    list[Gravity].power = list[Gyro].power +
                          list[Accelerometer].power +
                          list[MagneticField].power;
    list[Gravity].resolution = .00001;
    list[Gravity].maxRange = 9.81;

    // TODO: for GED tablet
    // list[Gravity].minDelay = 10000;
    list[Gravity].minDelay = 5000;

    return;
}

void MPLSensor::fillLinearAccel(struct sensor_t *list)
{
    VFUNC_LOG;

    list[LinearAccel].power = list[Gyro].power +
                          list[Accelerometer].power +
                          list[MagneticField].power;
    list[LinearAccel].resolution = list[Accelerometer].resolution;
    list[LinearAccel].maxRange = list[Accelerometer].maxRange;

    // TODO: for GED tablet
    // list[LinearAccel].minDelay = 10000;
    list[LinearAccel].minDelay = 5000;

    return;
}

int MPLSensor::inv_init_sysfs_attributes(void)
{
    VFUNC_LOG;

    unsigned char i = 0;
    char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN], tbuf[2];
    char *sptr;
    char **dptr;
    int num;

    // get proper (in absolute/relative) IIO path & build MPU's sysfs paths
    // inv_get_sysfs_abs_path(sysfs_path);
    if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) {
        ALOGE("MPLSensor failed get sysfs path");
        return -1;
    }
    sysfs_names_ptr = 
            (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN]));
    sptr = sysfs_names_ptr;
    if (sptr != NULL) {
        dptr = (char**)&mpu;
        do {
            *dptr++ = sptr;
            sptr += sizeof(char[MAX_SYSFS_NAME_LEN]);
        } while (++i < MAX_SYSFS_ATTRB);
    } else {
        LOGE("HAL:couldn't alloc mem for sysfs paths");
        return -1;
    }

    inv_get_iio_trigger_path(iio_trigger_path);

    sprintf(mpu.key, "%s%s", sysfs_path, "/key");
    sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable");
    sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length");
    sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state");
    sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en");
    sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name");
    sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger");

    sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware");
    sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded");
    sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on");
    sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on");
    sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate");
    sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on");

    // TODO: for self test
    sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test");

    sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature");
    sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable");
    sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
    sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix");
    sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en");
    sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en");
    sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en");

#ifdef THIRD_PARTY_ACCEL    //BMA250
    /* same as 'mpu.accel_enable' */
    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/enable");
    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/fifo_rate");
    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/NA");
    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/NA");
    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/NA");   
#else
    sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable");
    sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency");
    sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale");
    sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en");
    sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en");
    sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en");

    // TODO: for bias settings
    sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias");

    sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix");
#endif

    sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on");
    sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en");
    sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en");
    sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en");
    sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en");

#if 0
    // test print sysfs paths
    dptr = (char**)&mpu;
    for (i = 0; i < MAX_SYSFS_ATTRB; i++) {
        LOGE("HAL:sysfs path: %s", *dptr++);
    }
#endif
    return 0;
}