#include <SPI.h>

// Teensy default SPI: Pin 11-MOSI, 12-MISO, 13-SCLK, 10-CS (GND)
#define DRDY_PIN 2

SPISettings spiSettings(500000, MSBFIRST, SPI_MODE1);
#define INTER_COMMAND_DELAY_US 100
#define DEBUG_RAW_BYTES true

void safeDelay() {
  delayMicroseconds(INTER_COMMAND_DELAY_US);
}

void writeRegister(uint8_t reg, uint8_t val) {
  safeDelay();
  SPI.transfer(0x40 | reg);  
  SPI.transfer(0x00);        
  SPI.transfer(val);
}

uint8_t readRegister(uint8_t reg) {
  safeDelay();
  SPI.transfer(0x20 | reg); 
  SPI.transfer(0x00);        
  return SPI.transfer(0xFF);
}

bool waitForDRDY(unsigned long timeout_ms = 1000) {
  unsigned long start = millis();
  while (digitalRead(DRDY_PIN) == HIGH) {
    if ((millis() - start) > timeout_ms) return false;
    delayMicroseconds(10);
  }
  return true;
}

int32_t readADCData(bool &statusOK) {
  uint8_t status;
  uint8_t data[3];

  safeDelay();
  SPI.transfer(0x12);  // RDATA command
  status = SPI.transfer(0xFF);
  for (int i = 0; i < 3; i++) data[i] = SPI.transfer(0xFF);

  if (DEBUG_RAW_BYTES) {
    Serial.print("STATUS: 0x"); Serial.print(status, HEX);
    Serial.print(" | Bytes: ");
    for (int i = 0; i < 3; i++) {
      Serial.print("0x"); Serial.print(data[i], HEX); Serial.print(" ");
    }
    Serial.println();
  }

  statusOK = !(status & 0x80);  // Bit 7 = ADC error

  int32_t val = ((int32_t)data[0] << 16) |
                ((int32_t)data[1] << 8)  |
                ((int32_t)data[2]);
  if (val & 0x800000) val |= 0xFF000000;  // Sign-extend 24-bit to 32-bit

  return val;
}

void setupADS1261() {
  delay(100);  

 //writeRegister(0x01, 0x04);	// No reset, DRDY- Conversion data new
  writeRegister(0x02, 0x42);  // MODE0: SINC3 filter, 400 SPS
  writeRegister(0x03, 0x01);  // MODE1: Continuous conversion
  writeRegister(0x05, 0xD0);  // MODE3: SPITIM=1 (auto-reset), STATENB=1 (status on), Software PWDN
  writeRegister(0x06, 0x0B);  // REF: AIN0 - AIN1 (external 5V)
  writeRegister(0x10, 0x06);  // GAIN: 64
  writeRegister(0x11, 0x34);  // INPMUX: AIN2 - AIN3

  delay(10);


  Serial.print("Register readback = 0x");
  Serial.print(readRegister(0x02), HEX); Serial.print(",");
  Serial.print(readRegister(0x03), HEX); Serial.print(",");
  Serial.print(readRegister(0x05), HEX); Serial.print(",");
  Serial.print(readRegister(0x06), HEX); Serial.print(",");
  Serial.print(readRegister(0x10), HEX); Serial.print(",");
  Serial.println(readRegister(0x11), HEX);
}

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

  pinMode(DRDY_PIN, INPUT);
  SPI.begin();
  SPI.beginTransaction(spiSettings);  // As CS is tied to GND

  Serial.println("Initializing ADS1261...");

  
  SPI.transfer(0x20); SPI.transfer(0x00);
  uint8_t id = SPI.transfer(0xFF);
  Serial.print("Chip ID = 0x"); Serial.println(id, HEX);

  if ((id & 0xF0) != 0x80) {
    Serial.println("Invalid ID. Check SPI wiring.");
    while (1);
  }

  setupADS1261();
  Serial.println("ADS1261 initialized.");
}

void loop() {
  if (!waitForDRDY(1000)) {
    Serial.println("DRDY timeout");
    delay(500);
    return;
  }

  bool statusOK = false;
  int32_t adc_raw = readADCData(statusOK);

  if (statusOK) {
    Serial.print("ADC Raw = ");
    Serial.println(adc_raw);
  } else {
    Serial.println("ADC Status Error Detected!");
  }

  delay(500);
}