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

// Global values
const float STEP = 0.001;
volatile bool StateA, StateB; // initialized at startup by reading the legs.
volatile float DISTANCE = 0; // at startup 0

volatile byte newState, oldState; // initialized at startup by reading the legs.
volatile char ABprev;
const float increment[16] = {0,-STEP,STEP,0, STEP,0,0,-STEP, -STEP,0,0,STEP, 0,STEP,-STEP,0};
volatile int incStreak = 0; // used for step increasing during several consecutive scrolls of the second encoder
volatile float incStep; // save previous step to detect when increase the streak

// Expanders: PCF8574AP(https://www.nxp.com/docs/en/data-sheet/PCF8574_PCF8574A.pdf)
// Default pins A4-SDA, A5-SCL
// Correct adresses are 0x38 - 0x3F
PCF8574 CURR_PCF;
// // --- Lamps 1, 2 ---
// // PCF8574 PCF0(0b0100011);
// PCF8574 PCF0(0x39); // bin: 0b00111001
// // PCF8574 PCF0(0xB9);
// // --- Lamps 3, 4 ---
// // PCF8574 PCF1(0b0100101);
// PCF8574 PCF1(0x3D); // bin: 0b00111101
// // PCF8574 PCF1(0xBD);
// // --- Lamps 5, 6 ---
// // PCF8574 PCF2(0b0100110);
// PCF8574 PCF2(0x3F); // bin: 0b00111111
// // PCF8574 PCF2(0xBF);
// --- Lamps 7, 8 ---
// PCF8574 PCF3(0b0100111);
PCF8574 PCF3(0x3B); // bin: 0b00111011
// PCF8574 PCF3(0xBB);


void safeDistanceIncrease(float increaseStep){
  // Do not increase or decrease DISTANCE above or below 9999.999
  // because of 8 lamp limit
  if (abs(DISTANCE + increaseStep) > 9999.999){
    DISTANCE = 9999.999 * (pow(-1, (DISTANCE < 0)));
  } else {
    DISTANCE += increaseStep;
  }
}


// Interrupt on channel A of the rotary encoder
void interruptChannelA() {
  // change the channel state (to avoid digitalRead())
  StateA = !StateA;
  // adjust the coordinate
  if (StateA != StateB){
    safeDistanceIncrease(STEP);
  } else {
    safeDistanceIncrease(-STEP);
  }
}


// Interrupt on channel B of the rotary encoder
void interruptChannelB() {
  // change the channel state (to avoid digitalRead())
  StateB = !StateB;
  // adjust the coordinate
  if (StateA == StateB){
    safeDistanceIncrease(STEP);
  } else {
    safeDistanceIncrease(-STEP);
  }
}


ISR (PCINT0_vect) { // D8 or D9 or D10 have changed
  // Detect changes on lowest 3 bits of PINB. 7 is 0b111
  newState = PINB & 7;
  // if pin D10 has changed, state 100
  if ((newState ^ oldState) & 4) {
    DISTANCE = 0;
  // if pin D8 or D9 has changed
  } else {
    // Detect changes on lowest 2 bits of PINB. 3 is 0b11
    char AB = PINB & 3;
    // if inc direction do not changes, increase streak by 1, else reset
    if (incStep == increment[AB+ABprev*4]){
      incStreak++;
    } else {
      incStreak = 0;
    }
    incStep = increment[AB+ABprev*4];
    // Changing DISTANCE with new increase step values. 
    // All limits in states and multiplying of step are random numbers and can be adjusted
    if (10 > incStreak){
      safeDistanceIncrease(incStep);
    } else if (20 > incStreak){ // 10, 20, 30 are random nums
      safeDistanceIncrease(incStep * 10);
    } else if (30 > incStreak){
      safeDistanceIncrease(incStep * 100);
    } else if (40 > incStreak){
      safeDistanceIncrease(incStep * 1000);
    } else if (50 > incStreak){
      safeDistanceIncrease(incStep * 10000);
    } else if (60 > incStreak){
      safeDistanceIncrease(incStep * 100000);
    } else {
      safeDistanceIncrease(incStep * 1000000);
    }
    ABprev = AB;
  }
  oldState = newState;

}


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

  // // Check for correct address of connected PCF
  // if (!PCF0.begin()) {
  //   Serial.println("PCF0 Chip not responding.");
  // }
  // if (!PCF0.isConnected()) {
  //   Serial.println("PCF0 could not initialize... => not connected");
  //   while (1);
  // }
  // // Write 0 to all 8 pins for correct work
  // 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);
  }
  for (uint8_t i = 0; i < 3; i++) {
    PCF3.write8(170);
    delay(200);
    PCF3.write8(85);
    delay(200);
  }

  PCF3.write(0, 1);
  delay(125);

  for (int i = 0; i < 7; i++) {
    PCF3.shiftLeft();
    delay(125);
  }

  for (int i = 0; i < 7; i++) {
    PCF3.shiftRight();
    delay(125);
  }

  for (int i = 1; i < 7; i++) {
    PCF3.write(i, 1);
    delay(125);
  }
  // PCF3.write8(0);

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


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

  Serial.println("Setup started");

  setupPcfs();

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

  // Got from here -- https://habr.com/ru/articles/340448/
  PCICR |= (1 << PCIE0);  // interrupt will be fired on any change on pins d8, d9 or d10
  PCMSK0 |= 7;
  oldState = PINB & 7;
  ABprev = PINB & 3; // get first state of d8 and d9

  // Enable interrupt A
  attachInterrupt(digitalPinToInterrupt(3), interruptChannelA, CHANGE);
  // Enable interrupt B
  attachInterrupt(digitalPinToInterrupt(2), interruptChannelB, CHANGE);
  // Initialization of global variables
  StateA = digitalRead(3);
  StateB = digitalRead(2);

  Serial.println("Setup finished");
}


void writeDecoder(int decoder_i, int p1, int p2, int p4, int p8){
  switch (decoder_i){
    // case 0:
    // case 1:
    //   PCF0.write(0 + 4 * (decoder_i % 2), p1); // 1
    //   PCF0.write(1 + 4 * (decoder_i % 2), p2); // 2
    //   PCF0.write(2 + 4 * (decoder_i % 2), p4); // 4
    //   PCF0.write(3 + 4 * (decoder_i % 2), p8); // 8
    //   break;
    // case 2:
    // case 3:
    //   PCF1.write(0 + 4 * (decoder_i % 2), p1); // 1
    //   PCF1.write(1 + 4 * (decoder_i % 2), p2); // 2
    //   PCF1.write(2 + 4 * (decoder_i % 2), p4); // 4
    //   PCF1.write(3 + 4 * (decoder_i % 2), p8); // 8
    //   break;
    // case 4:
    // case 5:
    //   PCF2.write(0 + 4 * (decoder_i % 2), p1); // 1
    //   PCF2.write(1 + 4 * (decoder_i % 2), p2); // 2
    //   PCF2.write(2 + 4 * (decoder_i % 2), p4); // 4
    //   PCF2.write(3 + 4 * (decoder_i % 2), p8); // 8
    //   break;
    case 6:
    case 7:
      PCF3.write(0 + 4 * (decoder_i % 2), p1); // 1
      PCF3.write(1 + 4 * (decoder_i % 2), p2); // 2
      PCF3.write(2 + 4 * (decoder_i % 2), p4); // 4
      PCF3.write(3 + 4 * (decoder_i % 2), p8); // 8
      break;
  }
  // if ((decoder_i == 6) || (decoder_i == 7)){
  //   PCF3.write(0 + 4 * (decoder_i % 2), p1); // 1
  //   PCF3.write(1 + 4 * (decoder_i % 2), p2); // 2
  //   PCF3.write(2 + 4 * (decoder_i % 2), p4); // 4
  //   PCF3.write(3 + 4 * (decoder_i % 2), p8); // 8
  // }
  // CURR_PCF.write(0 + 4 * (decoder_i % 2), p1); // 1
  // CURR_PCF.write(1 + 4 * (decoder_i % 2), p2); // 2
  // CURR_PCF.write(2 + 4 * (decoder_i % 2), p4); // 4
  // CURR_PCF.write(3 + 4 * (decoder_i % 2), p8); // 8
}


void loop() {
  float tempDistance = DISTANCE;

  char tempString[9];

  dtostrf(tempDistance, 9, 3, tempString);

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

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

  // PCF8574 CURR_PCF;

  // 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++) {

    if (pcf_i < 3){
      if (tempString[char_i] == '.'){
        decoder_i--;
      }

      decoder_i++;

      if (decoder_i % 2 == 0){
        pcf_i++;
      }
      // Serial.println("Its work");
      continue;
    }
    
    // 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;
    // }

    // '.' case is skipped because dot showed always on indecators
    // 1, 2, 4, 8
    switch (tempString[char_i]){
      case ' ':
      case '0':
        // writeDecoder(decoder_i, 0, 1, 0, 1);
        writeDecoder(decoder_i, 0, 1, 0, 1);
        break;
      case '-':
        isMinus = true;
        writeDecoder(decoder_i, 0, 1, 0, 1);
        break;
      case '1':
        writeDecoder(decoder_i, 1, 0, 0, 1);
        break;
      case '2':
        writeDecoder(decoder_i, 0, 1, 0, 0);
        break;
      case '3':
        writeDecoder(decoder_i, 1, 1, 0, 0);
        break;
      case '4':
        writeDecoder(decoder_i, 0, 0, 1, 0);
        break;
      case '5':
        writeDecoder(decoder_i, 1, 0, 1, 0);
        break;
      case '6':
        writeDecoder(decoder_i, 0, 1, 1, 0);
        break;
      case '7':
        writeDecoder(decoder_i, 1, 1, 1, 0);
        break;
      case '8':
        writeDecoder(decoder_i, 0, 0, 0, 1);
        break;
      case '9':
        writeDecoder(decoder_i, 1, 0, 0, 1);
        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++;
    }
  }
  // if (isMinus || tempString[0] == '-'){
  //   PCF0.write(0, LOW); // 1
  //   PCF0.write(1, HIGH); // 2
  // } else {
  //   PCF0.write(0, HIGH); // 1
  //   PCF0.write(1, LOW); // 2
  // }

  delay(60);
}