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

class ADXL345FastSPI {
public:
  enum Rate { RATE_100HZ, RATE_200HZ, RATE_400HZ, RATE_800HZ, RATE_1600HZ, RATE_3200HZ };
  enum Range { RANGE_2G, RANGE_4G, RANGE_8G, RANGE_16G };

  ADXL345FastSPI(const uint8_t* csPins, uint8_t count) : count_(count > MAX_NUM ? MAX_NUM : count){
    for (uint8_t i = 0; i < count_; ++i) cs_[i] = csPins[i];
  }

  /* Pobierz konfigurację zakres akcelerometru */
  uint8_t getRange(uint8_t accel);

  /* Zwraca czy jest FUL_RES*/
  bool getFullRes(uint8_t accel);

  // Wersja podstawowa: podaj SPIClass* (np. &SPI)
  bool begin(SPIClass *spi, uint32_t spiHz, Rate rate, Range range, uint8_t options = 0);

  // Wersja wygodna: deleguje do powyższej, używając globalnego SPI
  inline bool begin(uint32_t spiHz, Rate rate, Range range, uint8_t options = 0) {
    return begin(&SPI, spiHz, rate, range, options);
  }

  // Informacje / dostępność
  inline uint8_t size() const { return presentCnt_; }
  inline bool    isPresent(uint8_t i) const { return (i < count_) && present_[i]; }
  bool availableAll();

  // Wariant B: zdejmuje po 1 NAJSTARSZEJ próbce z FIFO każdego obecnego sensora
  // Zwraca liczbę zebranych próbek (== size(), jeśli pełna ramka).
  uint8_t readAlignedOnce(int16_t* x, int16_t* y, int16_t* z, uint32_t* ts_us);

  // Zgodność: pojedynczy sensor – 1 próbka
  bool readNewSample(uint8_t idx, int16_t& x, int16_t& y, int16_t& z, bool& ready);

  // Zwraca ilośc podłączonych czujników po inicjalizacji - ale nie jest dynamiczna!
  inline uint8_t connectedSensorsCount() const { return presentCnt_; }

  // dynamiczne odświeżenie liczby działających czujników zawsze sprawdzi na bieżąco i zwróci przy działającym urządzeniu
  uint8_t refreshConnectedSensorsCount();

  // Maska aktywnych sensorów wg bieżącego stanu (bit i = sensor i)
  inline uint8_t activeMask() const {
    uint8_t m = 0;
    for (uint8_t i = 0; i < count_; ++i) if (present_[i]) m |= (1u << i);
    return m;
  }

  // Odświeża stan (ping) i zwraca zaktualizowaną maskę aktywnych sensorów
  uint8_t refreshActiveMask();

private:
  static constexpr uint8_t MAX_NUM = 4; // MAX Ilość ADXL345

  SPIClass* spi_ = &SPI;
  ADXL345FreshSPI dev_[MAX_NUM];
  uint8_t cs_[MAX_NUM]{};
  bool    present_[MAX_NUM]{};
  uint8_t count_ = 0;
  uint8_t presentCnt_ = 0;

  uint32_t spiHz_ = 5000000;
  float    odrHz_ = 3200.0f;

  static float mapRateToHz(Rate r) {
    switch (r) {
      case RATE_100HZ:  return 100.0f;
      case RATE_200HZ:  return 200.0f;
      case RATE_400HZ:  return 400.0f;
      case RATE_800HZ:  return 800.0f;
      case RATE_1600HZ: return 1600.0f;
      case RATE_3200HZ: return 3200.0f; 
      default: return 3200.0f;
    }
  }
  static ADXL345FreshSPI::Range mapRange(Range r) {
    switch (r) {
      case RANGE_2G:  return ADXL345FreshSPI::Range::G2;
      case RANGE_4G:  return ADXL345FreshSPI::Range::G4;
      case RANGE_8G:  return ADXL345FreshSPI::Range::G8;
      case RANGE_16G: return ADXL345FreshSPI::Range::G16;
      default: return ADXL345FreshSPI::Range::G16;
    }
  }
};