// Czujnik parkowania.
// Założenia:
//    - Sygnalizacja za pomocą paska LED - jeden LED oznacza 1 cm
//    - Pasek jest aktywny tylko podczas cofania (kontaktron symuluje wsteczny bieg)
//    - Podczas wlaczenia piszczy buzzer przez 1s a na LED jest animacja aktywacji
//    - Jeżeli odległość mniejsza niż X cm to piszczy i LEDy sygnalizują

#include <Adafruit_NeoPixel.h>
#include <HCSR04.h>

#define LED_PIN     4
#define LED_COUNT   8

#define TRIG_PIN    2
#define ECHO_PIN    3

#define BUZZER_PIN  5
#define REED_PIN    6

// Obiekt czujnika ultradźwiękowego
UltraSonicDistanceSensor distanceSensor(TRIG_PIN, ECHO_PIN);

// NeoPixel
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

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

  // LEDy
  strip.begin();
  strip.show();

  // Buzzer
  pinMode(BUZZER_PIN, OUTPUT);

  // Kontaktron (brak rezystora → używamy INPUT_PULLUP)
  pinMode(REED_PIN, INPUT_PULLUP);

  Serial.println("Start systemu...");
}

void buzzerNonBlocking(float freq) {
  static unsigned long lastToggleTime = 0;
  static bool buzzerState = false;

  if (freq <= 0) {
      digitalWrite(BUZZER_PIN, LOW);   // wyłącz buzzer
      return;
  }

  // okres półfali sygnału w ms
  float halfPeriodMs = (1000.0 / freq) / 2.0;

  unsigned long currentTime = millis();

  // jeśli minął czas półokresu, przełącz stan
  if (currentTime - lastToggleTime >= halfPeriodMs) {
      lastToggleTime = currentTime;
      buzzerState = !buzzerState;
      digitalWrite(BUZZER_PIN, buzzerState ? HIGH : LOW);
  }
}

void startAnimationNonBlocking(bool startAnimation) {
    static bool running = false;
    static unsigned long lastUpdate = 0;
    static int step = 0;
    static int fadeLevel = 255;

    // jeśli animacja ma być wyłączona → reset i wyjście
    if (!startAnimation) {
        running = false;
        step = 0;
        fadeLevel = 255;
        return;
    }

    // start animacji
    if (!running) {
        running = true;
        step = 0;
        fadeLevel = 255;
        strip.clear();
        strip.show();
    }

    unsigned long now = millis();

    // czas między krokami animacji
    const unsigned long interval = 120;  

    if (now - lastUpdate < interval) return;
    lastUpdate = now;

    // paleta kolorów kroków
    uint32_t colors[] = {
        strip.Color(255,   0,   0),   // czerwony
        strip.Color(255, 128,   0),   // pomarańczowy
        strip.Color(255, 255,   0),   // żółty
        strip.Color(  0, 255,   0)    // zielony
    };

    // sekwencja par diod od środka na zewnątrz
    const int pairs[4][2] = {
        {3, 4},
        {2, 5},
        {1, 6},
        {0, 7}
    };

    // ======================
    // 1) ZAPALANIE PAR LED
    // ======================

    if (step < 4) {
        // zapal odpowiednią parę LED kolorami
        int i1 = pairs[step][0];
        int i2 = pairs[step][1];

        strip.setPixelColor(i1, colors[step]);
        strip.setPixelColor(i2, colors[step]);
        strip.show();

        step++;
        return;
    }

    // ======================
    // 2) GRADIENTOWE WYGASZANIE WSZYSTKIEGO
    // ======================

    if (step == 4) {
        fadeLevel -= 15;  // zmień intensywność stopniowo

        if (fadeLevel < 0) {
            fadeLevel = 0;
        }

        for (int i = 0; i < strip.numPixels(); i++) {
            uint32_t c = strip.getPixelColor(i);
            
            // pobierz RGB
            uint8_t r = (c >> 16) & 0xFF;
            uint8_t g = (c >> 8)  & 0xFF;
            uint8_t b =  c        & 0xFF;

            // zredukuj proporcjonalnie
            r = (r * fadeLevel) / 255;
            g = (g * fadeLevel) / 255;
            b = (b * fadeLevel) / 255;

            strip.setPixelColor(i, r, g, b);
        }

        strip.show();

        if (fadeLevel == 0) {
            // reset animacji
            running = false;
            step = 0;
        }

        return;
    }
}

void loop() {
  static uint8_t state = 0;
  static float freq_buzzer = 0.0f;
  static bool welcomeAnimation = false;
  static uint32_t welcomeStartTime = 0;
  static uint32_t lastDistanceRead = 0;
  static float currentDistance = 0;
  


  switch(state)
  {
    case 0: // Oczekuje wlaczenia wstecznego biegu
      if (LOW == digitalRead(REED_PIN))
        state++;
      break;

    case 1: // Inicjalizacja
      welcomeAnimation = true;
      welcomeStartTime = millis();  // zapamiętaj czas startu animacji
      state++;
      break;
    
    case 2:
    if (millis() - welcomeStartTime >= 4000) {
      welcomeAnimation = false;
      strip.clear();
      strip.show();
      state++;   
    }
    break;
    
    case 3: // Normalne dzialanie czujnika
      if (HIGH == digitalRead(REED_PIN))
        state = 0;

      unsigned long now = millis();

      // ------------------------------------
      // 1) Pomiar co 500 ms
      // ------------------------------------
      if (now - lastDistanceRead >= 500) {
          lastDistanceRead = now;
          currentDistance = distanceSensor.measureDistanceCm(); // cm

          if (currentDistance <= 0) {
              currentDistance = 400; // ignorujemy błędy
          }
      }

      // ------------------------------------
      // 2) Częstotliwość buzzera zależnie od odległości
      // ------------------------------------

      if (currentDistance < 2) {
          freq_buzzer = 20.0;   // blisko → szybko
      }
      else if (currentDistance < 4) {
          freq_buzzer = 15.0;
      }
      else if (currentDistance < 7) {
          freq_buzzer = 10.0;
      }
      else if (currentDistance < 12) {
          freq_buzzer = 5.0;
      }
      else {
          freq_buzzer = 0.0;      // poza zasięgiem → cisza
      }

      // ------------------------------------
      // 3) Wskazanie na pasku LED (NeoPixel)
      //    Im bliżej – tym więcej diod + kolory
      // ------------------------------------

      strip.clear();

      int ledsToLight = 0;

      if      (currentDistance < 2)  ledsToLight = 8;
      else if (currentDistance < 4)  ledsToLight = 6;
      else if (currentDistance < 7)  ledsToLight = 4;
      else if (currentDistance < 12) ledsToLight = 2;
      else                           ledsToLight = 1;

      for (int i = 0; i < ledsToLight; i++) {
          // kolor zależny od odległości
          uint32_t color;

          if      (currentDistance < 2)  color = strip.Color(255, 0, 0);      // czerwony
          else if (currentDistance < 4)  color = strip.Color(255, 80, 0);     // pomarańcz
          else if (currentDistance < 7)  color = strip.Color(255, 255, 0);    // żółty
          else if (currentDistance < 12) color = strip.Color(0, 255, 0);      // zielony
          else                           color = strip.Color(0, 0, 255);     // niebieski

          strip.setPixelColor(i, color);
      }

      strip.show();

      break;
  }

  
  buzzerNonBlocking(freq_buzzer);
  startAnimationNonBlocking(welcomeAnimation);
}