#include <Servo.h>
#include <Adafruit_NeoPixel.h>
#include <TM1637Display.h>
// KONFIGURACJA DEBUG
#define DEBUG 0
// KONFIGURACJA SPRZĘTU
#define RING_PIN 5
#define NUM_LEDS 60
Adafruit_NeoPixel ring = Adafruit_NeoPixel(NUM_LEDS, RING_PIN, NEO_GRB + NEO_KHZ800);
const int ledsInUse = 30;

#define servoPin 4
#define trigPin 3
#define echoPin 2
Servo myservo;

#define DISPLAY_CLK 8
#define DISPLAY_DIO 10
TM1637Display display(DISPLAY_CLK, DISPLAY_DIO);
// PARAMETRY
// krok serwa
const int stepDeg = 6;
// opóźnienie między krokami (ms)
int dt = 50;
long duration, distance_cm;
int lastDetectedDistance = 0;
// true = w tym kierunku był alarm
bool alarmPoints[ledsInUse];
const int detectionThreshold = 400;
const int movementThreshold = 10;
int alarmTimer[ledsInUse];
const int alarmDurationSec = 5;
unsigned long lastAlarmUpdate = 0;
// STANY
int previousDistance[181];
bool detectionSetThisCycle = false;
// KOLORY
#define COLOR_SCAN 0x0000FF  // niebieski wskaźnik
// FUNKCJE DEBUG
void debugScan(int angle, int distance) {
#if DEBUG
  Serial.print("SCAN Angle=");
  Serial.print(angle);
  Serial.print(" Dist=");
  Serial.println(distance);
#endif
}

void debugAlarmTriggered(int angle, int ledIndex, int timer) {
#if DEBUG
  Serial.print("!RUCH! angle=");
  Serial.print(angle);
  Serial.print(" led=");
  Serial.print(ledIndex);
  Serial.print(" -> alarmTimer[");
  Serial.print(ledIndex);
  Serial.print("]=");
  Serial.println(timer);
#endif
}

void debugAlarmsTable() {
#if DEBUG
  Serial.print("Alarm table: ");
  for (int i = 0; i < ledsInUse; i++) {
    if (alarmTimer[i] > 0) {
      Serial.print("[");
      Serial.print(i);
      Serial.print("=");
      Serial.print(alarmTimer[i]);
      Serial.print("] ");
    }
  }
  Serial.println();
#endif
}
// POMOCNICZE
int logicalIndexToPhysical(int logicalIndex) {
  return (logicalIndex + 45) % NUM_LEDS;
}

int angleToLedIndex(int angle) {
  int idx = map(angle, 0, 180, 0, ledsInUse - 1);
  if (idx < 0) idx = 0;
  if (idx >= ledsInUse) idx = ledsInUse - 1;
  return idx;
}

long readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH, 30000);
  long dist = duration * 0.034 / 2;
  return (dist <= 2 || dist > 400) ? 400 : dist;
}
// ALARMY
void updateAlarms() {
  unsigned long now = millis();
  if (now - lastAlarmUpdate >= 1000) {
    lastAlarmUpdate = now;
    for (int i = 0; i < ledsInUse; i++) {
      if (alarmTimer[i] > 0) {
        alarmTimer[i]--;
        if (alarmTimer[i] == 0) {
          // wygaszony alarm
          alarmPoints[i] = false;
        }
      }
    }
    debugAlarmsTable();
  }
}
// RING LED
void updateRingDisplay(int currentAngle) {
  ring.clear();
  // czerwone alarmy
  for (int i = 0; i < ledsInUse; i++) {
    if (alarmPoints[i]) {
      int phys = logicalIndexToPhysical(i);
      ring.setPixelColor(phys, ring.Color(255, 0, 0));
    }
  }
  // niebieski wskaźnik
  int currIdx = angleToLedIndex(currentAngle);
  int currPhys = logicalIndexToPhysical(currIdx);
  if (!alarmPoints[currIdx]) {
    ring.setPixelColor(currPhys, COLOR_SCAN);
  }
  ring.show();
}
// GŁÓWNY UPDATE
void updateSystem(int angle) {
  myservo.write(angle);
  delay(dt);
  distance_cm = readDistance();
  if (previousDistance[angle] == -1) {
    previousDistance[angle] = distance_cm;
  } else {
    int distanceChange = abs(distance_cm - previousDistance[angle]);
#if DEBUG
    Serial.print("Δdist=");
    Serial.print(distanceChange);
    Serial.print(" prev=");
    Serial.print(previousDistance[angle]);
    Serial.print(" now=");
    Serial.println(distance_cm);
#endif
    if (!detectionSetThisCycle && distanceChange > movementThreshold && distance_cm < detectionThreshold) {
      int ledIndex = angleToLedIndex(angle);
      alarmTimer[ledIndex] = alarmDurationSec;
      alarmPoints[ledIndex] = true;
      lastDetectedDistance = distance_cm;
      // blokada do końca przebiegu
      detectionSetThisCycle = true;
      debugAlarmTriggered(angle, ledIndex, alarmTimer[ledIndex]);
    }
    previousDistance[angle] = distance_cm;
  }
  display.showNumberDec(lastDetectedDistance);
  updateRingDisplay(angle);
  debugScan(angle, distance_cm);
}
// SETUP
void setup() {
  Serial.begin(9600);
  for (int i = 0; i <= 180; i++) previousDistance[i] = -1;
  for (int i = 0; i < ledsInUse; i++) {
    alarmPoints[i] = false;
    alarmTimer[i] = 0;
  }
  myservo.attach(servoPin);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  ring.begin();
  ring.setBrightness(250);
  ring.show();
  display.setBrightness(7);
  display.showNumberDec(0);
}
// LOOP
void loop() {
  updateAlarms();
  // przód
  detectionSetThisCycle = false;
  for (int pos = 0; pos <= 180; pos += stepDeg) {
    updateSystem(pos);
  }
  // tył
  detectionSetThisCycle = false;
  for (int pos = 180; pos >= 0; pos -= stepDeg) {
    updateSystem(pos);
  }
}