#include "NewPing.h"

// ---------- Ultrasonic Sensor Pins ----------
#define TRIGGER_PIN_LEFT 9
#define ECHO_PIN_LEFT 10
#define TRIGGER_PIN_RIGHT 6
#define ECHO_PIN_RIGHT 7
#define MAX_DISTANCE_INCH 24.0
#define MAX_DISTANCE_CM (int)(MAX_DISTANCE_INCH / 0.393701)

// ---------- Solenoid Valve Pins ----------
#define LEFT_VALVE_PIN 4
#define RIGHT_VALVE_PIN 5

// ---------- Measurement and Control ----------
const float RAIL_LENGTH_INCH = 24.0;
const float POSITION_TOLERANCE_INCH = 0.4;
const int PULSE_DURATION_MS = 100;
const int PAUSE_BETWEEN_MOVES = 300;

// Function declarations
float averageDistanceInch(NewPing &sensor, int samples = 5);
float estimatePosition();
void moveLeftPulse();
void moveRightPulse();
void stopCarriage();
void checkSerialInput();



NewPing sonar_left(TRIGGER_PIN_LEFT, ECHO_PIN_LEFT, MAX_DISTANCE_CM);
NewPing sonar_right(TRIGGER_PIN_RIGHT, ECHO_PIN_RIGHT, MAX_DISTANCE_CM);

float targetPositionInch = -1;

void setup() {
  Serial.begin(9600);
  pinMode(LEFT_VALVE_PIN, OUTPUT);
  pinMode(RIGHT_VALVE_PIN, OUTPUT);
  stopCarriage();
  Serial.println("Enter target position (inches from left sensor):");
}

void loop() {
  checkSerialInput();

  if (targetPositionInch > 0) {
    float currentPosition = estimatePosition();

    if (currentPosition < 0) {
      Serial.println("⚠️ Invalid sensor readings.");
      return;
    }

    float error = targetPositionInch - currentPosition;

    Serial.print("Current: ");
    Serial.print(currentPosition, 2);
    Serial.print(" in, Target: ");
    Serial.print(targetPositionInch, 2);
    Serial.print(" in, Error: ");
    Serial.println(error, 2);

    if (abs(error) <= POSITION_TOLERANCE_INCH) {
      Serial.println("✅ Target reached.");
      stopCarriage();
      targetPositionInch = -1;
      Serial.println("Enter next target position (inches):");
    } else if (error > 0) {
      Serial.println("➡️ Moving RIGHT");
      moveRightPulse();
    } else {
      Serial.println("⬅️ Moving LEFT");
      moveLeftPulse();
    }

    delay(PAUSE_BETWEEN_MOVES);
  }
}

// ---------- Control Functions ----------
void moveLeftPulse() {
  digitalWrite(LEFT_VALVE_PIN, HIGH);
  digitalWrite(RIGHT_VALVE_PIN, LOW);
  delay(PULSE_DURATION_MS);
  stopCarriage();
}

void moveRightPulse() {
  digitalWrite(LEFT_VALVE_PIN, LOW);
  digitalWrite(RIGHT_VALVE_PIN, HIGH);
  delay(PULSE_DURATION_MS);
  stopCarriage();
}

void stopCarriage() {
  digitalWrite(LEFT_VALVE_PIN, HIGH);
  digitalWrite(RIGHT_VALVE_PIN, HIGH);
}

// ---------- Position Estimation ----------
float estimatePosition() {
  float dist_left = averageDistanceInch(sonar_left);
  float dist_right = averageDistanceInch(sonar_right);

  if (dist_left <= 0 || dist_right <= 0 || dist_left + dist_right > RAIL_LENGTH_INCH + 2) {
    return -1; // invalid
  }

  return dist_left; // position = distance from left
}

// ---------- Averaging Function ----------
float averageDistanceInch(NewPing &sensor, int samples = 5) {
  long sum = 0;
  int valid = 0;
  for (int i = 0; i < samples; i++) {
    int d = sensor.ping_cm();
    if (d > 0) {
      sum += d;
      valid++;
    }
    delay(20);
  }
  return valid > 0 ? (float)sum / valid * 0.393701 : -1;
}

// ---------- Serial Input ----------
void checkSerialInput() {
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();
    float val = input.toFloat();
    if (val > 0 && val < RAIL_LENGTH_INCH) {
      targetPositionInch = val;
      Serial.print("🎯 New target set: ");
      Serial.print(targetPositionInch, 2);
      Serial.println(" in");
    } else {
      Serial.println("❌ Invalid input. Enter a number between 0 and 24 (inches):");
    }
  }
}