#include <cstdint>
#include <binary.h>
#include <esp32-hal-gpio.h>
#include <Arduino.h>
#include "enc.h"

const int PinA[ENCODER_COUNT]={14,12,26,33};              // контат AB
const int PinC[ENCODER_COUNT]={27,13,25,32};               // контат BC

uint8_t pinA[ENCODER_COUNT] = {0,0,0,0};             // состояние контата AB
uint8_t pinC[ENCODER_COUNT] = {0,0,0,0};             // состояние контата BC

uint8_t blockA[ENCODER_COUNT] = {false,false,false,false};       // блокировка прерывания на контакте AB
uint8_t blockC[ENCODER_COUNT] = {false,false,false,false};       // блокировка прерывания на контакте BC

uint8_t encoderResult[ENCODER_COUNT] = {0,0,0,0};    // Результирующая переменная
int16_t encoderCount[ENCODER_COUNT] = {0,0,0,0};     // Счетчик щелчков энкодера
int16_t oldencoderCount[ENCODER_COUNT] = {0,0,0,0};  // Счетчик старое значение

int16_t getEncoderCount(int enc) {
    return encoderCount[enc];
}

int16_t getOldencoderCount(int enc) {
    return oldencoderCount[enc];
}

void setOldencoderCount(int enc, int16_t val) {
    oldencoderCount[enc] = val;
}

void encoderFilter (int enc) {
  encoderResult[enc] = encoderResult[enc] & B00001111;
  if (encoderResult[enc] == B1011) encoderCount[enc] ++;  // по часовой стрелке
  if (encoderResult[enc] == B0111) encoderCount[enc] --;  // против часосовой стрелки
  }

void pinACHANGE (int enc) {          // Изменилось состояние контакта АB
  if (blockA[enc] == true) return;
  pinA[enc] = !digitalRead(PinA[enc]);
  pinC[enc] = !digitalRead(PinC[enc]);  
  encoderResult[enc]  <<= 1;
  bitWrite(encoderResult[enc], 0, pinA[enc]);
  encoderResult[enc]  <<= 1;
  bitWrite(encoderResult[enc], 0, pinC[enc]);
  encoderFilter(enc);  
  if (!pinA[enc] && !pinC[enc]) blockA[enc] = false; else blockA[enc] = true; 
  blockC[enc] = false;
}

void pinCCHANGE (int enc) {          // Изменилось состояние контакта BC
  if (blockC[enc] == true) return;
  pinA[enc] = !digitalRead(PinA[enc]);
  pinC[enc] = !digitalRead(PinC[enc]);  
  encoderResult[enc]  <<= 1;
  bitWrite(encoderResult[enc], 0, pinA[enc]);
  encoderResult[enc]  <<= 1;
  bitWrite(encoderResult[enc], 0, pinC[enc]);
  encoderFilter(enc); 
  if (!pinA[enc] && !pinC[enc]) blockC[enc] = false; else blockC[enc] = true; 
  blockA[enc] = false;
}

#ifdef ENC1
void enc_1_A() {
  pinACHANGE(ENC1);
}

void enc_1_C() {
  pinCCHANGE(ENC1);
}
#endif

#ifdef ENC2
void enc_2_A() {
  pinACHANGE(ENC2);
}

void enc_2_C() {
  pinCCHANGE(ENC2);
}
#endif

#ifdef ENC3
void enc_3_A() {
  pinACHANGE(ENC3);
}

void enc_3_C() {
  pinCCHANGE(ENC3);
}
#endif

#ifdef ENC4
void enc_4_A() {
  pinACHANGE(ENC4);
}

void enc_4_C() {
  pinCCHANGE(ENC4);
}
#endif

void setupEncoder() {
  //привязываем прерывания
  //первый енкодер
  #ifdef ENC1
    pinMode (PinA[ENC1], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    pinMode (PinC[ENC1], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    attachInterrupt(digitalPinToInterrupt(PinA[ENC1]), enc_1_A, CHANGE);
    attachInterrupt(digitalPinToInterrupt(PinC[ENC1]), enc_1_C, CHANGE);
  #endif

  //второй энкодер
  #ifdef ENC2
    pinMode (PinA[ENC2], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    pinMode (PinC[ENC2], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    attachInterrupt(digitalPinToInterrupt(PinA[ENC2]), enc_2_A, CHANGE);
    attachInterrupt(digitalPinToInterrupt(PinC[ENC2]), enc_2_C, CHANGE);
  #endif
  
  //третий энкодер
  #ifdef ENC3
    pinMode (PinA[ENC3], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    pinMode (PinC[ENC3], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    attachInterrupt(digitalPinToInterrupt(PinA[ENC3]), enc_3_A, CHANGE);
    attachInterrupt(digitalPinToInterrupt(PinC[ENC3]), enc_3_C, CHANGE);
  #endif

  //четвертый энкодер
  #ifdef ENC4
    pinMode (PinA[ENC4], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    pinMode (PinC[ENC4], INPUT_PULLUP);   // Как вход и внутренняя подтяжка
    attachInterrupt(digitalPinToInterrupt(PinA[ENC4]), enc_4_A, CHANGE);
    attachInterrupt(digitalPinToInterrupt(PinC[ENC4]), enc_4_C, CHANGE);
  #endif
}

/*void loop() {
 
 if (oldencoderCount != encoderCount) {
  Serial.println (encoderCount);
  oldencoderCount = encoderCount;
 }

}*/