#include <Servo.h>

unsigned long currentTime;
unsigned long loopTime;

unsigned long tCurr;
unsigned long tLast = 0;
int t_pwm = 30;

const int pin_A = 2; // Encoder CLK
const int pin_B = 3; // Encoder DT
const int LED_PIN = 5;
unsigned char encoder_A;
unsigned char encoder_B;
unsigned char encoder_A_prev = 0;
int servoPos = 90;
double LED_kPWM = 0.0;
double LED_kPWM_STEP = 0.05;

Servo servo;

void setup() {
  Serial.begin(9600);
  pinMode(pin_A, INPUT);
  pinMode(pin_B, INPUT);
  pinMode(LED_PIN, OUTPUT);

  servo.attach(6);

  currentTime = millis();
  loopTime = currentTime;
}

void loop() {
  currentTime = millis();
  if (currentTime >= (loopTime + 5)) { // 100мс
    encoder_A = digitalRead(pin_A); // зчитуємо стан виходу А енкодера
    encoder_B = digitalRead(pin_B); // зчитуємо стан виходу B енкодера
    if ((!encoder_A) && (encoder_A_prev)) { // якщо стан змінився з «1» до «0»
      if (encoder_B) {
        // вихід В в стані «1», це означає обертання за годинниковою стрілкою,
        // тоді виконувати певні дії
        //Serial.println("Rotated clockwise ⏩");
        
        if ((servoPos >= 0) && (servoPos < 180) && (LED_kPWM <= 0)) {
          servo.write(servoPos += 2);
          LED_kPWM = 0;
          digitalWrite(LED_PIN, LOW);
        }
        else if ((servoPos <= 0) && (LED_kPWM >= 0)) {
          LED_kPWM -= LED_kPWM_STEP;
          generatePWM(LED_kPWM);
        }
        else if ((servoPos >= 180) && (LED_kPWM < 1)) {
          LED_kPWM += LED_kPWM_STEP;
          generatePWM(LED_kPWM);
        }
      }
      else {
        // вихід В у стані «0», отже це обертання проти годинникової стрілки
        // тоді виконувати певні дії
        //Serial.println("Rotated counterclockwise ⏪");

        if ((servoPos > 0) && (servoPos <= 180) && (LED_kPWM <= 0)) {
          servo.write(servoPos -= 2);
          LED_kPWM = 0;
          digitalWrite(LED_PIN, LOW);
        }
        else if ((servoPos <= 0) && (LED_kPWM < 1)) {
          LED_kPWM += LED_kPWM_STEP;
          generatePWM(LED_kPWM);
        }
        else if ((servoPos >= 180) && (LED_kPWM > 0)) {
          LED_kPWM -= LED_kPWM_STEP;
          generatePWM(LED_kPWM);
        }
      }
      
      // Serial.print("Servo pos: ");
      // Serial.print(servoPos);
      // Serial.print("\n");
      // Serial.print("LED kPWM: ");
      // Serial.print(LED_kPWM);
      // Serial.print("\n");
    }
    encoder_A_prev = encoder_A;
    loopTime = currentTime;
  }
}

void generatePWM(float kPWM) {
  tCurr = millis();
  
  if ( tCurr - tLast >= t_pwm ) {
    PORTD |= (1 << LED_PIN);  
    tLast = tCurr;
  }
  else 
    if ( tCurr - tLast >= kPWM * t_pwm ) {
      PORTD &= ~(1 << LED_PIN);
  }  
}