/*
  Project: Simple Theremin v1c
  Programmed by: ChatGPT and Jim F, Calgary AB
  Date Started: 22 Nov 2025
  Date Updated: 05 Dec 2025
*/

#define TRIG_PIN1 2   // Pitch sensor trig pin
#define ECHO_PIN1 3   // Pitch sensor echo pin
#define TRIG_PIN2 4   // Volume sensor trig pin
#define ECHO_PIN2 5   // Volume sensor echo pin

// Differential piezo drive pins
#define PIEZO_A 9
#define PIEZO_B 10

#define MAX_DISTANCE 400 // Max distance in cm

// Sensor smoothing
float pitchSmoothed = 0;
float volSmoothed   = 0;
float smoothingFactor = 0.2; // 0 = no smoothing, 1 = very smooth

// Frequency smoothing (portamento)
float freqCurrent = 55;      // starting frequency
float freqSmoothing = 0.1;   // lower = slower glide

// Volume smoothing
float amplitudeCurrent = 0;
float amplitudeSmoothing = 0.15; // lower = slower volume changes

void setup() {
  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);

  pinMode(PIEZO_A, OUTPUT);
  pinMode(PIEZO_B, OUTPUT);

  Serial.begin(9600);
  Serial.println("");
}

// Read distance in cm from ultrasonic sensor
long readDistanceCM(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 40000); // 40 ms timeout
  long distance = duration * 0.034 / 2;

  if (distance == 0 || distance > MAX_DISTANCE) distance = MAX_DISTANCE;
  return distance;
}

// Differential piezo tone output
void playToneDiff(int freq, int amplitude, int durationMs) {
  long period = 1000000L / freq;
  long highTime = (period / 2) * amplitude / 255;
  long lowTime  = (period / 2) - highTime;

  unsigned long start = millis();

  while (millis() - start < durationMs) {
    digitalWrite(PIEZO_A, HIGH);
    digitalWrite(PIEZO_B, LOW);
    delayMicroseconds(highTime);

    digitalWrite(PIEZO_A, LOW);
    digitalWrite(PIEZO_B, HIGH);
    delayMicroseconds(lowTime);
  }
}

void loop() {
  // Read distances from sensors
  long pitchDist = MAX_DISTANCE - readDistanceCM(TRIG_PIN1, ECHO_PIN1);
  delayMicroseconds(50);
  long volDist = MAX_DISTANCE - readDistanceCM(TRIG_PIN2, ECHO_PIN2);
  delayMicroseconds(50);

  // Smooth sensor readings
  pitchSmoothed += (pitchDist - pitchSmoothed) * smoothingFactor;
  volSmoothed   += (volDist - volSmoothed) * smoothingFactor;

  // Map pitch distance to target frequency
  int freqTarget = map(pitchSmoothed, 2, 400, 1500, 55);
  freqTarget = constrain(freqTarget, 55, 1500);

  // Map volume distance to target amplitude
  int amplitudeTarget = map(volSmoothed, 2, 400, 255, 0);
  amplitudeTarget = constrain(amplitudeTarget, 0, 255);

  // Smooth frequency (portamento)
  freqCurrent += (freqTarget - freqCurrent) * freqSmoothing;

  // Smooth amplitude
  amplitudeCurrent += (amplitudeTarget - amplitudeCurrent) * amplitudeSmoothing;

  // Play tone with smoothed frequency and amplitude
  playToneDiff((int)freqCurrent, (int)amplitudeCurrent, 15);

  // Serial output for debugging
  Serial.print((int)freqCurrent);
  Serial.print(",");
  Serial.println((int)amplitudeCurrent);
}