#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "I2C_ADXL345.h"

void PINMUX_RESET_CONFIG()
{
    *(volatile unsigned int *)0xFFD08704 = 0x0;  //i20mux
    *(volatile unsigned int *)0xFFD0849C = 0x1;  //sda mux
    *(volatile unsigned int *)0xFFD084A0 = 0x1;  //scl mux
}

void HPS_CONFIG(void)
{
    *(volatile unsigned int *)0xFFC0406C = 2;                               //ic enable
    while( ((*(volatile unsigned int *)0xFFC0409C) & 0x1) == 1);  //while i2c module is enabled, do nothing
    *(volatile unsigned int *)0xFFC04000 = 65;                    //con register
    *(volatile unsigned int *)0xFFC04004 = 0x53;                  //tar register
    *(volatile unsigned int *)0xFFC0401C = 90;                            //ic_fs_scl_hcnt
    *(volatile unsigned int *)0xFFC0401C = 160;                              //ic_fs_scl_lcnt
    *(volatile unsigned int *)0xFFC040A0 = 4;              //spklen
    *(volatile unsigned int *)0xFFC0406C = 1;              //ic enable
    while( ((*(volatile unsigned int *)0xFFC0409C) & 0x1) == 0);
    //*(volatile unsigned int *)0xFFD05014 = 0xFFFF0FFF; //DEASSERT I2C0 RESET  CHECK
}
void ADXL_WRITE_BYTE(uint8_t address, uint8_t value)
{
*(volatile unsigned int *)0xFFC04010 = address + 0x400;
 *(volatile unsigned int *)0xFFC04010 = value;
}

void ADXL_READ_BYTE(uint8_t address, uint8_t *value)
{
 *(volatile unsigned int *)0xFFC04010 = address + 0x400;
 *(volatile unsigned int *)0xFFC04010 = 0x100;
 while (*(volatile unsigned int *)0xFFC04078 == 0);
 *value = *(volatile unsigned int *)0xFFC04010;
}

void ADXL_READ_BYTES(uint8_t address, uint8_t len)
{
 int i;
 uint8_t threshold = *(volatile unsigned int *)0xFFC04078; //read how many receive fifo valid entries
*(volatile unsigned int *)0xFFC04010 = address + 0x400;
  for (i=0;i<len;i++)
 *(volatile unsigned int *)0xFFC04010 = 0x100;
  while((*(volatile unsigned int *)0xFFC04078) <= threshold + (i-1));
 }
void ADXL_INIT(void)
{
    ADXL_WRITE_BYTE(0x2C, 0x0D);   //bw_rate control register
    ADXL_WRITE_BYTE(0x31, 0x0B);   // data_format (r.justify)
    ADXL_WRITE_BYTE(0x38, 0x9F);    //fifo_ctl
    ADXL_WRITE_BYTE(0x2D, 0x08);    // power_ctl
}

void ADXL_XYZ(void)
{
    //uint8_t szData[6];
    ADXL_READ_BYTES(0x32,6);

    //data[0] = (szData[1] << 8) | szData[0];
    //data[1] = (szData[3] << 8) | szData[2];
    //data[2] = (szData[5] << 8) | szData[4];
}
bool ADXL345_Data_Ready(void)
{
    bool ready = false;
    uint8_t data;

    ADXL_READ_BYTE(0x30, &data);
    if (data & 0x80)
        ready = true;
    return ready;
}