/* Project 2 - Thursday 8am Group 1
ME 208
No AI was used */

#include <Servo.h>

Servo seedServo;

// Motor pins
const int motorA1 = 12;
const int motorA2 = 13;
const int motorB1 = 10;
const int motorB2 = 9;
const int enableA = 11;
const int enableB = 3;

// Ultrasonic sensor pins
const int trigPin = 6;
const int echoPin = 5;

// Servo positions
const int servoClosed = 0;
const int servoOpen = 90;

// Distance threshold (in cm)
const int stopDistance = 20;

// Timing
unsigned long lastSeedTime = 0;
const unsigned long seedInterval = 1000; // drop a seed every 3 seconds

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

  // Motor setup
  pinMode(motorA1, OUTPUT);
  pinMode(motorA2, OUTPUT);
  pinMode(motorB1, OUTPUT);
  pinMode(motorB2, OUTPUT);
  pinMode(enableA, OUTPUT);
  pinMode(enableB, OUTPUT);

  // Sensor setup
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Servo setup
  seedServo.attach(8);
  seedServo.write(servoClosed);

  // Set motor speed
  analogWrite(enableA, 180);
  analogWrite(enableB, 180);
}

void loop() {
  long distance = getDistance();
  

  if (distance < stopDistance) {
    // Obstacle detected
    stopMotors();
    delay(500);

    turnLeft();   // or turnRight();
    delay(1000);  // adjust turning time
    stopMotors();

    delay(500);
  } else {
    // Move forward
    moveForward();

    // Drop seeds periodically
    if (millis() - lastSeedTime >= seedInterval) {
      dropSeed();
      lastSeedTime = millis();
    }
  }
}

// ---- Movement Functions ----

void moveForward() {
  digitalWrite(motorA1, HIGH);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);
}

void stopMotors() {
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, LOW);
}

void turnLeft() {
  digitalWrite(motorA1, LOW);
  digitalWrite(motorA2, HIGH);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);
}

// ---- Seed Drop Function ----

void dropSeed() {
  seedServo.write(servoOpen);
  delay(500);
  seedServo.write(servoClosed);
  delay(500);
}

// ---- Ultrasonic Distance Function ----

long getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

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