#include <Wire.h>
#include <Adafruit_VL53L0X.h> //use 0 for SDA, use 2 for SCL
#include <Adafruit_NeoPixel.h> // use pin 4
#include <Servo.h> // use pin 3

// Constants
const int SERVO_PIN = 3;
const int NEOPIXEL_PIN = 4;
const int NUM_PIXELS = 1;
const int MIN_DISTANCE = 10; // in mm
const int MAX_DISTANCE = 2000; // in mm
const int SMOOTHING_FACTOR = 10; // Number of readings for smoothing
const unsigned long BRIGHTNESS_INCREMENT_INTERVAL = 1000; // in ms
const int SERVO_INCREMENT = 2; // Smooth transition step size
const int SERVO_DELAY = 20; // Delay for smooth movement

// Global Variables
Adafruit_VL53L0X lox = Adafruit_VL53L0X();
Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_PIXELS, NEOPIXEL_PIN, NEO_GRB + NEO_KHZ800);
Servo flowerServo;
int distanceReadings[SMOOTHING_FACTOR];
int readIndex = 0;
int total = 0;
int averageDistance = 0;
unsigned long personDetectedTime = 0;
int ledBrightness = 0;
int currentServoPosition = 0; // Tracks the servo position

void setup() {
  Serial.begin(115200);
  // Initialize VL53L0X
  if (!lox.begin()) {
    Serial.println(F("Failed to boot VL53L0X"));
    while (1);
  }
  // Initialize NeoPixel strip
  strip.begin();
  strip.show(); // Initialize all pixels to 'off'
  // Initialize Servo
  flowerServo.attach(SERVO_PIN);
  flowerServo.write(0); // Start with flowers closed
  currentServoPosition = 0;
  // Initialize distance readings array
  for (int i = 0; i < SMOOTHING_FACTOR; i++) {
    distanceReadings[i] = 0;
  }
}

void loop() {
  // Read distance
  VL53L0X_RangingMeasurementData_t measure;
  lox.rangingTest(&measure, false);
  int distance = (measure.RangeStatus != 4) ? measure.RangeMilliMeter : MAX_DISTANCE + 1;

  // Update the readings array
  total -= distanceReadings[readIndex];
  distanceReadings[readIndex] = distance;
  total += distance;
  readIndex = (readIndex + 1) % SMOOTHING_FACTOR;
  averageDistance = total / SMOOTHING_FACTOR;

  // Map distance to servo angle smoothly
  int targetServoAngle = (averageDistance >= MIN_DISTANCE && averageDistance <= MAX_DISTANCE) 
                         ? map(averageDistance, MIN_DISTANCE, MAX_DISTANCE, 90, 0) 
                         : 0;

  // Smoothly move servo to the target position
  if (currentServoPosition < targetServoAngle) {
    currentServoPosition += SERVO_INCREMENT;
    if (currentServoPosition > targetServoAngle) currentServoPosition = targetServoAngle;
  } else if (currentServoPosition > targetServoAngle) {
    currentServoPosition -= SERVO_INCREMENT;
    if (currentServoPosition < targetServoAngle) currentServoPosition = targetServoAngle;
  }

  flowerServo.write(currentServoPosition);
  delay(SERVO_DELAY); // Small delay to slow movement

  // Handle LED brightness based on proximity duration
  if (averageDistance >= MIN_DISTANCE && averageDistance <= MAX_DISTANCE) {
    if (personDetectedTime == 0) {
      personDetectedTime = millis();
    } else if (millis() - personDetectedTime >= BRIGHTNESS_INCREMENT_INTERVAL && ledBrightness < 255) {
      ledBrightness += 5; // Increase brightness
      personDetectedTime = millis(); // Reset timer
    }
  } else {
    ledBrightness = 0; // Reset brightness
    personDetectedTime = 0; // Reset detection timer
  }

  // Update NeoPixel brightness
  strip.fill(strip.Color(ledBrightness, ledBrightness, ledBrightness), 0, NUM_PIXELS);
  strip.show();
}