//https://www.luisllamas.es/mas-pines-digitales-con-arduino-y-el-expansor-es-i2c-pcf8574/
#include  "Wire.h"

#define I2C_OUTPUTS_ADDRESS 0x24
#define I2C_INPUTS_ADDRESS 0x22
#define SCL_PIN 5
#define SDA_PIN 4

void init_pcf8574(uint8_t address,uint8_t bits_configuration) {
  Wire.beginTransmission(address);
  Wire.write(~bits_configuration);
  Wire.endTransmission();
}

void write_i2c_gpios(uint8_t address, bool *digital_outputs){
  uint8_t data = 0;
  uint8_t write_mask = 1;

  for(uint8_t i = 0; i < 8; i ++){
    data = data | ((uint8_t(digital_outputs[i])) << i);
  }
  data = ~data;

  //data = 0b01010101;

  Wire.beginTransmission(address);
  Wire.write(data);
  Wire.endTransmission();
}

void read_i2c_gpios(uint8_t address, bool *digital_inputs){
  uint8_t value = 0;
  
  Wire.requestFrom(address, 2);
  if (Wire.available()) {
    value = Wire.read();
  }
  Wire.endTransmission();

  for (uint8_t i = 0; i < 8; i++){
    digital_inputs[i] = ((value & (1<<i)) != 0);
  }
}

void setup() {
  Serial.begin(115200);
  
  //Config i2c gpios
  Wire.begin(SDA_PIN, SCL_PIN);

  //Config outputs
  init_pcf8574(I2C_OUTPUTS_ADDRESS, 0b11111111);

  //Config inputs
  init_pcf8574(I2C_INPUTS_ADDRESS, 0b00000000);

}

void loop() {
  bool digital_outputs[8] ={0};
  bool digital_inputs[8] ={0};

  //Read inputs
  read_i2c_gpios(I2C_INPUTS_ADDRESS, digital_inputs);

  //Copy inputs to outputs
  for(uint8_t i = 0; i < 8; i++){
    //digital_outputs[i] = digital_inputs[i];
  }

  digital_outputs[3] = true;
  digital_outputs[4] = false;

  //Write outputs
  write_i2c_gpios(I2C_OUTPUTS_ADDRESS, digital_outputs);

  
  //Monitorización
  for(uint8_t i = 0; i < 8; i++){
    Serial.print(digital_inputs[i]);
  }
  Serial.println(" ");

  delay(500);
}