Radar Com Display De Pulso Onde Os Dispositivos

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <TinyGPS++.h>
#include <math.h>


// Hoisted type definitions
struct Contact {
  char id[13];
  float lat;
  float lon;
  uint32_t updatedAt;
  bool valid;
};


// Forward declarations
void sendAT(const char *command);
void drawRadar();
void rememberLocation(const char *id, float lat, float lon);
void sendLocationSms(const char *phone);
void processSms(const char *sender, const char *body);
void processGsmLine(char *line);
void readGsm();

constexpr int OLED_SDA = 21;
constexpr int OLED_SCL = 22;
constexpr int GPS_RX_PIN = 16;
constexpr int GSM_RX_PIN = 27;
constexpr int GSM_TX_PIN = 26;
constexpr uint8_t SCREEN_WIDTH = 128;
constexpr uint8_t SCREEN_HEIGHT = 64;
constexpr float RADAR_RANGE_M = 1000.0f;
constexpr uint32_t POSITION_FRESH_MS = 120000;

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
TinyGPSPlus gps;
HardwareSerial gpsSerial(1);
HardwareSerial gsmSerial(2);



constexpr uint8_t MAX_CONTACTS = 3;
Contact contacts[MAX_CONTACTS] = {};
float centerLat = 0.0f;
float centerLon = 0.0f;
bool centerValid = false;
char gsmLine[180];
size_t gsmLineLength = 0;
char pendingSender[28] = "";
bool waitingSmsBody = false;

void sendAT(const char *command) {
  gsmSerial.print(command);
  gsmSerial.print("\r");
}

void drawRadar() {
  display.clearDisplay();
  const int cx = 34;
  const int cy = 34;
  display.drawCircle(cx, cy, 10, SSD1306_WHITE);
  display.drawCircle(cx, cy, 20, SSD1306_WHITE);
  display.drawCircle(cx, cy, 29, SSD1306_WHITE);
  display.drawLine(cx - 29, cy, cx + 29, cy, SSD1306_WHITE);
  display.drawLine(cx, cy - 29, cx, cy + 29, SSD1306_WHITE);
  display.fillCircle(cx, cy, 2, SSD1306_WHITE);
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(70, 2);
  display.print("GSM RADAR");
  display.setCursor(70, 12);
  display.print(centerValid ? "GPS OK" : "AGUARDE GPS");
  display.setCursor(70, 22);
  display.print("RAIO 1km");

  uint8_t row = 0;
  for (uint8_t i = 0; i < MAX_CONTACTS; ++i) {
    if (!contacts[i].valid || !centerValid || millis() - contacts[i].updatedAt > POSITION_FRESH_MS) continue;
    const float northM = (contacts[i].lat - centerLat) * 111111.0f;
    const float eastM = (contacts[i].lon - centerLon) * 111111.0f * cosf(centerLat * DEG_TO_RAD);
    const float distanceM = sqrtf(northM * northM + eastM * eastM);
    const float scale = min(distanceM / RADAR_RANGE_M, 1.0f);
    const float divisor = distanceM > 0.1f ? distanceM : 1.0f;
    int x = cx + static_cast<int>((eastM / divisor) * scale * 28.0f);
    int y = cy - static_cast<int>((northM / divisor) * scale * 28.0f);
    display.fillCircle(x, y, 2, SSD1306_WHITE);
    display.setCursor(70, 34 + row * 10);
    display.print(contacts[i].id);
    display.print(' ');
    display.print(static_cast<int>(distanceM));
    display.print('m');
    ++row;
  }
  display.display();
}

void rememberLocation(const char *id, float lat, float lon) {
  int slot = -1;
  for (uint8_t i = 0; i < MAX_CONTACTS; ++i) {
    if (contacts[i].valid && strcmp(contacts[i].id, id) == 0) {
      slot = i;
      break;
    }
    if (!contacts[i].valid && slot < 0) slot = i;
  }
  if (slot < 0) slot = 0;
  strncpy(contacts[slot].id, id, sizeof(contacts[slot].id) - 1);
  contacts[slot].id[sizeof(contacts[slot].id) - 1] = '\0';
  contacts[slot].lat = lat;
  contacts[slot].lon = lon;
  contacts[slot].updatedAt = millis();
  contacts[slot].valid = true;
  drawRadar();
}

void sendLocationSms(const char *phone) {
  if (!gps.location.isValid()) return;
  char body[72];
  snprintf(body, sizeof(body), "LOC,PAINEL,%.6f,%.6f", gps.location.lat(), gps.location.lng());
  gsmSerial.print("AT+CMGS=\"");
  gsmSerial.print(phone);
  gsmSerial.print("\"\r");
  delay(250);
  gsmSerial.print(body);
  gsmSerial.write(26);
}

void processSms(const char *sender, const char *body) {
  char id[13] = {};
  float lat = 0.0f;
  float lon = 0.0f;
  if (sscanf(body, "LOC,%12[^,],%f,%f", id, &lat, &lon) == 3 && lat >= -90.0f && lat <= 90.0f && lon >= -180.0f && lon <= 180.0f) {
    rememberLocation(id, lat, lon);
  } else if (strcmp(body, "LOC?") == 0 && sender[0] != '\0') {
    sendLocationSms(sender);
  }
}

void processGsmLine(char *line) {
  if (strncmp(line, "+CMT:", 5) == 0) {
    const char *first = strchr(line, '\"');
    const char *last = first == nullptr ? nullptr : strchr(first + 1, '\"');
    if (first != nullptr && last != nullptr) {
      size_t len = min(static_cast<size_t>(last - first - 1), sizeof(pendingSender) - 1);
      memcpy(pendingSender, first + 1, len);
      pendingSender[len] = '\0';
    }
    waitingSmsBody = true;
  } else if (waitingSmsBody) {
    processSms(pendingSender, line);
    waitingSmsBody = false;
    pendingSender[0] = '\0';
  }
}

void readGsm() {
  while (gsmSerial.available()) {
    char c = static_cast<char>(gsmSerial.read());
    if (c == '\r') continue;
    if (c == '\n') {
      if (gsmLineLength > 0) {
        gsmLine[gsmLineLength] = '\0';
        processGsmLine(gsmLine);
        gsmLineLength = 0;
      }
    } else if (gsmLineLength < sizeof(gsmLine) - 1) {
      gsmLine[gsmLineLength++] = c;
    }
  }
}

void setup() {
  Serial.begin(115200);
  Wire.begin(OLED_SDA, OLED_SCL);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Iniciando radar GSM");
  display.display();
  gpsSerial.begin(9600, SERIAL_8N1, GPS_RX_PIN, -1);
  gsmSerial.begin(9600, SERIAL_8N1, GSM_RX_PIN, GSM_TX_PIN);
  delay(800);
  sendAT("AT");
  sendAT("ATE0");
  sendAT("AT+CMGF=1");
  sendAT("AT+CNMI=2,2,0,0,0");
}

void loop() {
  while (gpsSerial.available()) gps.encode(gpsSerial.read());
  if (gps.location.isUpdated()) {
    centerLat = gps.location.lat();
    centerLon = gps.location.lng();
    centerValid = true;
    drawRadar();
  }
  readGsm();
}