#include <SPI.h>

#define CS 10

#define POWER_CTL   0x2D
#define DATAX0      0x32
#define DATAY0      0x34
#define DATAZ0      0x36

#define X  0
#define Y  1
#define Z  2

void setup() {
  Serial.begin(115200);

  pinMode(CS, OUTPUT);
  digitalWrite(CS, HIGH);

  SPI.setDataMode(3);
  SPI.begin();
}

void loop() {
  // Prueba modo standby - Acelerómetro inactivo
  testAccel0();

  // Prueba de lectura en cada eje - Acelerómtro activoç
  testAccel1();

  // Prueba lectura registro POWER_CTL
  testPower();

}

void testPower() {
  for(int i=0; i<20; i++) {
    writePower(0);
    Serial.print("Valor esperado: 0\t Valor obtenido: ");
    Serial.println(readPower());
    writePower(8);
    Serial.print("Valor esperado: 8\t Valor obtenido: ");
    Serial.println(readPower());
    delay(300);
  }
}

void testAccel0() {
  writePower(0);
  for(int i=0; i<20; i++) {
    Serial.print("Valor esperado: 0, 0, 0\t\t Valor obtenido: ");
    Serial.print(readAccel(X));
    Serial.print(", ");
    Serial.print(readAccel(Y));
    Serial.print(", ");
    Serial.println(readAccel(Z));
    delay(300);
  }
}

void testAccel1() {
  writePower(8);
  for(int i=0; i<20; i++) {
    Serial.print("Valor esperado: AX, AY, AZ\t Valor obtenido: ");
    Serial.print(readAccel(X));
    Serial.print(", ");
    Serial.print(readAccel(Y));
    Serial.print(", ");
    Serial.println(readAccel(Z));
    delay(300);
  }
}

uint8_t readPower() {
  uint8_t buffer[2] = {POWER_CTL|0x80, 0};
  
  digitalWrite(CS, LOW);
  SPI.transfer(buffer, 2);
  digitalWrite(CS, HIGH);

  return buffer[1];
}

void writePower(uint8_t value) {
  uint8_t buffer[2] = {POWER_CTL, value};
  
  digitalWrite(CS, LOW);
  SPI.transfer(buffer, 2);
  digitalWrite(CS, HIGH);
}

int readAccel(int axis) {
  uint8_t buffer[2];
  int data;
  uint8_t addr;

  if(axis == X) addr = DATAX0;
  else if(axis == Y) addr = DATAY0;
  else if(axis == Z) addr = DATAZ0;

  buffer[0] = addr + 0x80;

  digitalWrite(CS, LOW);
  SPI.transfer(buffer, 2);
  digitalWrite(CS, HIGH);

  data = buffer[1];

  buffer[0] = addr + 0x81;

  digitalWrite(CS, LOW);
  SPI.transfer(buffer, 2);
  digitalWrite(CS, HIGH);

  data += buffer[1] << 8;

  return data;
}