#include <SPI.h>

const int CS_PIN = 10;

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

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

  SPI.begin(); // SCK = 13, MISO = 12, MOSI = 11
  SPI.beginTransaction(SPISettings(5000000, MSBFIRST, SPI_MODE3)); //1 MHz 

  //Device ID lesen (soll 0xE5 sein)
  uint8_t deviceID = readRegister(0x00);
  Serial.print("Device ID: 0x");
  Serial.println(deviceID, HEX);

  delay(100);
  
  writeRegister(0x31, 0x01);  //DATA-Format
  writeRegister(0x2D, 0x08);  // Measurement Mode
  writeRegister(0x2C, 0x0C);  // 400 Hz ODR
  writeRegister(0x38, 0x00);  // FIFO aus

}

void loop() {
  // X-, Y-, Z-Daten lesen
  uint8_t x0 = readRegister(0x32);
  uint8_t x1 = readRegister(0x33);
  uint8_t y0 = readRegister(0x34);
  uint8_t y1 = readRegister(0x35);
  uint8_t z0 = readRegister(0x36);
  uint8_t z1 = readRegister(0x37);
  
  int16_t x = (x1 << 8) | x0;
  int16_t y = (y1 << 8) | y0;
  int16_t z = (z1 << 8) | z0;

  Serial.print("X = "); Serial.print(x);
  Serial.print("\t| Y = "); Serial.print(y);
  Serial.print("\t| Z = "); Serial.println(z);
  
  delay(100);
}

//Register write
void writeRegister(uint8_t registerAddress, uint8_t value) {
  //Erstes Byte =  Read/Write (Bit 15) + mehrere Bytes (Bit 14) + Adresse (Bit 13-8) + Daten (Bit 7 - 0)
  uint8_t address = (registerAddress & 0x3F); // Bit 15=0 (Write), Bit 14=0 (Einzelbyte)
  uint16_t command = ((address << 8) | value);
  digitalWrite(CS_PIN, LOW);
  
  SPI.transfer16(command);

  digitalWrite(CS_PIN, HIGH);
}

//Register read
uint16_t readRegister(uint8_t registerAddress){
    //Erstes Byte =  Read/Write (Bit 15) + mehrere Bytes (Bit 14) + Adresse (Bit 13-8) + Daten (Bit 7 - 0)
    uint8_t address = ((registerAddress & 0x3F) | 0x80); // Bit 15=1 (Read), Bit 14=0 (Einzelbyte)

    digitalWrite(CS_PIN, LOW);
    int16_t data = SPI.transfer16((address<<8));
    digitalWrite(CS_PIN, HIGH);

    return data;
}