#include <Wire.h>
#include "Adafruit_VL6180X.h"

// Modified example code from:
// https://learn.adafruit.com/adafruit-vl6180x-time-of-flight-micro-lidar-distance-sensor-breakout/wiring-and-test
// 
// 2021-07-23 add filter

Adafruit_VL6180X vl = Adafruit_VL6180X();
uint8_t status;
float range;
float smoothed;
int filterStrength;

void setup() 
{
	filterStrength = 3;			// average of 4 values
	Serial.begin(115200);
	while (!Serial) {
		delay(1); 
	} 
	Serial.println("Adafruit VL6180x test!");
	if (! vl.begin()) {
		Serial.println("Failed to find sensor");
		while (1);
	}
	Serial.println("Sensor found!");
}

void loop() 
{
	range = vl.readRange(); 	// using single shot ranging
	Filter(smoothed, range, filterStrength);
	status = vl.readRangeStatus(); 
	if (status == VL6180X_ERROR_NONE) { 
		Serial.print("(PRB:Z-"); Serial.print(smoothed, 2); Serial.println(")");  
	}
	delay(50); 					// range execution time up to 10 ms at 100 mm
}

void Filter(float &OrigVal, int NewVal, int Strength){
	OrigVal= ((OrigVal * Strength) + NewVal) / (Strength + 1);  
}