#include <Wire.h>
#include <PCF8574.h>

PCF8574 PCF0(0x27); // Maybe, default pins A4-SDA, A5-SCL: 
// PCF8574 PCF0; // Maybe, default pins A4-SDA, A5-SCL: 
// PCF8574 PCF1(0x26);

void setup_pcfs() {
  Serial.println("PCFs setupes started");

  if (!PCF0.begin()) {
    Serial.println("PCF0 Chip not responding.");
  } else {
    Serial.println("PCF0.begin() works");
  }



  if (!PCF0.isConnected()) {
    Serial.println("PCF0 could not initialize... => not connected");
    while (1);
  }

  // for (int i = 0; i < 16; i++) {
  //   PCF0.write(i, LOW);
  // }

  Serial.println("PCFs setupes were finished.");
}


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

  Serial.println("Setup started");

  // 1. commented pinModes, connected A4, A5 -- error
  // 2. uncommented pinModes 4, 5, after setup_pcfs, connected A4, A5 -- 
  // 3. uncommented pinModes 4, 5, before setup_pcfs, connected A4, A5 -- 
  // 4. pinMode 4, 5

  setup_pcfs();

  pinMode(A4, OUTPUT); // SDA
  pinMode(A5, OUTPUT); // SCL

  Serial.println("Setup finished");
}


void loop() {
  char tempString[9] = "1111";

  Serial.print("number to show:");
  Serial.println(tempString);

  // initialize index of pcf(lamp)
  int i_pcf = 0;
  PCF8574 curr_pcf;
  bool is_minus = false;
  // Start with first character to omit sign of number and show it last
  // Last character of char tempString[9]; dtostrf(distance, 9, 3, tempString); is '&'. I do not know why
  for (int i = 0; i < strlen(tempString) - 1; i++) {
    Serial.print("char to show:");
    Serial.println(tempString[i]);

    switch (i_pcf){
      case 0:
        curr_pcf = PCF0;
        break;
      // case 1:
      //   curr_pcf = PCF1;
      //   break;
    }
    
    // IMPORTANT!!! Write LOW to all 8 pins to show new number
    // I thinl we can ignore turning off lamp after '.' symbol because 
    // next number will be showed faster then it will be noticed
    // curr_pcf.write8(LOW);

    // '.' case is skipped because dot showed always on indecators
    switch (tempString[i]){
      case ' ':
      case '0':
        // Index is based on number of decoder inputs, number of expander inputs
        // and index of expanders
        Serial.println("There is 0");
        // curr_pcf.write(0 + 4 * i - 16 * i_pcf, LOW); // 1
        // curr_pcf.write(1 + 4 * i - 16 * i_pcf, HIGH); // 2
        // curr_pcf.write(2 + 4 * i - 16 * i_pcf, LOW);  // 4
        // curr_pcf.write(3 + 4 * i - 16 * i_pcf, HIGH);  // 8
        PCF0.write(0, LOW); // 1
        PCF0.write(1, HIGH); // 2
        PCF0.write(2, LOW);  // 4
        PCF0.write(3, HIGH);  // 8
        // i_pcf++;
        break;
      case '-':
        is_minus = true;
        Serial.println("There is -, but showed 0");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, LOW); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, HIGH); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, LOW); // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, HIGH); // 8
        // i_pcf++;
        break;
      case '1':
        Serial.println("There is 1");
        PCF0.write(0, HIGH); // 1
        PCF0.write(1, LOW); // 2
        PCF0.write(2, LOW); // 4
        PCF0.write(3, LOW); // 8
        // i_pcf++;
        break;
      case '2':
        Serial.println("There is 2");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, LOW); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, HIGH); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, LOW);  // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, LOW);  // 8
        // i_pcf++;
        break;
      case '3':
        Serial.println("There is 3");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, HIGH); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, HIGH); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, LOW);  // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, LOW);  // 8
        // i_pcf++;
        break;
      case '4':
        Serial.println("There is 4");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, LOW); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, LOW); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, HIGH); // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, LOW); // 8
        // i_pcf++;
        break;
      case '5':
        Serial.println("There is 5");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, HIGH); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, LOW); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, HIGH);  // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, LOW);  // 8
        // i_pcf++;
        break;
      case '6':
        Serial.println("There is 6");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, LOW); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, HIGH); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, HIGH);  // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, LOW);  // 8
        // i_pcf++;
        break;
      case '7':
        Serial.println("There is 7");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, HIGH); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, HIGH); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, HIGH);  // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, LOW);  // 8
        // i_pcf++;
        break;
      case '8':
        Serial.println("There is 8");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, LOW); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, LOW); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, LOW);  // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, HIGH);  // 8
        // i_pcf++;
        break;
      case '9':
        Serial.println("There is 9");
        curr_pcf.write(0 + 4 * i - 16 * i_pcf, HIGH); // 1
        curr_pcf.write(1 + 4 * i - 16 * i_pcf, LOW); // 2
        curr_pcf.write(2 + 4 * i - 16 * i_pcf, LOW);  // 4
        curr_pcf.write(3 + 4 * i - 16 * i_pcf, HIGH);  // 8
        // i_pcf++;
        break;
    }
    // if (i + 1 == 4){
    //   i_pcf++;
    // }
    // Serial.print("pcf index after showing number: ");
    // Serial.println(i_pcf);
  }
  if (is_minus || tempString[0] == '-'){
    Serial.println("There is -");
    // What settings for + or -?
    PCF0.write(0, HIGH); // 1
    PCF0.write(1, HIGH); // 2
    PCF0.write(2, HIGH); // 4
    PCF0.write(3, HIGH); // 8
  } else {
    Serial.println("There is +");
    // What settings for + or -?
    PCF0.write(0, LOW); // 1
    PCF0.write(1, LOW); // 2
    PCF0.write(2, LOW); // 4
    PCF0.write(3, LOW); // 8
  }

  delay(100);
}