#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// LCD I2C address and size
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Photodiode pins
const int leftPin = A0;
const int rightPin = A1;
const int upPin = A2;

// Stepper motor pins
const int dirPinH = 2;
const int stepPinH = 3;
const int dirPinV = 4;
const int stepPinV = 5;

const int stepDelay = 800;  // Microseconds delay between steps
const int threshold = 50;   // Light difference threshold for movement

void setup() {
  pinMode(dirPinH, OUTPUT);
  pinMode(stepPinH, OUTPUT);
  pinMode(dirPinV, OUTPUT);
  pinMode(stepPinV, OUTPUT);

  lcd.init();
  lcd.backlight();
  lcd.print("3-Sensor Tracker");
  delay(2000);
  lcd.clear();
}

void loop() {
  // Read and invert photodiode values
  int leftVal = 1023 - analogRead(leftPin);
  int rightVal = 1023 - analogRead(rightPin);
  int upVal = 1023 - analogRead(upPin);

  int horizDiff = leftVal - rightVal;
  int vertDiff = upVal - ((leftVal + rightVal) / 2); // Up vs average of L & R

  // Horizontal movement
  if (abs(horizDiff) > threshold) {
    if (leftVal > rightVal) {
      moveStepper(dirPinH, stepPinH, false); // Move LEFT
    } else {
      moveStepper(dirPinH, stepPinH, true);  // Move RIGHT
    }
  }

  // Vertical movement
  if (abs(vertDiff) > threshold) {
    if (vertDiff > 0) {
      moveStepper(dirPinV, stepPinV, false); // Move UP
    } else {
      moveStepper(dirPinV, stepPinV, true);  // Move DOWN
    }
  }

  // LCD Display with space clearing
  lcd.setCursor(0, 0);
  lcd.print("L:");
  lcd.print(leftVal);
  lcd.print("   ");  // Clear leftovers
  lcd.print("R:");
  lcd.print(rightVal);
  lcd.print("   ");

  lcd.setCursor(0, 1);
  lcd.print("U:");
  lcd.print(upVal);
  lcd.print("   ");
  lcd.print("VD:");
  lcd.print(vertDiff);
  lcd.print("   ");

  delay(200);
}

void moveStepper(int dirPin, int stepPin, bool direction) {
  digitalWrite(dirPin, direction ? HIGH : LOW);
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(stepDelay);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(stepDelay);
}