// For Arduino R4 Minima with L298N driver

const int in1 = A0;  // Top array control 1
const int in2 = A1;  // Top array control 2
const int in3 = A2;  // Bottom array control 1
const int in4 = A3;  // Bottom array control 2

// Adjusted delay for 40 kHz
int delayTime = 8.6;  // 8.4 to 8.7  microseconds should get closer to 40 kHz

void setup() {
  // Set control pins as outputs
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  // Initialize all pins to LOW
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  Serial.begin(9600);
  Serial.println("Acoustic Levitator - Tuned for 40kHz");
}

void loop() {
  // Phase 1: Top array active in one direction, bottom array in opposite direction
  digitalWrite(in1, HIGH);  // Top array +
  digitalWrite(in2, LOW);   // Top array -
  digitalWrite(in3, LOW);   // Bottom array -
  digitalWrite(in4, HIGH);  // Bottom array +
  delayMicroseconds(delayTime);

  // Phase 2: Reverse the polarity on both arrays
  digitalWrite(in1, LOW);   // Top array -
  digitalWrite(in2, HIGH);  // Top array +
  digitalWrite(in3, HIGH);  // Bottom array +
  digitalWrite(in4, LOW);   // Bottom array -
  delayMicroseconds(delayTime);
}