#include <Arduino.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SDA_PIN         21
#define SCL_PIN         22

#define PIR1_PIN        14
#define PRZYCISK_UZBROJ_PIN  33
#define PRZYCISK_RESET_PIN   32
#define PRZYCISK_DRZWI_PIN   25

#define LED_PIN         2
#define BUZZER_PIN      4

#define SZEROKOSC_EKRANU  128
#define WYSOKOSC_EKRANU   64
#define OLED_RESET        -1

Adafruit_SSD1306 ekran(SZEROKOSC_EKRANU, WYSOKOSC_EKRANU, &Wire, OLED_RESET);

bool systemUzbrojony = false;
bool alarmAktywny = false;

static QueueHandle_t kolejkaEkranu = NULL;

void ZadanieSterowania(void *pvParameters);
void ZadanieWyswietlacza(void *pvParameters);

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

  Wire.begin(SDA_PIN, SCL_PIN);

  if (!ekran.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    while (true) {
      delay(100);
    }
  }
  ekran.clearDisplay();
  ekran.setTextSize(1);
  ekran.setTextColor(WHITE);
  ekran.setCursor(0, 0);
  ekran.println("System alarmowy PSK");
  ekran.display();
  delay(1500);

  pinMode(PIR1_PIN, INPUT);
  pinMode(PRZYCISK_UZBROJ_PIN, INPUT_PULLUP);
  pinMode(PRZYCISK_RESET_PIN, INPUT_PULLUP);
  pinMode(PRZYCISK_DRZWI_PIN, INPUT_PULLUP);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);

  kolejkaEkranu = xQueueCreate(5, sizeof(const char *));
  if (!kolejkaEkranu) {
    while (true) {
      delay(100);
    }
  }

  xTaskCreate(ZadanieSterowania, "ZadanieSterowania", 2048, NULL, 2, NULL);
  xTaskCreate(ZadanieWyswietlacza, "ZadanieWyswietlacza", 2048, NULL, 1, NULL);
}

void loop() {}

void ZadanieSterowania(void *pvParameters) {
  for (;;) {
    if (digitalRead(PRZYCISK_UZBROJ_PIN) == LOW) {
      systemUzbrojony = !systemUzbrojony;
      alarmAktywny = false;
      digitalWrite(LED_PIN, LOW);
      digitalWrite(BUZZER_PIN, LOW);

      const char *komunikat = (systemUzbrojony ? "System uzbrojony" : "System rozbrojony");
      xQueueSend(kolejkaEkranu, &komunikat, portMAX_DELAY);

      vTaskDelay(pdMS_TO_TICKS(50));
    }

    if (digitalRead(PRZYCISK_RESET_PIN) == LOW) {
      alarmAktywny = false;
      digitalWrite(LED_PIN, LOW);
      digitalWrite(BUZZER_PIN, LOW);

      const char *komReset = "Alarm skasowany";
      xQueueSend(kolejkaEkranu, &komReset, portMAX_DELAY);

      vTaskDelay(pdMS_TO_TICKS(50));
    }

    if (systemUzbrojony && (digitalRead(PRZYCISK_DRZWI_PIN) == LOW) && !alarmAktywny) {
      alarmAktywny = true;
      digitalWrite(LED_PIN, HIGH);
      digitalWrite(BUZZER_PIN, HIGH);

      const char *komDrzwi = "ALARM! Otwarcie drzwi!";
      xQueueSend(kolejkaEkranu, &komDrzwi, portMAX_DELAY);

      vTaskDelay(pdMS_TO_TICKS(50));
    }

    if (systemUzbrojony && !alarmAktywny) {
      if (digitalRead(PIR1_PIN) == HIGH) {
        alarmAktywny = true;
        digitalWrite(LED_PIN, HIGH);
        digitalWrite(BUZZER_PIN, HIGH);

        const char *komPIR = "ALARM! Wykryto ruch!";
        xQueueSend(kolejkaEkranu, &komPIR, portMAX_DELAY);
      }
    }

    if (!alarmAktywny) {
      digitalWrite(BUZZER_PIN, LOW);
    }

    vTaskDelay(pdMS_TO_TICKS(10));
  }
}

void ZadanieWyswietlacza(void *pvParameters) {
  for (;;) {
    const char *komunikat;
    if (xQueueReceive(kolejkaEkranu, &komunikat, portMAX_DELAY) == pdTRUE) {
      ekran.clearDisplay();
      ekran.setCursor(0, 0);
      ekran.setTextSize(1);
      ekran.setTextColor(WHITE);
      ekran.println(komunikat);
      ekran.display();
    }

    vTaskDelay(pdMS_TO_TICKS(20));
  }
}