#include "ADXL345FreshSPI.h"

static const char *TAG_FRESH = "ADXLFRESH";

static inline int16_t u8pair_to_i16(uint8_t lo, uint8_t hi) {
  return (int16_t)((hi << 8) | lo);
}

bool ADXL345FreshSPI::begin(SPIClass* s, uint8_t csPin, uint32_t clockHz) {
  spi = s; cs = csPin; spiHz = clockHz;
  pinMode(cs, OUTPUT); digitalWrite(cs, HIGH);
  //spi->begin(); // tymczas
  delay(1);

  if (!ping()) return false;

  // 1. Wymuś STANDBY przed jakąkolwiek konfiguracją
  write8(ADXL345_REG_POWER_CTL, 0x00); 
  delay(1);

  // 2. Skonfiguruj format danych (Range i Full_Res)
  if (!setRange(Range::G16, true)) { // było if (!setRange(Range::G2, true)) return false;
    ESP_LOGI(TAG_FRESH, "Range G16 ERROR!");
    return false;
  } 
  // 3. Skonfiguruj ODR
  if (!setODR_Hz(100.0f)) return false;      // domyślnie 100 Hz
  // 4. Dopiero teraz włącz pomiar
  if (!configurePowerMeasure()) return false;
  showRangeFull();
  return true;
}


bool ADXL345FreshSPI::ping() {
  uint8_t id=0; if (!read8(ADXL345_REG_DEVID, id)) return false;
  return id == 0xE5;
}

bool ADXL345FreshSPI::configurePowerMeasure() {
  return write8(ADXL345_REG_POWER_CTL, ADXL345_POWER_MEASURE);
}


bool ADXL345FreshSPI::setRange(Range r, bool fullRes_) {
  fullRes = fullRes_;
  // Wymuś STANDBY (MEASURE=0) – kluczowe dla zmiany RANGE/FULL_RES
  write8(ADXL345_REG_POWER_CTL, 0x00);
  delayMicroseconds(5);

  uint8_t fmt = 0;
  if (!read8(ADXL345_REG_DATA_FORMAT, fmt)) return false;

  fmt &= ~ADXL345_DATA_FORMAT_RANGE_MASK;   // bity 1:0
  fmt |= (uint8_t)r;

  if (fullRes) fmt |= ADXL345_DATA_FORMAT_FULL_RES;   // bit 3
  else fmt &= ~ADXL345_DATA_FORMAT_FULL_RES;

  if (!write8(ADXL345_REG_DATA_FORMAT, fmt)) return false;

  // Kontrola: odczyt po zapisie
  //uint8_t verify = 0;
  //read8(ADXL345_REG_DATA_FORMAT, verify);
  // tu możesz logować verify

  // Wróć do MEASURE
  write8(ADXL345_REG_POWER_CTL, ADXL345_POWER_MEASURE);
  scale_g_per_lsb = fullRes ? 0.0039f : (1.0f/256.0f) * (2 << (uint8_t)r);
  return true;
}


uint8_t ADXL345FreshSPI::odrCodeFromHz(float hz) {
  struct { uint8_t code; float f; } map[] = {
    {0x06, 6.25f},{0x07,12.5f},{0x08,25.f},{0x09,50.f},{0x0A,100.f},
    {0x0B,200.f},{0x0C,400.f},{0x0D,800.f},{0x0E,1600.f},{0x0F,3200.f}
  };
  uint8_t best=0x0A; float bestErr=1e9f;
  for (auto &e: map){ float err=fabsf(e.f-hz); if (err<bestErr){bestErr=err; best=e.code;}}
  return best;
}

bool ADXL345FreshSPI::setODR_Hz(float hz) {
  return write8(ADXL345_REG_BW_RATE, odrCodeFromHz(hz));
}

bool ADXL345FreshSPI::enableFIFO(FIFOmode mode, uint8_t triggerLevel) {
  triggerLevel = constrain(triggerLevel, (uint8_t)1, (uint8_t)32);
  uint8_t m = ADXL345_FIFO_BYPASS;
  switch(mode){
    case FIFOmode::BYPASS:  m = ADXL345_FIFO_BYPASS; break;
    case FIFOmode::FIFO:    m = ADXL345_FIFO_FIFO;   break;
    case FIFOmode::STREAM:  m = ADXL345_FIFO_STREAM; break;
    case FIFOmode::TRIGGER: m = ADXL345_FIFO_TRIGGER;break;
  }
  return write8(ADXL345_REG_FIFO_CTL, (uint8_t)(m | ((triggerLevel-1) & 0x1F)));
}

bool ADXL345FreshSPI::enableDataReadyInterrupt(bool enable) {
  uint8_t ie=0; if (!read8(ADXL345_REG_INT_ENABLE, ie)) return false;
  if (enable)  ie |= ADXL345_DATA_READY_BIT;
  else         ie &= ~ADXL345_DATA_READY_BIT;
  return write8(ADXL345_REG_INT_ENABLE, ie);
}

bool ADXL345FreshSPI::available() {
  uint8_t src=0; if (!read8(ADXL345_REG_INT_SOURCE, src)) return false;
  return (src & ADXL345_DATA_READY_BIT) != 0;
}

bool ADXL345FreshSPI::readFresh(SampleI16& out, uint32_t timeout_ms) {
  uint32_t start = millis();
  while (!available()) {
    if ((millis() - start) > timeout_ms) return false;
    delayMicroseconds(200);
  }
  uint8_t buf[6];
  if (!readMulti(ADXL345_REG_DATAX0, buf, 6)) return false;
  out.x = u8pair_to_i16(buf[0], buf[1]);
  out.y = u8pair_to_i16(buf[2], buf[3]);
  out.z = u8pair_to_i16(buf[4], buf[5]);
  out.ts_us = micros();
  return true;
}

bool ADXL345FreshSPI::readFresh(SampleSI& out, uint32_t timeout_ms) {
  SampleI16 raw;
  if (!readFresh(raw, timeout_ms)) return false;
  countsToSI(raw, out);
  return true;
}

size_t ADXL345FreshSPI::readFIFOBurst(SampleI16* buf, size_t maxCount) {
  if (!buf || maxCount==0) return 0;
  uint8_t status=0; if (!read8(ADXL345_REG_FIFO_STATUS, status)) return 0;
  uint8_t entries = status & 0x3F; // 0..32
  size_t n = min<size_t>(entries, maxCount);
  for (size_t i=0;i<n;i++){
    uint8_t d[6];
    if (!readMulti(ADXL345_REG_DATAX0, d, 6)) return i;
    buf[i].x = u8pair_to_i16(d[0], d[1]);
    buf[i].y = u8pair_to_i16(d[2], d[3]);
    buf[i].z = u8pair_to_i16(d[4], d[5]);
    buf[i].ts_us = micros();
  }
  return n;
}

size_t ADXL345FreshSPI::readFIFOBurst(SampleSI* buf, size_t maxCount) {
  if (!buf || maxCount==0) return 0;
  size_t n = 0;
  while (n < maxCount) {
    uint8_t status=0; if (!read8(ADXL345_REG_FIFO_STATUS, status)) break;
    uint8_t entries = status & 0x3F;
    if (entries == 0) break;
    uint8_t d[6];
    uint32_t t = micros();
    if (!readMulti(ADXL345_REG_DATAX0, d, 6)) break;
    SampleI16 s;
    s.x = u8pair_to_i16(d[0], d[1]);
    s.y = u8pair_to_i16(d[2], d[3]);
    s.z = u8pair_to_i16(d[4], d[5]);
    s.ts_us = t;
    countsToSI(s, buf[n]);
    n++;
  }
  return n;
}

void ADXL345FreshSPI::countsToSI(const SampleI16& in, SampleSI& out) {
  out.ax_g  = in.x * scale_g_per_lsb;
  out.ay_g  = in.y * scale_g_per_lsb;
  out.az_g  = in.z * scale_g_per_lsb;
  out.ax_ms2 = out.ax_g * g_ms2;
  out.ay_ms2 = out.ay_g * g_ms2;
  out.az_ms2 = out.az_g * g_ms2;
  out.ts_us = in.ts_us;
}

// ---------- Low-level SPI -----------
bool ADXL345FreshSPI::write8(uint8_t reg, uint8_t val) {
  spi->beginTransaction(SPISettings(spiHz, MSBFIRST, SPI_MODE3));
  spiSelect();
  spi->transfer(reg & 0x3F);       // write, single
  spi->transfer(val);
  spiDeselect();
  spi->endTransaction();
  return true;
}

bool ADXL345FreshSPI::read8(uint8_t reg, uint8_t& val) {
  spi->beginTransaction(SPISettings(spiHz, MSBFIRST, SPI_MODE3));
  spiSelect();
  spi->transfer(0x80 | (reg & 0x3F)); // read, single
  val = spi->transfer(0x00);
  spiDeselect();
  spi->endTransaction();
  return true;
}

bool ADXL345FreshSPI::readMulti(uint8_t reg, uint8_t *dst, size_t n) {
  if (n==0) return true;
  spi->beginTransaction(SPISettings(spiHz, MSBFIRST, SPI_MODE3));
  spiSelect();
  spi->transfer(0xC0 | (reg & 0x3F)); // read, multi (MB=1, R/W=1)
  for (size_t i=0;i<n;i++) dst[i] = spi->transfer(0x00);
  spiDeselect();
  spi->endTransaction();
  return true;
}

void ADXL345FreshSPI::spiSelect()   { digitalWrite(cs, LOW);  }
void ADXL345FreshSPI::spiDeselect() { digitalWrite(cs, HIGH); }


uint8_t ADXL345FreshSPI::getADXLRange() {
  uint8_t format = 0;
  if (!read8(0x31, format)) return 255; // albo 0
  switch (format & 0x03) {
    case 0: return 2;
    case 1: return 4;
    case 2: return 8;
    case 3: return 16;
  }
  return 255;
}


bool ADXL345FreshSPI::getADXLFullRes() {
  uint8_t format = 0;
  if (!read8(0x31, format)) return false;
  //bool ok = read8(0x31, format);
  //ESP_LOGI(TAG_FRESH, "read8 ok=%d DATA_FORMAT=0x%02X", ok, format);
  return (format & 0x08) != 0;
}

void ADXL345FreshSPI::showRangeFull(String txt) {
  uint8_t format = getADXLRange();
  bool full_res = getADXLFullRes();
  ESP_LOGI(TAG_FRESH, "%s ADXL345: RANGE=%dG FULL_RES=%d", txt, format, full_res);
}