/*
  Radar scanner — Robot Maker Toolkit
  Arduino UNO + SG90 servo + HC-SR04 + SSD1306 OLED

  The servo sweeps the ultrasonic sensor back and forth; each reading is
  plotted as a blip on a polar display, with a sweep line that follows the
  servo. A cheap radar you can watch working.

  PIN NOTE: the SG90 module page uses D9 for the servo signal, but the
  HC-SR04 page already uses D9 for TRIG. This project keeps the sensor on
  its documented pins and moves the servo to D11 — any digital pin can
  drive a servo, so nothing is lost.

  Libraries: Servo (bundled), Adafruit SSD1306, Adafruit GFX.
*/

#include <Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

const int TRIG_PIN  = 9;
const int ECHO_PIN  = 10;
const int SERVO_PIN = 11;

const int SCREEN_WIDTH = 128;
const int SCREEN_HEIGHT = 64;
const int OLED_RESET = -1;
const uint8_t OLED_ADDR = 0x3C;   // a few modules are 0x3D

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Servo sweeper;

const unsigned long ECHO_TIMEOUT_US = 25000UL;
const int MAX_RANGE_CM = 100;     // anything beyond this plots at the rim
const int STEP_DEG = 3;           // sweep resolution

// Origin of the polar plot: bottom-centre of the screen.
const int CX = SCREEN_WIDTH / 2;
const int CY = SCREEN_HEIGHT - 1;
const int R  = SCREEN_HEIGHT - 4;

// One blip per sweep column, remembered so the whole arc stays on screen.
const int SLOTS = 180 / STEP_DEG + 1;
uint8_t blip[SLOTS];              // distance in cm, 0 = nothing seen

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  sweeper.attach(SERVO_PIN);

  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    // Nothing to draw on — blink the built-in LED so the failure is visible.
    pinMode(LED_BUILTIN, OUTPUT);
    while (true) {
      digitalWrite(LED_BUILTIN, HIGH); delay(200);
      digitalWrite(LED_BUILTIN, LOW);  delay(200);
    }
  }
  display.clearDisplay();
  display.display();
  for (int i = 0; i < SLOTS; i++) blip[i] = 0;
}

float readDistanceCm() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long us = pulseIn(ECHO_PIN, HIGH, ECHO_TIMEOUT_US);
  if (us == 0) return -1.0;
  return (us * 0.0343) / 2.0;
}

void drawFrame(int angle) {
  display.clearDisplay();

  // Range arcs at 1/3 and 2/3 of full range.
  display.drawCircle(CX, CY, R / 3, SSD1306_WHITE);
  display.drawCircle(CX, CY, (R * 2) / 3, SSD1306_WHITE);
  display.drawCircle(CX, CY, R, SSD1306_WHITE);
  display.drawFastHLine(CX - R, CY, R * 2, SSD1306_WHITE);

  // Blips collected so far.
  for (int i = 0; i < SLOTS; i++) {
    if (blip[i] == 0) continue;
    float a = radians(i * STEP_DEG);
    int len = map(blip[i], 0, MAX_RANGE_CM, 0, R);
    int x = CX + (int)(cos(a) * len);
    int y = CY - (int)(sin(a) * len);
    display.fillCircle(x, y, 1, SSD1306_WHITE);
  }

  // Sweep line at the servo's current angle.
  float a = radians(angle);
  display.drawLine(CX, CY, CX + (int)(cos(a) * R), CY - (int)(sin(a) * R), SSD1306_WHITE);

  // Current reading, top-left.
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  int slot = angle / STEP_DEG;
  if (slot >= 0 && slot < SLOTS && blip[slot] > 0) {
    display.print(blip[slot]);
    display.print(F(" cm"));
  } else {
    display.print(F("--"));
  }

  display.display();
}

void sweep(int from, int to, int step) {
  for (int angle = from; step > 0 ? angle <= to : angle >= to; angle += step) {
    sweeper.write(angle);
    delay(40);                      // let the horn actually arrive

    float cm = readDistanceCm();
    int slot = angle / STEP_DEG;
    if (slot >= 0 && slot < SLOTS) {
      blip[slot] = (cm > 0 && cm <= MAX_RANGE_CM) ? (uint8_t)cm : 0;
    }
    drawFrame(angle);
  }
}

void loop() {
  sweep(0, 180, STEP_DEG);
  sweep(180, 0, -STEP_DEG);
}