An ESP8266-based Parking Alignment Display

/*! \file Parking Monitor Display with Clock
 *
 * This ESP8266-based project drives a collection of WS2812B LED modules
 * to display information regarding the relative positioning of an automobile
 * with respect to its desired parking spot.
 *
 * The ESP8266 module's TCP/IP stack is exploited to automatically obtain the
 * time via NTP.  The current time is subsequently displayed when the system
 * is idle due to a lack of any vehicle movement.
 *
 * Support for Benewake's TF Mini-S LIDAR sensor and the very common
 * HC-SR04 ultrasonic module is provided.
 *
 * Typical configuration is to use the expensive but extremely precise
 * LIDAR sensor for determining the depth of the vehicle within the garage
 * and one or two of the very inexpensive but less accurate ultrasonic sensors
 * to monitor the left/right offset of the vehicle with respect to its
 * desired parking spot.
 *
 * The distance remaining and offset from the center of the parking spot
 * are displayed when a vehicle enters the garage.  Animated graphics also
 * provide a visual indication as to how much more distance needs to be
 * traveled.
 *
 * \sa http://www.fargos.net/parkingMonitor/parkingMonitor.html
 *
 * \author Geoff Carpenter gcc@fargos.net http://www.fargos.net/gcc.html
 * \date 2024/03/06 Initial public release
 */

#include <Adafruit_GFX.h>
#include <FastLED.h>
#include <FastLED_NeoMatrix.h>
#include <SoftwareSerial.h>
#include <sys/time.h>
#include <time.h>

/* Obtain local customizations */
#include "ParkingLocalConfig.h"

#ifndef ENABLE_NTP
#define ENABLE_NTP 1 /*!< Enable use of NTP to obtain current time */
#endif
#if ENABLE_NTP
#include <ESP8266WiFi.h>
#include <TZ.h>
#endif

#ifndef NUM_LED_ROWS
#define NUM_LED_ROWS 7 /*!< Number of rows of LEDs to control */
#endif
#ifndef NUM_LED_COLS
#define NUM_LED_COLS 50 /*!< Number of LEDs in each row */
#endif

#if NUM_LED_ROWS > 6
#include "segoeui_5.h" /* use segoeui_5.h when 7 strips are available */
#define FONT_ARRAY segoeui5pt7b
#else
#include "segoeui_4.h" /* use segoeui_4.h only 6 strips are available */
#define FONT_ARRAY segoeui4pt7b
#endif

/* NOTE:  all measurements are internally maintained as centimeters,
 * but can be displayed as inches on the LED panel.
 */
#ifndef USE_METRIC_DISPLAY
#define USE_METRIC_DISPLAY 0 /*!< Indicate if display should show centimeters rather than inches. */
#endif

#ifndef USE_LIDAR
#define USE_LIDAR 1 /*!< Enable use of TFmini-S LIDAR sensor */
#endif
#ifndef USE_ULTRASONIC
#define USE_ULTRASONIC 1 /*!< Enable use of HC-SR04 ultrasonic sensor */
#endif
#ifndef USE_DEPTH_SENSOR
#define USE_DEPTH_SENSOR 1 /*!< Enable use of depth sensor */
#endif
#ifndef USE_OFFSET_SENSORS
#define USE_OFFSET_SENSORS 2 /*!< Number of offset sensors to use */
#endif

#ifndef OFFSET1_SENSOR_ON_LEFT
#define OFFSET1_SENSOR_ON_LEFT 1 /*!< Indicates if sensor 1 is to left of vehicle */
#endif
#ifndef OFFSET2_SENSOR_ON_LEFT
#define OFFSET2_SENSOR_ON_LEFT 1 /*!< Indicates if sensor 2 is to left of vehicle */
#endif

#define ANIMATION_INTERVAL 100 /*!< Interval between animation frames in milliseconds */

#ifndef SIMULATE_LIDAR
#define SIMULATE_LIDAR 0 /*!< If non-zero, simulates LIDAR depth sensor for testomg.  Normally 0. */
#endif
#ifndef SIMULATE_ULTRASONIC
#define SIMULATE_ULTRASONIC 0 /*!< If non-zero, simulates ultrasonic offset sensors for testing.  Normally 0. */
#endif

#if ENABLE_NTP
#ifndef USE_WIFI_SSID
#error "Must define USE_WIFI_SSID in ParkingLocalConfig.h"
#endif
#ifndef USE_WIFI_PASSWORD
#error "Must define USE_WIFI_PASSWORD in ParkingLocalConfig.h"
#endif
#ifndef USE_LOCAL_TZ
#error "Must define USE_LOCAL_TZ in ParkingLocalConfig.h"
#endif
#ifndef USE_DEVICE_HOSTNAME
#define USE_DEVICE_HOSTNAME "ParkingMonitor" /*!< Hostname offered in DHCP request */
#endif
#endif /* ENABLE_NTP */

/* Parameters to define maximum expected values as derived from the
 * respective sensor data sheets.  Can be adjusted if desired, such as
 * if dealing with a single bay garage.
 */
#define MAX_DEPTH_CM  600 /*!< Maximum feasible value from depth sensor; TFmini-S technically capable of 12 meters, but accuracy degrades after 6 meters. */
#define MAX_OFFSET_CM 400 /*!< Maximum feasible value from offset sensor. */

/* Parameters to define minimum expected values; below indicates blocked sensor
 */
#ifndef MIN_DEPTH_CM
#define MIN_DEPTH_CM 10 /*!< Minimum feasible value from depth sensor. */
#endif
#ifndef MIN_OFFSET1_CM
#define MIN_OFFSET1_CM 2 /*!< Minimum feasible value from front offset sensor */
#endif
#ifndef MIN_OFFSET2_CM
#define MIN_OFFSET2_CM 2 /*!< Minimum feasible value from rear offset sensor */
#endif

/* Parameters to define the desired depth of the parking spot */
#ifndef MIN_DESIRED_DEPTH_CM
#define MIN_DESIRED_DEPTH_CM 40 /*!< Minimum physically-permitted depth within garage */
#endif
#ifndef MAX_DESIRED_DEPTH_CM
#define MAX_DESIRED_DEPTH_CM 500 /*!< Maximum reportable depth within garage */
#endif
#ifndef TARGET_DESIRED_DEPTH_CM
#define TARGET_DESIRED_DEPTH_CM 60 /*!< Desired stop depth */
#endif
#ifndef DESIRED_DEPTH_TOLERANCE
#define DESIRED_DEPTH_TOLERANCE_CM 10 /*!< Acceptable depth is TARGET_DESIRED_DEPTH_CM +/- DESIRED_DEPTH_TOLERANCE_CM */
#endif
#ifndef MIN_INTERESTING_DEPTH_CHANGE_CM
#define MIN_INTERESTING_DEPTH_CHANGE_CM 5 /*!< Amount of depth change required to be interesting */
#endif

/* Parameters to define desired offset to parking spot for sensor 1 */
#ifndef MIN_DESIRED_OFFSET1_CM
#define MIN_DESIRED_OFFSET1_CM 50 /*!< Minimum physically-permitted offset for sensor 1 */
#endif
#ifndef MAX_DESIRED_OFFSET1_CM
#define MAX_DESIRED_OFFSET1_CM 150 /*!< Maximum physically-permitted offset for sensor 1 */
#endif
#ifndef TARGET_DESIRED_OFFSET1_CM
#define TARGET_DESIRED_OFFSET1_CM 100 /*!< Desired offset from sensor 1 */
#endif

/* Parameters to define desired offset to parking spot for sensor 2 */
#ifndef MIN_DESIRED_OFFSET2_CM
#define MIN_DESIRED_OFFSET2_CM 50 /*!< Minimum physically-permitted offset for sensor 2 */
#endif
#ifndef MAX_DESIRED_OFFSET2_CM
#define MAX_DESIRED_OFFSET2_CM 150 /*!< Maximum physically-permitted offset for sensor 2 */
#endif
#ifndef TARGET_DESIRED_OFFSET2_CM
#define TARGET_DESIRED_OFFSET2_CM 100 /*!< Desired offset from sensor 2 */
#endif

#ifndef DESIRED_OFFSET_TOLERANCE_CM
#define DESIRED_OFFSET_TOLERANCE_CM 10 /*!< Tolerance for offset readings */
#endif
#ifndef MIN_INTERESTING_OFFSET_CHANGE_CM
#define MIN_INTERESTING_OFFSET_CHANGE_CM 5 /*!< Amount of offset change required to be interesting. */
#endif

#define IDLE_SECS                  20 /*!< Number of seconds of no movement before movement flags are cleared. */
#define SECONDS_UNTIL_DISPLAY_TIME 60 /*!< Number of seconds of no movement before current time of day is displayed */
#define ANNOUNCE_ARRIVE_DELAY      2  /*!< Number of seconds to hold static arrival message */
#define ANNOUNCE_DEPART_DELAY      5  /*!< Number of seconds to hold static departure message */
#define MAXIMUM_SENSOR_FAIL_COUNT  30 /*!< Number of unsuccessful requests before a sensor is declared as failed. */

#ifndef LOG_ENABLED
/*! \brief Enables debug output to serial console.
 *
 * Normally 0, but can be set to nonzero to enable debug output to
 * the serial console. 2 also outputs the file name instead of just the
 * line number.
 * \note This must be nonzero to enable any debug output.  All debug
 * output is inhibited if LOG_ENABLED is set to 0.
 * \sa ENABLE_CONSOLE_COMMANDS
 */
#define LOG_ENABLED 0
#endif
#ifndef ENABLE_CONSOLE_COMMANDS
#define ENABLE_CONSOLE_COMMANDS 0 /*!< Enable entering of debug commands via console serial line */
#endif
#if (LOG_ENABLED == 0) && (ENABLE_CONSOLE_COMMANDS != 0)
#error "LOG_ENABLED must be set when ENABLE_CONSOLE_COMMANDS is non-zero"
#endif
#ifndef DISPLAY_HEARTBEAT_PIXEL
#define DISPLAY_HEARTBEAT_PIXEL 0 /*!< If non-zero, moves a colored pixel along the LED strip during each time update */
#endif

/*! \brief Console baud rate
 *
 * This value should match the baud rate selected in the Arduino IDE's
 * serial monitor window or a dedicated terminal program like Putty.
 */
#define CONSOLE_BAUD_RATE 9600

/* These log interfaces are compatible with the advanced thread-safe
 * logging API made available by FARGOS Development, LLC.
 See http://www.fargos.net/documents/FARGOSutilsLibrary.html
 */
#if LOG_ENABLED > 0
#include <Streaming.h>
#if LOG_ENABLED == 2
#define LOG_COUT(level) Serial << F(__FILE__) << F(":") << __LINE__ << F("\t") << F(#level) << F("\t")
#else
#define LOG_COUT(level) Serial << F(":") << __LINE__ << F("\t") << F(#level) << F("\t")
#endif
#define LOG_ENDLINE endl
#endif

/*! Current sensor readings are displayed if the analog pin is fed 3.3 VDC.
 * Normal output is enabled if the analog pin is connected to ground.
 */
#define DEBUG_PIN A0
/* NOTE: D4 is also the LED pin */
#define LIDAR_RX_PIN D1 /*!< Receive pin for LIDAR sensor (its green wire) */
#define LIDAR_TX_PIN D2 /*!< Transmit pin for LIDAR sensor (its white wire) */

/*! Pin used to trigger measurement request for ultrasonic sensor 1 */
#define SONIC1_TRIGGER_PIN D3
/*! Pin used to time duration of the echo pin of ultrasonic sensor 1 being held high */
#define SONIC1_DATA_PIN D5

/*! Pin used to trigger measurement request for ultrasonic sensor 2 */
#define SONIC2_TRIGGER_PIN D6
/*! Pin used to time duration of the echo pin of ultrasonic sensor 2 being held high */
#define SONIC2_DATA_PIN D7

/*! Pin used to send stream of color encodings to string of WS2812B LED modules */
#define LED_CONTROL_PIN D0

#define DO_LIDAR_CONFIG             1       /*!< Indicates if LIDAR sensor should be reconfigured each startup */
#define SAVE_LIDAR_CONFIG           0       /*!< Indicates if current settings should be saved by LIDAR sensor */
#define DO_LIDAR_FACTORY_RESET      0       /*!< Set non-zero to force a factory reset of TFmini-S */
#define INITIAL_LIDAR_BAUD_RATE     9600    /*!< Factory setting is 115200 */
#define NEW_LIDAR_BAUD_RATE         9600    /*!< Set slow enough for reliable operation using SoftwareSerial */
#define ULTRASONIC_TRIGGER_DURATION 10      /*!< Duration of trigger pulse, minimum 10 microseconds */
#define ULTRASONIC_MAX_SAMPLE_TIME  50000UL /*!< maximum time to wait for response on echo line */

#define DEFAULT_BRIGHTNESS 32 /*!< max possible is 255 */
#ifndef PANEL_ORIGIN
/*! Default panel configuration is a zig-zag chain with the initial connection point being in
 * the upper right hand corner.
 */
#define PANEL_ORIGIN (NEO_MATRIX_TOP + NEO_MATRIX_RIGHT)
#endif
#ifndef STRIP_LAYOUT
/*! Default panel configuration is a collection of strips, connected in alternating back-and-forth
 * manner.
 */
#define STRIP_LAYOUT NEO_MATRIX_ZIGZAG
#endif

// This could also be defined as matrix->color(255,0,0) but those defines
// are meant to work for adafruit_gfx backends that are lacking color()
#define LED_BLACK 0

#define LED_RED_VERYLOW (3 << 11)
#define LED_RED_LOW     (7 << 11)
#define LED_RED_MEDIUM  (15 << 11)
#define LED_RED_HIGH    (31 << 11)

#define LED_GREEN_VERYLOW (1 << 5)
#define LED_GREEN_LOW     (15 << 5)
#define LED_GREEN_MEDIUM  (31 << 5)
#define LED_GREEN_HIGH    (63 << 5)

#define LED_BLUE_VERYLOW 3
#define LED_BLUE_LOW     7
#define LED_BLUE_MEDIUM  15
#define LED_BLUE_HIGH    31

#define LED_ORANGE_VERYLOW (LED_RED_VERYLOW + LED_GREEN_VERYLOW)
#define LED_ORANGE_LOW     (LED_RED_LOW + LED_GREEN_LOW)
#define LED_ORANGE_MEDIUM  (LED_RED_MEDIUM + LED_GREEN_MEDIUM)
#define LED_ORANGE_HIGH    (LED_RED_HIGH + LED_GREEN_HIGH)

#define LED_PURPLE_VERYLOW (LED_RED_VERYLOW + LED_BLUE_VERYLOW)
#define LED_PURPLE_LOW     (LED_RED_LOW + LED_BLUE_LOW)
#define LED_PURPLE_MEDIUM  (LED_RED_MEDIUM + LED_BLUE_MEDIUM)
#define LED_PURPLE_HIGH    (LED_RED_HIGH + LED_BLUE_HIGH)

#define LED_CYAN_VERYLOW (LED_GREEN_VERYLOW + LED_BLUE_VERYLOW)
#define LED_CYAN_LOW     (LED_GREEN_LOW + LED_BLUE_LOW)
#define LED_CYAN_MEDIUM  (LED_GREEN_MEDIUM + LED_BLUE_MEDIUM)
#define LED_CYAN_HIGH    (LED_GREEN_HIGH + LED_BLUE_HIGH)

#define LED_WHITE_VERYLOW (LED_RED_VERYLOW + LED_GREEN_VERYLOW + LED_BLUE_VERYLOW)
#define LED_WHITE_LOW     (LED_RED_LOW + LED_GREEN_LOW + LED_BLUE_LOW)
#define LED_WHITE_MEDIUM  (LED_RED_MEDIUM + LED_GREEN_MEDIUM + LED_BLUE_MEDIUM)
#define LED_WHITE_HIGH    (LED_RED_HIGH + LED_GREEN_HIGH + LED_BLUE_HIGH)

#define LED_RGB(r, g, b) (((r) << 1) | ((g) << 5) | (B))
#define LED_VERYLOW      3
#define LED_LOW          5
#define LED_MEDIUM       7
#define LED_HIGH         15
#define LED_VERYHIGH     31

#ifndef FORCE_INTENSE_COLORS
#define FORCE_INTENSE_COLORS 0 /*!< if non-zero, use bright colors regardless if debugging */
#endif
#if (LOG_ENABLED != 0) && (FORCE_INTENSE_COLORS == 0)
/* If connected to a computer's USB port, use low intensity colors for
 * reduced power requirements.
 */
#define LED_RED    LED_RED_LOW
#define LED_BLUE   LED_BLUE_LOW
#define LED_GREEN  LED_GREEN_LOW
#define LED_ORANGE LED_ORANGE_LOW
#else
#define LED_RED    LED_RED_HIGH
#define LED_BLUE   LED_BLUE_HIGH
#define LED_GREEN  LED_GREEN_HIGH
#define LED_ORANGE LED_ORANGE_HIGH
#endif

#define LED_TIME_COLOR LED_BLUE

#if ENABLE_CONSOLE_COMMANDS > 0
static int32_t forceDepth = -1;
static int32_t forceOffset = -1;
#endif

#if USE_LIDAR
/*! \brief Control and obtain object depth from a TFmini-S LIDAR sensor.
 */
class TFmini_S_LIDARsensor {
  public:
    enum {
      TFMINI_DEFAULT_BAUD_RATE = 115200, //!< factory default
      MAX_TFMINI_RESPONSE_TIME=50 //!< Maximum milliseconds to wait for measurement
    };

  protected:
    SoftwareSerial serialPort;
    uint16_t       selectedFrameRate;
    uint8_t        bfrIndex;
    uint8_t        failedCount;
    unsigned char  recvBfr[9];

    uint8_t computeChecksum(unsigned char bytes[], uint8_t len)
    {
        uint8_t checksum = 0;
        for (uint8_t i = 0; i < len; i += 1) {
            checksum += bytes[i];
        }
        return (checksum);
    }

    bool waitForData(unsigned long maxWait)
    {
        if (serialPort.available()) return (true);

        unsigned long curTime = millis();
        unsigned long stopTime = curTime + maxWait;
        while (curTime < stopTime) { // max wait time not yet reached
            if (serialPort.available()) return (true);
            curTime = millis();
        }
        return (false);
    }

    int8_t readResponse(uint8_t expectedLen, unsigned long maxWaitTime = 10)
    {
        bool dataAvailable;

        bfrIndex = 0;
        unsigned long stopMillis = millis() + maxWaitTime;
        do {
            while (serialPort.available()) {
                recvBfr[bfrIndex] = serialPort.read();
                bfrIndex += 1;
                switch (bfrIndex) {
                case 1:
                    if (recvBfr[0] != 0x5a) { // start of message not yet found
#if LOG_ENABLED
                        LOG_COUT(warn) << F("not header=") << recvBfr[0] << LOG_ENDLINE;
#endif
                        bfrIndex = 0; // restart
                    }
                    break;
                case 2:
                    if (recvBfr[1] != expectedLen) { // corrupted response
#if LOG_ENABLED
                        LOG_COUT(warn) << F("wrong len=") << recvBfr[1] << LOG_ENDLINE;
#endif
                        bfrIndex = 0; // reset
                    }
                    break;
                default:
                    if (bfrIndex == expectedLen) {
                        return (bfrIndex);
                    }
                    break;
                } // end switch bfrIndex
            }     // end while available
            unsigned long currentMillis = millis();
            if (currentMillis >= stopMillis) break;
            dataAvailable = waitForData(stopMillis - currentMillis);
        } while (dataAvailable);
        return (bfrIndex);
    }

    int8_t readDataFrame(unsigned long maxWaitTime)
    {
        do {
            while (serialPort.available()) {
                recvBfr[bfrIndex] = serialPort.read();
                switch (bfrIndex) {
                case 0:
                case 1:
                    if (recvBfr[bfrIndex] != 0x59) { // haven't found header of 0x59 0x59
                        bfrIndex = 0;
                        continue;
                    }
                    break;
                case 2:
                    if (recvBfr[2] == 0x59) { // assume stream of header markers
                        bfrIndex = 1;
                        continue;
                    }
                    break;
                case 8: {
                    uint16_t checksum = 0;

                    for (uint8_t j = 0; j < 8; j += 1) {
                        checksum += recvBfr[j];
                    }
                    bfrIndex = 0;
                    if (recvBfr[8] == (checksum & 255)) { // success
                        return (9);
                    }
                    // failed
#if LOG_ENABLED
                    LOG_COUT(error) << F("bad checksum=") << (int)(checksum & 255) << F(" vs ") << (int)recvBfr[8] << LOG_ENDLINE;
                    for (uint8_t i = 0; i <= 8; i += 1) {
                        LOG_COUT(info) << F("bfr[") << i << F("] = ") << recvBfr[i] << LOG_ENDLINE;
                    }
#endif
                    return (0);
                }
                    // NOTREACHED
                default:
                    break;
                } // end switch bfrIndex
                bfrIndex += 1;
            } // end while serial data available
            if (maxWaitTime != 0) {
                bool dataAvailable = waitForData(maxWaitTime);
                if (dataAvailable == false) break;
            }
        } while (maxWaitTime != 0);
        return (bfrIndex);
    }

  public:
    TFmini_S_LIDARsensor(uint8_t rxPin, uint8_t txPin) :
        serialPort(rxPin, txPin)
    {
        selectedFrameRate = 100; // default from specification
        failedCount = 0;
        bfrIndex = 0;
        pinMode(rxPin, INPUT);
        if (txPin != rxPin) {
            pinMode(txPin, OUTPUT);
        }
    }

    ~TFmini_S_LIDARsensor() {}

    void init(uint32_t baudRate = TFMINI_DEFAULT_BAUD_RATE)
    {
        // Initialize the data rate for the SoftwareSerial port
        serialPort.begin(baudRate);
    }

#if DO_LIDAR_CONFIG
    bool changeBaudRate(uint32_t newRate)
    {
        unsigned char cmd[8];

        uint32_t desiredRate = newRate;

        cmd[0] = 0x5a;
        cmd[1] = 0x08;
        cmd[2] = 0x06;
        cmd[3] = newRate & 255;
        newRate >>= 8;
        cmd[4] = newRate & 255;
        newRate >>= 8;
        cmd[5] = newRate & 255;
        newRate >>= 8;
        cmd[6] = newRate & 255;
        cmd[7] = computeChecksum(cmd, sizeof(cmd) - 1);
        serialPort.write(cmd, sizeof(cmd));

        bool dataAvail = waitForData(100); // wait a bit for response
        if (dataAvail == false) {
#if LOG_ENABLED
            LOG_COUT(error) << F("No response to baud rate change") << LOG_ENDLINE;
#endif
            return (false);
        }
        int8_t len = readResponse(8);
#if LOG_ENABLED
        LOG_COUT(info) << F("response len=") << len << LOG_ENDLINE;
#endif
        if (len == 8) {
#if LOG_ENABLED
            LOG_COUT(info) << F("Change baud rate response=") << recvBfr[2] << LOG_ENDLINE;
#endif
            if (recvBfr[2] == 6) { // was change baud rate response
#if LOG_ENABLED
                LOG_COUT(info) << F("saving settings") << LOG_ENDLINE;
#endif
#if SAVE_LIDAR_CONFIG
                saveSettings();
#endif
                serialPort.begin(desiredRate);
                return (true);
            }
        }
        return (false);
    }

    bool changeFrameRate(uint16_t newRate)
    {
        unsigned char cmd[6];

        selectedFrameRate = newRate;
        // send command to change LIDAR device frame rate
        cmd[0] = 0x5a;
        cmd[1] = 0x06;
        cmd[2] = 0x03;
        cmd[3] = newRate & 255;
        newRate >>= 8;
        cmd[4] = newRate & 255;
        cmd[5] = computeChecksum(cmd, sizeof(cmd) - 1);
        serialPort.write(cmd, sizeof(cmd));

        bool dataAvail = waitForData(100); // wait a bit for response
        if (dataAvail == false) {
#if LOG_ENABLED
            LOG_COUT(error) << F("No response to frame rate change") << LOG_ENDLINE;
#endif
            return (false);
        }
        int8_t len = readResponse(6);
#if LOG_ENABLED
        LOG_COUT(info) << F("response len=") << len << LOG_ENDLINE;
#endif
        if (len == 6) {
#if LOG_ENABLED
            LOG_COUT(info) << F("Change Frame Rate response=") << recvBfr[3] << LOG_ENDLINE;
#endif
            if (recvBfr[2] == 3) { // was change frame response
                return (true);
            }
        }
        return (false);
    }

#if DO_LIDAR_FACTORY_RESET
    bool restoreFactorySettings()
    {
        unsigned char cmd[4];
        cmd[0] = 0x5a;
        cmd[1] = 0x04;
        cmd[2] = 0x10;
        cmd[3] = 0x6e;
        serialPort.write(cmd, sizeof(cmd));

        bool dataAvail = waitForData(2000); // wait 2 seconds for response
        if (dataAvail == false) {
            LOG_COUT(error) << F("No response to reset to factory settings") << LOG_ENDLINE;
            return (false);
        }

        int8_t len = readResponse(5);
        if (len == 5) {
            LOG_COUT(info) << F("restore factory settings response=") << (int)recvBfr[3] << LOG_ENDLINE;
            if (recvBfr[2] == 0x10) {
                return (true);
            }
        }
        return (false);
    }
#endif

#if SAVE_LIDAR_CONFIG
    bool saveSettings()
    {
        unsigned char cmd[4];
        cmd[0] = 0x5a;
        cmd[1] = 0x04;
        cmd[2] = 0x11;
        cmd[3] = 0x6f;
        serialPort.write(cmd, sizeof(cmd));

        bool dataAvail = waitForData(2000); // wait 2 seconds for response
        if (dataAvail == false) {
            LOG_COUT(error) << F("No response to save settings") << LOG_ENDLINE;
            return (false);
        }

        int8_t len = readResponse(5);
        if (len == 5) {
            LOG_COUT(info) << F("Save settings response=") << (int)recvBfr[3] << LOG_ENDLINE;
            if (recvBfr[2] == 0x11) {
                return (true);
            }
        }
        return (false);
    }
#endif

    void requestMeasurement()
    {
        unsigned char cmd[4];
        cmd[0] = 0x5a;
        cmd[1] = 0x04;
        cmd[2] = 0x04;
        cmd[3] = 0x62;
        serialPort.write(cmd, sizeof(cmd));
    }
#endif

    bool sensorHasFailed() const
    {
        return (failedCount >= MAXIMUM_SENSOR_FAIL_COUNT);
    }

    bool getDistanceReading(uint16_t *distance, uint16_t *strength, uint16_t *temp = nullptr)
    {
#if SIMULATE_LIDAR
        /* generate simulate results rather than query a real sensor */
        static uint16_t simDistance = MAX_DEPTH_CM + 10;
        static int16_t  moveDirection = -1;
        static time_t   lastTime;
#if ENABLE_CONSOLE_COMMANDS > 0
        if (forceDepth != -1) {
            *distance = forceDepth;
            return (true);
        }
#endif
        unsigned long secondsSinceBoot = millis() / 1000;
        if (secondsSinceBoot != lastTime) {
            lastTime = secondsSinceBoot;
            if (simDistance < (MIN_DEPTH_CM / 2)) {
                moveDirection = 1;
            } else if (simDistance > (MAX_DEPTH_CM + 60)) {
                moveDirection = -1;
            } else if ((secondsSinceBoot > 360) && (moveDirection == 0)) {
                moveDirection = -2;
            } else if ((secondsSinceBoot > 300) && (secondsSinceBoot <= 360)) {
                moveDirection = 0;
            }
            if (secondsSinceBoot > 10) {
                simDistance += moveDirection;
            }
        }
        *distance = simDistance;
        if (strength != nullptr) {
            *strength = 200;
        }
        if (temp != nullptr) {
            *temp = 30;
        }
        return (true);
        /* NOTREACHED */
#endif /* SIMULATE_LIDAR */
        if (selectedFrameRate == 0) { // request measurement on demand
            requestMeasurement();
            bool dataArrived = waitForData(MAX_TFMINI_RESPONSE_TIME); // wait for response
            bfrIndex = 0;
            if (dataArrived == false) {
#if LOG_ENABLED
                LOG_COUT(warn) << F("No LIDAR response after request") << LOG_ENDLINE;
#endif
                if (failedCount < MAXIMUM_SENSOR_FAIL_COUNT) {
                    failedCount += 1;
                }
                return (false);
            }
        }
        int8_t readLen = readDataFrame((selectedFrameRate == 0) ? 2 : 0);
#if LOG_ENABLED
        if (readLen != 9) {
            LOG_COUT(error) << F("only got ") << readLen << F(" bytes in response") << LOG_ENDLINE;
            for (uint8_t i = 0; i < readLen; i += 1) {
                LOG_COUT(info) << F("bfr[") << i << F("] = ") << recvBfr[i] << LOG_ENDLINE;
            }
        }
#endif
        if (readLen == 9) { // entire data frame read
            *distance = recvBfr[2] + recvBfr[3] * 256;
            if (strength != nullptr) {
                *strength = recvBfr[4] + recvBfr[5] * 256;
            }
            if (temp != nullptr) {
                *temp = recvBfr[6] + recvBfr[7] * 256;
            }
            failedCount = 0; // reset
            return (true);
        }
#if LOG_ENABLED
        LOG_COUT(warn) << F("No LIDAR response bfrIndex=") << bfrIndex << LOG_ENDLINE;
#endif
        if (failedCount < MAXIMUM_SENSOR_FAIL_COUNT) {
            failedCount += 1;
        }
        return (false);
    }
}; // end class TFmini_S_LIDARsensor
#endif

#if USE_ULTRASONIC
/*! Poll HC-SR04 ultrasonic sensor for object distance.
 */
class HC_SR04sensor {
  protected:
    uint8_t triggerPin;
    uint8_t echoPin;
    uint8_t failedCount;

  public:
    HC_SR04sensor(uint8_t trigPin, uint8_t echoPin) :
        triggerPin(trigPin),
        echoPin(echoPin)
    {
        failedCount = 0;
    }

    ~HC_SR04sensor() {}

    void init()
    {
        pinMode(triggerPin, OUTPUT);
        pinMode(echoPin, INPUT);
        digitalWrite(triggerPin, LOW); // force low
    }

    bool sensorHasFailed() const { return (failedCount >= MAXIMUM_SENSOR_FAIL_COUNT); }

    bool getDistanceReading(uint32_t *distance, unsigned long *strength)
    {
#if SIMULATE_ULTRASONIC
        static uint32_t simOffset = MIN_OFFSET1_CM;
        static int32_t  dir = 1;
        static time_t   lastTime;
#if ENABLE_CONSOLE_COMMANDS > 0
        if (forceOffset != -1) {
            *distance = forceOffset;
            if (strength != nullptr) {
                *strength = 2000;
            }
            return (true);
        }
#endif
        unsigned long secondsSinceBoot = millis() / 1000;
        if (secondsSinceBoot != lastTime) {
            if (simOffset > (MAX_OFFSET_CM + 10)) dir = -1;
            else if (simOffset < (MIN_OFFSET1_CM / 2)) dir = 1;
            simOffset += dir;
        }
        *distance = simOffset;
        if (strength != nullptr) {
            *strength = 1000;
        }
        return (true);
#endif
        digitalWrite(triggerPin, HIGH); // raise trigger signal, must hold for at least 10 microseconds
        delayMicroseconds(ULTRASONIC_TRIGGER_DURATION);

        noInterrupts();                // disable interrupts for precise timing
        digitalWrite(triggerPin, LOW); // test triggered by signal being dropped low

        unsigned long duration = pulseIn(echoPin, HIGH, ULTRASONIC_MAX_SAMPLE_TIME);
        interrupts();
        if (duration == 0) { // failed reading
            if (strength != nullptr) {
                *strength = 0;
            }
#if LOG_ENABLED
            LOG_COUT(error) << F("No response on echo pin=") << echoPin << F(" trigger=") << triggerPin << LOG_ENDLINE;
#endif
            if (failedCount < MAXIMUM_SENSOR_FAIL_COUNT) {
                failedCount += 1;
            }
            return (false);
        }
#if LOG_ENABLED
        if (duration > 36000) {
            LOG_COUT(warn) << F("Duration too long=") << duration << LOG_ENDLINE;
        }
#endif
        failedCount = 0;
        float dist = (duration * .0343) / 2; // centimeters per microsecond, then halved
// inches per microsecond = 0.0135039, halved=0.00675195
#if LOG_ENABLED > 1
        LOG_COUT(info) << F("Distance=") << dist << F(" cm") << LOG_ENDLINE;
#endif
        *distance = static_cast<int>(dist);
        if (strength != nullptr) {
            *strength = duration;
        }
        return (true);
    }
};     // end class HC_SR04sensor
#endif /* USE_ULTRASONIC */

/*! \brief Control strips of WS2812B LED modules.
 *
 * Displays text and animated graphics on LED panel comprised of
 * multiple rows of WS2812B LED modules.
 *
 * \note Makes heavy use of FastLED_NeoMatrix.
 */
template <uint8_t DATA_PIN, int8_t COLS, int8_t ROWS = 1> class LEDstrips {
  public:
    enum {
        MAX_MESSAGE_DISPLAY_LEN = 128,
        MIN_UPDATE_INTERVAL_MILLIS = 100, //!< minimum update interval in milliseconds
        NUM_LEDS = ROWS * COLS,
        TIME_TEXT_X_ORIGIN = 4,
#if NUM_LED_ROWS > 6
        TEXT_Y_ORIGIN = 6,
#else
        TEXT_Y_ORIGIN = 5,
#endif
        MAX_DISPLAY_PHASES = 10,
        ANIMATION_PHASES = 4
    };
    enum eDisplayContent {
        SCROLL_BLOCK_LEFT = 1,
        SCROLL_BLOCK_RIGHT = 2,
        SCROLL_DIRECTION_MASK = (SCROLL_BLOCK_LEFT | SCROLL_BLOCK_RIGHT),
        SCROLL_LEFT_SIDE = 4,
        SCROLL_RIGHT_SIDE = 8,
        SCROLL_MIDDLE = 16,
        SCROLL_SIDE_MASK = (SCROLL_LEFT_SIDE | SCROLL_RIGHT_SIDE | SCROLL_MIDDLE),
        SCROLL_ENTIRE_BLOCK_LEFT = (SCROLL_LEFT_SIDE | SCROLL_RIGHT_SIDE | SCROLL_BLOCK_LEFT | SCROLL_MIDDLE),
        DISPLAY_TIME = 32,
        DISPLAY_MESSAGE = 64,
        DISPLAY_GRAPHIC = 128,
        DISPLAY_BLINKING_MESSAGE = 256,
        DISPLAY_SCROLLED_MESSAGE = (DISPLAY_MESSAGE | SCROLL_ENTIRE_BLOCK_LEFT),
        DISPLAY_LEFT_WARNING = (DISPLAY_GRAPHIC | SCROLL_LEFT_SIDE | SCROLL_BLOCK_RIGHT),
        DISPLAY_RIGHT_WARNING = (DISPLAY_GRAPHIC | SCROLL_RIGHT_SIDE | SCROLL_BLOCK_LEFT)

    };

  protected:
    CRGB              ledDataWithSafety[NUM_LEDS + 1];
    CRGB *const       ledData;
    FastLED_NeoMatrix matrix;
    uint16_t          messageColor;
    uint16_t          messageBackground;
    int16_t           messageText_X_origin;
    int16_t           markerDirection;
    uint16_t          markerColor;
    int16_t           marker_X_origin;
    uint16_t          targetBoxColor;
    int16_t           targetBox_X;
#if DISPLAY_HEARTBEAT_PIXEL
    uint16_t lastPixel;
    uint8_t  heartbeatColor;
#endif

  public:
    char     currentTimeText[12];
    char     depthMessage[16];
    char     offsetMessage[16];
    char     messageToDisplay[MAX_MESSAGE_DISPLAY_LEN];
    uint16_t activeContentMode;
    bool     timeChanged;

  protected:
    static constexpr int16_t XY(uint8_t x, uint8_t y)
    {
        // odd rows run backwards
        return ((y & 1) ? ((y * COLS) + (COLS - 1) - x) : ((y * COLS) + x));
    }

    static int16_t XYsafe(uint8_t x, uint8_t y)
    {
        if ((x >= COLS) || (y >= ROWS)) return (-1);
        return (XY(x, y));
    }

#if 0
  void moveBlockLeft1(uint8_t firstCol, uint8_t lastCol) {
    for (uint8_t c = firstCol; c < lastCol; c += 1) {
      for (uint8_t r = 0; r < ROWS; r += 1) {
        uint8_t destPixel = XYsafe(c, r);
        uint8_t sourcePixel = XYsafe(c + 1, r);
        ledData[destPixel] = ledData[sourcePixel];
      }
    }
    for (uint8_t r = 0; r < ROWS; r += 1) {
      uint8_t destPixel = XYsafe(lastCol, r);
      ledData[destPixel] = CRGB::Black;
    }
  }

  void moveBlockRight1(uint8_t firstCol, uint8_t lastCol) {
    for (uint8_t c = lastCol; c >= firstCol; c -= 1) {
      for (uint8_t r = 0; r < ROWS; r += 1) {
        uint8_t sourcePixel = XYsafe(c, r);
        uint8_t destPixel = XYsafe(c + 1, r);
        ledData[destPixel] = ledData[sourcePixel];
      }
    }
    for (uint8_t r = 0; r < ROWS; r += 1) {
      uint8_t destPixel = XYsafe(firstCol, r);
      ledData[destPixel] = CRGB::Black;
    }
  }
#endif

    void drawOffsetMarker(int32_t x, uint16_t color, int16_t dir, uint8_t phaseNumber)
    {
        //        LOG_COUT(info) << F("drawMarker x=") << x << F(" color=") << color << LOG_ENDLINE;
        if (phaseNumber == 0) {
            matrix.fillRect(0, 0, COLS, ROWS, LED_BLACK);
        }
        int x_dir = (dir == 0) ? 0 : ((dir >= 0) ? -1 : 1);
        int x_offset = phaseNumber * x_dir;

        int cur_x = x + x_offset;
        if (x_dir >= 0) {
            while (cur_x < (COLS / 2)) {
                for (int y = phaseNumber; y < (ROWS - phaseNumber); y += 1) {
                    matrix.drawPixel(cur_x, y, color);
                }
                cur_x += 5;
            }
        } else {
            while (cur_x > (COLS / 2)) {
                for (int y = phaseNumber; y < (ROWS - phaseNumber); y += 1) {
                    matrix.drawPixel(cur_x, y, color);
                }
                cur_x -= 5;
            }
        }
    }

    void drawTargetBox(int32_t x, uint16_t color, uint8_t phaseNumber)
    {
        int32_t h = ROWS - (phaseNumber * 2);
        if (h < 1) h = 1;
        matrix.drawRect(x + phaseNumber, phaseNumber, h, h, color);
    }

    void drawMessage(uint8_t phaseNumber)
    {
#if 0
    int16_t messageTextBox_X;
    int16_t messageTextBox_Y;
    uint16_t messageTextBox_width;
    uint16_t messageTextBox_height;

    matrix.getTextBounds(messageToDisplay, messageText_X_origin, TEXT_Y_ORIGIN,
                         &messageTextBox_X, &messageTextBox_Y,
                         &messageTextBox_width, &messageTextBox_height);
    // TODO:  use black for fill
    matrix.fillRect(messageTextBox_X, messageTextBox_Y,
                    messageTextBox_width, messageTextBox_height, messageBackground);
#endif
        matrix.fillRect(0, 0, COLS, ROWS, LED_BLACK);
        if ((phaseNumber & 1) == 0) { // even numbered phase
            matrix.setTextColor(messageColor, messageBackground);
            matrix.setCursor(messageText_X_origin, TEXT_Y_ORIGIN);
            matrix.print(messageToDisplay);
        }
#if LOG_ENABLED
        LOG_COUT(info) << F("phase=") << phaseNumber << F(" drawMessage x=") << messageText_X_origin << F(" mess=") << messageToDisplay << LOG_ENDLINE;
#endif
    }

    void drawTimeDisplay(uint8_t phaseNumber)
    {
#if LOG_ENABLED > 1
        unsigned long curTime = millis();
        LOG_COUT(info) << F("drawTimeDisplay millis=") << curTime << F(" timeChanged=") << timeChanged << F(" text=") << currentTimeText << LOG_ENDLINE;
#endif
        if (timeChanged == false) {
            return;
        }
        timeChanged = false;

        // clear bounding box
        matrix.fillRect(0, 0, COLS, ROWS, LED_BLACK);
        matrix.setTextColor(LED_TIME_COLOR, LED_BLACK);
        matrix.setCursor(TIME_TEXT_X_ORIGIN, TEXT_Y_ORIGIN);
        matrix.print(currentTimeText);
#if DISPLAY_HEARTBEAT_PIXEL
        ledData[lastPixel] = CRGB::Black;
        lastPixel += 1;
        if (lastPixel >= NUM_LEDS) {
            lastPixel = 0;
            heartbeatColor += 1;
            if (heartbeatColor >= 3) heartbeatColor = 0;
        }
        switch (heartbeatColor) {
        case 0:
            ledData[lastPixel] = CRGB::Red;
            break;
        case 1:
            ledData[lastPixel] = CRGB::Green;
            break;
        case 2:
            ledData[lastPixel] = CRGB::Blue;
            break;
        }
#if LOG_ENABLED
        LOG_COUT(info) << F("lastPixel=") << lastPixel << LOG_ENDLINE;
#endif
#endif /* DISPLAY_HEARTBEAT_PIXEL */
    }

  public:
    LEDstrips() :
        ledData(ledDataWithSafety + 1),
        matrix(ledData, COLS, ROWS, PANEL_ORIGIN + STRIP_LAYOUT)
    {
        messageColor = LED_RED;
        markerColor = LED_BLACK;
        timeChanged = false;
        depthMessage[0] = '\0';
        offsetMessage[0] = '\0';
    }

    void init()
    {
        // pinMode(LED_CONTROL_PIN, OUTPUT);
        FastLED.addLeds<WS2812B, DATA_PIN, GRB>(ledData, NUM_LEDS).setCorrection(TypicalSMD5050);

        matrix.begin();

        matrix.cp437(true); // use correct character codes
        matrix.setTextWrap(false);
        matrix.setBrightness(DEFAULT_BRIGHTNESS);
        matrix.fillScreen(LED_BLACK);

        matrix.setFont(&FONT_ARRAY);
        matrix.setTextSize(1);
#if DISPLAY_HEARTBEAT_PIXEL
        lastPixel = NUM_LEDS - 1;
        heartbeatColor = 0;
#endif
#if LOG_ENABLED
        LOG_COUT(info) << F("MAC=") << WiFi.macAddress() << LOG_ENDLINE;
#endif
        setMessage(WiFi.macAddress().c_str(), 0, LED_PURPLE_HIGH);
        for (int8_t i = 0; i < 10; i += 1) {
            drawMessage(0);
            FastLED.show();
            FastLED.delay(1000);
        }
    }

    void setMessage(const char *message, uint8_t origin_x = 0, uint16_t color = LED_BLUE, uint16_t background = LED_BLACK)
    {
        if (message[0] == '\0') {
            activeContentMode = DISPLAY_TIME;
            messageToDisplay[0] = '\0';
            return;
        }
        strncpy(messageToDisplay, message, sizeof(messageToDisplay) - 1);
        messageToDisplay[sizeof(messageToDisplay) - 1] = '\0';
        messageText_X_origin = origin_x;
        messageColor = color;
        messageBackground = background;
        activeContentMode = DISPLAY_MESSAGE;
    }

    void setOffsetLine(int16_t x, int32_t relativeOffset, uint16_t color, uint8_t sensorId)
    {
        marker_X_origin = x;
        markerDirection = static_cast<int16_t>(relativeOffset);
        markerColor = color;
    }

    void setTargetBoxOrigin(int16_t x, uint16_t color)
    {
        targetBox_X = x;
        targetBoxColor = color;
    }

    void refreshDisplay(uint8_t phaseNumber)
    {
#if LOG_ENABLED > 1
        LOG_COUT(info) << F("activeContentMode=") << activeContentMode << F(" phase=") << phaseNumber << LOG_ENDLINE;
#endif
        switch (activeContentMode) {
        case DISPLAY_TIME:
            if (phaseNumber == 0) {
#if LOG_ENABLED
                LOG_COUT(info) << F("DISPLAY_TIME phase=") << phaseNumber << LOG_ENDLINE;
#endif
                drawTimeDisplay(phaseNumber);
            }
            break;
        case DISPLAY_MESSAGE:
        case DISPLAY_BLINKING_MESSAGE:
            if (phaseNumber < (MAX_DISPLAY_PHASES - ANIMATION_PHASES)) {
#if LOG_ENABLED > 1
                LOG_COUT(info) << F("DISPLAY_MESSAGE") << LOG_ENDLINE;
#endif
                if ((phaseNumber == 0) || (activeContentMode == DISPLAY_BLINKING_MESSAGE)) {
                    drawMessage(phaseNumber);
                }
            } else {
                uint8_t drawPhase = phaseNumber - (MAX_DISPLAY_PHASES - ANIMATION_PHASES);
                if (markerColor != LED_BLACK) {
                    drawOffsetMarker(marker_X_origin, markerColor, markerDirection, drawPhase);
                }
                if (targetBoxColor != LED_BLACK) {
                    if (drawPhase > 0) { // erase previous rectangle outline
                        drawTargetBox(targetBox_X, LED_BLACK, drawPhase - 1);
                    }
                    drawTargetBox(targetBox_X, targetBoxColor, drawPhase);
                }
            }
            break;
        case DISPLAY_SCROLLED_MESSAGE:
            // FALLTHROUGH for now
        default:
#if LOG_ENABLED
            LOG_COUT(warn) << F("Unsupported display mode=") << activeContentMode << LOG_ENDLINE;
#endif
            break;
        }
    }
}; // end class LEDstrips<>

template <uint8_t DATA_PIN, int8_t COLS, int8_t ROWS> class ParkingMonitor {
  protected:
    enum eDisplayMode {
        NOT_MOVING = 0,
        MOVING_FORWARD = 1,
        MOVING_BACKWARD = 2,
        MOVING_LEFT = 4,
        MOVING_RIGHT = 8,
        MOVEMENT_MASK = MOVING_FORWARD | MOVING_BACKWARD | MOVING_LEFT | MOVING_RIGHT,
        DISPLAY_CURRENT_TIME = 16,
        DISPLAY_DEPTH = 32,
        DISPLAY_DEPTH_MOVEMENT_MASK = MOVING_FORWARD | MOVING_BACKWARD,
        DISPLAY_OFFSET = 64,
        DISPLAY_INITIAL = 128,
        DISPLAY_ARRIVING = DISPLAY_DEPTH | MOVING_FORWARD | DISPLAY_INITIAL,
        DISPLAY_DEPARTING = DISPLAY_DEPTH | MOVING_BACKWARD | DISPLAY_INITIAL,
        DISPLAY_MASK = DISPLAY_CURRENT_TIME | DISPLAY_DEPTH | DISPLAY_OFFSET | DISPLAY_INITIAL,
        DISPLAY_STOP_HERE = 256,
        DISPLAY_CAUTION = 512,
        DISPLAY_TOO_FAR = 1024,
        DISPLAY_BLOCKED_SENSOR = 2048,
        DISPLAY_FAILED_SENSOR = 4096,
        DISPLAY_ALERT_MASK_WITHOUT_BLOCKED = DISPLAY_STOP_HERE | DISPLAY_CAUTION | DISPLAY_TOO_FAR | DISPLAY_FAILED_SENSOR,
        DISPLAY_ALERT_MASK_WITHOUT_FAILED = DISPLAY_STOP_HERE | DISPLAY_CAUTION | DISPLAY_TOO_FAR | DISPLAY_BLOCKED_SENSOR,
        DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED = DISPLAY_STOP_HERE | DISPLAY_CAUTION | DISPLAY_TOO_FAR,
        DISPLAY_ALERT_MASK = DISPLAY_ALERT_MASK_WITHOUT_BLOCKED | DISPLAY_BLOCKED_SENSOR
    };
    enum eSensorIdMask { DEPTH_SENSOR_MASK = 1, OFFSET1_SENSOR_MASK = 2, OFFSET2_SENSOR_MASK = 4 };

    LEDstrips<DATA_PIN, COLS, ROWS> ledController;
#if USE_DEPTH_SENSOR
    TFmini_S_LIDARsensor depthDevice;
#endif
#if USE_OFFSET_SENSORS > 0
    HC_SR04sensor offsetDevice1;
#if USE_OFFSET_SENSORS > 1
    HC_SR04sensor offsetDevice2;
#endif
#endif
    time_t  lastTime;
    time_t  lastUpdateDepthTime;
    time_t  lastUpdateOffsetTime;
    time_t  nextIdleTime;
    time_t  timeUntilDisplayChange;
    int32_t lastDepth;
#if USE_OFFSET_SENSORS > 0
    int32_t lastOffset[USE_OFFSET_SENSORS];
#endif
    uint16_t displayMode;
    uint8_t  displayPhase;
    uint8_t  blockedSensorMask;
    uint8_t  failedSensorMask;

  public:
    enum { MAX_DISPLAY_PHASES = LEDstrips<DATA_PIN, COLS, ROWS>::MAX_DISPLAY_PHASES };

    ParkingMonitor() :
#if USE_DEPTH_SENSOR
        depthDevice(LIDAR_RX_PIN, LIDAR_TX_PIN),
#endif
#if USE_OFFSET_SENSORS > 0
        offsetDevice1(SONIC1_TRIGGER_PIN, SONIC1_DATA_PIN),
#if USE_OFFSET_SENSORS > 1
        offsetDevice2(SONIC2_TRIGGER_PIN, SONIC2_DATA_PIN),
#endif
#endif
        lastTime(0),
        lastUpdateDepthTime(0),
        lastUpdateOffsetTime(0),
        nextIdleTime(0),
        timeUntilDisplayChange(0),
        displayMode(DISPLAY_CURRENT_TIME),
        displayPhase(0),
        blockedSensorMask(0),
        failedSensorMask(0)
    {
#if USE_OFFSET_SENSORS > 0
        for (uint8_t i = 0; i < USE_OFFSET_SENSORS; i += 1) {
            lastOffset[i] = 0;
        }
#endif
    }

    void init()
    {
#if USE_DEPTH_SENSOR
#if LOG_ENABLED
        LOG_COUT(info) << F("Initial LIDAR baud rate=") << INITIAL_LIDAR_BAUD_RATE << F(" desired baud rate=") << NEW_LIDAR_BAUD_RATE << LOG_ENDLINE;
#endif
        uint32_t currentBaudRate = INITIAL_LIDAR_BAUD_RATE;
        depthDevice.init(currentBaudRate);
#if DO_LIDAR_FACTORY_RESET
        bool resetSuccessful = false;
        for (int t = 0; t < 5; t += 1) {
#if LOG_ENABLED
            LOG_COUT(info) << F("Attempt reset rate=" << currentBaudRate << " attempt=" << t << LOG_ENDLINE;
#endif
            resetSuccessful = depthDevice.restoreFactorySettings();
            if (resetSuccessful) {
                depthDevice.init(); // reinit with default baud rate
                currentBaudRate = depthDevice.TF_MINI_DEFAULT_BAUD_RATE;
                break;
            }
        }
#endif /* DO_LIDAR_FACTORY_RESET */
        if (currentBaudRate != NEW_LIDAR_BAUD_RATE) {
            for (int t = 0; t < 5; t += 1) {
#if LOG_ENABLED
                LOG_COUT(info) << F("Change baud rate to ") << NEW_LIDAR_BAUD_RATE << LOG_ENDLINE;
#endif
                bool ok = depthDevice.changeBaudRate(NEW_LIDAR_BAUD_RATE);
                if (ok) {
                    break;
                }
#if LOG_ENABLED
                LOG_COUT(warn) << F("Failed to change baud rate attempt=") << t << LOG_ENDLINE;
#endif
                delay(1000); // delay 1 second
            }
        }
#if DO_LIDAR_CONFIG
        for (int t = 0; t < 5; t += 1) {
#if LOG_ENABLED
            LOG_COUT(info) << F("Change frame rate") << LOG_ENDLINE;
#endif
            bool ok = depthDevice.changeFrameRate(0); // 0 = request on demand
            if (ok) {
#if SAVE_LIDAR_CONFIG
#if LOG_ENABLED
                LOG_COUT(info) << F("saving settings") << LOG_ENDLINE;
#endif
                depthDevice.saveSettings();
#endif /* SAVE_LIDAR_CONFIG */
                break;
            }
#if LOG_ENABLED
            LOG_COUT(warn) << F("Failed to change frame rate attempt=") << t << LOG_ENDLINE;
#endif
            delay(1000); // delay 1 second
        }
#endif /* DO_LIDAR_CONFIG */
#endif /* USE_DEPTH_SENSOR */
        ledController.init();
#if USE_OFFSET_SENSORS > 0
        offsetDevice1.init();
#if USE_OFFSET_SENSORS > 1
        offsetDevice2.init();
#endif /* USE_OFFSET_SENSORS > 1 */
#endif /* USE_OFFSET_SENSORS > 0 */
#if ENABLE_NTP
        configTime(USE_LOCAL_TZ, "pool.ntp.org");
        WiFi.mode(WIFI_STA);
        WiFi.setHostname(USE_DEVICE_HOSTNAME);
        WiFi.begin(USE_WIFI_SSID, USE_WIFI_PASSWORD);
#if LOG_ENABLED
        LOG_COUT(info) << F("MACaddress=") << WiFi.macAddress() << LOG_ENDLINE;
#endif
#endif /* ENABLE_NTP */
    }

    void updateDepth(time_t now, uint32_t newDepth)
    {
        int32_t delta = lastDepth - newDepth;
        int32_t abs_delta = (delta >= 0) ? delta : -delta;
        if (abs_delta < MIN_INTERESTING_DEPTH_CHANGE_CM) { // no useful change
            if ((now - lastUpdateDepthTime) > IDLE_SECS) {
#if LOG_ENABLED
                LOG_COUT(info) << F("no depth change in secs=") << now - lastUpdateDepthTime << F(" lastUpdate=") << lastUpdateDepthTime << LOG_ENDLINE;
#endif
                displayMode &= ~(DISPLAY_INITIAL | DISPLAY_DEPTH_MOVEMENT_MASK | DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED); // clear movement flags
#if LOG_ENABLED
                LOG_COUT(info) << F("displayMode now=") << displayMode << LOG_ENDLINE;
#endif
                //                displayMode |= DISPLAY_CURRENT_TIME;
            }
            return; // no interesting change
        }
#if LOG_ENABLED
        LOG_COUT(info) << F("newDepth=") << newDepth << LOG_ENDLINE;
#endif
        lastDepth = newDepth;
        lastUpdateDepthTime = now;
        nextIdleTime = now + SECONDS_UNTIL_DISPLAY_TIME;

        if ((displayMode & (DISPLAY_INITIAL | DISPLAY_DEPTH_MOVEMENT_MASK)) == 0) {
            // no movement previously detected
            displayMode |= DISPLAY_INITIAL;
#if LOG_ENABLED
            LOG_COUT(info) << F("Set INITIAL depth=") << newDepth << F(" displayMode=") << displayMode << F(" text=") << ledController.depthMessage
                           << LOG_ENDLINE;
#endif
        }
        displayMode &= ~(MOVING_FORWARD | MOVING_BACKWARD);
        displayMode |= (delta >= 0) ? (DISPLAY_DEPTH | MOVING_FORWARD) : (DISPLAY_DEPTH | MOVING_BACKWARD);

        int32_t     depthRemaining = newDepth - TARGET_DESIRED_DEPTH_CM;
        const char *direction = (depthRemaining >= 0) ? "F" : "B";

        snprintf(ledController.depthMessage, sizeof(ledController.depthMessage),
#if USE_METRIC_DISPLAY
                 "%d cm %s", depthRemaining,
#else
                 "%.0f %s", depthRemaining * 0.3937,
#endif
                 direction);
        if ((displayMode & DISPLAY_CURRENT_TIME) != 0) { // showing time
            displayMode &= ~DISPLAY_CURRENT_TIME;        // stop display of time
        }

        blockedSensorMask &= ~DEPTH_SENSOR_MASK; // clear blocked depth sensor flag
        if ((blockedSensorMask == 0) && (displayMode & DISPLAY_BLOCKED_SENSOR)) {
            displayMode &= ~DISPLAY_BLOCKED_SENSOR; // clear flag
        }
        if (newDepth < MIN_DEPTH_CM) {
            displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED | DISPLAY_CURRENT_TIME);
            displayMode |= DISPLAY_BLOCKED_SENSOR;
            blockedSensorMask |= DEPTH_SENSOR_MASK;
            ledController.depthMessage[0] = '\0';
            strcpy(ledController.offsetMessage, "L1 blocked");
            return;
        } else if (newDepth <= (MIN_DESIRED_DEPTH_CM + DESIRED_DEPTH_TOLERANCE_CM)) {
            displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED | DISPLAY_CURRENT_TIME);
            displayMode |= DISPLAY_TOO_FAR;
        } else if (newDepth <= (TARGET_DESIRED_DEPTH_CM + DESIRED_DEPTH_TOLERANCE_CM)) {
            displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED | DISPLAY_CURRENT_TIME);
            displayMode |= DISPLAY_STOP_HERE;
        } else if (newDepth <= (MAX_DESIRED_DEPTH_CM)) { // valid data
            displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED | DISPLAY_CURRENT_TIME);
            displayMode |= DISPLAY_CAUTION;
        } else if (newDepth <= MAX_DEPTH_CM) { // valid depth
            displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED | DISPLAY_CURRENT_TIME);
        } else { // too far away
#if LOG_ENABLED
            LOG_COUT(info) << F("depth too far away") << LOG_ENDLINE;
#endif
            displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED | DISPLAY_DEPTH | DISPLAY_INITIAL | DISPLAY_DEPTH_MOVEMENT_MASK);
            displayMode |= DISPLAY_CURRENT_TIME; // set display time flag
        }
#if LOG_ENABLED
        LOG_COUT(info) << F("delta=") << delta << F(" depth=") << newDepth << F(" displayMode=") << displayMode << LOG_ENDLINE;
#endif
    }

    void displaySensorData()
    {
        char bfr[32];

        snprintf(bfr, sizeof(bfr),
#if USE_DEPTH_SENSOR
                 "%d "
#endif
#if USE_OFFSET_SENSORS > 0
                 "%d "
#endif
#if USE_OFFSET_SENSORS > 1
                 "%d"
#endif
#if USE_DEPTH_SENSOR
                 ,
                 lastDepth
#endif
#if USE_OFFSET_SENSORS > 0
                 ,
                 lastOffset[0]
#endif
#if USE_OFFSET_SENSORS > 1
                 ,
                 lastOffset[1]
#endif
        );
        ledController.setMessage(bfr);
        ledController.refreshDisplay(0);
        FastLED.show();
    }

    void updateOffset(time_t now, uint32_t newOffset, uint8_t sensorId)
    {
        int32_t delta = lastOffset[sensorId] - newOffset;
        int32_t abs_delta = (delta >= 0) ? delta : -delta;
        if (abs_delta < MIN_INTERESTING_OFFSET_CHANGE_CM) { // no change
            if ((now - lastUpdateOffsetTime) > IDLE_SECS) {
                displayMode &= ~(MOVING_LEFT | MOVING_RIGHT); // clear movement flags
            }
            return;
        }
#if LOG_ENABLED
        LOG_COUT(info) << F("set newOffset=") << newOffset << LOG_ENDLINE;
#endif
        lastOffset[sensorId] = newOffset;
        lastUpdateOffsetTime = now;
        blockedSensorMask &= ~(OFFSET1_SENSOR_MASK << sensorId); // assume cleared
        if ((blockedSensorMask == 0) && (displayMode & DISPLAY_BLOCKED_SENSOR)) {
            displayMode &= ~DISPLAY_BLOCKED_SENSOR; // clear flag
        }

        int32_t offsetRemaining;
        int32_t dirToMove; // -1=left, 1=right
        bool    belowMinOffset, aboveMaxOffset;

        switch (sensorId) {
        case 0:
            belowMinOffset = (newOffset <= MIN_OFFSET1_CM);
            aboveMaxOffset = (newOffset >= MAX_OFFSET_CM);
            dirToMove = (OFFSET1_SENSOR_ON_LEFT == 1) ? 1 : -1; // constant expression
            offsetRemaining = TARGET_DESIRED_OFFSET1_CM - newOffset;
            break;
        case 1:
            belowMinOffset = (newOffset <= MIN_OFFSET2_CM);
            aboveMaxOffset = (newOffset >= MAX_OFFSET_CM);
            dirToMove = (OFFSET2_SENSOR_ON_LEFT == 1) ? 1 : -1; // constant expression
            offsetRemaining = TARGET_DESIRED_OFFSET2_CM - newOffset;
            break;
        }
        if (aboveMaxOffset == true) { // nothing present
            displayMode &= ~(DISPLAY_OFFSET | MOVING_LEFT | MOVING_RIGHT);
            if ((displayMode & DISPLAY_BLOCKED_SENSOR) == 0) { // no sensor is blocked
#if LOG_ENABLED
                LOG_COUT(info) << F("offset sensor no reading and no blocked sensors") << LOG_ENDLINE;
#endif
            }
            return;
        }
        if (belowMinOffset == true) {
            displayMode &= ~(DISPLAY_CURRENT_TIME);
            displayMode |= DISPLAY_BLOCKED_SENSOR;
            blockedSensorMask |= (OFFSET1_SENSOR_MASK << sensorId); // flag sensor
            snprintf(ledController.offsetMessage, sizeof(ledController.offsetMessage), ("U%d blocked"), sensorId + 1);
#if LOG_ENABLED
            LOG_COUT(info) << F("offset sensor blocked sensor=") << sensorId << F(" offset=") << newOffset << LOG_ENDLINE;
#endif
            return;
        }
        displayMode &= ~(DISPLAY_CURRENT_TIME | MOVING_LEFT | MOVING_RIGHT);

        int32_t leftOrRightOffset = dirToMove * offsetRemaining;
        int32_t abs_offset = (leftOrRightOffset >= 0) ? leftOrRightOffset : -leftOrRightOffset;
        // for display purposes, we indicate what direction is needed to return to the center
        // rather than the last detected movement direction
        const char *direction = (leftOrRightOffset >= 0) ? ("R") : ("L");
        displayMode |= (leftOrRightOffset >= 0) ? (DISPLAY_OFFSET | MOVING_LEFT) : (DISPLAY_OFFSET | MOVING_RIGHT);
#if LOG_ENABLED
        LOG_COUT(info) << F("newOffset=") << newOffset << F(" offsetRemaing=") << offsetRemaining << F(" leftOrRightOffset=") << leftOrRightOffset
                       << F(" dirToMove=") << dirToMove << F(" dir=") << direction << F(" display=") << displayMode << LOG_ENDLINE;
#endif
        snprintf(ledController.offsetMessage, sizeof(ledController.offsetMessage),
#if USE_METRIC_DISPLAY
                 ("%d cm %s"), abs_offset,
#else
                 ("%.0f %s"), abs_offset * 0.3937,
#endif
                 direction);
    }

    void updateTime(time_t now)
    {
#if LOG_ENABLED > 1
        LOG_COUT(info) << F("updateTime now=") << now << LOG_ENDLINE;
#endif
        time_t delta = now - lastTime;
        if (delta > 0) {
            lastTime = now;
            ledController.timeChanged = true;
            if (now >= 1704085200) { // time set as later than January 1, 2024
                struct tm *curTime = localtime(&now);
                snprintf(ledController.currentTimeText, sizeof(ledController.currentTimeText), ("%2.2d :%2.2d :%2.2d"), curTime->tm_hour, curTime->tm_min,
                         curTime->tm_sec);
            } else {
                strcpy(ledController.currentTimeText, "NO TIME");
            }
#if LOG_ENABLED
        } else if (delta < 0) {
            LOG_COUT(error) << F("updateTime delta=") << delta << LOG_ENDLINE;
#endif
        }
    }

    bool updateDisplay(time_t now, uint8_t animationPhase)
    {
#if LOG_ENABLED
        LOG_COUT(info) << F("updateDisplay phase=") << animationPhase <<
#if USE_DEPTH_SENSOR
            F(" lastDepth=") << lastDepth <<
#endif
#if USE_OFFSET_SENSORS > 0
            F(" lastOffset1=") << lastOffset[0] <<
#if USE_OFFSET_SENSORS > 1
            F(" lastOffset2=") << lastOffset[1] <<
#endif
#endif
            F(" toNextIdle=") << nextIdleTime - now << F(" now=") << now << F(" nextIdle=") << nextIdleTime << F(" displayMode=") << displayMode
                       << F(" activeContentMode=") << ledController.activeContentMode << LOG_ENDLINE;
#endif /* LOG_ENABLED */
        if ((nextIdleTime != 0) && (now >= nextIdleTime)) {
            //      ledController.activeContentMode = ledController.DISPLAY_TIME;
#if LOG_ENABLED
            LOG_COUT(info) << F("IDLE TIME REACHED") << LOG_ENDLINE;
#endif
            displayMode &= ~(DISPLAY_INITIAL | MOVEMENT_MASK | DISPLAY_ALERT_MASK_WITHOUT_BLOCKED_OR_FAILED);
            displayMode |= DISPLAY_CURRENT_TIME;
            nextIdleTime = 0;
        }
#if USE_OFFSET_SENSORS > 0
        // clear offset marker
#if USE_OFFSET_SENSORS > 1
//        ledController.setOffsetLine(-1, 0, LED_BLACK, 1);
#endif
        ledController.setOffsetLine(-1, 0, LED_BLACK, 0);
#endif
        uint16_t alert = displayMode & DISPLAY_ALERT_MASK;
        switch (alert) {
        case DISPLAY_TOO_FAR:
            ledController.setMessage("BACK UP!", 3, LED_RED, LED_BLACK);
            //            timeUntilDisplayChange = 10;
            break;
        case DISPLAY_BLOCKED_SENSOR:
        case DISPLAY_FAILED_SENSOR:
            ledController.setMessage(ledController.offsetMessage, 0, LED_RED);
            //            timeUntilDisplayChange = 10;
            break;
        case DISPLAY_STOP_HERE:
            ledController.setMessage("STOP HERE", 0, LED_GREEN, LED_BLACK);
            //            timeUntilDisplayChange = 10;
            break;
        case DISPLAY_CAUTION: {
            char text[128];
            text[0] = '\0';
            if (displayMode & DISPLAY_DEPTH) {
                strcat(text, ledController.depthMessage);
            }
            if (displayMode & DISPLAY_OFFSET) {
                if (text[0] != '\0') {
                    strcat(text, " ");
                }
#if LOG_ENABLED
                if (ledController.offsetMessage[0] == '\0') {
                    LOG_COUT(error) << F("OFFSET set but no text") << LOG_ENDLINE;
                }
#endif
                strcat(text, ledController.offsetMessage);
            }
            uint8_t l = static_cast<uint8_t>(strlen(text));
            uint8_t x = (l >= 10) ? 0 : 5 - (l / 2);
#if LOG_ENABLED
            LOG_COUT(info) << F("text=") << text << F(" l=") << l << F(" x=") << x << LOG_ENDLINE;
#endif
            ledController.setMessage(text, x * 5, LED_ORANGE);
        } break;
        default:
            break;
        } // end switch alert
#if USE_OFFSET_SENSORS > 0
        {
            int32_t relativeOffset, scaledOffset;

#if USE_OFFSET_SENSORS > 1
            // handle sensor 2 first
            if ((lastOffset[1] > MIN_OFFSET2_CM) && (lastOffset[1] < MAX_OFFSET_CM)) { // valid signal
                if (lastOffset[1] < MIN_DESIRED_OFFSET2_CM) {
                    scaledOffset = 0;
                    relativeOffset = TARGET_DESIRED_OFFSET2_CM - MIN_DESIRED_OFFSET2_CM;
                } else if (lastOffset[1] > MAX_DESIRED_OFFSET2_CM) {
                    scaledOffset = COLS - 1;
                    relativeOffset = TARGET_DESIRED_OFFSET2_CM - MAX_DESIRED_OFFSET2_CM;
                } else {
                    // following constants are computed at compile-time
                    const int32_t range1 = MAX_DESIRED_OFFSET2_CM - TARGET_DESIRED_OFFSET2_CM;
                    const int32_t range2 = TARGET_DESIRED_OFFSET2_CM - MIN_DESIRED_OFFSET2_CM;
                    const int32_t rangeScale = (range1 >= range2) ? range1 : range2;

                    relativeOffset = TARGET_DESIRED_OFFSET2_CM - lastOffset[1];
                    scaledOffset = (relativeOffset * COLS) / (rangeScale * 2);
#if OFFSET2_SENSOR_ON_LEFT == 0
                    scaledOffset = (COLS - 1) - scaledOffset; // increasing values mean more to the left
                    relativeOffset *= -1;                     // invert sign
#endif
                    scaledOffset = (COLS / 2) - scaledOffset;
                    if (scaledOffset < 0) scaledOffset = 0;
                    else if (scaledOffset >= COLS) scaledOffset = COLS - 1;
                }

                uint16_t color = LED_BLUE_HIGH;
                if ((scaledOffset <= (COLS / 4)) || (scaledOffset >= (COLS - (COLS / 4)))) {
                    color = LED_RED;
                }
                ledController.setOffsetLine(scaledOffset, (scaledOffset <= (COLS / 2) ? -1 : 1), color, 1);
            } // valid signal
#endif        /* end USE_OFFSET_SENSORS > 1 */
            // handle sensor 1
            if ((lastOffset[0] >= MIN_OFFSET1_CM) && (lastOffset[0] < MAX_OFFSET_CM)) { // valid signal
                if (lastOffset[0] < MIN_DESIRED_OFFSET1_CM) {
                    scaledOffset = 0;
                    relativeOffset = TARGET_DESIRED_OFFSET1_CM - MIN_DESIRED_OFFSET1_CM;
                } else if (lastOffset[0] > MAX_DESIRED_OFFSET1_CM) {
                    scaledOffset = COLS - 1;
                    relativeOffset = TARGET_DESIRED_OFFSET1_CM - MAX_DESIRED_OFFSET1_CM;
                } else {
                    // following constants are computed at compile-time
                    const int32_t range1 = MAX_DESIRED_OFFSET1_CM - TARGET_DESIRED_OFFSET1_CM;
                    const int32_t range2 = TARGET_DESIRED_OFFSET1_CM - MIN_DESIRED_OFFSET1_CM;
                    const int32_t rangeScale = (range1 >= range2) ? range1 : range2;

                    relativeOffset = TARGET_DESIRED_OFFSET1_CM - lastOffset[0];
                    scaledOffset = (relativeOffset * COLS) / (rangeScale * 2);
#if OFFSET1_SENSOR_ON_LEFT == 0
                    scaledOffset = (COLS - 1) - scaledOffset; // increasing values mean more to the left
                    relativeOffset *= -1;                     // invert sign
#endif
                    scaledOffset = (COLS / 2) - scaledOffset;
                    if (scaledOffset < 0) scaledOffset = 0;
                    else if (scaledOffset >= COLS) scaledOffset = COLS - 1;
                }
                uint16_t color = LED_BLUE_HIGH;
                if ((scaledOffset < (COLS / 4)) || (scaledOffset >= (COLS - (COLS / 4)))) {
                    color = LED_RED;
                }
#if LOG_ENABLED
                LOG_COUT(info) << F("scaledOffset=") << scaledOffset << F(" offset=") << lastOffset[0] << F(" relative=") << relativeOffset << F(" color=")
                               << color << LOG_ENDLINE;
#endif
                ledController.setOffsetLine(scaledOffset, (scaledOffset <= (COLS / 2) ? -1 : 1), color, 0);
            } // valid signal
        }
#endif /* USE_OFFSET_SENSORS > 0 */
#if USE_DEPTH_SENSOR
        {
            // following constants are computed at compile-time
            const int32_t range1 = MAX_DESIRED_DEPTH_CM - TARGET_DESIRED_DEPTH_CM;
            const int32_t range2 = TARGET_DESIRED_DEPTH_CM - MIN_DESIRED_DEPTH_CM;
            const int32_t rangeScale = (range1 >= range2) ? range1 : range2;

            int32_t depthRemaining = lastDepth - TARGET_DESIRED_DEPTH_CM;
            int32_t scaledDepth = (depthRemaining * COLS) / (rangeScale * 2);
            int32_t centeredOffset = ((COLS / 2) - ledController.ANIMATION_PHASES) + scaledDepth; // center
#if LOG_ENABLED
            LOG_COUT(info) << F("depthRemaining=") << depthRemaining << F(" scaled=") << scaledDepth << F(" centered=") << centeredOffset << LOG_ENDLINE;
#endif
            if (centeredOffset < 0) centeredOffset = 0;
            else if (centeredOffset >= (COLS - (ledController.ANIMATION_PHASES * 2))) centeredOffset = COLS - (ledController.ANIMATION_PHASES * 2);
            ledController.setTargetBoxOrigin(static_cast<int16_t>(centeredOffset),
                                             (lastDepth >= (TARGET_DESIRED_DEPTH_CM - DESIRED_DEPTH_TOLERANCE_CM)) ? LED_GREEN_HIGH : LED_RED_HIGH);
        }
#endif /* USE_DEPTH_SENSOR */

        if (now < timeUntilDisplayChange) {
#if LOG_ENABLED
            LOG_COUT(info) << F("seconds until display change=") << timeUntilDisplayChange - now << LOG_ENDLINE;
#endif
            return (true);
        }
        uint8_t maxPhases = ledController.MAX_DISPLAY_PHASES;
        switch (displayMode & (DISPLAY_MASK | MOVING_FORWARD | MOVING_BACKWARD)) {
        case DISPLAY_ARRIVING:
        case DISPLAY_ARRIVING | DISPLAY_OFFSET:
#if LOG_ENABLED
            LOG_COUT(info) << F("SET ARRIVE MESSAGE") << LOG_ENDLINE;
#endif
            ledController.setMessage("HELLO", 10, LED_GREEN);
            timeUntilDisplayChange = now + ANNOUNCE_ARRIVE_DELAY;
            displayMode &= ~DISPLAY_INITIAL;
            maxPhases = 1;
            break;
        case DISPLAY_DEPARTING:
        case DISPLAY_DEPARTING | DISPLAY_OFFSET:
#if LOG_ENABLED
            LOG_COUT(info) << F("SET DEPART MESSAGE") << LOG_ENDLINE;
#endif
            ledController.setMessage("BE SAFE", 10, LED_GREEN);
            timeUntilDisplayChange = now + ANNOUNCE_DEPART_DELAY;
            displayMode &= ~DISPLAY_INITIAL;
            maxPhases = 1;
            break;
#if LOG_ENABLED
        case DISPLAY_INITIAL | DISPLAY_DEPTH | DISPLAY_OFFSET:
        case DISPLAY_INITIAL | DISPLAY_DEPTH:
        case DISPLAY_DEPTH | DISPLAY_OFFSET:
        case DISPLAY_DEPTH:
            LOG_COUT(warn) << F("DEPTH but no movement") << LOG_ENDLINE;
            break;
#endif
#if LOG_ENABLED
        case DISPLAY_DEPTH | MOVING_FORWARD:
        case DISPLAY_DEPTH | MOVING_BACKWARD:
            LOG_COUT(warn) << F("DEPTH AND DIRECTION BUT NO OFFSET") << LOG_ENDLINE;
            break;
#endif
#if LOG_ENABLED
        case DISPLAY_DEPTH | DISPLAY_OFFSET | MOVING_FORWARD:
        case DISPLAY_DEPTH | DISPLAY_OFFSET | MOVING_BACKWARD:

            LOG_COUT(info) << F("normal depth and offset and movement") << LOG_ENDLINE;
            break;
#endif
        case DISPLAY_INITIAL | DISPLAY_OFFSET: // no valid depth data
#if LOG_ENABLED
            LOG_COUT(info) << F("Initial offset only") << LOG_ENDLINE;
#endif
            displayMode &= ~DISPLAY_INITIAL; // clear initial flag
            /* FALLTHROUGH */
        case DISPLAY_OFFSET:
#if LOG_ENABLED
            LOG_COUT(warn) << F("OFFSET but NO DEPTH") << LOG_ENDLINE;
#endif
            /* FALLTHROUGH */
        case DISPLAY_CURRENT_TIME | DISPLAY_OFFSET:
        case DISPLAY_CURRENT_TIME | DISPLAY_DEPTH:
        case DISPLAY_CURRENT_TIME | DISPLAY_DEPTH | DISPLAY_OFFSET:
        case DISPLAY_CURRENT_TIME:
            ledController.activeContentMode = ledController.DISPLAY_TIME;
            ledController.refreshDisplay(0);
            FastLED.show();
            FastLED.delay(10);
#if LOG_ENABLED > 1
            LOG_COUT(info) << F("did show for time") << LOG_ENDLINE;
#endif
            return (true);
        case 0:
#if LOG_ENABLED
            LOG_COUT(warn) << F("no display mode set") << LOG_ENDLINE;
#endif
            break;
        default:
#if LOG_ENABLED
            LOG_COUT(warn) << F("UNSUPPORTED displayMode=") << displayMode << F(" mask=") << (displayMode & (DISPLAY_MASK | MOVING_FORWARD | MOVING_BACKWARD))
                           << LOG_ENDLINE;
//            delay(5000);
#endif
            break;
        }
        //        for (displayPhase = 0; displayPhase < maxPhases; displayPhase += 1) {
        ledController.refreshDisplay(animationPhase);
        FastLED.show();
        FastLED.delay(10);
#if LOG_ENABLED > 1
        LOG_COUT(info) << F("did show for regular animationPhase=") << animationPhase << F(" maxPhases=") << maxPhases << LOG_ENDLINE;
#endif
        if ((animationPhase + 1) >= maxPhases) {
            return (true);
        }
        return (false);
    }

    void pollSensors(time_t now)
    {
      /* If enabled, read secondary ultrasonic device first, then query LIDAR sensor followed by
       * reading of primary ultrasonic device.  This minimizes time wasted enforcing 60 milliseconds
       * between readings.
       */
#if USE_OFFSET_SENSORS > 0
        uint32_t      offset;
        unsigned long duration;
        bool          gotResponse;
#endif
#if USE_OFFSET_SENSORS > 1
        unsigned long startMillis = millis();
        // do furthest away sensor first
        gotResponse = offsetDevice2.getDistanceReading(&offset, &duration);
        if (gotResponse) {                                   // sensor working
            failedSensorMask &= ~(OFFSET1_SENSOR_MASK << 1); // clear failed depth sensor flag
            if ((failedSensorMask == 0) && (displayMode & DISPLAY_FAILED_SENSOR)) {
                displayMode &= ~DISPLAY_FAILED_SENSOR; // clear failed flag
            }
#if LOG_ENABLED > 1
            float inches = offset * 0.3937;
            LOG_COUT(info) << F("SONIC2 distance=") << offset << F(" cm ") << inches << F(" in duration=") << duration << LOG_ENDLINE;
#endif
            updateOffset(now, offset, 1);
        } else {
            bool sensorFailed = offsetDevice2.sensorHasFailed();
            if (sensorFailed) {
#if LOG_ENABLED
                LOG_COUT(error) << F("SONIC2 failed") << LOG_ENDLINE;
#endif
                displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_FAILED | DISPLAY_CURRENT_TIME);
                displayMode |= DISPLAY_FAILED_SENSOR;
                failedSensorMask |= (OFFSET1_SENSOR_MASK << 1);
                strcpy(ledController.offsetMessage, ("U2 failed"));
                ledController.depthMessage[0] = '\0';
                updateOffset(now, MAX_OFFSET_CM, 1);
            }
        }
#endif

#if USE_DEPTH_SENSOR
        uint16_t distance;
        uint16_t strength;

        bool haveReading = depthDevice.getDistanceReading(&distance, &strength);
        if (haveReading) {
            failedSensorMask &= ~DEPTH_SENSOR_MASK; // clear failed depth sensor flag
            if ((failedSensorMask == 0) && (displayMode & DISPLAY_FAILED_SENSOR)) {
                displayMode &= ~DISPLAY_FAILED_SENSOR; // clear failed flag
            }
#if LOG_ENABLED > 1
            float inches = distance * 0.3937;
            LOG_COUT(info) << F("LIDAR distance=") << distance << F(" cm ") << inches << F(" in strength=") << strength << LOG_ENDLINE;
#endif
            // distance 65535 = signal strength < 100
            // distance 65534 = signal strength saturation
            // distance 65532 = ambient light saturation
            if ((strength > 100) && (distance < 65532)) { // valid signal strength
                updateDepth(now, distance);
            }
        } else { // sensor seems to not be working....
            if (depthDevice.sensorHasFailed()) {
                displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_FAILED | DISPLAY_CURRENT_TIME | DISPLAY_DEPTH | DISPLAY_INITIAL | DISPLAY_DEPTH_MOVEMENT_MASK);
                displayMode |= DISPLAY_FAILED_SENSOR;
                failedSensorMask |= DEPTH_SENSOR_MASK;
                ledController.depthMessage[0] = '\0';
                strcpy(ledController.offsetMessage, "L1 failed");
                updateDepth(now, 0);
            }
        }
#endif /* USE_DEPTH_SENSOR */

#if USE_OFFSET_SENSORS > 1
        unsigned long nowMillis = millis();
        unsigned long sampleTime = nowMillis - startMillis;
        if (sampleTime < 60) { // enforce 60 milliseconds between samples
            FastLED.delay(60 - sampleTime);
        }
#endif /* USE_OFFSET_SENSORS > 1 */
#if USE_OFFSET_SENSORS > 0
        gotResponse = offsetDevice1.getDistanceReading(&offset, &duration);
        if (gotResponse) {
            failedSensorMask &= ~(OFFSET1_SENSOR_MASK << 0); // clear failed depth sensor flag
            if ((failedSensorMask == 0) && (displayMode & DISPLAY_FAILED_SENSOR)) {
                displayMode &= ~DISPLAY_FAILED_SENSOR; // clear failed flag
            }
#if LOG_ENABLED > 1
            float inches = offset * 0.3937;
            LOG_COUT(info) << F("SONIC1 distance=") << offset << F(" cm ") << inches << F(" in duration=") << duration << LOG_ENDLINE;
#endif
            updateOffset(now, offset, 0);
        } else {
            bool sensorFailed = offsetDevice1.sensorHasFailed();
            if (sensorFailed) {
#if LOG_ENABLED
                LOG_COUT(error) << F("SONIC1 failed") << LOG_ENDLINE;
#endif
                displayMode &= ~(DISPLAY_ALERT_MASK_WITHOUT_FAILED | DISPLAY_CURRENT_TIME);
                displayMode |= DISPLAY_FAILED_SENSOR;
                failedSensorMask |= (OFFSET1_SENSOR_MASK << 0);
                strcpy(ledController.offsetMessage, ("U1 failed"));
                ledController.depthMessage[0] = '\0';
                updateOffset(now, MAX_OFFSET_CM, 0);
            }
        }
#endif /* USE_OFFSET_SENSORS > 0 */
    }
}; // end class ParkingMonitor<>

static ParkingMonitor<LED_CONTROL_PIN, NUM_LED_COLS, NUM_LED_ROWS> monitor;

void setup()
{
    delay(1000); // stabilize after power-on
#if LOG_ENABLED
    // Setup hardware serial port
    Serial.begin(CONSOLE_BAUD_RATE);
    delay(500); // stabilize after power-on
    while (!Serial)
        ; // wait for serial port to connect. Needed for native USB port only
    LOG_COUT(info) << F("Initializing...") << LOG_ENDLINE;
#endif
    pinMode(LED_BUILTIN, OUTPUT);
    monitor.init();
#if LOG_ENABLED
    LOG_COUT(info) << F("end setup") << LOG_ENDLINE;
#endif
}

#if ENABLE_CONSOLE_COMMANDS > 0
static uint8_t consoleReadOffset;
static char    consoleCmdBfr[64];
static bool    pauseFlag = false;

static bool processConsoleCommand(const time_t now, char *cmd)
{
    uint16_t val;
    char    *nextCmd;
    char    *cmdStart = cmd;
    while (*cmdStart != '\0') {
        nextCmd = cmdStart;
        while ((*nextCmd != '\0') && (*nextCmd != ','))
            nextCmd += 1;
        if (*nextCmd == ',') {
            *nextCmd = '\0';
            nextCmd += 1;
        }
#if LOG_ENABLED
        LOG_COUT(debug) << F(" cmd=") << cmdStart << LOG_ENDLINE;
#endif
        if ((cmdStart[0] == 'd') && (cmdStart[1] == ':')) {
            forceDepth = atoi(cmdStart + 2);
        } else if ((cmdStart[0] == 'o') && (cmdStart[1] == ':')) {
            forceOffset = atoi(cmdStart + 2);
        } else if (strcmp(cmdStart, "stop") == 0) {
            pauseFlag = true;
        } else if (strcmp(cmdStart, "go") == 0) {
            pauseFlag = false;
        }
        cmdStart = nextCmd;
    }
    return (pauseFlag);
}

static bool readAndProcessConsoleCommands(const time_t now)
{
    static uint8_t consoleReadOffset;

    while (Serial.available()) {
        bool sawEOL = false;
        do {
            uint8_t c = Serial.read();
            if ((c == '\r') || (c == '\n')) {
                sawEOL = true;
                break;
            }
            consoleCmdBfr[consoleReadOffset++] = c;
        } while (Serial.available());
        //    Serial.stopListening();
        if (sawEOL == false) break;

        consoleCmdBfr[consoleReadOffset] = '\0';
#if LOG_ENABLED
        LOG_COUT(debug) << F("len=") << consoleReadOffset << F(" bfr=") << consoleCmdBfr << LOG_ENDLINE;
#endif
        consoleReadOffset = 0; // reset
        bool result = processConsoleCommand(now, consoleCmdBfr);
    }
    return (pauseFlag);
}
#endif

void loop()
{
    static uint64_t lastDisplayCall;
    static uint8_t  loopCount;
    static uint8_t  heartbeatLEDstate; // blink mode for builtin LED
    static uint8_t  animationPhase = monitor.MAX_DISPLAY_PHASES - 1;

    if (++loopCount == 10) { // reset
        loopCount = 0;
        heartbeatLEDstate = 1 - heartbeatLEDstate; // toggle high vs. low
        digitalWrite(LED_BUILTIN, heartbeatLEDstate);
    }
#if LOG_ENABLED > 2
    LOG_COUT(info) << F("begin loop") << LOG_ENDLINE;
#endif
    time_t now = time(nullptr);
    monitor.updateTime(now);

#if ENABLE_CONSOLE_COMMANDS
    bool pauseNow = readAndProcessConsoleCommands(now);
    if (pauseNow == true) {
        FastLED.delay(1000); // delay 1 second
        return;
    }
#endif
    monitor.pollSensors(now);
    uint64_t currentMillis = millis();
    int64_t  msSinceLastDisplay = currentMillis - lastDisplayCall;
    if (msSinceLastDisplay < ANIMATION_INTERVAL) {
        FastLED.delay(ANIMATION_INTERVAL - msSinceLastDisplay);
        currentMillis = millis();
    }
    lastDisplayCall = currentMillis;
    if (++animationPhase >= monitor.MAX_DISPLAY_PHASES) {
        animationPhase = 0;
    }
    int debugState = analogRead(DEBUG_PIN);
#if LOG_ENABLED > 1
    LOG_COUT(info) << F("debug pin level=") << debugState << LOG_ENDLINE;
#endif
    if (debugState > 1000) { // pin was attached to 3.3v
        monitor.displaySensorData();
    }
    bool didLastPhase = monitor.updateDisplay(now, animationPhase);
    if (didLastPhase) { // reset phase counter
        animationPhase = monitor.MAX_DISPLAY_PHASES - 1;
    }
    //    FastLED.delay(100); // slow for debugging

#if LOG_ENABLED > 2
    LOG_COUT(info) << F("end loop") << LOG_ENDLINE;
#endif
}

/* vim: set filetype=cpp : */
/* vim: set expandtab shiftwidth=4 tabstop=4 : */