#include <Wire.h>
#include <Adafruit_MCP23X17.h>
#include <Adafruit_ADS1X15.h>

// I2C addresses
#define MCP_ADDR 0x20
#define ADS_ADDR 0x48

Adafruit_MCP23X17 mcp;
Adafruit_ADS1115 ads;

// Arrays for 64 channels
int16_t values[64];
int16_t previous[64];
int16_t highs[64];
int16_t lows[64];



void printBinary16(uint16_t value) {
  for (int i = 15; i >= 0; i--) {
    Serial.print(bitRead(value, i));
  }
  Serial.println();
}
//===========================================================================
void initArrays() {
  for (int i = 0; i < 64; i++) {
    values[i]   = 0;
    previous[i] = -1;
    highs[i]    = -32768;
    lows[i]     = 32767;
  }
}
//===========================================================================
// Set the same channel on ALL 4 muxes at once (GPA0-7 + GPB0-7)
void setAllMuxes(uint8_t channel) {
  if (channel > 15) return;

  // Turn ON pins 0-7 (Port A) and OFF pins 8-15 (Port B)
  // binary: 0000000011111111, hex: 0x00FF, decimal: 255
  //  mcp.writeGPIOAB(0x00FF);

  // Turn OFF pins 0-7 (Port A) and ON pins 8-15 (Port B)
  // binary: 1111111100000000, hex: 0xFF00, decimal: 65280
  //  mcp.writeGPIOAB(0xFF00);
  /*
    uint8_t s0 = (channel >> 0) & 0x01;
    uint8_t s1 = (channel >> 1) & 0x01;
    uint8_t s2 = (channel >> 2) & 0x01;
    uint8_t s3 = (channel >> 3) & 0x01;

    // Bulk write is faster than 16 individual digitalWrite
    uint16_t gpio = 0;

    // GPA0-3 (Mux 0) and GPA4-7 (Mux 1)
    if (s0) gpio |= (1 << 0) | (1 << 4);
    if (s1) gpio |= (1 << 1) | (1 << 5);
    if (s2) gpio |= (1 << 2) | (1 << 6);
    if (s3) gpio |= (1 << 3) | (1 << 7);

    // GPB0-3 (Mux 2) and GPB4-7 (Mux 3)
    if (s0) gpio |= (1 << 8)  | (1 << 12);
    if (s1) gpio |= (1 << 9)  | (1 << 13);
    if (s2) gpio |= (1 << 10) | (1 << 14);
    if (s3) gpio |= (1 << 11) | (1 << 15);
  */

  switch (channel) {
    case 0:
      mcp.writeGPIOAB(0);
      break;
    case 1:
      mcp.writeGPIOAB(4369);
      break;
    case 2:
      mcp.writeGPIOAB(8738);
      break;
    case 3:
      mcp.writeGPIOAB(13107);
      break;
    case 4:
      mcp.writeGPIOAB(17476);
      break;
    case 5:
      mcp.writeGPIOAB(21845);
      break;
    case 6:
      mcp.writeGPIOAB(26214);
      break;
    case 7:
      mcp.writeGPIOAB(30583);
      break;
    case 8:
      mcp.writeGPIOAB(34952);
      break;
    case 9:
      mcp.writeGPIOAB(39321);
      break;
    case 10:
      mcp.writeGPIOAB(43690);
      break;
    case 11:
      mcp.writeGPIOAB(48059);
      break;
    case 12:
      mcp.writeGPIOAB(52428);
      break;
    case 13:
      mcp.writeGPIOAB(56797);
      break;
    case 14:
      mcp.writeGPIOAB(61166);
      break;
    case 15:
      mcp.writeGPIOAB(65535);
      break;
    default:
      mcp.writeGPIOAB(0);
      break;
  }
delay(50);
  //mcp.writeGPIOAB(gpio);   // Single I2C transaction for all 16 pins
}
//===========================================================================
// Reset all select pins to LOW in one write
void resetMuxPins() {
  mcp.writeGPIO(0x0000);
}
//===========================================================================
// Read one "layer" (4 channels at the same mux position)
void readLayer(uint8_t layer) {          // layer = 0..15
  setAllMuxes(layer);
  delayMicroseconds(120);                // Settling time – tune down if possible

  int16_t val0 = ads.readADC_SingleEnded(0);
  int16_t val1 = ads.readADC_SingleEnded(1);
  int16_t val2 = ads.readADC_SingleEnded(2);
  int16_t val3 = ads.readADC_SingleEnded(3);

  uint8_t base = layer * 4;   // Wait, no: each mux has 16 channels

  // Correct indexing: channel = mux * 16 + layer
  uint8_t ch0 = 0 * 16 + layer;
  uint8_t ch1 = 1 * 16 + layer;
  uint8_t ch2 = 2 * 16 + layer;
  uint8_t ch3 = 3 * 16 + layer;

  // Update storage
  updateChannel(ch0, val0);
  updateChannel(ch1, val1);
  updateChannel(ch2, val2);
  updateChannel(ch3, val3);

  resetMuxPins();   // Return all selects to LOW
}
//===========================================================================
void updateChannel(uint8_t ch, int16_t val) {
  values[ch] = val;
  if (val > highs[ch]) highs[ch] = val;
  if (val < lows[ch])  lows[ch] = val;
}
//===========================================================================
void setup() {
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);        // 400 kHz I2C for faster communication

  if (!mcp.begin_I2C(MCP_ADDR)) {
    Serial.println("MCP23017 not found!");
    while (1);
  }

  // Set all 16 pins (0-15) as outputs
  for(int pin = 0; pin < 16; pin++) {
    mcp.pinMode(pin, OUTPUT);
  }

  // All pins as output (already default after begin, but ensure)
  mcp.writeGPIO(0x0000);   // All LOW

  if (!ads.begin(ADS_ADDR)) {
    Serial.println("ADS1115 not found!");
    while (1);
  }

  // Faster settings for ADS1115
  ads.setDataRate(RATE_ADS1115_860SPS);   // Fastest single-shot rate
  ads.setGain(GAIN_TWOTHIRDS);         // Default ±6.144V

  initArrays();

  Serial.println("Fast 64-channel reader ready (4 channels per mux select)");

  for (uint8_t layer = 0; layer < 16; layer++) {
    setAllMuxes(layer);
    delay(200);
  }
}
//===========================================================================
void loop() {
  bool anyChange = false;

  for (uint8_t layer = 0; layer < 16; layer++) {
    readLayer(layer);

    // Check for changes on the 4 channels just read
    uint8_t chs[4] = {0 * 16 + layer, 1 * 16 + layer, 2 * 16 + layer, 3 * 16 + layer};

    for (int i = 0; i < 4; i++) {
      uint8_t ch = chs[i];
      if (values[ch] != previous[ch]) {
        Serial.printf("Ch %02d: %6d  (H:%6d L:%6d)\n", ch, values[ch], highs[ch], lows[ch]);
        previous[ch] = values[ch];
        anyChange = true;
      }
    }
  }

  if (!anyChange) {
    static uint32_t lastHb = 0;
    if (millis() - lastHb > 3000) {
      //Serial.println("No changes");
      lastHb = millis();
    }
  }

  // Very short pause between full scans
  delay(10);
}
//===========================================================================