/*
  Pocket radar

  pixel LUT
  https://wokwi.com/projects/327642884399432274

  add pause button
*/

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

const int SWEEP_SPD = 1000;
const int MAX_VECTOR = 175;
const int MIN_VECTOR = 5;
const int SERVO_PIN = 3;
const int ECHO_PIN = 11;
const int TRIG_PIN = 12;
const int CENTER_X = 63;  // x center of the sweep
const int CENTER_Y = 63;  // y center of the sweep
const int RADIUS = 61;    // radius for pixel tickmarks
const int OLED_RESET = -1;

// lookup tables for sin/cos calculations
byte lut_x_pixel[36]; // 180 deg / 5 deg
byte lut_y_pixel[36];
int increment = 5;
unsigned long prevMillis = 0;

Adafruit_SSD1306 display(128, 64, &Wire, OLED_RESET);
Servo servo;

long getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH);
  long cm = (duration / 2) / 29.1;  // Divide by 29.1 or multiply by 0.0343
  //inches = (duration / 2) / 74; // Divide by 74 or multiply by 0.0135
  return cm;
}

void makeLUT()  {
  // pre-calculate x and y positions into look-up tables
  for (int angle = 5; angle < 180; angle += 5) {
    lut_x_pixel[angle / 5] =  sin(radians(angle - 90)) * RADIUS + CENTER_X;
    lut_y_pixel[angle / 5] = -cos(radians(angle - 90)) * RADIUS + CENTER_Y;
    /*
      Serial.print("a: ");
      Serial.print(angle);
      Serial.print(F("\tX: "));
      Serial.print(lut_x_pixel[angle / 5]);
      Serial.print(F("\tY: "));
      Serial.println(lut_y_pixel[angle / 5]);
    */
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  servo.attach(SERVO_PIN);
  makeLUT();
  display.setRotation(2);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.setFont();
  display.setTextSize(0);      // Normal 1:1 pixel scale
  display.setTextColor(WHITE, BLACK); // Draw white text
  display.display();
  delay(100);
  display.clearDisplay(); // clears the screen and buffer
  display.drawCircle( // origin x, y, radius, color
    (display.width() / 2) - 1, display.height(),
    display.height() - 1,
    SSD1306_WHITE);
  display.drawCircle(
    (display.width() / 2) - 1, display.height(),
    (display.height() - 1) / 2,
    SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Range:");
  display.setCursor(104, 0);
  display.print("Deg:");
  display.display();
}

void loop() {
  static int bearing = MIN_VECTOR;
  char buffer[4];
  long range = 0;

  if (millis() - prevMillis >= SWEEP_SPD)  {
    prevMillis = millis();

    bearing = bearing + increment;
    if (bearing >= MAX_VECTOR || bearing <= MIN_VECTOR)   {
      increment = -increment;
    }
    servo.write(bearing);

    display.drawLine(
      CENTER_X, CENTER_Y,
      lut_x_pixel[bearing / 5], lut_y_pixel[bearing / 5],
      SSD1306_WHITE);
    display.display();
    delay(50);
    display.drawLine(
      CENTER_X, CENTER_Y,
      lut_x_pixel[bearing / 5], lut_y_pixel[bearing / 5],
      SSD1306_BLACK);
    display.display();

    range = getDistance();
    int screenRange = map (range, 2, 400, 0, RADIUS);

    display.fillCircle( // origin x, y, radius, color
      sin(radians(bearing - 90)) * screenRange + CENTER_X - 1,
      -cos(radians(bearing - 90)) * screenRange + CENTER_Y,
      2,
      SSD1306_WHITE);
    display.display();
    delay(50);
    display.fillCircle( // origin x, y, radius, color
      sin(radians(bearing - 90)) * screenRange + CENTER_X - 1,
      -cos(radians(bearing - 90)) * screenRange + CENTER_Y,
      2,
      SSD1306_BLACK);

    // readouts
    snprintf(buffer, 4, "%3d", range);
    display.setCursor(0, 8);
    display.print(buffer);
    snprintf(buffer, 4, "%3d", bearing);
    display.setCursor(104, 8);
    display.print(buffer);
    display.display();

    Serial.print("Range: ");
    Serial.print(range);
    Serial.print("\tBearing: ");
    Serial.println(bearing);
  }
}