#include "segment.pio.h"

uint8_t digits[] = {
  0b11000000, 0b11111001, 0b10100100, 0b10110000,
  0b10011001, 0b10010010, 0b10000010, 0b11111000,
  0b10000000, 0b10011000,
};

const uint8_t first_segment_pin = 2;
const uint8_t first_digit_pin = 10;

// Ultrasoon sensoren
const int trig1 = 14;
const int echo1 = 15;
const int trig2 = 16;
const int echo2 = 17;

// LEDs en buzzer
const int greenLed = 18;
const int redLed   = 19;
const int buzzer   = 20;

int counter = 0;
unsigned long lastTriggerTime = 0;
const int triggerDistance = 10;  
const int debounceDelay = 1000;  

unsigned long lastBlinkTime = 0;
bool alarmState = false;

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

  pinMode(trig1, OUTPUT);
  pinMode(echo1, INPUT);
  pinMode(trig2, OUTPUT);
  pinMode(echo2, INPUT);

  pinMode(greenLed, OUTPUT);
  pinMode(redLed, OUTPUT);
  pinMode(buzzer, OUTPUT);

  auto offset = pio_add_program(pio0, &segment_program);
  segment_program_init(pio0, 0, offset, first_segment_pin, first_digit_pin);

  startupAnimation();
}

long readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000);
  long distance = duration * 0.034 / 2;

  return distance;
}

void displayRaw(uint32_t rawValue) {
  pio_sm_put(pio0, 0, rawValue);
}

void displayNumber(uint value) {
  if (value > 9999) value = 9999;

  pio_sm_put(pio0, 0,
    digits[value / 1000 % 10] << 24 |
    digits[value / 100 % 10] << 16 |
    digits[value / 10 % 10] << 8 |
    digits[value % 10]
  );
}

// 🔥 Segment-voor-segment opbouw (±3 sec)
void startupAnimation() {

  uint8_t segmentPattern = 0xFF;  // alles uit (common cathode logica omgekeerd)

  for (int bit = 7; bit >= 0; bit--) {

    segmentPattern &= ~(1 << bit);   // zet één segment aan

    uint32_t fullDisplay =
      segmentPattern << 24 |
      segmentPattern << 16 |
      segmentPattern << 8  |
      segmentPattern;

    displayRaw(fullDisplay);
    delay(300);
  }

  delay(500);   // alles even volledig aan laten
  displayNumber(0);
}

void handleAlarm() {

  if (counter > 15) {
    digitalWrite(greenLed, LOW);

    if (millis() - lastBlinkTime > 300) {
      lastBlinkTime = millis();
      alarmState = !alarmState;

      digitalWrite(redLed, alarmState);
      digitalWrite(buzzer, alarmState);
    }

  } else {
    digitalWrite(greenLed, HIGH);
    digitalWrite(redLed, LOW);
    digitalWrite(buzzer, LOW);
  }
}

void loop() {

  long dist1 = readDistance(trig1, echo1);
  long dist2 = readDistance(trig2, echo2);

  if (millis() - lastTriggerTime > debounceDelay) {

    if (dist1 > 0 && dist1 < triggerDistance) {
      counter++;
      lastTriggerTime = millis();
    }

    if (dist2 > 0 && dist2 < triggerDistance) {
      if (counter > 0) {
        counter--;
      }
      lastTriggerTime = millis();
    }
  }

  displayNumber(counter);
  handleAlarm();
}