// Pit controller

#include <WiFi.h>
#include <esp_now.h>
#include <ESP32Servo.h>

// Define the pins for the two servos
#define SERVO1_PIN 9
#define SERVO2_PIN 7

// Define pulse widths (in microseconds)
#define DEFAULT_PULSE 2000    // Default position
#define ACTUATED_PULSE 1000   // Actuated position

// Define the reset time in milliseconds
#define RESET_TIME_MS 500

// Create two Servo objects for the two servos
Servo servo1;
Servo servo2;

// Global variables to manage servo reset timing
volatile bool servoActuated = false;     // Tracks if servos are currently actuated
unsigned long actuatedTimestamp = 0;       // Stores the time when the servos were actuated

// Updated ESP‑Now receive callback function with the correct signature
void OnDataRecv(const esp_now_recv_info_t *recv_info, const uint8_t *incomingData, int len) {
  // We expect one boolean (1 byte)
  bool command = false;
  if (len == sizeof(command)) {
    memcpy(&command, incomingData, sizeof(command));
    if (command) {
      // Actuate both servos to the actuated position
      servo1.writeMicroseconds(ACTUATED_PULSE);
      servo2.writeMicroseconds(ACTUATED_PULSE);
      // Record the time when the servos were actuated
      actuatedTimestamp = millis();
      servoActuated = true;
    } else {
      // Immediately reset both servos to the default position
      servo1.writeMicroseconds(DEFAULT_PULSE);
      servo2.writeMicroseconds(DEFAULT_PULSE);
      servoActuated = false;
    }
  }
}

void setup() {
  // Initialize WiFi in station mode (required for ESP‑Now)
  WiFi.mode(WIFI_STA);

  // Attach the servos to their respective pins and set to default position
  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servo1.writeMicroseconds(DEFAULT_PULSE);
  servo2.writeMicroseconds(DEFAULT_PULSE);

  // Initialize ESP‑Now
  if (esp_now_init() != ESP_OK) {
    // Optionally handle initialization errors
    return;
  }
  // Register the ESP‑Now receive callback function
  esp_now_register_recv_cb(OnDataRecv);
}

void loop() {
  // Check if the servos are in the actuated state and if RESET_TIME_MS has elapsed
  if (servoActuated && (millis() - actuatedTimestamp >= RESET_TIME_MS)) {
    // Reset both servos back to the default position after the delay
    servo1.writeMicroseconds(DEFAULT_PULSE);
    servo2.writeMicroseconds(DEFAULT_PULSE);
    servoActuated = false;
  }
  // Yield a little time for other tasks
  delay(10);
}