#include <avr/interrupt.h>
#include <avr/io.h>

#define MASK_PIND 0b11110000

char keysState[16];
char newKeysState[16];
char debouncedKeys[16];

uint8_t confidenceLevel = 0;
uint8_t row1;

volatile unsigned long ms = 0;

bool isDebounced;

struct KeyTimeMap {
  char key;
  int time;
};

struct KeyTimeMap keyTimeMap[16];

void setupTimer() {
  TCCR1B |= (1 << WGM12); // настраиваем режим CTC (Clear Timer on Compare Match)
  OCR1A = 250; // задаем значение, при котором будет срабатывать прерывание (в данном случае 15999, так как Atmega328p работает на частоте 16 МГц и нужно получить прерывание раз в 1 мс)
  TIMSK1 |= (1 << OCIE1A); // включаем прерывание по совпадению со значением регистра OCR1A
  sei(); // разрешаем глобальные прерывания
  TCCR1B |= (1 << CS11) | (1 << CS10); // запускаем таймер с предделителем 64 (16000000/64=250000)
}


// unused
void setupSpi() {
  /* Установка выводов SCK, MOSI и SS как выходные */
  DDRB |= (1 << DDB5) | (1 << DDB3) | (1 << 2);

  /* Включение SPI, установка режима ведущего, установка частоты */
  SPCR = (1 << SPE) | (1 << MSTR) | (1 << SPR0);
}


void setup() {

  DDRD = 0b00001111;
  PORTD = 0b11110000;

  DDRC = 0b111111;
  PORTC = 0b001111;

  // setupSpi();
  setupTimer();
  Serial.begin(9600);
  Serial.println("Program start");
}

ISR(TIMER1_COMPA_vect) {
  ++ms;
}

void countKeysPressedTime(char *keys) {
  if (!isDebounced) return;
  for (int i = 0; i < 16; i++) {
    keyTimeMap[i].key = i;
    if (keys[i] == 0) {
      keyTimeMap[i].time += 5;
    } else {
      keyTimeMap[i].time = 0;
    }
  }
}

void debounce(char *keys) {
  if (ms % 5 != 0) return;
  isDebounced = 0;
  checkKeysState(newKeysState);

  for (int i = 0; i < 16; i++) {
    if (newKeysState[i] != keys[i]) {
      confidenceLevel = 0;
    }
  }

  if (confidenceLevel < 6) confidenceLevel++;
  if (confidenceLevel == 5) {
    for (int i = 0; i < 16; i++) {
      debouncedKeys[i] = keys[i];
    }
    isDebounced = 1;
    confidenceLevel = 0;
  }
}

void checkKeysState(char *keys) {
  PORTC &= ~(1 << 3);
  row1 = PIND & MASK_PIND;
  if (row1 & (1 << 4)) keys[0] = 1; else keys[0] = 0;
  if (row1 & (1 << 5)) keys[1] = 1; else keys[1] = 0;
  if (row1 & (1 << 6)) keys[2] = 1; else keys[2] = 0;
  if (row1 & (1 << 7)) keys[3] = 1; else keys[3] = 0;
  PORTC |= (1 << 3);

  PORTC &= ~(1 << 2);
  row1 = PIND & MASK_PIND;
  if (row1 & (1 << 4)) keys[4] = 1; else keys[4] = 0;
  if (row1 & (1 << 5)) keys[5] = 1; else keys[5] = 0;
  if (row1 & (1 << 6)) keys[6] = 1; else keys[6] = 0;
  if (row1 & (1 << 7)) keys[7] = 1; else keys[7] = 0;
  PORTC |= (1 << 2);

  PORTC &= ~(1 << 1);
  row1 = PIND & MASK_PIND;
  if (row1 & (1 << 4)) keys[8] = 1; else keys[8] = 0;
  if (row1 & (1 << 5)) keys[9] = 1; else keys[9] = 0;
  if (row1 & (1 << 6)) keys[10] = 1; else keys[10] = 0;
  if (row1 & (1 << 7)) keys[11] = 1; else keys[11] = 0;
  PORTC |= (1 << 1);

  PORTC &= ~(1 << 0);
  row1 = PIND & MASK_PIND;
  if (row1 & (1 << 4)) keys[12] = 1; else keys[12] = 0;
  if (row1 & (1 << 5)) keys[13] = 1; else keys[13] = 0;
  if (row1 & (1 << 6)) keys[14] = 1; else keys[14] = 0;
  if (row1 & (1 << 7)) keys[15] = 1; else keys[15] = 0;
  PORTC |= (1 << 0);
}

void loop() {
  checkKeysState(keysState);
  debounce(keysState);
  countKeysPressedTime(debouncedKeys);
  // if (isDebounced) {
  //   Serial.println("---start---");
  //   for (int i = 0; i < 16; i++) {
  //     Serial.print(debouncedKeys[i], " ");
  //   }
  //   Serial.println("---end---");
  // }
  Serial.print("---start---");
  for (int i = 0; i < 16; i++) {
    Serial.print(keyTimeMap[i].key, " ");
    Serial.print(keyTimeMap[i].time);
    Serial.print(" ");
  }
  Serial.println("---end---");
}