#include <Wire.h>
#include <Adafruit_NeoPixel.h>

//#define TOF   // Comment out this line if the VL53L0X sensor is to be used.

#ifdef TOF
#include <VL53L0X.h>
#define LONG_RANGE
#endif

//
// Definitions, Constants, global variables
//
constexpr bool DEBUG {true};   // false to disable the debug output

class Timer {
public:
  void start() { timeStamp = millis(); }
  bool operator()(const unsigned long duration) const { return (millis() - timeStamp >= duration) ? true : false; }

private:
  unsigned long timeStamp {0};
};

struct ColorSegment {
  uint32_t check(uint16_t ledNum) const { return (ledNum < from || ledNum > to) ? 0 : color; }

  const uint8_t from;
  const uint8_t to;
  const uint32_t color;
};

#ifndef TOF
constexpr uint8_t R_ECHOPIN {9};
constexpr uint8_t R_TRIGPIN {8};
#endif

constexpr uint16_t MAX_DISTANCE_THESHOLD {170};   // Maximum specified measuring range (especially for the VL53L0X)
constexpr uint16_t DISTANCE_HYSTERESIS {5};

// Data pin for the WS2812B LED leds
constexpr uint8_t LEDPIN {6};
constexpr uint8_t MAX_LEDS {16};

constexpr uint16_t RANGESTARTMM {150};   // Range in mm
constexpr uint16_t RANGEENDMM {10};      // Range in mm

// initialize LEDs
// in this case 16 LEDs with addresses 0 to 15
Adafruit_NeoPixel ledStrip = Adafruit_NeoPixel(MAX_LEDS, LEDPIN, NEO_GRB + NEO_KHZ800);

// constexpr uint32_t MAX_LIGHT_DURATION {1000 * 60 * 2};  // 2 Minutes
constexpr uint32_t MAX_LIGHT_DURATION {10000};

// Sensorobject
#ifdef TOF
VL53L0X distanceSensor;
#endif

Timer timer;

 // 16 LEDs
ColorSegment colorSegments[] {
    {0,  7,        0xFF00  }, // Green
    {8,  11,       0xFFA500}, // Orange
    {12, MAX_LEDS, 0xFF0000}  // Red
};

 // 12 LEDs
// ColorSegment colorSegments[] {
//     {0, 5,        0xFF00  }, // Green
//     {6, 8,        0xFFA500}, // Orange
//     {9, MAX_LEDS, 0xFF0000}  // Red
// };

//
// Functions
//
void printDistPix(int16_t dist, int8_t pix) {
  Serial.print(F("distance = "));
  Serial.print(dist);
  Serial.print(" => ");
  Serial.print(F("lastPixel = "));
  Serial.println(pix);
}

#ifdef TOF
uint16_t checkDistance(VL53L0X &sensor) {
  // Read distance in mm
  uint16_t dist = sensor.readRangeSingleMillimeters();
  // If TIMEOUT then output error serially and set dist to 0
  if (distanceSensor.timeoutOccurred()) {
    Serial.println("TIMEOUT");
    dist = 0;
  }
  return dist / 10; // Return cm
}
#else
uint16_t checkDistance(uint8_t trig, uint8_t echo) {
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  return static_cast<uint16_t>(pulseIn(echo, HIGH) / 58);
}
#endif

template <size_t ITEMS> void setLeds(Adafruit_NeoPixel &leds, ColorSegment (&cs)[ITEMS], int8_t ledNum) {
  leds.clear();   //  Fill the whole NeoPixel leds with 0 / black / off
  if (ledNum > MAX_LEDS - 1) { ledNum = MAX_LEDS - 1; }
  if (ledNum > -1) {
    for (auto &c : cs) {
      auto activeColor = c.check(ledNum);
      if (activeColor) {
        for (auto i = 0; i <= ledNum; ++i) { leds.setPixelColor(i, activeColor); }
        break;
      }
    }
  }
  leds.show();   // Update LEDs
}

//
// Main program
//
void setup() {
  if (DEBUG) { Serial.begin(74880); }
  Wire.begin();
#ifdef TOF
  // Initialize and configure distance sensor
  if (!distanceSensor.init()) {
    if(DEBUG) { Serial.println(F("VL53L0X Sensor nicht gefunden! Ablauf angehalten.")); }
    while (1) {}
  }
  distanceSensor.setTimeout(500);
  distanceSensor.setMeasurementTimingBudget(100000);
  distanceSensor.setSignalRateLimit(0.1);
  distanceSensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
  distanceSensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
#else
  // Configure HC-SR04
  pinMode(R_TRIGPIN, OUTPUT);
  pinMode(R_ECHOPIN, INPUT);
#endif
  // Start LED ring and set brightness
  ledStrip.begin();
  ledStrip.setBrightness(100);   // Brightness 0-255
  ledStrip.show();
}

void loop() {
  static uint16_t prevDistance {0};
  static bool isTurnedOn {false};
#ifdef TOF
  uint16_t distance {checkDistance(distanceSensor)};
#else
  uint16_t distance {checkDistance(R_TRIGPIN, R_ECHOPIN)};
#endif
  // Limitation of the measuring range (distance), prevents unwanted led flickering
  if (distance < MAX_DISTANCE_THESHOLD && prevDistance != distance) {
    // Use a hysteresis to prevent a possible flickering of the LEDs
    if (abs(prevDistance - distance) > DISTANCE_HYSTERESIS) {
      prevDistance = distance;
      int8_t lastPixel = map(distance, RANGEENDMM, RANGESTARTMM, MAX_LEDS - 1, 0);
      if (DEBUG) { printDistPix(distance, lastPixel); }
      setLeds(ledStrip, colorSegments, lastPixel);
      timer.start();
      isTurnedOn = true;
    }
  }

  // Turn off LEDs when the timer has run out.
  if (timer(MAX_LIGHT_DURATION) && isTurnedOn) {
    if (DEBUG) { Serial.println(F("Switch Off")); }
    ledStrip.clear();
    ledStrip.show();
    isTurnedOn = false;
  }
}