#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
#include <OneWire.h>
#include <DallasTemperature.h>

// OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// פינים
#define TRIG_PIN 5
#define ECHO_PIN 18
#define SERVO_PIN 13
#define TEMP_PIN 4  // חיישן DS18B20

// סרוו
Servo myServo;

// חיישן טמפרטורה
OneWire oneWire(TEMP_PIN);
DallasTemperature sensors(&oneWire);

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

  // הגדרות חיישן מרחק
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // סרוו
  myServo.setPeriodHertz(50);
  myServo.attach(SERVO_PIN);

  // OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED not found");
    while (true);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Radar + Temp Init...");
  display.display();
  delay(1000);

  // חיישן טמפרטורה
  sensors.begin();
}

long readDistanceCM() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH, 30000); // מקסימום 30ms
  return duration * 0.034 / 2;
}

float readTemperatureC() {
  sensors.requestTemperatures();
  return sensors.getTempCByIndex(0);
}

void loop() {
  scanRadar(30, 120, 5);
  scanRadar(120, 30, -5);
}

void scanRadar(int fromAngle, int toAngle, int step) {
  for (int angle = fromAngle; (step > 0) ? angle <= toAngle : angle >= toAngle; angle += step) {
    myServo.write(angle);
    delay(200);

    long distance = readDistanceCM();
    float temperature = readTemperatureC();
    drawRadar(angle, distance, temperature);
  }
}

void drawRadar(int angle, long distance, float temperature) {
  display.clearDisplay();

  int x = map(angle, 30, 120, 0, SCREEN_WIDTH);
  int y = constrain(map(distance, 0, 100, SCREEN_HEIGHT - 1, 0), 0, SCREEN_HEIGHT - 1);

  display.drawLine(SCREEN_WIDTH / 2, SCREEN_HEIGHT, x, y, SSD1306_WHITE);
  if (distance < 100) {
    display.fillCircle(x, y, 3, SSD1306_WHITE);
  }

  display.setCursor(0, 0);
  display.print("Angle: ");
  display.print(angle);

  display.setCursor(0, 10);
  display.print("Dist: ");
  display.print(distance);
  display.println("cm");

  display.setCursor(0, 20);
  display.print("Temp: ");
  display.print(temperature);
  display.println(" C");

  display.display();
}