#include <ch.h>
#include <hal.h>
#include "mma7660_acc.h"

#define MMA7660_REG_XOUT    0x00
#define MMA7660_REG_YOUT    0x01
#define MMA7660_REG_ZOUT    0x02
#define MMA7660_REG_TILT    0x03
#define MMA7660_REG_SRST    0x04
#define MMA7660_REG_SPCNT   0x05
#define MMA7660_REG_INTSU   0x06
#define MMA7660_REG_MODE    0x07
#define MMA7660_REG_SR      0x08
#define MMA7660_REG_PDET    0x09
#define MMA7660_REG_PD      0x0A

#define MMA7660_ADDR        0x4C
#define MMA7660_TIMEOUT     MS2ST(100)

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

static void i2c_recover()
{
    // Try to recover from errors
    i2cStop(&I2CD2);
    i2cStart(&I2CD2, &g_i2cconfig);
}

static bool writereg(uint8_t reg, uint8_t value)
{
    uint8_t txbuf[2] = {reg, value};
    msg_t status = i2cMasterTransmitTimeout(&I2CD2, MMA7660_ADDR,
                                            txbuf, 2,
                                            NULL, 0,
                                            MMA7660_TIMEOUT);
    if (status != RDY_OK)
        i2c_recover();
    
    return (status == RDY_OK);
}

static BSEMAPHORE_DECL(g_newSample, TRUE);

static void callback(EXTDriver *extp, expchannel_t channel) {
    chSysLockFromIsr();
    chBSemSignalI(&g_newSample);
    chSysUnlockFromIsr();
}

static const EXTConfig extconfig = {
    {[GPIOB_INT] = {EXT_CH_MODE_RISING_EDGE | EXT_CH_MODE_AUTOSTART | EXT_MODE_GPIOB, &callback}}
};

bool mma7660_poweron(int samplerate)
{
    // Turn on sensor power
    palSetPad(GPIOB, GPIOB_SVDD);
    chThdSleepMilliseconds(2);
    
    // Enable I2C
    i2cStart(&I2CD2, &g_i2cconfig);
    palSetPadMode(GPIOB, GPIOB_SCL, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_PUSHPULL);
    palSetPadMode(GPIOB, GPIOB_SDA, PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN);
    
    // Enable interrupt on new data
    extStart(&EXTD1, &extconfig);
    
    // Set the samplerate and turn on
    return writereg(MMA7660_REG_SR, samplerate) &&
           writereg(MMA7660_REG_INTSU, 0x10) && // Interrupt on new data
           writereg(MMA7660_REG_MODE, 0xC1);    // Active, interrupt on
}

void mma7660_poweroff()
{
    i2cStop(&I2CD2);
    palSetPadMode(GPIOB, GPIOB_SCL, PAL_MODE_INPUT);
    palSetPadMode(GPIOB, GPIOB_SDA, PAL_MODE_INPUT);
    palClearPad(GPIOB, GPIOB_SVDD);
    extStop(&EXTD1);
}

bool mma7660_read(int *x, int *y, int *z)
{
    if (!palReadPad(GPIOB, GPIOB_INT))
        return false;
    
    uint8_t txbuf[1] = {MMA7660_REG_XOUT};
    uint8_t rxbuf[3] = {};
    msg_t status = i2cMasterTransmitTimeout(&I2CD2, MMA7660_ADDR,
                                            txbuf, 1,
                                            rxbuf, 3,
                                            MMA7660_TIMEOUT);
    if (status != RDY_OK)
        i2c_recover();
    
    *x = (int8_t)(rxbuf[0] << 2) * 1000 / 85;
    *y = (int8_t)(rxbuf[1] << 2) * 1000 / 85;
    *z = (int8_t)(rxbuf[2] << 2) * 1000 / 85;
    
    return (status == RDY_OK);
}

void mma7660_wait()
{
    while (!palReadPad(GPIOB, GPIOB_INT))
        chBSemWait(&g_newSample);
}