// --------------------------------------
// i2c basic com with PCA9555
//
// Le 20 Juin 2018 by Mej
// 
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////
// Wire and functions

#include <Wire.h>

void set_I2C_register(byte ADDRESS, byte REGISTER, byte VALUE)
{
  Wire.beginTransmission(ADDRESS);
  Wire.write(REGISTER);
  Wire.write(VALUE);
  Wire.endTransmission();
}

byte get_I2C_register(byte ADDRESS, byte REGISTER)
{
  Wire.beginTransmission(ADDRESS);
  Wire.write(REGISTER);
  Wire.endTransmission();
  Wire.requestFrom(ADDRESS, 1); //read 1 byte
  byte x = Wire.read();
  return x;
}

//timing monitoring variables
unsigned long t_0 = millis(); // gives time since startup in ms. Overflow after 50 days.
unsigned long t = millis();
unsigned long loop_cnt = 0; //loop count
const int D = 100; //delay between loops in ms

/////////////////////////////////////////////////////////////////////////////////////////////////////////
//## PCA9555 (I²C GPIO expender) address
//Address
const byte PCA9555 =  B0100001; //  0x21
byte data, data0, data1;
 
void setup()
{
 
  Serial.begin(115200);
  delay(1000);
  //while (!Serial);             // wait for serial monitor to be monitoring ^^

  //Set and scan I²C bus
  Wire.begin();
  Serial.println("\nI2C Scanning...");
  byte error, address;
  int nDevices =0;
  for(address = 16; address < 100; address++ )
  {
    // The i2c_scanner uses the return value of
    // the Write.endTransmisstion to see if
    // a device did acknowledge to the address.
    // Copy/Pasted/Tweeked from https://playground.arduino.cc/Main/I2cScanner
    delay(1);
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
 
    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16)
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");
 
      nDevices++;
    }
    else if (error==4)
    {
      Serial.print("Unknown error at address 0x");
      if (address<16)
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");


  //Set config of PCA9555
  data0 = B00000000; //port 0 Input=1/Output=0 (LED on MSB)
  data1 = B00000000; //port 0 Input=1/Output=0 (LED on LSB)
  Wire.beginTransmission(PCA9555);
  Wire.write(0x06); //Config port 0
  Wire.write(data0);
  Wire.write(data1);
  Wire.endTransmission();
  
}
 
 
void loop()
{
 
  loop_cnt = loop_cnt+1;
  t_0 = millis();


  /// Writte ports (inputs)

  //LED on MSB for port0
  if(loop_cnt%2==0){ 
    data0 = B10000000;}
  if(loop_cnt%2==1){
    data0 = B00000000;}

  //LED on LSB for port1
  if(loop_cnt%2==0){
    data1 = B00000001;}
  if(loop_cnt%2==1){
    data1 = B00000000;}

  //Writte PCA
  if(loop_cnt%2==0){
    data = B11111111;}
  if(loop_cnt%2==1){
    data = B00000000;}
  Wire.beginTransmission(PCA9555);
  Wire.write(0x02); //Output port 0
  Wire.write(data);
  Wire.write(data);
  Wire.endTransmission();
  
//  /// Read ports (inputs)
//  data0 = get_I2C_register(PCA9555, 0x00);
//  data1 = get_I2C_register(PCA9555, 0x01);
 
  

  //////////////////////////////////////////////////// print 95555 state
  Serial.print("PCA9555 0x");
  Serial.print(PCA9555,HEX);
  Serial.print("  Port0= B");
  Serial.print(data0,BIN);
  Serial.print("  ");
  Serial.print(" Port1= B");
  Serial.print(data1,BIN);
  Serial.print("  ");


  t = millis();
  Serial.print("t[s]= ");
  Serial.print(t/1000.0, 3);
  Serial.print("\t t_loop[ms]= ");
  Serial.println(t-t_0, DEC);
  
  delay(D);

}