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

// Initialize LCD (I2C address 0x27, 20x4 display)
LiquidCrystal_I2C lcd(0x27, 20, 4);

// RTC object
RTC_DS3231 rtc;

// Photodiode sensor pins
const int leftSensorPin = A0;
const int rightSensorPin = A1;
const int upSensorPin = A2;
const int downSensorPin = A3;

// Simulated INA219 analog input pins (potentiometers)
const int voltagePotPin = A4;
const int currentPotPin = A5;

// Stepper motor control pins (TB6600 drivers)
const int dirPinH = 2;
const int stepPinH = 3;
const int dirPinV = 4;
const int stepPinV = 5;

// Pushbutton pins
const int pbLightMotor = 22;
const int pbVoltCurrent = 23;

// Step counting and angle tracking
long stepCountH = 0;
long stepCountV = 0;
const float degPerStep = 1.8;  // adjust according to microstepping

// Display mode selector
enum DisplayMode { LIGHT_MOTOR, VOLT_CURR };
DisplayMode currentDisplay = LIGHT_MOTOR;

// Movement control
const int stepDelay = 800;
const int threshold = 50;
const int lightLevelThreshold = 200;

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

  pinMode(pbLightMotor, INPUT_PULLUP);
  pinMode(pbVoltCurrent, INPUT_PULLUP);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Dual Axis Tracker");
  delay(2000);
  lcd.clear();

  if (!rtc.begin()) {
    lcd.setCursor(0, 0);
    lcd.print("RTC not found!");
    while (1);
  }
}

void loop() {
  int leftVal  = 1023 - analogRead(leftSensorPin);
  int rightVal = 1023 - analogRead(rightSensorPin);
  int upVal    = 1023 - analogRead(upSensorPin);
  int downVal  = 1023 - analogRead(downSensorPin);

  // Check light availability
  if (isSunlightAvailable(leftVal, rightVal, upVal, downVal)) {
    handleMovement(leftVal, rightVal, dirPinH, stepPinH, stepCountH);
    handleMovement(upVal, downVal, dirPinV, stepPinV, stepCountV);
  } else {
    trackUsingRTC();
  }

  // Display mode switching with pushbuttons
  if (digitalRead(pbLightMotor) == LOW) {
    currentDisplay = LIGHT_MOTOR;
    delay(200);
    lcd.clear();
  }
  if (digitalRead(pbVoltCurrent) == LOW) {
    currentDisplay = VOLT_CURR;
    delay(200);
    lcd.clear();
  }

  // Update display
  if (currentDisplay == LIGHT_MOTOR) {
    showLightMotorInfo(leftVal, rightVal, upVal, downVal);
  } else {
    showSimulatedINA219();
  }

  delay(200);
}

// Light availability check
bool isSunlightAvailable(int l, int r, int u, int d) {
  return (l > lightLevelThreshold || r > lightLevelThreshold || u > lightLevelThreshold || d > lightLevelThreshold);
}

// RTC fallback tracking logic
void trackUsingRTC() {
  DateTime now = rtc.now();
  int hour = now.hour();

  // Simplified sun position: 6am-6pm maps to 0-180°
  float hDeg = map(hour, 6, 18, 0, 180);
  if (hDeg < 0) hDeg = 0;
  if (hDeg > 180) hDeg = 180;

  float vDeg = 45;  // Constant vertical angle for now

  moveToAngle(hDeg, vDeg);
}

// Move both motors to target angles
void moveToAngle(float targetH, float targetV) {
  long targetStepsH = targetH / degPerStep;
  long targetStepsV = targetV / degPerStep;

  moveAxisToPosition(dirPinH, stepPinH, stepCountH, targetStepsH);
  moveAxisToPosition(dirPinV, stepPinV, stepCountV, targetStepsV);
}

// Move individual motor axis
void moveAxisToPosition(int dirPin, int stepPin, long &currentSteps, long targetSteps) {
  long stepsToMove = targetSteps - currentSteps;
  bool direction = stepsToMove > 0;
  for (long i = 0; i < abs(stepsToMove); i++) {
    moveStepper(dirPin, stepPin, direction, currentSteps);
  }
}

// Basic step function with counter update
void moveStepper(int dirPin, int stepPin, bool direction, long &counter) {
  digitalWrite(dirPin, direction ? HIGH : LOW);
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(stepDelay);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(stepDelay);

  counter += (direction ? 1 : -1);
  if (counter < 0) counter += long(360.0 / degPerStep);
  if (counter >= long(360.0 / degPerStep)) counter -= long(360.0 / degPerStep);
}

// Handle movement for one axis
void handleMovement(int val1, int val2, int dirPin, int stepPin, long &counter) {
  if (abs(val1 - val2) > threshold) {
    bool dir = (val1 < val2);
    moveStepper(dirPin, stepPin, dir, counter);
  }
}

// Light sensor + motor angle display
void showLightMotorInfo(int left, int right, int up, int down) {
  lcd.setCursor(0, 0);
  lcd.print("L:"); lcd.print(left);
  lcd.print(" R:"); lcd.print(right);

  lcd.setCursor(0, 1);
  lcd.print("U:"); lcd.print(up);
  lcd.print(" D:"); lcd.print(down);

  lcd.setCursor(0, 2);
  lcd.print("H:"); lcd.print(stepCountH * degPerStep, 0);
  lcd.print((char)223); lcd.print(" V:");
  lcd.print(stepCountV * degPerStep, 0); lcd.print((char)223);

  lcd.setCursor(0, 3);
  lcd.print("Mode: Light & Motor ");
}

// Voltage & Current simulated display
void showSimulatedINA219() {
  int voltRaw = analogRead(voltagePotPin);
  int currRaw = analogRead(currentPotPin);

  float voltage = map(voltRaw, 0, 1023, 0, 175) / 10.0;
  float current = map(currRaw, 0, 1023, 0, 500) / 1000.0;

  lcd.setCursor(0, 0);
  lcd.print("Simulated INA219   ");

  lcd.setCursor(0, 1);
  lcd.print("Voltage: "); lcd.print(voltage, 2); lcd.print(" V");

  lcd.setCursor(0, 2);
  lcd.print("Current: "); lcd.print(current, 3); lcd.print(" A");

  lcd.setCursor(0, 3);
  lcd.print("Mode: Voltage/Current");
}