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

const float step = 0.001;
// const float step = 1000;
volatile bool StateA, StateB; //глобальные, инициализируются при запуске чтением ножек.
volatile float distance = 0; //глобальная, при запуске 0
// volatile bool countFlag = 1;
// 0x20 - 0x27
PCF8574 PCF0(0x27); // Maybe, default pins A4-SDA, A5-SCL: 
// PCF8574 PCF1(0x26);

// Прерывание по каналу 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_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);
  }

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

  // if (!PCF1.isConnected()) {
  //   Serial.println("PCF1 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.println("Setup started");
  
  Serial.begin(115200);

  setup_pcfs();

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

  Serial.println("Setup finished");
}

// String correct_string(float dist) {
//   // Create a string buffer to store the converted string
//   char tempString[9];

//   // Convert the floating-point number to a string
//   dtostrf(dist, 9, 3, tempString);

//   bool is_minus = false;
//   // < 9 - 5 because 0.000 is always not ' ' and not '-'
//   // for (int i = 0; i < strlen(tempString) - 5; i++) {
//   for (int i = 0; i < 9 - 5; i++) {
//     if (tempString[i] == ' ') {
//       tempString[i] = '0';
//     } else if (tempString[i] == '-') {
//       is_minus = true;
//       tempString[i] = '0';
//   }

//   if (is_minus) {
//     tempString[0] = '-';
//   } else {
//     tempString[0] = '+';
//   }
//   };
//   // Serial.println(tempString);

//   return tempString;
// }


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);

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

  // PCF0.toggle(0);
  // PCF0.toggle(1);
  // delay(1000);
  // initialize index of pcf(lamp)
  int i_pcf = 0;
  PCF8574 curr_pcf;
  bool is_minus = false;
  // iterate through all chars of distanse
  // for (int i = 0; i < tempString.length(); i++) {
  // 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 i = 1; 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;
    }

    // Serial.print("pcf index before showing number: ");
    // Serial.println(i_pcf);
    
    // 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 + 4 * i - 16 * i_pcf, LOW); // 1
        PCF0.write(1 + 4 * i - 16 * i_pcf, HIGH); // 2
        PCF0.write(2 + 4 * i - 16 * i_pcf, LOW);  // 4
        PCF0.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);
  }
  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(60);
}

/////////////////////////////////////

// volatile float distance = 0;
// volatile char ABprev = 0;
// const float step = 0.001;
// const float increment[16] = {0,-step,step,0, step,0,0,-step, -step,0,0,step, 0,step,-step,0};

// ISR (PCINT0_vect) { // D8 or D9 has changed
//   // Внимание, ардуиновский digitalRead() противопоказан, 
//   // когда нам критична скорость работы
//   char AB = PINB & 3;
//   distance += increment[AB+ABprev*4];
//   ABprev = AB;
// }

// void setup() {
//   pinMode(8, INPUT);  // A
//   pinMode(9, INPUT);  // B
//   PCICR |= (1 << PCIE0);  // interrupt will be fired on any change on pins d8 and d9
//   PCMSK0 |= 3;
//   ABprev = PINB & 3;
//   Serial.begin(115200);
// }

// void loop() {
//   Serial.println(distance, 3);
//   delay(100);
// }

//////////////////////////

// volatile bool StateA, StateB; //глобальные, инициализируются при запуске чтением ножек.
// bool prevStateA, prevStateB; //глобальные, инициализируются при запуске чтением ножек.
// volatile long Coord; //глобальная, при запуске 0

// // // Прерывание по каналу A датчика угла поворота
// // void interruptChannelA() {
// //   // меняем состояние канала (чтобы не делать digitalRead())
// //   StateA = !StateA; 
// //   // корректируем координату
// //   Coord += (StateA != StateB) ? 1 : -1;
// // }

// // // Прерывание по каналу B датчика угла поворота
// // void interruptChannelB() {
// //   // меняем состояние канала (чтобы не делать digitalRead())
// //   StateB = !StateB;
// //   // корректируем координату
// //   Coord += (StateA == StateB) ? 1 : -1;
// // }

// void setup() {
//   pinMode(8, INPUT);  // A
//   pinMode(9, INPUT);  // B
//   // PCICR |= (1 << PCIE0);  // interrupt will be fired on any change on pins d8 and d9
//   // PCMSK0 |= 3;
//   // ABprev = PINB & 3;
//   Serial.begin(115200);

//   // Инициализация глобальных переменных
//   prevStateA = digitalRead(8);
//   prevStateB = digitalRead(9);
//   // Coord = 0;
// }

// void loop() {
//   // Читаем состояние каналов A и B датчика угла поворота
//   StateA = digitalRead(8);
//   StateB = digitalRead(9);

//   // // State was changed
//   // prevStateA = !prevStateA;
//   // // State was changed
//   // prevStateB = !prevStateB;

//   if ((prevStateA != StateA) && (prevStateB == StateB)) {
//     if (StateA != StateB) {
//       Coord++;
//     } else {
//       Coord--;
//     }
//   } else if ((prevStateA == StateA) && (prevStateB != StateB)) {
//     if (StateA == StateB) {
//       Coord++;
//     } else {
//       Coord--;
//     }
//   };

//   // if (prevStateB != StateB) {
//   //   if (StateA == StateB) {
//   //     Coord++;
//   //   } else {
//   //     Coord--;
//   //   }
//   // }
//   // if ((prevStateA != StateA) && (prevStateB != StateB)) {
//   //   Coord = Coord + 0;
//   // }; 
//   // if ((prevStateA != StateA) && (prevStateB == StateB)) {
//   //   // Coord increase?
//   //   Coord = Coord + 1
//   // } else if ((prevStateA == StateA) && (prevStateB != StateB)) {
//   //   // Coord decrease?
//   //   Coord = Coord + 2
//   // // } else if (prevStateA == StateA) & (prevStateB != StateB) {}
//   // } else {
//   //   Coord = Coord + 3
//   // }
//   prevStateA = StateA;
//   prevStateB = StateB;

//   // Serial.println(StateA);
//   // Serial.println(StateB);

//   Serial.println(Coord);
//   delay(100);
// }


//////////////////////////