#include <SPI.h>

static const int CS_PIN = 10;   // Chip Select pin for the MT6835
static const uint16_t CMD_READ    = 0b0011000000000000;
static const uint16_t CMD_WRITE   = 0b0110000000000000;
static const uint16_t CMD_EEPROM  = 0b1100000000000000;
static const uint16_t REG_RES_HI = 0x0007;
static const uint16_t REG_RES_LO = 0x0008;
static const uint16_t RES_VALUE = 8191;
static const SPISettings settings6835(50000, MSBFIRST, SPI_MODE3);

void writeRegister8(uint16_t reg, uint8_t value)
{
  Serial.print(reg | CMD_WRITE, HEX);
  Serial.println(value, HEX);
  SPI.beginTransaction(settings6835);
  delay(1);
  digitalWrite(CS_PIN, LOW);
  SPI.transfer16(reg | CMD_WRITE);
  SPI.transfer(value);
  delay(1);
  digitalWrite(CS_PIN, HIGH);
  SPI.endTransaction();
}

uint8_t readRegister8(uint16_t reg)
{
  Serial.print(reg | CMD_READ, HEX);
  SPI.beginTransaction(settings6835);
  digitalWrite(CS_PIN, LOW);
  delay(1);
  SPI.transfer16(reg | CMD_READ);
  uint8_t val = SPI.transfer(0x00);
  delay(1);
  digitalWrite(CS_PIN, HIGH);
  SPI.endTransaction();
  Serial.println(val, HEX);
  return val;
}

uint8_t writeToEeprom()
{
  Serial.print(CMD_EEPROM, HEX);
  SPI.beginTransaction(settings6835);
  digitalWrite(CS_PIN, LOW);
  delay(1);
  SPI.transfer16(CMD_EEPROM);
  uint8_t val = SPI.transfer(0x00);
  delay(1);
  digitalWrite(CS_PIN, HIGH);
  SPI.endTransaction();
  Serial.println(val, HEX);
  return val == 0x55;
}

void setup()
{
  uint8_t res_hi;
  uint8_t res_lo;
  uint16_t res;
  // Initialize Serial (for debugging)
  Serial.begin(115200);
  while (!Serial) { }
  Serial.println("Configuring MT6835");

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

  // Initialize SPI
  SPI.begin();

  delay(100);  // Allow some startup time for the MT6835

  // Read current resolution value
  res_hi = readRegister8(REG_RES_HI);
  res_lo = readRegister8(REG_RES_LO);
  res = (uint16_t)res_hi << 6;
  res |= (uint16_t)res_lo >> 2;
  Serial.print("Current resolution: ");
  Serial.println(res, DEC);

  delay(100);

  if (res == RES_VALUE)
  {
    Serial.println("Values match - writing to EEPROM!");
    writeToEeprom();
  }
  else
  {
    Serial.print("Setting resolution to ");
    Serial.print(RES_VALUE, DEC);
    Serial.println(" PPR...");
    // Write new value to resolution registers
    res_hi = (uint8_t)(RES_VALUE >> 6);
    res_lo = (uint8_t)((RES_VALUE << 2) & 0xFC);
    writeRegister8(REG_RES_HI, res_hi);
    writeRegister8(REG_RES_LO, res_lo);

    delay(100);

    // Read current value again
    res_hi = readRegister8(REG_RES_HI);
    res_lo = readRegister8(REG_RES_LO);
    res = (uint16_t)res_hi << 6;
    res |= (uint16_t)res_lo >> 2;
    Serial.print("Resolution readback: ");
    Serial.println(res, DEC);

    if (res == RES_VALUE)
    {
      Serial.println("Values match - writing to EEPROM!");
      writeToEeprom();
    }
  }

  SPI.end();
}

void loop()
{
  // Your main code can read angle data or use the incremental outputs.
  // For instance, to read the absolute angle from the sensor:
  // (This is just a typical pattern; you must adjust for your sensor’s protocol.)
  /*
    uint16_t angleData = 0;
    digitalWrite(CS_PIN, LOW);

    // Send read command / address to read angle from sensor
    SPI.transfer(0x02);  // Example angle register (16-bit)
    
    uint8_t highByte = SPI.transfer(0x00);
    uint8_t lowByte  = SPI.transfer(0x00);

    digitalWrite(CS_PIN, HIGH);

    angleData = (highByte << 8) | lowByte;
    // angleData is now the raw angle, typically 0 to (2^N - 1) for N-bit resolution.

    float angleDeg = ((float)angleData / 4096.0) * 360.0; // If 4096 counts per revolution
    Serial.print("Angle: ");
    Serial.print(angleDeg, 2);
    Serial.println(" deg");

    delay(250);
  */
}