#include <avr/io.h>
#include <util/delay.h>

#include "bma180.h"
#include "bma180_internal.h"

static void 
bma180_init_SPI(void)
{
    //make SPI master
    //SCL idle high, sample data on rising edge
    //Fosc/4 is SPI frequency
    //enable SPI
    SPCR = _BV(MSTR) | _BV(SPE) | _BV(CPHA) | _BV(CPOL);
}

static char 
bma180_rxdata(void)
{
    //while((SPSR&0x80) == 0x80);
    SPDR = 0x55;
    while((SPSR&0x80) == 0x00);

    return SPDR;
}

static void 
bma180_txdata(char data)
{
    //while((SPSR&0x80) == 0x80);
    SPDR = data;
    while((SPSR&0x80) == 0x00);
}

static void 
bma180_write(uint8_t address, char data)
{
    //write any data byte to any single address
    //adds a 0 to the MSB of the address byte (WRITE mode)

    address &= 0x7F;    

    //printf("\nWriting 0x%x to 0x%x\n", data, address);

    BMA180_CS_PORT &= ~_BV(BMA180_CS_PIN);
    _delay_ms(1);
    bma180_txdata(address);
    _delay_ms(1);
    bma180_txdata(data);
    _delay_ms(1);
    BMA180_CS_PORT |= _BV(BMA180_CS_PIN);
}

static char 
bma180_read(uint8_t address)
{
    //returns the contents of any 1 byte register from any address
    //sets the MSB for every address byte (READ mode)

    char byte;

    address |= 0x80;

    BMA180_CS_PORT &= ~_BV(BMA180_CS_PIN);
    bma180_txdata(address);
    byte = bma180_rxdata();
    BMA180_CS_PORT |= _BV(BMA180_CS_PIN);

    return byte;
}

static int 
bma180_set_config(uint8_t range, uint8_t bw)
{
    char temp, temp1;
    
    // if connected correctly, ID register should be 3
    if(bma180_read(ID) != 3)
        return -1;
        

    // Set ee_w bit
    temp = bma180_read(CTRLREG0);
    temp |= 0x10;
    bma180_write(CTRLREG0, temp);  // Have to set ee_w to write any other registers

    // Set BW
    temp = bma180_read(BWTCS);
    temp1 = bw;
    temp1 = temp1<<4;
    temp &= (~BWMASK);
    temp |= temp1;
    bma180_write(BWTCS, temp);     // Keep tcs<3:0> in BWTCS, but write new BW

    // Set Range
    temp = bma180_read(OLSB1);
    temp1 = range;
    temp1 = (temp1<<RANGESHIFT);
    temp &= (~RANGEMASK);
    temp |= temp1;
    bma180_write(OLSB1, temp); //Write new range data, keep other bits the same
    
    return 0;
}

void
bma180_init(uint8_t range, uint8_t bw)
{
    bma180_init_SPI();

    // CS output 
    // XXX checkme, sample code has the opposite?
    BMA180_CS_DDR |= _BV(BMA180_CS_PIN);

    bma180_set_config(range, bw);

}

static uint16_t 
bma_get_reading_n(uint8_t reg_lsb, uint8_t reg_msb)
{
    uint16_t ret;
    uint8_t val = 0;
    while (val != 1)
    {
        val = bma180_read(reg_lsb) & 0x01;
    }
    val = bma180_read(reg_msb);
    ret = val << 8;

    val = bma180_read(reg_lsb);
    ret |= val;
    // low bits aren't the reading
    ret >>= 2;
    return ret;
}

void
bma180_get_reading(uint16_t *x, uint16_t *y, uint16_t *z, uint8_t *temperature)
{
    *x = bma_get_reading_n(ACCXLSB, ACCXMSB);
    *y = bma_get_reading_n(ACCYLSB, ACCYMSB);
    *z = bma_get_reading_n(ACCZLSB, ACCZMSB);

    *temperature = bma180_read(TEMPERATURE);
}