#include <PCF8574.h>

PCF8574 pcf20(0x27);
PCF8574 pcf21(0x28);

//const float step = 0.001;
 const float step = 1;
volatile bool StateA, StateB; //глобальные, инициализируются при запуске чтением ножек.
volatile float distance = 0; //глобальная, при запуске 0

// Прерывание по каналу A датчика угла поворота
void interruptChannelA() {
  // if (countFlag == 1){
    // меняем состояние канала (чтобы не делать digitalRead())
    StateA = !StateA;
    // корректируем координату
    distance += (StateA != StateB) ? step : -step;
    // countFlag = 0;
  // }
//  //  Why interrupt is detached -- https://habr.com/ru/articles/340448/#comment_10480290 
//  detachInterrupt(1);
//  attachInterrupt(0, interruptChannelB, CHANGE);
}

// Прерывание по каналу B датчика угла поворота
void interruptChannelB() {
 // меняем состояние канала (чтобы не делать digitalRead())
 StateB = !StateB;
 // корректируем координату
 distance += (StateA == StateB) ? step : -step;
// //  Why interrupt is detaching -- https://habr.com/ru/articles/340448/#comment_10480290 
//  detachInterrupt(0);
//  attachInterrupt(1, interruptChannelA, CHANGE);
}
void setup() {
  Serial.begin(115200);
  delay(500);
  pinMode(3, INPUT); // A
  pinMode(2, INPUT); // B
  attachInterrupt(digitalPinToInterrupt(3), interruptChannelA, CHANGE);
  attachInterrupt(digitalPinToInterrupt(2), interruptChannelB, CHANGE);
  StateA = digitalRead(3);
  StateB = digitalRead(2);
//  Serial.print("PCF8574_LIB_VERSION:\t");
//  Serial.println(PCF8574_LIB_VERSION);

  if (!pcf20.begin()) {
    Serial.println("Chip not responding.");
  }

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

  for (uint8_t i = 0; i < 3; i++) {
    pcf20.write8(255);
    delay(100);
    pcf20.write8(0);
    delay(200);
  }
  delay(800);

  pcf20.write(0, 1);
  delay(125);
}

void loop() {
  float temp_distance = distance;
  //char myString[8];
 // String tempString = correct_string(distance);
 // Serial.println(correct_string(distance));
  //char tempString[9];

  //dtostrf(distance, 9, 3, tempString);

  char tempString[9] = "43211.123";

  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 = pcf20;
        break;
      case 1:
       curr_pcf = pcf21;
       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
        // 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");
        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, 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);
  }
}