/*
  Program title:
  VL53L5CXTOFScanner8x8grid_distandzonecount_v5
  Written by Jeff Ritchie with assistance from Open AI Codex
  Date: May 20, 2026

Prompt used to create this program:
  I am building a motion sensor using a Pololu VL53L5CX time-of-flight sensor
  connected by I2C to a Seeed Studio XIAO ESP32C3. The sensor uses SDA GPIO6
  and SCL GPIO7. I want to classify one person moving in front of the sensor as:
  no person present, standing still, moving left to right, moving right to left,
  approaching the sensor, or walking away from the sensor. The sensor is mounted
  at the bottom of a vertical NeoPixel display and angled upward. Revise the
  8x8 background-calibrated classifier so it uses horizontal position, depth,
  and foreground zone count together. Use x change for side-to-side motion, z
  change for approach/walk-away, and foreground zone count change as a second
  signal for approach/walk-away. Print the motion state, x, z, foreground zone
  count, and motion scores for debugging and Fab Academy documentation.
*/

#include <Wire.h>
#include <SparkFun_VL53L5CX_Library.h>

#define SDA_PIN 6
#define SCL_PIN 7

SparkFun_VL53L5CX sensor;
VL53L5CX_ResultsData data;

const int GRID_SIZE = 8;
const int ZONE_COUNT = 64;

const int MIN_VALID_MM = 150;
const int MAX_VALID_MM = 3000;

const int BACKGROUND_DELTA_MM = 300;
const int MIN_FOREGROUND_ZONES = 3;
const int FOREGROUND_WINDOW_MM = 800;

const float X_MOTION_THRESHOLD = 0.55;
const float Z_MOTION_THRESHOLD_MM = 90.0;
const float AREA_MOTION_THRESHOLD = 2.0;

const float STILL_X_THRESHOLD = 0.30;
const float STILL_Z_THRESHOLD_MM = 60.0;
const float STILL_AREA_THRESHOLD = 1.5;

const unsigned long CLASSIFY_INTERVAL_MS = 350;

int background[ZONE_COUNT];

float smoothX = -1;
float smoothZ = -1;
float smoothArea = -1;

float previousX = -1;
float previousZ = -1;
float previousArea = -1;

unsigned long lastClassifyTime = 0;

String motionState = "NO_PERSON";

// Function declarations help avoid Arduino prototype-generation issues.
void calibrateBackground();
void updateMotionClassifier();
int findClosestForegroundDistance();
bool isForeground(int index, int distance);
bool validDistance(int distance);
float smoothValue(float oldValue, float newValue, float amount);
void resetTracking();
void printState(float x, float z, float area, int closest, float approachScore, float awayScore, float sideScore);

void setup() {
  Serial.begin(115200);
  delay(1000);

  Serial.println();
  Serial.println("VL53L5CX 8x8 Motion Classifier: X + Z + Area");

  Wire.begin(SDA_PIN, SCL_PIN);
  Wire.setClock(400000);

  Serial.println("Initializing VL53L5CX...");

  if (sensor.begin() == false) {
    Serial.println("Sensor not found. Check wiring.");
    while (1) {
      delay(10);
    }
  }

  sensor.setResolution(64);
  sensor.setRangingFrequency(10);
  sensor.startRanging();

  Serial.println("Keep the area in front of the sensor empty.");
  Serial.println("Calibrating background in 3 seconds...");
  delay(3000);

  calibrateBackground();

  Serial.println("Background calibration complete.");
  Serial.println("Begin testing motion.");
}

void loop() {
  if (sensor.isDataReady()) {
    if (sensor.getRangingData(&data)) {
      updateMotionClassifier();
    }
  }

  delay(5);
}

void calibrateBackground() {
  const int samplesNeeded = 20;
  long sums[ZONE_COUNT];
  int counts[ZONE_COUNT];

  for (int i = 0; i < ZONE_COUNT; i++) {
    sums[i] = 0;
    counts[i] = 0;
    background[i] = MAX_VALID_MM;
  }

  int samplesCollected = 0;

  while (samplesCollected < samplesNeeded) {
    if (sensor.isDataReady()) {
      if (sensor.getRangingData(&data)) {
        for (int i = 0; i < ZONE_COUNT; i++) {
          int d = data.distance_mm[i];

          if (validDistance(d)) {
            sums[i] += d;
            counts[i]++;
          }
        }

        samplesCollected++;
        Serial.print(".");
      }
    }

    delay(10);
  }

  Serial.println();

  for (int i = 0; i < ZONE_COUNT; i++) {
    if (counts[i] > 0) {
      background[i] = sums[i] / counts[i];
    } else {
      background[i] = MAX_VALID_MM;
    }
  }
}

void updateMotionClassifier() {
  int closest = findClosestForegroundDistance();

  if (closest < 0) {
    resetTracking();
    motionState = "NO_PERSON";
    printState(-1, -1, 0, -1, 0, 0, 0);
    return;
  }

  float totalWeight = 0;
  float weightedX = 0;
  float weightedZ = 0;
  int foregroundZones = 0;

  for (int i = 0; i < ZONE_COUNT; i++) {
    int distance = data.distance_mm[i];

    if (!isForeground(i, distance)) {
      continue;
    }

    if (distance <= closest + FOREGROUND_WINDOW_MM) {
      int col = i % GRID_SIZE;
      float weight = (float)((closest + FOREGROUND_WINDOW_MM) - distance + 1);

      weightedX += col * weight;
      weightedZ += distance * weight;
      totalWeight += weight;
      foregroundZones++;
    }
  }

  if (foregroundZones < MIN_FOREGROUND_ZONES || totalWeight <= 0) {
    resetTracking();
    motionState = "NO_PERSON";
    printState(-1, -1, foregroundZones, closest, 0, 0, 0);
    return;
  }

  float currentX = weightedX / totalWeight;
  float currentZ = weightedZ / totalWeight;
  float currentArea = foregroundZones;

  if (smoothX < 0 || smoothZ < 0 || smoothArea < 0) {
    smoothX = currentX;
    smoothZ = currentZ;
    smoothArea = currentArea;

    previousX = currentX;
    previousZ = currentZ;
    previousArea = currentArea;

    motionState = "STANDING_STILL";
    printState(smoothX, smoothZ, smoothArea, closest, 0, 0, 0);
    return;
  }

  smoothX = smoothValue(smoothX, currentX, 0.35);
  smoothZ = smoothValue(smoothZ, currentZ, 0.35);
  smoothArea = smoothValue(smoothArea, currentArea, 0.35);

  unsigned long now = millis();

  if (now - lastClassifyTime >= CLASSIFY_INTERVAL_MS) {
    float deltaX = smoothX - previousX;
    float deltaZ = smoothZ - previousZ;
    float deltaArea = smoothArea - previousArea;

    float sideScore = abs(deltaX) / X_MOTION_THRESHOLD;

    float approachScore = 0;
    float awayScore = 0;

    if (deltaZ < 0) {
      approachScore += abs(deltaZ) / Z_MOTION_THRESHOLD_MM;
    }

    if (deltaArea > 0) {
      approachScore += abs(deltaArea) / AREA_MOTION_THRESHOLD;
    }

    if (deltaZ > 0) {
      awayScore += abs(deltaZ) / Z_MOTION_THRESHOLD_MM;
    }

    if (deltaArea < 0) {
      awayScore += abs(deltaArea) / AREA_MOTION_THRESHOLD;
    }

    bool mostlyStill =
      abs(deltaX) < STILL_X_THRESHOLD &&
      abs(deltaZ) < STILL_Z_THRESHOLD_MM &&
      abs(deltaArea) < STILL_AREA_THRESHOLD;

    if (mostlyStill) {
      motionState = "STANDING_STILL";
    } else if (approachScore > awayScore && approachScore > sideScore && approachScore >= 1.0) {
      motionState = "APPROACHING";
    } else if (awayScore > approachScore && awayScore > sideScore && awayScore >= 1.0) {
      motionState = "WALKING_AWAY";
    } else if (sideScore >= 1.0) {
      if (deltaX > 0) {
        motionState = "LEFT_TO_RIGHT";
      } else {
        motionState = "RIGHT_TO_LEFT";
      }
    } else {
      motionState = "STANDING_STILL";
    }

    previousX = smoothX;
    previousZ = smoothZ;
    previousArea = smoothArea;
    lastClassifyTime = now;

    printState(smoothX, smoothZ, smoothArea, closest, approachScore, awayScore, sideScore);
  }
}

int findClosestForegroundDistance() {
  int closest = MAX_VALID_MM + 1;

  for (int i = 0; i < ZONE_COUNT; i++) {
    int distance = data.distance_mm[i];

    if (isForeground(i, distance) && distance < closest) {
      closest = distance;
    }
  }

  if (closest == MAX_VALID_MM + 1) {
    return -1;
  }

  return closest;
}

bool isForeground(int index, int distance) {
  if (!validDistance(distance)) {
    return false;
  }

  return distance < background[index] - BACKGROUND_DELTA_MM;
}

bool validDistance(int distance) {
  return distance >= MIN_VALID_MM && distance <= MAX_VALID_MM;
}

float smoothValue(float oldValue, float newValue, float amount) {
  return oldValue + amount * (newValue - oldValue);
}

void resetTracking() {
  smoothX = -1;
  smoothZ = -1;
  smoothArea = -1;

  previousX = -1;
  previousZ = -1;
  previousArea = -1;

  lastClassifyTime = 0;
}

void printState(float x, float z, float area, int closest, float approachScore, float awayScore, float sideScore) {
  Serial.print("state=");
  Serial.print(motionState);

  Serial.print("  x=");
  Serial.print(x, 2);

  Serial.print("  z=");
  Serial.print(z, 0);
  Serial.print("mm");

  Serial.print("  area=");
  Serial.print(area, 1);

  Serial.print("  closest=");
  Serial.print(closest);
  Serial.print("mm");

  Serial.print("  approachScore=");
  Serial.print(approachScore, 2);

  Serial.print("  awayScore=");
  Serial.print(awayScore, 2);

  Serial.print("  sideScore=");
  Serial.println(sideScore, 2);
}