/* This task collects data from the sensors through I2C bus.
 * It is fairly speed critical, as we need to keep the I2C bus fully occupied
 * in order to get best performance.
 */


#include <ch.h>
#include <hal.h>
#include "sensor_task.h"
#include "lsm303.h"
#include "parameters.h"
#include "mag_calib.h"

#ifdef SPHERE_RX
#include "speedsensor.h"
#endif

#ifdef SPHERE_TX
#include "rf_task.h"
#endif

volatile int mag_x, mag_y, mag_z;
volatile int acc_x, acc_y, acc_z;
volatile unsigned sensor_readings;
volatile unsigned mag_errors;
volatile unsigned acc_errors;
volatile unsigned sensor_start_time;
EventSource sensor_data_event;

void sensor_get_mag(v3d *mag)
{
    chSysLock();
    int x = mag_x;
    int y = mag_y;
    int z = mag_z;
    chSysUnlock();
    
    mag->x = fix16_div(fix16_from_int(x) - mag_calib.bias_x, mag_calib.scale_x);
    mag->y = fix16_div(fix16_from_int(y) - mag_calib.bias_y, mag_calib.scale_y);
    mag->z = fix16_div(fix16_from_int(z) - mag_calib.bias_z, mag_calib.scale_z);
}

void sensor_get_acc(v3d *acc)
{
    chSysLock();
    int x = acc_x;
    int y = acc_y;
    int z = acc_z;
    chSysUnlock();
    
    acc->x = fix16_div(x - params.acc_bias_x, params.acc_scale_x);
    acc->y = fix16_div(y - params.acc_bias_y, params.acc_scale_y);
    acc->z = fix16_div(z - params.acc_bias_z, params.acc_scale_z);
}

static void check_i2c_state();

static Thread *sensorThread = 0;
static WORKING_AREA(sensorThread_wa, 256);

static msg_t sensor_task(void *arg)
{
    chRegSetThreadName("sensor");
    
    // Read sensors at 200 Hz
    systime_t next_read = chTimeNow();
    
#ifdef SPHERE_RX
    // Update speed sensor at 10 Hz
    systime_t next_speed = next_read;
#endif
    
    while (!chThdShouldTerminate())
    {
        if (next_read <= chTimeNow()) // If we miss a round, just continue
            next_read = chTimeNow() + 1;
        
        chThdSleepUntil(next_read);
        next_read += MS2ST(5);
        
        int x,y,z;
        
        check_i2c_state();
        
        // Magnetometer
        if (lsm303_mag_read_results(&x, &y, &z))
        {
            chSysLock();
            mag_x = x; mag_y = y; mag_z = z;
            chSysUnlock();
        }
        else
        {
            mag_errors++;
        }
        
        check_i2c_state();
        
        // Accelerometer
        if (lsm303_acc_read_results(&x, &y, &z))
        {
            chSysLock();
            acc_x = x; acc_y = y; acc_z = z;
            sensor_readings++;
            chSysUnlock();
        }
        else
        {
            acc_errors++;
        }

#ifdef SPHERE_RX
        if (chTimeNow() >= next_speed)
        {
            next_speed += MS2ST(100);
            speedsensor_update();
        }
#endif

#ifdef SPHERE_TX
        orientation_update();
#endif
        
        {
            v3d mag;
            sensor_get_mag(&mag);
            mag_calib_update(&mag);
        }
        
        chEvtBroadcast(&sensor_data_event);
    }
    
    return 0;
}


static const I2CConfig i2c_config = {
    OPMODE_I2C,
    50000,
    STD_DUTY_CYCLE
};

static void check_i2c_state()
{
    if (I2CD2.state == I2C_LOCKED)
    {
        printf("I2C error, restarting I2C...\r\n");
        i2cStop(&I2CD2);
        i2cStart(&I2CD2, &i2c_config);
    }
}

void sensor_init()
{
    chEvtInit(&sensor_data_event);
}

void sensor_start()
{
    i2cStart(&I2CD2, &i2c_config);    
    
    // Configure sensors
    lsm303_acc_write(LSM303_ACC_CTRL1, 0x67); // All channels @ 200Hz
    lsm303_acc_write(LSM303_ACC_CTRL2, 0x00); // No high-pass
    lsm303_acc_write(LSM303_ACC_CTRL3, 0x00); // No interrupts
    lsm303_acc_write(LSM303_ACC_CTRL4, 0x88); // Block update, high resolution
    
    lsm303_mag_write(LSM303_MAG_CTRLA, 0x1C); // 220 Hz output rate
    lsm303_mag_write(LSM303_MAG_CTRLB, 0x20); // 1.3 Gauss range
    lsm303_mag_write(LSM303_MAG_MODE, 0x00); // Continuous conversion
    
#ifdef SPHERE_RX
    speedsensor_start();
#endif
    
    mag_calib_init();
    
    sensor_readings = acc_errors = mag_errors = 0;
    sensor_start_time = chTimeNow();
    
    sensorThread = chThdCreateStatic(sensorThread_wa, sizeof(sensorThread_wa), NORMALPRIO + 10, sensor_task, NULL);
}

void sensor_stop()
{
    chThdTerminate(sensorThread);
    chThdWait(sensorThread);
    
    // Sleep mode
    lsm303_acc_write(LSM303_ACC_CTRL1, 0x08);
    lsm303_mag_write(LSM303_MAG_MODE, 0x03);

#ifdef SPHERE_RX
    // Emit wakeup signal on movement
    lsm303_acc_write(LSM303_ACC_CTRL2, 0xC1); // High-pass on
    lsm303_acc_write(LSM303_ACC_CTRL1, 0x3F); // 25 Hz low-power
    
    chThdSleepMilliseconds(1000); // Wait for high-pass to stabilize
    
    lsm303_acc_write(LSM303_ACC_INT1_THS, 30);
    lsm303_acc_write(LSM303_ACC_INT1_DUR, 2);
    lsm303_acc_write(LSM303_ACC_INT1_CFG, 0x2A);
    lsm303_acc_write(LSM303_ACC_CTRL3, 0x40); // AOI1 interrupt
#endif
    
    chThdSleepMilliseconds(10);
    i2cStop(&I2CD2);
    
#ifdef SPHERE_RX
    speedsensor_stop();
#endif
}