#include "ADXL345FastSPI.h"

static const char *TAG_FRESH = "ADXLFAST";

bool ADXL345FastSPI::begin(SPIClass *spi, uint32_t spiHz, Rate rate, Range range, uint8_t /*options*/){
  spi_   = spi;
  spiHz_ = spiHz;
  odrHz_ = mapRateToHz(rate);
  auto r = mapRange(range);

  //spi_->begin(); 28.01.2026
  presentCnt_ = 0;
  for (uint8_t i = 0; i < count_; ++i) {
    bool ok = dev_[i].begin(spi_, cs_[i], spiHz_)
           && dev_[i].setRange(r, true)
           && dev_[i].setODR_Hz(odrHz_);

    dev_[i].showRangeFull(String(i));


    if (ok) {
      dev_[i].enableFIFO(ADXL345FreshSPI::FIFOmode::STREAM, 32); // wariant B
      present_[i] = true;
      presentCnt_++;
    } else {
      present_[i] = false;
    }
  }
  return presentCnt_ > 0;
}

bool ADXL345FastSPI::availableAll() {
  for (uint8_t i = 0; i < count_; ++i) {
    if (!present_[i]) continue;
    // else {
    //   dev_[i].showRangeFull("AVAILABLE ADXL");
    // }

    if (!dev_[i].available()) return false;
  }
  return true;
}

uint8_t ADXL345FastSPI::readAlignedOnce(int16_t* x, int16_t* y, int16_t* z, uint32_t* ts_us){
  // 1) Czekaj aż każdy ma ≥1 próbkę (DATA_READY => FIFO>0)
  while (!availableAll()) {
    // micro-spin; w razie potrzeby można dodać yield() / feed watchdog poza hot-path
  }

  // 2) Zdejmij najstarszą z każdego sensora
  uint8_t got = 0;
  for (uint8_t i = 0; i < count_; ++i) {
    if (!present_[i]) continue;

    ADXL345FreshSPI::SampleI16 one[1];
    size_t n = dev_[i].readFIFOBurst(one, 1);
    if (n == 0) {
      ADXL345FreshSPI::SampleI16 s;
      if (!dev_[i].readFresh(s, 1)) break;
      one[0] = s;
    }
    if (x)     x[i]     = one[0].x;
    if (y)     y[i]     = one[0].y;
    if (z)     z[i]     = one[0].z;
    if (ts_us) ts_us[i] = one[0].ts_us;
    got++;
  }
  return got;
}

bool ADXL345FastSPI::readNewSample(uint8_t idx, int16_t& x, int16_t& y, int16_t& z, bool& ready)
{
  if (idx >= count_ || !present_[idx]) { ready = false; return false; }

  ADXL345FreshSPI::SampleI16 one[1];
  size_t n = dev_[idx].readFIFOBurst(one, 1);
  if (n == 0) {
    ADXL345FreshSPI::SampleI16 s;
    if (!dev_[idx].readFresh(s, 1)) { ready = false; return false; }
    one[0] = s;
  }
  x = one[0].x; y = one[0].y; z = one[0].z;
  ready = true;
  return true;
}

uint8_t ADXL345FastSPI::refreshConnectedSensorsCount(){
  uint8_t cnt = 0;
  for (uint8_t i = 0; i < count_; ++i) {
    // ping() czyta DEVID (0xE5) wewnętrznie i zwraca true/false
    if (dev_[i].ping()) {
      present_[i] = true;
      cnt++;
    } else {
      present_[i] = false;
    }
  }
  presentCnt_ = cnt;
  return cnt;
}

/*
Zwraca maskę bitową aktywnych sensorów (np. do szybkiej diagnostyki: który CS nie odpowiada)
Uzycie: 
uint8_t mask = adxl.refreshActiveMask();
ESP_LOGI(TAG_MAIN, "Aktywne sensory (bitmask): 0x%02X, count=%u",
         mask, adxl.connectedSensorsCount());

// Sprawdzenie konkretnego sensora:
if (mask & (1u << 3)) {
  ESP_LOGI(TAG_MAIN, "Sensor #3 online");
} else {
  ESP_LOGW(TAG_MAIN, "Sensor #3 offline");
}
*/
uint8_t ADXL345FastSPI::refreshActiveMask(){
  uint8_t m = 0;
  uint8_t cnt = 0;

  for (uint8_t i = 0; i < count_; ++i) {
    if (dev_[i].ping()) {
      present_[i] = true;
      m |= (1u << i);
      cnt++;
    } else {
      present_[i] = false;
    }
  }
  presentCnt_ = cnt;
  return m;
}

// Pobiera zakres wybranego akcelerometru
uint8_t ADXL345FastSPI::getRange(uint8_t accel){
    return dev_[accel].getADXLRange();
}

bool ADXL345FastSPI::getFullRes(uint8_t accel){
    return dev_[accel].getADXLFullRes();
}