#include <Servo.h>

// Ultrasonic Sensor Pins
const int trigPin = 8;
const int echoPin = 7;

// Servo Motors
Servo horizontalServo;   // Horizontal Servo (X-axis movement)
Servo verticalServo;     // Vertical Servo (Y-axis movement)
const int servoPin = 9;
const int verticalServoPin = 10;

// Variables
long duration;
int distance;
int verticalAngle = 180; // Start at the top (180°) for vertical servo
bool scanning = false;

// Store distances for each horizontal scan line
String scanLines[19];  // Array to store sonar symbols for each line (up to 19 vertical scan positions)
String scanDistances[19];  // Array to store distance measurements for each scan position

void setup() {
  // Initialize Serial Monitor
  Serial.begin(9600);
  Serial.println("2D Scan Simulation with Vertical and Horizontal Servo Motors");
  Serial.println("Type 'scan' to start 2D sonar sweep.");

  // Initialize Pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Attach Servo Motors
  horizontalServo.attach(servoPin);           // Attach horizontal servo to pin 9
  verticalServo.attach(verticalServoPin);     // Attach vertical servo to pin 10
  horizontalServo.write(180);  // Start the horizontal servo at 180 degrees (right)
  verticalServo.write(verticalAngle);    // Start the vertical servo at 180 degrees (top)
}

void loop() {
  // Check for user input to start scanning
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim(); // Clean up the command input

    if (command.equalsIgnoreCase("scan")) {
      scanning = true;
      Serial.println("Starting 2D sonar sweep...");
    }
  }

  // Perform 2D scan if scanning is enabled
  if (scanning) {
    perform2DScan();
    scanning = false;  // Stop scanning after one sweep
    Serial.println("2D sonar sweep completed.");

    // Print the sonar image and distances after the scan
    printScanResults();
  }
}

// Function to measure the distance from the ultrasonic sensor
int measureDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2; // Convert to cm
}

// Function to perform the 2D sonar scan with both motors moving in 10° increments
void perform2DScan() {
  int lineIndex = 0;  // Index to store data in arrays

  // Scan at different vertical positions from top to bottom
  for (verticalAngle = 180; verticalAngle >= 80; verticalAngle -= 10) { // Start at 180° and move down to 0°
    verticalServo.write(verticalAngle);  // Move vertical servo down
    delay(500);  // Wait for the servo to move

    // Scan at different horizontal positions from right to left
    String scanLine = "";  // Initialize the scan line
    String scanDistanceLine = "";  // Initialize the distance line for this scan

    for (int angle = 180; angle >= 0; angle -= 10) { // 10° increments for horizontal sweep (right to left)
      horizontalServo.write(angle);  // Move horizontal servo
      delay(500);  // Allow servo to reach the angle

      // Measure distance
      distance = measureDistance();

      // Store the sonar output and distance measurement
      scanLine += displaySonar(distance);  // Add symbol (█ or ○)
      scanDistanceLine += String(distance) + " ";  // Add the distance value

    }

    // Store the completed scan line and distance line
    scanLines[lineIndex] = scanLine;
    scanDistances[lineIndex] = scanDistanceLine;
    lineIndex++;

    Serial.println(scanLine);  // Print the scan line (sonar symbols)
  }

  // Reset horizontal servo to 180 degrees (right)
  horizontalServo.write(180);
}

// Function to represent the sonar output with solid (█) for detected objects and hole (○) for empty space
String displaySonar(int distance) {
  if (distance <= 50) {
    return "█";  // Solid (object detected)
  } else {
    return "○";  // Hole (no object detected)
  }
}

// Function to print the scan results and distances
void printScanResults() {
  Serial.println("\n2D Sonar Scan Results:");
  
  // Print the scan image (sonar symbols)
  for (int i = 0; i < 19; i++) {
    if (scanLines[i] != "") {
      Serial.println(scanLines[i]);  // Print sonar symbols
    }
  }

  // Print the distances below the scan lines
  Serial.println("\nScan Distances (cm):");
  for (int i = 0; i < 19; i++) {
    if (scanDistances[i] != "") {
      Serial.println(scanDistances[i]);  // Print the corresponding distances
    }
  }
}