#include <ch.h>
#include <hal.h>
#include "lsm303.h"
#include <stdio.h>

void lsm303_acc_write(uint8_t reg, uint8_t value)
{
    uint8_t txbuf[2] = {reg, value};
    i2cMasterTransmitTimeout(&I2CD2, LSM303_ACC_ADDR,
                             txbuf, 2,
                             NULL, 0,
                             LSM303_TIMEOUT);
}

uint8_t lsm303_acc_read8(uint8_t reg)
{
    uint8_t txbuf[1] = {reg};
    uint8_t rxbuf[2] = {};
    i2cMasterTransmitTimeout(&I2CD2, LSM303_ACC_ADDR,
                             txbuf, 1,
                             rxbuf, 2,
                             LSM303_TIMEOUT);
    return rxbuf[0];
}

bool lsm303_acc_read_results(int *x, int *y, int *z)
{
    uint8_t txbuf[1] = {LSM303_ACC_OUT_X | 0x80};
    uint8_t rxbuf[6] = {};
    msg_t status = i2cMasterTransmitTimeout(&I2CD2, LSM303_ACC_ADDR,
                                txbuf, 1,
                                rxbuf, 6,
                                LSM303_TIMEOUT);
    
    *x = (int16_t)(rxbuf[0] | (rxbuf[1] << 8));
    *y = (int16_t)(rxbuf[2] | (rxbuf[3] << 8));
    *z = (int16_t)(rxbuf[4] | (rxbuf[5] << 8));
    
    return (status == RDY_OK);
}

void lsm303_mag_write(uint8_t reg, uint8_t value)
{
    uint8_t txbuf[2] = {reg, value};
    i2cMasterTransmitTimeout(&I2CD2, LSM303_MAG_ADDR,
                             txbuf, 2,
                             NULL, 0,
                             LSM303_TIMEOUT);
}

uint8_t lsm303_mag_read8(uint8_t reg)
{
    uint8_t txbuf[1] = {reg};
    uint8_t rxbuf[2] = {};
    i2cMasterTransmitTimeout(&I2CD2, LSM303_MAG_ADDR,
                             txbuf, 1,
                             rxbuf, 2,
                             LSM303_TIMEOUT);
    return rxbuf[0];
}

bool lsm303_mag_read_results(int *x, int *y, int *z)
{
    uint8_t txbuf[1] = {LSM303_MAG_OUT_X};
    uint8_t rxbuf[6] = {};
    msg_t status = i2cMasterTransmitTimeout(&I2CD2, LSM303_MAG_ADDR,
                             txbuf, 1,
                             rxbuf, 6,
                             LSM303_TIMEOUT);
    
    
    *x = (int16_t)(rxbuf[1] | (rxbuf[0] << 8));
    *z = (int16_t)(rxbuf[3] | (rxbuf[2] << 8));
    *y = (int16_t)(rxbuf[5] | (rxbuf[4] << 8));
    
    return (status == RDY_OK);
}