#include "Accel.h"


void i2c_write_Accel(char address, char data)
{
    int ack;
    i2c_start();
    ack = i2c_send(WRITE_ACCEL);
    ack = i2c_send(address);
    ack = i2c_send(data);
    do{
        i2c_start();
        ack = i2c_send(WRITE_ACCEL);
    } while (ack);
    i2c_stop();
    IFS1bits.I2C2MIF = 0;
}

void accel_init(){
    i2c_write_Accel(POWER_CTL,0x00);
    i2c_write_Accel(DATA_FORMAT,0x01);
    i2c_write_Accel(POWER_CTL,0x08);
    //i2c_write_Accel(INTR_ENABLE,0x00); // disable interrupts while configuring
    i2c_write_Accel(ACT_INACT_CTL,0x07);
    i2c_write_Accel(THRES_INACT,0x20);
    i2c_write_Accel(TIME_INACT,0x04);
    i2c_write_Accel(INTR_MAP,0xF7);
    i2c_write_Accel(INTR_ENABLE,0x08);
}

char i2c_read_Accel(char address)
{
    char read_value;
    i2c_start();
    read_value = i2c_send(WRITE_ACCEL);
    //read_value = i2c_send(0x00); // high byte
    read_value = i2c_send(address);
    i2c_restart();
    read_value = i2c_send(READ_ACCEL);
    read_value = i2c_receive();
    I2C2CONbits.ACKDT = 1;
    I2C2CONbits.ACKEN = 1;
    while(!IFS1bits.I2C2MIF);
    IFS1bits.I2C2MIF = 0;
    I2C2CONbits.ACKDT = 0;
    i2c_stop();
    return read_value;
}

struct accel_data accel_measure(void)
{
    struct accel_data ad1;
    float inter = i2c_read_Accel(INT_SOURCE);
    int x;
    int y;
    int z;
    float xg,yg,zg;
    ad1.xl = i2c_read_Accel(DATAX0);
    ad1.xh = i2c_read_Accel(DATAX1);
    ad1.yl = i2c_read_Accel(DATAY0);
    ad1.yh = i2c_read_Accel(DATAY1);
    ad1.zl = i2c_read_Accel(DATAZ0);
    ad1.zh = i2c_read_Accel(DATAZ1);
    return ad1;
}