#pragma once
#include <Arduino.h>
#include <SPI.h>
#include "ADXL345FreshSPI.h"

class ADXL345ArraySPI {
public:
  static constexpr uint8_t MAX_SENSORS = 4;

  explicit ADXL345ArraySPI(const uint8_t cs_pins[MAX_SENSORS]) {
    for (uint8_t i=0;i<MAX_SENSORS;i++){ cs_[i]=cs_pins[i]; }
  }

  // Inicjalizacja wszystkich (SPI MODE3, STREAM FIFO, full-res, wybrany ODR/Range).
  // Zwraca liczbę poprawnie wykrytych czujników.
  uint8_t beginAll(SPIClass *spi, uint32_t spiHz, float odr_hz, ADXL345FreshSPI::Range range = ADXL345FreshSPI::Range::G16) {
    presentMask_ = 0;

    // Zbezpieczenie: inicjalizacja CS na HIGH przed startem.
    for (uint8_t i=0;i<MAX_SENSORS;i++){
      pinMode(cs_[i], OUTPUT);
      digitalWrite(cs_[i], HIGH);
    }
    delayMicroseconds(5);

    for (uint8_t i=0;i<MAX_SENSORS;i++){
      if (!acc_[i].begin(spi, cs_[i], spiHz)) continue;
      acc_[i].write8(0x2D, 0x00);        // POWER_CTL = standby (MEASURE=0)
      if (!acc_[i].setRange(range, true)) continue;
      acc_[i].write8(0x2D, 0x08);        // POWER_CTL = measure
      if (!acc_[i].setODR_Hz(odr_hz)) continue;
      // Tryb STREAM – najbezpieczniejszy dla jitteru CPU
      acc_[i].enableFIFO(ADXL345FreshSPI::FIFOmode::STREAM, 32);
      presentMask_ |= (1u << i);
    }
    return countPresent();
  }

  inline uint8_t size() const { return MAX_SENSORS; }
  inline bool isPresent(uint8_t i) const { return (presentMask_ & (1u << i)) != 0; }
  inline uint8_t countPresent() const { return __builtin_popcount(presentMask_); }

  // Sprawdza, czy KAŻDY obecny sensor ma >=1 nową próbkę (DATA_READY).
  bool availableAll() {
    for (uint8_t i=0;i<MAX_SENSORS;i++){
      if (!isPresent(i)) continue;
      if (!acc_[i].available()) return false;
    }
    return true;
  }

  // Zdejmuje po 1 NAJSTARSZEJ próbce z FIFO każdego obecnego sensora.
  // Zwraca liczbę faktycznie zebranych próbek (== liczbie obecnych sensorów, jeśli sukces).
  uint8_t readAlignedOnce(int16_t x[], int16_t y[], int16_t z[], uint32_t ts_us[]) {
    uint8_t got = 0;
    for (uint8_t i=0;i<MAX_SENSORS;i++){
      if (!isPresent(i)) continue;

      ADXL345FreshSPI::SampleI16 one[1];
      size_t n = acc_[i].readFIFOBurst(one, 1);  // NAJSTARSZA próbka z FIFO
      if (n == 0) {
        // sporadycznie: awaryjnie dociągnij świeżą (rzadkie)
        ADXL345FreshSPI::SampleI16 s;
        if (!acc_[i].readFresh(s, 1)) return got; // przerwij zbieranie ramki
        one[0] = s;
      }
      x[i] = one[0].x; y[i] = one[0].y; z[i] = one[0].z; ts_us[i] = one[0].ts_us;
      got++;
    }
    return got;
  }

  ADXL345FreshSPI& at(uint8_t i) { return acc_[i]; }

private:
  uint8_t cs_[MAX_SENSORS]{};
  ADXL345FreshSPI acc_[MAX_SENSORS];
  uint32_t presentMask_ = 0;
};