/* Copyright (C) 2009 The Android Open Source Project
**
** This software is licensed under the terms of the GNU General Public
** License version 2, as published by the Free Software Foundation, and
** may be copied, distributed, and modified under those terms.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
** GNU General Public License for more details.
*/

#include <math.h>
#include "android/hw-sensors.h"
#include "android/utils/debug.h"
#include "android/utils/misc.h"
#include "android/utils/system.h"
#include "android/hw-qemud.h"
#include "android/globals.h"
#include "hw/hw.h"
#include "sysemu/char.h"
#include "qemu/timer.h"
#include "android/sensors-port.h"

#define  E(...)    derror(__VA_ARGS__)
#define  W(...)    dwarning(__VA_ARGS__)
#define  D(...)  VERBOSE_PRINT(sensors,__VA_ARGS__)
#define  V(...)  VERBOSE_PRINT(init,__VA_ARGS__)

/* define T_ACTIVE to 1 to debug transport communications */
#define  T_ACTIVE  0

#if T_ACTIVE
#define  T(...)  VERBOSE_PRINT(sensors,__VA_ARGS__)
#else
#define  T(...)   ((void)0)
#endif

/* this code supports emulated sensor hardware
 *
 * Note that currently, only the accelerometer is really emulated, and only
 * for the purpose of allowing auto-rotating the screen in keyboard-less
 * configurations.
 *
 *
 */


static const struct {
    const char*  name;
    int          id;
} _sSensors[MAX_SENSORS] = {
#define SENSOR_(x,y)  { y, ANDROID_SENSOR_##x },
  SENSORS_LIST
#undef SENSOR_
};


static int
_sensorIdFromName( const char*  name )
{
    int  nn;
    for (nn = 0; nn < MAX_SENSORS; nn++)
        if (!strcmp(_sSensors[nn].name,name))
            return _sSensors[nn].id;
    return -1;
}

static const char*
_sensorNameFromId( int  id )
{
    int  nn;
    for (nn = 0; nn < MAX_SENSORS; nn++)
        if (id == _sSensors[nn].id)
            return _sSensors[nn].name;
    return NULL;
}

/* For common Sensor Value struct */
typedef struct {
    float a, b, c;
} SensorValues;

typedef struct {
    float   x, y, z;
} Acceleration;


typedef struct {
    float  x, y, z;
} MagneticField;


typedef struct {
    float  azimuth;
    float  pitch;
    float  roll;
} Orientation;


typedef struct {
    float  celsius;
} Temperature;


typedef struct {
    float  value;
} Proximity;

typedef struct {
    char       enabled;
    union {
        SensorValues   value;
        Acceleration   acceleration;
        MagneticField  magnetic;
        Orientation    orientation;
        Temperature    temperature;
        Proximity      proximity;
    } u;
} Sensor;

/*
 * - when the qemu-specific sensors HAL module starts, it sends
 *   "list-sensors"
 *
 * - this code replies with a string containing an integer corresponding
 *   to a bitmap of available hardware sensors in the current AVD
 *   configuration (e.g. "1" a.k.a (1 << ANDROID_SENSOR_ACCELERATION))
 *
 * - the HAL module sends "set:<sensor>:<flag>" to enable or disable
 *   the report of a given sensor state. <sensor> must be the name of
 *   a given sensor (e.g. "accelerometer"), and <flag> must be either
 *   "1" (to enable) or "0" (to disable).
 *
 * - Once at least one sensor is "enabled", this code should periodically
 *   send information about the corresponding enabled sensors. The default
 *   period is 200ms.
 *
 * - the HAL module sends "set-delay:<delay>", where <delay> is an integer
 *   corresponding to a time delay in milli-seconds. This corresponds to
 *   a new interval between sensor events sent by this code to the HAL
 *   module.
 *
 * - the HAL module can also send a "wake" command. This code should simply
 *   send the "wake" back to the module. This is used internally to wake a
 *   blocking read that happens in a different thread. This ping-pong makes
 *   the code in the HAL module very simple.
 *
 * - each timer tick, this code sends sensor reports in the following
 *   format (each line corresponds to a different line sent to the module):
 *
 *      acceleration:<x>:<y>:<z>
 *      magnetic-field:<x>:<y>:<z>
 *      orientation:<azimuth>:<pitch>:<roll>
 *      temperature:<celsius>
 *      sync:<time_us>
 *
 *   Where each line before the sync:<time_us> is optional and will only
 *   appear if the corresponding sensor has been enabled by the HAL module.
 *
 *   Note that <time_us> is the VM time in micro-seconds when the report
 *   was "taken" by this code. This is adjusted by the HAL module to
 *   emulated system time (using the first sync: to compute an adjustment
 *   offset).
 */
#define  HEADER_SIZE  4
#define  BUFFER_SIZE  512

typedef struct HwSensorClient   HwSensorClient;

typedef struct {
    QemudService*       service;
    Sensor              sensors[MAX_SENSORS];
    HwSensorClient*     clients;
    AndroidSensorsPort* sensors_port;
} HwSensors;

struct HwSensorClient {
    HwSensorClient*  next;
    HwSensors*       sensors;
    QemudClient*     client;
    QEMUTimer*       timer;
    uint32_t         enabledMask;
    int32_t          delay_ms;
};

static void
_hwSensorClient_free( HwSensorClient*  cl )
{
    /* remove from sensors's list */
    if (cl->sensors) {
        HwSensorClient**  pnode = &cl->sensors->clients;
        for (;;) {
            HwSensorClient*  node = *pnode;
            if (node == NULL)
                break;
            if (node == cl) {
                *pnode = cl->next;
                break;
            }
            pnode = &node->next;
        }
        cl->next    = NULL;
        cl->sensors = NULL;
    }

    /* close QEMUD client, if any */
    if (cl->client) {
        qemud_client_close(cl->client);
        cl->client = NULL;
    }
    /* remove timer, if any */
    if (cl->timer) {
        timer_del(cl->timer);
        timer_free(cl->timer);
        cl->timer = NULL;
    }
    AFREE(cl);
}

/* forward */
static void  _hwSensorClient_tick(void*  opaque);


static HwSensorClient*
_hwSensorClient_new( HwSensors*  sensors )
{
    HwSensorClient*  cl;

    ANEW0(cl);

    cl->sensors     = sensors;
    cl->enabledMask = 0;
    cl->delay_ms    = 800;
    cl->timer       = timer_new(QEMU_CLOCK_VIRTUAL, SCALE_NS, _hwSensorClient_tick, cl);

    cl->next         = sensors->clients;
    sensors->clients = cl;

    return cl;
}

/* forward */

static void  _hwSensorClient_receive( HwSensorClient*  cl,
                                      uint8_t*         query,
                                      int              querylen );

/* Qemud service management */

static void
_hwSensorClient_recv( void*  opaque, uint8_t*  msg, int  msglen,
                      QemudClient*  client )
{
    HwSensorClient*  cl = opaque;

    _hwSensorClient_receive(cl, msg, msglen);
}

static void
_hwSensorClient_close( void*  opaque )
{
    HwSensorClient*  cl = opaque;

    /* the client is already closed here */
    cl->client = NULL;
    _hwSensorClient_free(cl);
}

/* send a one-line message to the HAL module through a qemud channel */
static void
_hwSensorClient_send( HwSensorClient*  cl, const uint8_t*  msg, int  msglen )
{
    D("%s: '%s'", __FUNCTION__, quote_bytes((const void*)msg, msglen));
    qemud_client_send(cl->client, msg, msglen);
}

static int
_hwSensorClient_enabled( HwSensorClient*  cl, int  sensorId )
{
    return (cl->enabledMask & (1 << sensorId)) != 0;
}

/* this function is called periodically to send sensor reports
 * to the HAL module, and re-arm the timer if necessary
 */
static void
_hwSensorClient_tick( void*  opaque )
{
    HwSensorClient*  cl = opaque;
    HwSensors*       hw  = cl->sensors;
    int64_t          delay = cl->delay_ms;
    int64_t          now_ns;
    uint32_t         mask  = cl->enabledMask;
    Sensor*          sensor;
    char             buffer[128];

    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_ACCELERATION)) {
        sensor = &hw->sensors[ANDROID_SENSOR_ACCELERATION];
        snprintf(buffer, sizeof buffer, "acceleration:%g:%g:%g",
                 sensor->u.acceleration.x,
                 sensor->u.acceleration.y,
                 sensor->u.acceleration.z);
        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
    }

    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_MAGNETIC_FIELD)) {
        sensor = &hw->sensors[ANDROID_SENSOR_MAGNETIC_FIELD];
        /* NOTE: sensors HAL expects "magnetic", not "magnetic-field" name here. */
        snprintf(buffer, sizeof buffer, "magnetic:%g:%g:%g",
                 sensor->u.magnetic.x,
                 sensor->u.magnetic.y,
                 sensor->u.magnetic.z);
        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
    }

    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_ORIENTATION)) {
        sensor = &hw->sensors[ANDROID_SENSOR_ORIENTATION];
        snprintf(buffer, sizeof buffer, "orientation:%g:%g:%g",
                 sensor->u.orientation.azimuth,
                 sensor->u.orientation.pitch,
                 sensor->u.orientation.roll);
        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
    }

    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_TEMPERATURE)) {
        sensor = &hw->sensors[ANDROID_SENSOR_TEMPERATURE];
        snprintf(buffer, sizeof buffer, "temperature:%g",
                 sensor->u.temperature.celsius);
        _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));
    }

    if (_hwSensorClient_enabled(cl, ANDROID_SENSOR_PROXIMITY)) {
        sensor = &hw->sensors[ANDROID_SENSOR_PROXIMITY];
        snprintf(buffer, sizeof buffer, "proximity:%g",
                 sensor->u.proximity.value);
        _hwSensorClient_send(cl, (uint8_t*) buffer, strlen(buffer));
    }

    now_ns = qemu_clock_get_ns(QEMU_CLOCK_VIRTUAL);

    snprintf(buffer, sizeof buffer, "sync:%" PRId64, now_ns/1000);
    _hwSensorClient_send(cl, (uint8_t*)buffer, strlen(buffer));

    /* rearm timer, use a minimum delay of 20 ms, just to
     * be safe.
     */
    if (mask == 0)
        return;

    if (delay < 20)
        delay = 20;

    delay *= 1000000LL;  /* convert to nanoseconds */
    timer_mod(cl->timer, now_ns + delay);
}

/* handle incoming messages from the HAL module */
static void
_hwSensorClient_receive( HwSensorClient*  cl, uint8_t*  msg, int  msglen )
{
    HwSensors*  hw = cl->sensors;

    D("%s: '%.*s'", __FUNCTION__, msglen, msg);

    /* "list-sensors" is used to get an integer bit map of
     * available emulated sensors. We compute the mask from the
     * current hardware configuration.
     */
    if (msglen == 12 && !memcmp(msg, "list-sensors", 12)) {
        char  buff[12];
        int   mask = 0;
        int   nn;

        for (nn = 0; nn < MAX_SENSORS; nn++) {
            if (hw->sensors[nn].enabled)
                mask |= (1 << nn);
        }

        snprintf(buff, sizeof buff, "%d", mask);
        _hwSensorClient_send(cl, (const uint8_t*)buff, strlen(buff));
        return;
    }

    /* "wake" is a special message that must be sent back through
     * the channel. It is used to exit a blocking read.
     */
    if (msglen == 4 && !memcmp(msg, "wake", 4)) {
        _hwSensorClient_send(cl, (const uint8_t*)"wake", 4);
        return;
    }

    /* "set-delay:<delay>" is used to set the delay in milliseconds
     * between sensor events
     */
    if (msglen > 10 && !memcmp(msg, "set-delay:", 10)) {
        cl->delay_ms = atoi((const char*)msg+10);
        if (cl->enabledMask != 0)
            _hwSensorClient_tick(cl);

        return;
    }

    /* "set:<name>:<state>" is used to enable/disable a given
     * sensor. <state> must be 0 or 1
     */
    if (msglen > 4 && !memcmp(msg, "set:", 4)) {
        char*  q;
        int    id, enabled, oldEnabledMask = cl->enabledMask;
        msg += 4;
        q    = strchr((char*)msg, ':');
        if (q == NULL) {  /* should not happen */
            D("%s: ignore bad 'set' command", __FUNCTION__);
            return;
        }
        *q++ = 0;

        id = _sensorIdFromName((const char*)msg);
        if (id < 0 || id >= MAX_SENSORS) {
            D("%s: ignore unknown sensor name '%s'", __FUNCTION__, msg);
            return;
        }

        if (!hw->sensors[id].enabled) {
            D("%s: trying to set disabled %s sensor", __FUNCTION__, msg);
            return;
        }
        enabled = (q[0] == '1');

        if (enabled)
            cl->enabledMask |= (1 << id);
        else
            cl->enabledMask &= ~(1 << id);

        if (cl->enabledMask != (uint32_t)oldEnabledMask) {
            D("%s: %s %s sensor", __FUNCTION__,
                (cl->enabledMask & (1 << id))  ? "enabling" : "disabling",  msg);
        }

        /* If emulating device is connected update sensor state there too. */
        if (hw->sensors_port != NULL) {
            if (enabled) {
                sensors_port_enable_sensor(hw->sensors_port, (const char*)msg);
            } else {
                sensors_port_disable_sensor(hw->sensors_port, (const char*)msg);
            }
        }

        _hwSensorClient_tick(cl);
        return;
    }

    D("%s: ignoring unknown query", __FUNCTION__);
}

/* Saves sensor-specific client data to snapshot */
static void
_hwSensorClient_save( QEMUFile*  f, QemudClient*  client, void*  opaque  )
{
    HwSensorClient* sc = opaque;

    qemu_put_be32(f, sc->delay_ms);
    qemu_put_be32(f, sc->enabledMask);
    timer_put(f, sc->timer);
}

/* Loads sensor-specific client data from snapshot */
static int
_hwSensorClient_load( QEMUFile*  f, QemudClient*  client, void*  opaque  )
{
    HwSensorClient* sc = opaque;

    sc->delay_ms = qemu_get_be32(f);
    sc->enabledMask = qemu_get_be32(f);
    timer_get(f, sc->timer);

    return 0;
}

static QemudClient*
_hwSensors_connect( void*  opaque,
                    QemudService*  service,
                    int  channel,
                    const char* client_param )
{
    HwSensors*       sensors = opaque;
    HwSensorClient*  cl      = _hwSensorClient_new(sensors);
    QemudClient*     client  = qemud_client_new(service, channel, client_param, cl,
                                                _hwSensorClient_recv,
                                                _hwSensorClient_close,
                                                _hwSensorClient_save,
                                                _hwSensorClient_load );
    qemud_client_set_framing(client, 1);
    cl->client = client;

    return client;
}

/* change the value of the emulated sensor vector */
static void
_hwSensors_setSensorValue( HwSensors*  h, int sensor_id, float a, float b, float c )
{
    Sensor* s = &h->sensors[sensor_id];

    s->u.value.a = a;
    s->u.value.b = b;
    s->u.value.c = c;
}

/* Saves available sensors to allow checking availability when loaded.
 */
static void
_hwSensors_save( QEMUFile*  f, QemudService*  sv, void*  opaque)
{
    HwSensors* h = opaque;

    // number of sensors
    qemu_put_be32(f, MAX_SENSORS);
    AndroidSensor i;
    for (i = 0 ; i < MAX_SENSORS; i++) {
        Sensor* s = &h->sensors[i];
        qemu_put_be32(f, s->enabled);

        /* this switch ensures that a warning is raised when a new sensor is
         * added and is not added here as well.
         */
        switch (i) {
        case ANDROID_SENSOR_ACCELERATION:
            qemu_put_float(f, s->u.acceleration.x);
            qemu_put_float(f, s->u.acceleration.y);
            qemu_put_float(f, s->u.acceleration.z);
            break;
        case ANDROID_SENSOR_MAGNETIC_FIELD:
            qemu_put_float(f, s->u.magnetic.x);
            qemu_put_float(f, s->u.magnetic.y);
            qemu_put_float(f, s->u.magnetic.z);
            break;
        case ANDROID_SENSOR_ORIENTATION:
            qemu_put_float(f, s->u.orientation.azimuth);
            qemu_put_float(f, s->u.orientation.pitch);
            qemu_put_float(f, s->u.orientation.roll);
            break;
        case ANDROID_SENSOR_TEMPERATURE:
            qemu_put_float(f, s->u.temperature.celsius);
            break;
        case ANDROID_SENSOR_PROXIMITY:
            qemu_put_float(f, s->u.proximity.value);
            break;
        case MAX_SENSORS:
            break;
        }
    }
}


static int
_hwSensors_load( QEMUFile*  f, QemudService*  s, void*  opaque)
{
    HwSensors* h = opaque;

    /* check number of sensors */
    int32_t num_sensors = qemu_get_be32(f);
    if (num_sensors > MAX_SENSORS) {
        D("%s: cannot load: snapshot requires %d sensors, %d available\n",
          __FUNCTION__, num_sensors, MAX_SENSORS);
        return -EIO;
    }

    /* load sensor state */
    AndroidSensor i;
    for (i = 0 ; i < (AndroidSensor)num_sensors; i++) {
        Sensor* s = &h->sensors[i];
        s->enabled = qemu_get_be32(f);

        /* this switch ensures that a warning is raised when a new sensor is
         * added and is not added here as well.
         */
        switch (i) {
        case ANDROID_SENSOR_ACCELERATION:
            s->u.acceleration.x = qemu_get_float(f);
            s->u.acceleration.y = qemu_get_float(f);
            s->u.acceleration.z = qemu_get_float(f);
            break;
        case ANDROID_SENSOR_MAGNETIC_FIELD:
            s->u.magnetic.x = qemu_get_float(f);
            s->u.magnetic.y = qemu_get_float(f);
            s->u.magnetic.z = qemu_get_float(f);
            break;
        case ANDROID_SENSOR_ORIENTATION:
            s->u.orientation.azimuth = qemu_get_float(f);
            s->u.orientation.pitch   = qemu_get_float(f);
            s->u.orientation.roll    = qemu_get_float(f);
            break;
        case ANDROID_SENSOR_TEMPERATURE:
            s->u.temperature.celsius = qemu_get_float(f);
            break;
        case ANDROID_SENSOR_PROXIMITY:
            s->u.proximity.value = qemu_get_float(f);
            break;
        case MAX_SENSORS:
            break;
        }
    }

    /* The following is necessary when we resume a snaphost
     * created by an older version of the emulator that provided
     * less hardware sensors.
     */
    for ( ; i < MAX_SENSORS; i++ ) {
        h->sensors[i].enabled = 0;
    }

    return 0;
}


/* change the emulated proximity */
static void
_hwSensors_setProximity( HwSensors*  h, float value )
{
    Sensor*  s = &h->sensors[ANDROID_SENSOR_PROXIMITY];
    s->u.proximity.value = value;
}

/* change the coarse orientation (landscape/portrait) of the emulated device */
static void
_hwSensors_setCoarseOrientation( HwSensors*  h, AndroidCoarseOrientation  orient )
{
    /* The Android framework computes the orientation by looking at
     * the accelerometer sensor (*not* the orientation sensor !)
     *
     * That's because the gravity is a constant 9.81 vector that
     * can be determined quite easily.
     *
     * Also, for some reason, the framework code considers that the phone should
     * be inclined by 30 degrees along the phone's X axis to be considered
     * in its ideal "vertical" position
     *
     * If the phone is completely vertical, rotating it will not do anything !
     */
    const double  g      = 9.81;
    const double  angle  = 20.0;
    const double  cos_angle = cos(angle/M_PI);
    const double  sin_angle = sin(angle/M_PI);

    switch (orient) {
    case ANDROID_COARSE_PORTRAIT:
        _hwSensors_setSensorValue( h, ANDROID_SENSOR_ACCELERATION, 0., g*cos_angle, g*sin_angle );
        break;

    case ANDROID_COARSE_LANDSCAPE:
        _hwSensors_setSensorValue( h, ANDROID_SENSOR_ACCELERATION, g*cos_angle, 0., g*sin_angle );
        break;
    default:
        ;
    }
}


/* initialize the sensors state */
static void
_hwSensors_init( HwSensors*  h )
{
    h->sensors_port = NULL;

    h->service = qemud_service_register("sensors", 0, h, _hwSensors_connect,
                                        _hwSensors_save, _hwSensors_load);

    if (android_hw->hw_accelerometer) {
        h->sensors[ANDROID_SENSOR_ACCELERATION].enabled = 1;
    }

    if (android_hw->hw_sensors_proximity) {
        h->sensors[ANDROID_SENSOR_PROXIMITY].enabled = 1;
    }

    if (android_hw->hw_sensors_magnetic_field) {
        h->sensors[ANDROID_SENSOR_MAGNETIC_FIELD].enabled = 1;
    }

    if (android_hw->hw_sensors_orientation) {
        h->sensors[ANDROID_SENSOR_ORIENTATION].enabled = 1;
    }

    if (android_hw->hw_sensors_temperature) {
        h->sensors[ANDROID_SENSOR_TEMPERATURE].enabled = 1;
    }

    /* XXX: TODO: Add other tests when we add the corresponding
        * properties to hardware-properties.ini et al. */

    _hwSensors_setCoarseOrientation(h, ANDROID_COARSE_PORTRAIT);
    _hwSensors_setProximity(h, 1);
}

static HwSensors    _sensorsState[1];

void
android_hw_sensors_init( void )
{
    HwSensors*  hw = _sensorsState;

    if (hw->service == NULL) {
        _hwSensors_init(hw);
        D("%s: sensors qemud service initialized", __FUNCTION__);
    }
}

void
android_hw_sensors_init_remote_controller(void) {
    HwSensors* hw = _sensorsState;

    if (!hw->sensors_port) {
        /* Try to see if there is a device attached that can be used for
        * sensor emulation. */
        hw->sensors_port = sensors_port_create(hw);
        if (hw->sensors_port == NULL) {
            V("Realistic sensor emulation is not available, since the remote controller is not accessible:\n %s",
            strerror(errno));
        }
    }
}

/* change the coarse orientation value */
extern void
android_sensors_set_coarse_orientation( AndroidCoarseOrientation  orient )
{
    android_hw_sensors_init();
    _hwSensors_setCoarseOrientation(_sensorsState, orient);
}

/* Get sensor name from sensor id */
extern const char*
android_sensors_get_name_from_id( int sensor_id )
{
    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
        return NULL;

    return _sensorNameFromId(sensor_id);
}

/* Get sensor id from sensor name */
extern int
android_sensors_get_id_from_name( char* sensorname )
{
    HwSensors* hw = _sensorsState;

    if (sensorname == NULL)
        return SENSOR_STATUS_UNKNOWN;

    int id = _sensorIdFromName(sensorname);

    if (id < 0 || id >= MAX_SENSORS)
        return SENSOR_STATUS_UNKNOWN;

    if (hw->service != NULL) {
        if (! hw->sensors[id].enabled)
            return SENSOR_STATUS_DISABLED;
    } else
        return SENSOR_STATUS_NO_SERVICE;

    return id;
}

/* Interface of reading the data for all sensors */
extern int
android_sensors_get( int sensor_id, float* a, float* b, float* c )
{
    HwSensors* hw = _sensorsState;

    *a = 0;
    *b = 0;
    *c = 0;

    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
        return SENSOR_STATUS_UNKNOWN;

    Sensor* sensor = &hw->sensors[sensor_id];
    if (hw->service != NULL) {
        if (! sensor->enabled)
            return SENSOR_STATUS_DISABLED;
    } else
        return SENSOR_STATUS_NO_SERVICE;

    *a = sensor->u.value.a;
    *b = sensor->u.value.b;
    *c = sensor->u.value.c;

    return SENSOR_STATUS_OK;
}

/* Interface of setting the data for all sensors */
extern int
android_sensors_set( int sensor_id, float a, float b, float c )
{
    HwSensors* hw = _sensorsState;

    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
        return SENSOR_STATUS_UNKNOWN;

    if (hw->service != NULL) {
        if (! hw->sensors[sensor_id].enabled)
            return SENSOR_STATUS_DISABLED;
    } else
        return SENSOR_STATUS_NO_SERVICE;

    _hwSensors_setSensorValue(hw, sensor_id, a, b, c);

    return SENSOR_STATUS_OK;
}

/* Get Sensor from sensor id */
extern uint8_t
android_sensors_get_sensor_status( int sensor_id )
{
    HwSensors* hw = _sensorsState;

    if (sensor_id < 0 || sensor_id >= MAX_SENSORS)
        return SENSOR_STATUS_UNKNOWN;

    return hw->sensors[sensor_id].enabled;
}