/*
This was created by Troy Davis and Caitlin Smart 
in conjunction with the ECE492 Design project
beerware licenceing applies
*/

#include "accelerometer.h"
#include "i2c_ctrl.h"
#include <stdio.h>

// TEST INCLUDES for ostimedly
#include "priv/alt_file.h"


void accelerometer_write(unsigned char reg_addr, unsigned short data){

    unsigned short i2c_complete_data_word;
    
    i2c_complete_data_word = ( reg_addr << 8 | data );
    //address of accelerometer is always 0x1D
    i2c_write( ACCELEROMETER_I2C_ADDRESS, i2c_complete_data_word );
    
}

unsigned char accelerometer_read(unsigned char reg_addr){

    unsigned char ret_reg_data;
    
    //address of accelerometer is always 0x1D
    ret_reg_data = i2c_read( ACCELEROMETER_I2C_ADDRESS, reg_addr);
    return ret_reg_data;
    
}

void init_accelerometer(){
    //dummy write
    //accelerometer_write(0x0D,0x2A);
    //check accelerometer whoami register and compare to known value to confirm comunication

    int a;
    if( (a = (int)accelerometer_read(WHO_AM_I)) == 0x2A){
        printf("Communication - OK\n");    
    }
    else{
        printf("Communication -FAILED 0x%x\n", a);    
    }
   
    
    MMA8452Standby(); // Must be in standby to change registers
    OSTimeDlyHMSM(0,0,0,1); // 1 us delay to spread out initialization.
    //Initialize register FF_MT_CFG 
    accelerometer_write(FF_MT_CFG, 0x20);
    OSTimeDlyHMSM(0,0,0,1); // 1 us delay to spread out initialization.
    //Initialize CTRL_REG4 To enable motion interrupt
    accelerometer_write(CTRL_REG4, 0x84); //was 0xE0
    OSTimeDlyHMSM(0,0,0,1); // 1 us delay to spread out initialization.
    //Initialize CTRL_REG5 to 0x04. To capture motion interrupt
    accelerometer_write(CTRL_REG5, 0x80); //was 0xE0
    OSTimeDlyHMSM(0,0,0,1); // 1 us delay to spread out initialization.
    //Initialize FF_MT_THS. To Set threshold of interrupt sensativity
    accelerometer_write(FF_MT_THS, 0x9F); //was 0xE0  
    OSTimeDlyHMSM(0,0,0,1);// 1 us delay to spread out initialization.
    // Set up the full scale range to 2, 4, or 8g.
    byte fsr = GSCALE;
    if(fsr > 8) fsr = 8; //Easy error check
    fsr >>= 2; // Neat trick, see page 22. 00 = 2G, 01 = 4A, 10 = 8G

    //accelerometer_write(XYZ_DATA_CFG, fsr);

    //The default data rate is 800Hz and we don't modify it in this example code
    //OSTimeDlyHMSM(0,0,1,100); // 50 us delay to spread out initialization.
    MMA8452Active(); // Set to active to start reading
    //OSTimeDlyHMSM(0,0,1,100); // 50 us delay to spread out initialization.
    //loop();

}

// Sets the MMA8452 to standby mode. It must be in standby to change most register settings
void MMA8452Standby()
{
  byte c = i2c_read(ACCELEROMETER_I2C_ADDRESS, CTRL_REG1);
  byte data = c & ~(CR_STANDBY);
  OSTimeDlyHMSM(0,0,0,1);// 1 us delay to spread out activation, violated i2c timing otherwise.
  accelerometer_write(CTRL_REG1, data);
}

// Sets the MMA8452 to active mode. Needs to be in this mode to output data
void MMA8452Active()
{
  byte c = i2c_read(ACCELEROMETER_I2C_ADDRESS, CTRL_REG1);
  byte data = c | CR_STANDBY;
  OSTimeDlyHMSM(0,0,0,1);// 1 us delay to spread out activation, violated i2c timing otherwise.
  accelerometer_write(CTRL_REG1, data);
}

