#include <SPI.h>

const int CS_PIN = 10;

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

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

  SPI.begin(6, 2, 7, 10); // SCK, MISO, MOSI
  SPI.beginTransaction(SPISettings(6000000, MSBFIRST, SPI_MODE0)); //6 MHz 

  delay(10);
  //Soft-Reset
  //writeRegister(0x2A, 0b00000010);
  writeRegister(0x2A, 0x52); //Seite 69
  uint8_t softReset = readRegister(0x2A);
  Serial.print("Soft-Reset:\t0x");
  Serial.print(softReset, HEX);
  Serial.print("\t0b");
  Serial.println(softReset, BIN);
  delay(10);
  //NVM Control
  //writeRegister(0x29, 0b11000000);
  //delay(10);
  //Modus-Wahl
  writeRegister(0x26, 0b00000000); //Standby-Mode
  uint8_t mode1 = readRegister(0x26);
  Serial.print("Mode_1:\t\t0x");
  Serial.print(mode1, HEX);
  Serial.print("\t0b");
  Serial.println(mode1, BIN);

  writeRegister(0x27, 0b11100000); //0x27 = DIGITAL ENABLE Reg 
  uint8_t dig_En = readRegister(0x27);
  Serial.print("Dig_En:\t\t0x");
  Serial.print(dig_En, HEX);
  Serial.print("\t0b");
  Serial.println(dig_En, BIN);

  writeRegister(0x26, 0b00001100); //0x26 = OP_Mode-Reg.
  uint8_t mode2 = readRegister(0x26);
  Serial.print("Mode_2:\t\t0x");
  Serial.print(mode2, HEX);
  Serial.print("\t0b");
  Serial.println(mode2, BIN);

  //Status
  uint8_t status0 = readRegister(0x11);
  uint8_t status1 = readRegister(0x12);
  uint8_t status2 = readRegister(0x13);
  uint8_t status3 = readRegister(0x14);

  //Status
  Serial.print("Status0:\t0x");
  Serial.print(status0, HEX);
  Serial.print("\t0b");
  Serial.println(status0, BIN);
  Serial.print("Status1:\t0x");
  Serial.print(status1, HEX);
  Serial.print("\t0b");
  Serial.println(status1, BIN);
  Serial.print("Status2:\t0x");
  Serial.print(status2, HEX);
  Serial.print("\t0b");
  Serial.println(status2, BIN);
  Serial.print("Status3:\t0x");
  Serial.print(status3, HEX);
  Serial.print("\t0b");
  Serial.println(status3, BIN);
}

void loop() {
  //x-Achse lesen
  uint8_t ID1 = readRegister(0x00);
  uint8_t ID2 = readRegister(0x01);
  uint8_t ID3 = readRegister(0x02);
  uint8_t ID4 = readRegister(0x03);
  uint8_t xH_Data = readRegister(0x15);
  uint8_t xL_Data = readRegister(0x16);
  uint8_t yH_Data = readRegister(0x17);
  uint8_t yL_Data = readRegister(0x18);
  uint8_t zH_Data = readRegister(0x19);
  uint8_t zL_Data = readRegister(0x1A);
  
  //ID lesen
  Serial.print("ID1: ");
  Serial.println(ID1,HEX);
  Serial.print("ID2: ");
  Serial.println(ID2,HEX);
  Serial.print("ID3: ");
  Serial.println(ID3,HEX);
  Serial.print("ID4: ");
  Serial.println(ID4,HEX);
  //x-Achse
  Serial.print("xL_Data: ");
  Serial.println(xL_Data);
  Serial.print("xH_Data: ");
  Serial.println(xH_Data);
  //y-Achse
  Serial.print("yL_Data: ");
  Serial.println(yL_Data);
  Serial.print("yH_Data: ");
  Serial.println(yH_Data);
  //z-Achse
  Serial.print("zL_Data: ");
  Serial.println(zL_Data);
  Serial.print("zH_Data: ");
  Serial.println(zH_Data);
  delay(400);
}

//Register write
void writeRegister(uint8_t registerAddress, uint8_t value) {
  //uint16_t command = ((registerAddress << 9) | value); // R/W-Bit = 0
  uint16_t command = (((uint16_t)registerAddress << 9) | value);
  digitalWrite(CS_PIN, LOW);
  
  SPI.transfer16(command);

  digitalWrite(CS_PIN, HIGH);
}

//Register read
uint8_t readRegister(uint8_t registerAddress){
    
    digitalWrite(CS_PIN, LOW);

    //uint16_t data = SPI.transfer16((registerAddress << 9) | (0x01<<8));
    int8_t data = SPI.transfer16((registerAddress<<9) | (0x01<<8));
    digitalWrite(CS_PIN, HIGH);

    return data;
}