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

const float STEP = 0.001;
// const float STEP = 123.456;
volatile bool StateA, StateB; //глобальные, инициализируются при запуске чтением ножек.
volatile float DISTANCE = 0; //глобальная, при запуске 0
// 0x20 - 0x27. Correct adress is 0x39
// 0x39 -- 00111001
PCF8574 PCF3(0b0100111); // Maybe, default pins A4-SDA, A5-SCL: 
PCF8574 PCF2(0b0100110);
PCF8574 PCF1(0b0100101);
PCF8574 PCF0(0b0100011);
PCF8574 curr_pcf;

// Прерывание по каналу 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 resetDistance(){
  DISTANCE = 0;
}

void setup_pcfs() {
  // https://www.youtube.com/watch?v=jgesIr0MFoY
  // https://www.youtube.com/watch?v=YGdIB4pdKtg
  // https://wokwi.com/projects/372234522949232641
  // PCF8574

  Serial.println("PCFs setupes started");

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

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

  PCF0.write8(0);

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

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

  PCF1.write8(0);

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

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

  PCF2.write8(0);

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

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

  PCF3.write8(0);

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


ISR (PCINT0_vect) { // D8 or D9 has changed
  DISTANCE = 0;
}


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

  Serial.println("Setup started");

  setup_pcfs();

  // Serial.begin(9600);
  pinMode(3, INPUT); // A
  pinMode(2, INPUT); // B
  pinMode(4, INPUT); // B

  // Got from here -- https://habr.com/ru/articles/340448/
  PCICR |= (1 << PCIE0);  // interrupt will be fired on any change on pins d8 and d9
  PCMSK0 |= 3;

  // pinMode(A4, OUTPUT); // SDA
  // pinMode(A5, OUTPUT); // SCL
  // pinMode(A2, OUTPUT); // SDA
  // pinMode(A3, OUTPUT); // SCL
  // Включаем прерывание A
  attachInterrupt(digitalPinToInterrupt(3), interruptChannelA, CHANGE);
  // Включаем прерывание B
  attachInterrupt(digitalPinToInterrupt(2), interruptChannelB, CHANGE);
  // Инициализация глобальных переменных
  StateA = digitalRead(3);
  StateB = digitalRead(2);

  Serial.println("Setup finished");
}

void loop() {
  float temp_distance = DISTANCE;
  // char myString[8];

  // String tempString = correct_string(distance);
  // Serial.println(correct_string(distance));

  char tempString[9];

  dtostrf(temp_distance, 9, 3, tempString);

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

  int pcf_i = 0;
  int decoder_i = 1;
  bool is_minus = false;

  // iterate through all chars of distanse
  // 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 why
  for (int char_i = 1; char_i < strlen(tempString); char_i++) {
    // Serial.print("char to show:");
    // Serial.print(tempString[char_i]);
    // Serial.print(" ");

    switch (pcf_i){
      case 0:
        curr_pcf = PCF0;
        break;
      case 1:
        curr_pcf = PCF1;
        break;
      case 2:
        curr_pcf = PCF2;
        break;
      case 3:
        curr_pcf = PCF3;
        break;
    }
    // if (pcf_i == 4){
    //   break;
    // }

    // Serial.print("pcf index before showing number: ");
    // Serial.println(i_pcf);
    // Serial.println(decoder_i % 2);
    // Serial.print(0 + 4 * (decoder_i % 2));
    
    // 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(0);

    // '.' case is skipped because dot showed always on indecators
    switch (tempString[char_i]){
      case ' ':
      case '0':
        // Index is based on number of decoder inputs, number of expander inputs
        // and index of expanders
        // Serial.print("There is 0 ");
        // Serial.print(0 + 4 * (decoder_i % 2));
        // Serial.println(1);
        // Serial.print(2 + 4 * (decoder_i % 2));
        // Serial.println(0);
        // Serial.print(3 + 4 * (decoder_i % 2));
        // Serial.println(1);
        curr_pcf.write(0 + 4 * (decoder_i % 2), 0); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 1); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 0);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 1);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 0, 1, 0, 1);
        
        // i_pcf++;
        break;
      case '-':
        is_minus = true;
        // Serial.println("There is -, but showed 0");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 0); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 1); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 0);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 1);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 0, 1, 0, 1);
        // i_pcf++;
        break;
      case '1':
        // Serial.println("There is 1");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 1); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 0); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 0);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 0);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 1, 0, 0, 0);
        // i_pcf++;
        break;
      case '2':
        // Serial.println("There is 2");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 0); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 1); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 0);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 0);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 0, 1, 0, 0);
        // i_pcf++;
        break;
      case '3':
        // Serial.println("There is 3");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 1); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 1); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 0);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 0);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 1, 1, 0, 0);
        // i_pcf++;
        break;
      case '4':
        // Serial.println("There is 4");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 0); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 0); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 1);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 0);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 0, 0, 1, 0);
        // i_pcf++;
        break;
      case '5':
        // Serial.println("There is 5");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 1); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 0); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 1);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 0);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 1, 0, 1, 0);
        // i_pcf++;
        break;
      case '6':
        // Serial.println("There is 6");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 0); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 1); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 1);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 0);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 0, 1, 1, 0);
        // i_pcf++;
        break;
      case '7':
        // Serial.println("There is 7");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 1); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 1); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 1);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 0);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 1, 1, 1, 0);
        // i_pcf++;
        break;
      case '8':
        // Serial.println("There is 8");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 0); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 0); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 0);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 1);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 0, 0, 0, 1);
        // i_pcf++;
        break;
      case '9':
        // Serial.println("There is 9");
        curr_pcf.write(0 + 4 * (decoder_i % 2), 1); // 1
        curr_pcf.write(1 + 4 * (decoder_i % 2), 0); // 2
        curr_pcf.write(2 + 4 * (decoder_i % 2), 0);  // 4
        curr_pcf.write(3 + 4 * (decoder_i % 2), 1);  // 8
        // write_pcf_vals(pcf_i, decoder_i, 1, 0, 0, 1);
        // i_pcf++;
        break;
      case '.':
        // Do not increase decoder index, because dot is skipped and always showed
        decoder_i--;
        // break;
    }

    decoder_i++;

    if (decoder_i % 2 == 0){
      pcf_i++;
    }
    // Serial.println(PCF0.valueOut());
    // Serial.println(PCF1.valueOut());
    // Serial.print("pcf index after showing number: ");
    // Serial.println();
  }
  if (is_minus || tempString[0] == '-'){
    // Serial.println("There is -");
    // What settings for + or -?
    PCF0.write(0, LOW); // 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, HIGH); // 1
    PCF0.write(1, LOW); // 2
    // PCF0.write(2, LOW); // 4
    // PCF0.write(3, LOW); // 8
  }

  delay(60);
}