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

// Global values
const int8_t STEP = 1;
volatile int32_t DISTANCE = 0; // at startup 0

volatile bool StateA, StateB; // initialized at startup by reading the legs.

volatile byte newState, oldState; // initialized at startup by reading the legs.
// Adding interaption to PIND need to increase to int16_t(?)
volatile int8_t AB, ABprev;
const int8_t increment[16] = {0,-STEP,STEP,0, STEP,0,0,-STEP, -STEP,0,0,STEP, 0,STEP,-STEP,0};
// int8_t is enought for incStreak. If it turns negative it will add only 1
volatile int8_t incStreak = 0; // used for step increasing during several consecutive scrolls of the second encoder
volatile int32_t incStep; // save previous step to detect when increase the streak

volatile float tempDistance;
volatile int8_t decoder_i;
volatile bool isMinus;
char tempString[9];

// Expanders: PCF8574AP(https://www.nxp.com/docs/en/data-sheet/PCF8574_PCF8574A.pdf)
// Default pins A4-SDA, A5-SCL
// Correct adresses are 0x38 - 0x3F
// --- 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(int32_t 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;
  // }
  DISTANCE = max(min(DISTANCE + increaseStep, 9999999), -9999999);
}


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


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

ISR (PCINT2_vect) { // D4 or D5 or D6 have changed
  // d4 011>1<0000
  // d5 01>1<10000
  // d6 0>1<110000
  // pins d4, d5, d6, detect changes on 0>000<0000 bits of PIND. 112 is 0b1110000
  newState = PIND & 112;
  // pin d6, detect changes on 000>0<0000 bits of PIND. 64 is 01000000
  // Reset DISTANCE and incStreak
  if ((newState ^ oldState) & 64) {
    DISTANCE = 0;
    incStreak = 0;
  // if pin D4 or D5 has changed. 00>00<0000. 48 is 00110000
  } else {
    AB = PIND & 48;
    // Serial.print("AB:");
    // Serial.println(AB);
    // Serial.print("ABprev:");
    // Serial.println(ABprev);
    // if inc direction do not changes, increase streak by 1, else reset
    if (incStep == increment[(AB+ABprev*4)/16]){
      incStreak++;
    } else {
      incStreak = 0;
    }
    incStep = increment[(AB+ABprev*4)/16];
    // 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);
  }
  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 << PCIE2);  // interrupt will be fired on any change on pins d8, d9 or d10
  // https://docs.arduino.cc/hacking/software/PortManipulation
  // PCMSK0 |= 7;
  // PORTD: 0 to 7, pins d3, d4, d5, bin: 01110000, dec: 112
  // 00000000
  // 01110000
  PCMSK2 |= 112;
  oldState = PIND & 112;
  // get first state of d4 and d5
  // pins d3, d4, bin: 01100000, dec: 96
  // d4 00000>0<00
  // d5 0000>0<000
  // d6 000>0<0000
  ABprev = PIND & 48; // bin: 1100

  // 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(int8_t decoder_i, bool p1, bool p2, bool p4, bool 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;
  }
}


void loop() {
  tempDistance = DISTANCE;

  // char tempString[9];

  // dtostrf(tempDistance, 9, 3, tempString);
  dtostrf(tempDistance, 8, 0, tempString);

  Serial.print("number:");
  // Serial.println(tempDistance / 1000.0, 3);
  Serial.println(tempString);

  decoder_i = 1;
  isMinus = false;

  // iterate through all chars of distanse
  // Start with first character to omit sign of number and show it last
  for (int8_t char_i = 1; char_i < strlen(tempString); char_i++) {
    // 1, 2, 4, 8
    switch (tempString[char_i]){
      case ' ':
      case '0':
        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 (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);
}