#include "Arduino.h"
#include "PCF8574.h"
#include <ModbusIP_ESP8266.h>
#include <WiFi.h>

// Set i2c address
//PCF8574 pcf8574_1(0x24,4,5);
//PCF8574 pcf8574_2(0x25,4,5);

// Set i2c address for output
PCF8574 pcf8574_1(0x24,4,5);  // channel 1-8     adress:100100
PCF8574 pcf8574_2(0x25,4,5);  //channel 9-16    adress:100101

// Set i2c address for input
PCF8574 pcf8574_3(0x22,4,5);  // channel 1-8     adress: 100010
PCF8574 pcf8574_4(0x21,4,5);  //channel 9-16    adress: 100001

const int COIL0 = 100; //GPIO0 
const int COIL1 = 101; //GPIO2 DO
const int COIL2 = 102; //GPIO4 DO
const int COIL3 = 103; //GPIO5 DO
const int COIL4 = 104; //GPIO12 DO
const int COIL5 = 105; //GPIO13 DO
const int COIL6 = 106; //GPIO14 DO
const int COIL7 = 107; //GPIO15 DO
const int COIL8 = 108; //GPIO16 DO
const int COIL9 = 109; 
const int COIL10 = 110; //GPIO17 DI
const int COIL11 = 111; //GPIO18 DI
const int COIL12 = 112; //GPIO19 DI
const int COIL13 = 113; //GPIO21 DI
const int COIL14 = 114; //GPIO22 DI
const int COIL15 = 115; //GPIO23 DI
const int COIL16 = 116; //GPIO27 DI
const int COIL17 = 117; //GPIO32 DI
const int COIL18 = 118; //GPIO33 DI
const int COIL19 = 119;
const int COIL20 = 120;

//ModbusIP object
ModbusIP mb;


void setup()
{
  Serial.begin(115200);

  WiFi.begin(".......", ".........");
  WiFi.config("10.41.3.150");
  while (WiFi.status() != WL_CONNECTED) {
  delay(500);
  Serial.print(".");
  }
 
  Serial.println("");
  Serial.println("WiFi connected");  
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());

  mb.server();


  pcf8574_1.pinMode(P0, OUTPUT);
  pcf8574_1.pinMode(P1, OUTPUT);
  pcf8574_1.pinMode(P2, OUTPUT);
  pcf8574_1.pinMode(P3, OUTPUT);
  pcf8574_1.pinMode(P4, OUTPUT);
  pcf8574_1.pinMode(P5, OUTPUT);
  pcf8574_1.pinMode(P6, OUTPUT);
  pcf8574_1.pinMode(P7, OUTPUT);

  pcf8574_2.pinMode(P0, OUTPUT);
  pcf8574_2.pinMode(P1, OUTPUT);
  pcf8574_2.pinMode(P2, OUTPUT);
  pcf8574_2.pinMode(P3, OUTPUT);
  pcf8574_2.pinMode(P4, OUTPUT);
  pcf8574_2.pinMode(P5, OUTPUT);
  pcf8574_2.pinMode(P6, OUTPUT);
  pcf8574_2.pinMode(P7, OUTPUT);

  pcf8574_3.pinMode(P0, INPUT);
  pcf8574_3.pinMode(P1, INPUT);
  pcf8574_3.pinMode(P2, INPUT);
  pcf8574_3.pinMode(P3, INPUT);
  pcf8574_3.pinMode(P4, INPUT);
  pcf8574_3.pinMode(P5, INPUT);
  pcf8574_3.pinMode(P6, INPUT);
  pcf8574_3.pinMode(P7, INPUT);

  pcf8574_4.pinMode(P0, INPUT);
  pcf8574_4.pinMode(P1, INPUT);
  pcf8574_4.pinMode(P2, INPUT);
  pcf8574_4.pinMode(P3, INPUT);
  pcf8574_4.pinMode(P4, INPUT);
  pcf8574_4.pinMode(P5, INPUT);
  pcf8574_4.pinMode(P6, INPUT);
  pcf8574_4.pinMode(P7, INPUT);
  mb.addCoil(COIL0);
  mb.addCoil(COIL1);
  mb.addCoil(COIL2);
  mb.addCoil(COIL3);
  mb.addCoil(COIL4);
  mb.addCoil(COIL5);
  mb.addCoil(COIL6);
  mb.addCoil(COIL7);
  mb.addCoil(COIL8);
  mb.addCoil(COIL9);
  mb.addCoil(COIL10);
  mb.addCoil(COIL11);
  mb.addCoil(COIL12);
  mb.addCoil(COIL13);
  mb.addCoil(COIL14);
  mb.addCoil(COIL15);
  mb.addCoil(COIL16);
  mb.addCoil(COIL17);
  mb.addCoil(COIL18);
  mb.addCoil(COIL19);
  mb.addCoil(COIL20);

 Serial.print("Init pcf8574...");
    if (pcf8574_1.begin()){
        Serial.println("pcf8574_1_OK");
    }else{
        Serial.println("pcf8574_1_KO");
    }

  Serial.print("Init pcf8574...");
  if (pcf8574_2.begin()){
    Serial.println("pcf8574_2_OK");
  }else{
    Serial.println("pcf8574_2_KO");
  }
   Serial.print("Init pcf8574...");
    if (pcf8574_3.begin()){
        Serial.println("pcf8574_3_OK");
    }else{
        Serial.println("pcf8574_3_KO");
    }

  Serial.print("Init pcf8574...");
  if (pcf8574_4.begin()){
    Serial.println("pcf8574_4_OK");
  }else{
    Serial.println("pcf8574_4_KO");
  }
  

}

void loop()
{
    //Call once inside loop() - all magic here
  mb.task();
  pcf8574_1.digitalWrite(P0, mb.Coil(COIL0));
  pcf8574_1.digitalWrite(P1, mb.Coil(COIL1));
  pcf8574_1.digitalWrite(P2, mb.Coil(COIL2));
  pcf8574_1.digitalWrite(P3, mb.Coil(COIL3));
  pcf8574_1.digitalWrite(P4, mb.Coil(COIL4));
  pcf8574_1.digitalWrite(P5, mb.Coil(COIL5));
  pcf8574_1.digitalWrite(P6, mb.Coil(COIL6));
  pcf8574_1.digitalWrite(P7, mb.Coil(COIL7));

  //mb.Coil(COIL10, pcf8574_3.digitalRead(P0));
  //mb.Coil(COIL11, pcf8574_3.digitalRead(P1));
  //mb.Coil(COIL12, pcf8574_3.digitalRead(P2));
  //mb.Coil(COIL13, pcf8574_3.digitalRead(P3));
  //mb.Coil(COIL14, pcf8574_3.digitalRead(P4));
  //mb.Coil(COIL15, pcf8574_3.digitalRead(P5));
  //mb.Coil(COIL16, pcf8574_3.digitalRead(P6));
  //mb.Coil(COIL17, pcf8574_3.digitalRead(P7));
uint8_t val1 = pcf8574_3.digitalRead(P0);
uint8_t val2 = pcf8574_3.digitalRead(P1);
uint8_t val3 = pcf8574_3.digitalRead(P2);
uint8_t val4 = pcf8574_3.digitalRead(P3);
uint8_t val5 = pcf8574_3.digitalRead(P4);
uint8_t val6 = pcf8574_3.digitalRead(P5);
uint8_t val7 = pcf8574_3.digitalRead(P6);
uint8_t val8 = pcf8574_3.digitalRead(P7);
  mb.Coil(COIL10, val1);
  mb.Coil(COIL11, val2);
  mb.Coil(COIL12, val3);
  mb.Coil(COIL13, val4);
  mb.Coil(COIL14, val5);
  mb.Coil(COIL15, val6);
  mb.Coil(COIL16, val7);
  mb.Coil(COIL17, val8);

}