#include <I2cDiscreteIoExpander.h>

#define Add0 9
#define Add1 8
#define Add2 7
#define Add3 6
#define Add4 5
#define Add5 4


// instantiate I2cDiscreteIoExpander object
I2cDiscreteIoExpander device[6] = {0,1,2,3,4,5};

void setup()
{
pinMode(Add0, INPUT_PULLUP);
pinMode(Add1, INPUT_PULLUP);
pinMode(Add2, INPUT_PULLUP);
pinMode(Add3, INPUT_PULLUP);
pinMode(Add4, INPUT_PULLUP);
pinMode(Add5, INPUT_PULLUP);

  // initialize i2c interface
  Wire.begin();

  // initialize serial interface
  Serial.begin(115200);
  device[0].disableBitwiseInversion();
  device[1].disableBitwiseInversion();
  device[2].disableBitwiseInversion();
  device[3].disableBitwiseInversion();
  device[4].disableBitwiseInversion();
  device[5].disableBitwiseInversion();
 
  device[0].digitalWrite(0xffff);
  device[1].digitalWrite(0xffff);
  device[2].digitalWrite(0xffff);
  device[3].digitalWrite(0xffff);
  device[4].digitalWrite(0xffff);
  device[5].digitalWrite(0xffff);

  delay(100);

  Serial.println("Clearing any int/mirroring inputs");
  mirrorInputOntoOutputs(0);
  mirrorInputOntoOutputs(1);
  mirrorInputOntoOutputs(2);
  mirrorInputOntoOutputs(3);
  mirrorInputOntoOutputs(4);
  mirrorInputOntoOutputs(5);

}


void flashLED(uint8_t n) {
  for (uint8_t i=0; i<n; i++) {
    device[0].digitalWrite(0x7fff);
    delay(100);
    device[0].digitalWrite(0xffff);
    delay(80);
  }

}

uint16_t getChangedInputValue(uint8_t idx) {

    Serial.println("Get Input");
    device[idx].digitalRead();
    return device[idx].getPorts();

}

void mirrorInputOntoOutputs(uint8_t idx) {

    uint16_t bothports = getChangedInputValue(idx);
    uint16_t inputs = lowByte(bothports);
    // Mirror the inputs to the outputs
    // leave the inputs, as inputs (i.e. writing high)
    uint16_t newValue = word(inputs, 0xff) ; // (inputs << 8) | 0xff;
    Serial.print("Inputs read as 0x");
    Serial.print(inputs, HEX);
    //Serial.println(" being mirrored to out");
    device[idx].digitalWrite(newValue);
    Serial.print("Wrote 2 bytes: port0 -> 0x");
    Serial.print(lowByte(newValue), HEX);
    Serial.print(" port1 -> 0x");
    Serial.print(highByte(newValue), HEX);
    Serial.print(" to device ");
    Serial.println((int)idx);
}

//bool handleInt(uint8_t idx) {

 // if (haveInt[idx]) {
 //   haveInt[idx] = false;
 //   mirrorInputOntoOutputs(idx);
//    return true;
 // } 
  
 // return false;
//}
void loop() {

  for(;;){

    if (digitalRead(Add0)== LOW){
      Serial.println("===== test1=====");
      //delay(20);
      mirrorInputOntoOutputs(0);
    }
    if (digitalRead(Add1)== LOW){
      Serial.println("===== test2=====");
      //delay(20);
      mirrorInputOntoOutputs(1);
    }
     if (digitalRead(Add2)== LOW){
      Serial.println("===== test3=====");
      //delay(20);
      mirrorInputOntoOutputs(2);
    }
    if (digitalRead(Add3)== LOW){
      Serial.println("===== test4=====");
      //delay(20);
      mirrorInputOntoOutputs(3);
    }
     if (digitalRead(Add4)== LOW){
      Serial.println("===== test5=====");
      //delay(20);
      mirrorInputOntoOutputs(4);
    }
    if (digitalRead(Add5)== LOW){
      Serial.println("===== test6=====");
      //delay(20);
      mirrorInputOntoOutputs(5);
    }
  }
/*
  device[0].digitalWrite(0xffff);
  device[1].digitalWrite(0xffff);
  device[2].digitalWrite(0xffff);
  device[3].digitalWrite(0xffff);
  device[4].digitalWrite(0xffff);
  device[5].digitalWrite(0xffff);
 
 delay (1000);
  device[0].digitalWrite(0x0000);
  device[1].digitalWrite(0x0000);
  device[2].digitalWrite(0x0000);
  device[3].digitalWrite(0x0000);
  device[4].digitalWrite(0x0000);
  device[5].digitalWrite(0x0000);

 
  if (handleInt(0) || handleInt(1)) {
    digitalWrite(PIN_GOT_INTERRUPT_INDICATOR, HIGH);
  } else {

    digitalWrite(PIN_GOT_INTERRUPT_INDICATOR, LOW);
  }
  delay(50);
  }
  */
}