#include <I2cDiscreteIoExpander.h>


//#define PIN_PCF_INTERRUPT_INPUT   3
//#define PIN_GOT_INTERRUPT_INDICATOR 10
#define Trig_A0   4
//#define Trig_A1   5
//#define Trig_A2   6
//#define Trig_A3   7
//#define Trig_A4   8
//#define Trig_A5   9
#define Open 0
#define Close 1

static uint16_t room_state[6]={0xffff,0xffff,0xffff,0xffff,0xffff,0xffff} ;


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

void action(int id, uint16_t room ,int state){

 Serial.print("R");
 Serial.print(id+1);
   
switch (room){
        case 0x1:  Serial.print("01"); break;        
        case 0x2:  Serial.print("02"); break;
        case 0x4:  Serial.print("03"); break;
        case 0x8:  Serial.print("04"); break;
        case 0x10:  Serial.print("05"); break;
        case 0x20:  Serial.print("06"); break;
        case 0x40:  Serial.print("07"); break;
        case 0x80:  Serial.print("08"); break;
        case 0x100:  Serial.print("09"); break;       
        case 0x200:  Serial.print("10"); break;
        case 0x400:  Serial.print("11"); break;
        case 0x800:  Serial.print("12"); break;
        case 0x1000:  Serial.print("13"); break;
        case 0x2000:  Serial.print("14"); break;
        case 0x4000:  Serial.print("15"); break;
        case 0x8000:  Serial.print("16"); break;
        
      }
  if(state == 0){
  Serial.print(" Open # ");
 }else Serial.print(" Close # ");
}

volatile bool haveInt[2]=  {false, false};
void pcfInt0() {
  haveInt[0] = true;
}

void pcfInt1() {
  haveInt[1] = true;
}


void setup()
{
pinMode(Trig_A0, INPUT_PULLUP);
//pinMode(Trig_A1, INPUT_PULLUP);
//pinMode(Trig_A2, INPUT_PULLUP);
//pinMode(Trig_A3, INPUT_PULLUP);
//pinMode(Trig_A4, INPUT_PULLUP);
//pinMode(Trig_A5, INPUT_PULLUP);

  // initialize i2c interface
  Wire.begin();

  // initialize serial interface
  Serial.begin(9600);

  device[0].disableBitwiseInversion();
 // device[1].disableBitwiseInversion();
 // device[2].disableBitwiseInversion();
 // device[3].disableBitwiseInversion();
 // device[4].disableBitwiseInversion();
 // device[5].disableBitwiseInversion();
 

  

  //flashLED(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);

//  Serial.println("ATTACHING INTs");
 // attachInterrupt(digitalPinToInterrupt(3), pcfInt1, FALLING);

  delay(1000);

  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("Got Data");
    device[idx].digitalRead();
    return device[idx].getPorts();

}

void mirrorInputOntoOutputs(uint8_t idx) {

    uint16_t Act_room;
    int Rstate;
    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, 0xffff) ; // (inputs << 8) | 0xff;

    Serial.print("Inputs read as 0x"); Serial.println(bothports, HEX);
    Serial.print("Room state     0x"); Serial.println(room_state[0], HEX);

    //Serial.println(" being mirrored to out");
    if (room_state[idx] == bothports){
      Serial.println("equal");
    }else if (room_state[idx] < bothports){
      Act_room = bothports - room_state[idx];
      Rstate = Close;
      action(idx,Act_room ,Rstate);
    }else if (room_state[idx] > bothports){
      Act_room = room_state[idx] - bothports ;
      Rstate = Open;
      action(idx,Act_room ,Rstate);
         }

   // 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);
       room_state[idx] = bothports;
}

bool handleInt(uint8_t idx) {

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

  for(;;){
    mirrorInputOntoOutputs(0);

    if (digitalRead(Trig_A0)== LOW){
      Serial.println("===== test1=====");
      //delay(20);
      mirrorInputOntoOutputs(0);
    }
  /*
    if (digitalRead(Trig_A1)== LOW){
      Serial.println("===== test2=====");
      //delay(20);
      mirrorInputOntoOutputs(1);
    }
    if (digitalRead(Trig_A2)== LOW){
      Serial.println("===== test3=====");
      //delay(20);
      mirrorInputOntoOutputs(2);
    }
    if (digitalRead(Trig_A3)== LOW){
      Serial.println("===== test1=====");
      //delay(20);
      mirrorInputOntoOutputs(3);
    }
    if (digitalRead(Trig_A4)== LOW){
      Serial.println("===== test2=====");
      //delay(20);
      mirrorInputOntoOutputs(4);
    }
    if (digitalRead(Trig_A5)== LOW){
      Serial.println("===== test3=====");
      //delay(20);
     mirrorInputOntoOutputs(5);
  
    }
    */
    /*
     if (digitalRead(7)== LOW){
      Serial.println("===== test3=====");
      //delay(20);
      mirrorInputOntoOutputs(2);
    }
    if (digitalRead(6)== LOW){
      Serial.println("===== test4=====");
      //delay(20);
      mirrorInputOntoOutputs(3);
    }
     if (digitalRead(5)== LOW){
      Serial.println("===== test5=====");
      //delay(20);
      mirrorInputOntoOutputs(4);
    }
    if (digitalRead(4)== LOW){
      Serial.println("===== test6=====");
      //delay(20);
      mirrorInputOntoOutputs(5);
    }
    */
  }
  
/* 
  device[0].digitalWrite(0xffff);
  device[1].digitalWrite(0xffff);
 
 delay (1000);
  device[0].digitalWrite(0x0000);
  device[1].digitalWrite(0x0000);

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

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