Infrared Reflective Sensor TCRT5000 · 2025.04.17

#pragma once

// Infrared Reflective Sensor Reader
// improving the accuracy of a very cheap TCRT5000 sensor
// info@muman.ch, 2025.04.14
// All rights reversed

/*
IR reflective sensors have a IR LED and an IR transistor sensor 
which sensesthe reflected infrared from the LED. A typical low-cost 
sensor is the TCRT5000:
https://www.vishay.com/docs/80107/80107.pdf
https://www.vishay.com/docs/82904/irreceiversprecencesensorapps.pdf
https://www.vishay.com/docs/83760/tcrt5000.pdf

Note: This application is for detecting the distance of objects using 
the sensor in analogue mode. It's not for measuring rotation speed, 
for that you should use a module with a built-in comparator chip like 
the LM393 which has a digital output. For a suitable circuit, Google
"TCRT5000 circuits" and look at the images.

For our purposes, a very simple circuit can be used, no opamps are 
needed. The IR LED is turned on/off by a digital output, and the
transistor IR sensor value is read via an analogue input.

       Pull-up resistor
     +-------|10K|------- +5V or 3.3V
     |
     +------------------> ANALOG INPUT
     |
     |  Current limiting resistor, 330 ohms for ~11mA drive
     |   +---|330|------< DIGITAL OUTPUT, IR LED ON/OFF
     |   |
   +-------\
   | C   A  |  TCRT5000
   | E   K  |
   +-------/
     |   |
     +---+--------------- GND

In the above circuit, the analogue input gets typical readings in
the range 0..900 when connected to a 10-bit analogue input (0..1023). 
This corresponds to a reflector distance of 300mm..10mm. The closer 
it is, the higher the reading.

Improved Accuracy
The influence of ambient infrared light and ambient temperature is 
removed by subtracting the value read when the IR LED is off from 
the value read when it's on. This makes the readings more accurate 
and reduces drift dramatically. The sensor has a "daylight blocking 
filter" but this doesn't seem to work very well (just drawing the
curtains changes the reading by 10%, and God knows what bright 
sunlight would do). It does not compensate for the drift caused by 
temperature changes either, even the IR LED itself causes heating.

Here you will find two analogue read routines:
read() : Blocks until the reading is complete, takes about 800uS
poll() : Non-blocking version which returns immediately with 'true'
         if a new reading is ready, call this regularly from loop()

And one digital routine:
readSwitch() : A proximity switch with hysteresis, which calls poll()

For poll() and readSwitch() you can set the 'msBetweenReads' value to 
define how often the sensor is read (default is every 100mS, 10 times 
per second). For readSwitch(), 'readingsToAverage' can be zero (the 
default) because the switch has hysteresis so averaging is not needed.

Matt's Tip: English is silly. Pronounce 'read' as 'red' if it's in 
the past tense.


CALCULATING A REFLECTOR'S DISTANCE

The sensor reading depends on the reflectivity and size of the target 
object, and any other reflective objects in the vicinity. Even the 
wires of the prototype will affect the reading, so keep them out of 
the way. The reading should be 0 if there's no reflection.

If you need the sensor to measure something like weight or tension 
by moving an armature, the best setup is to house the sensor and a 
matt-white movable reflector in a non-reflective enclosure (line the
enclosure with black antistatic foam), then calibrate it using a 
ruler. The exponential curve is steep at long distances and flattens 
out as the distance approaches 0. The closer the reflector, the more 
accurate the reading will be (but not closer than 10mm).

The cheap TCRT5000 detects distance from 10..150mm. Accuracy can be
better than 1mm up to 100mm distance, and 0.1mm at 10-30mm distance.

To convert the reading to a distance, you need to create a calibration
table, enter it in an Excel spreadsheet, and use Excel's 'Trendline'
feature to create the equation for converting the reading to the 
distance in millimeters...

How to use Excel to determine the equation for distance calculation:

1) Mount the sensor and reflector, and write some code to display the 
   readings from the sensor's analog input using calls to read() or
   poll()
2) Create a data set of distance vs. reading, with as many points as
   possible between 0 and 150mm distance
3) Enter the points into two vertical columns in an Excel spreadsheet,
   with the analogue readings in the first column A (high to low)
   and the matching mm distances in the second column B (low to high)
   this ensures you'll get the correct axes on the scatter chart
4) Select all the data values in the two columns and do 
   'Insert -> Charts -> Scatter chart with smooth lines and markers'
   you should see a chart with the distances on the vertical Y axis 
   and the analogue readings on the horizontal X axis
   with a nice exponential curve
5) Right-click on the curve to select it and show the context menu
   choose 'Add Trendline'
   you should see the 'Format Trendline' properties window on the right
6) On the 'Trendline Options' page, select the 'Logarithmic' radio button
   this should show a trend line curve that closely matches the data points
7) Select the 'Display Equation on Chart' checkbox
   this shows the equation that matches the curve, copy this equation and 
   paste it somewhere safe, e.g.
       y = -29.72ln(x) + 210.68
8) Code this equation in the calculateDistance() function below
   for ln(x), use logf((float)reading)
9) Is it bedtime yet?

Alternatively, you could use MATLAB's 'Curve Fitting Toolbox' to create 
a polynomial equation which might have an even better fit to your data. 
But you have to pay for that.
*/

#include "Utils.h"
#include "RollingAverage.h"


namespace MattLabs
{
	class IrReflectiveSensor
	{
	public:
		IrReflectiveSensor(int ledOutPin, int analogueInPin, int readingsToAverage = 0);
		int read();

		bool poll(int* reading);
		bool readSwitch(bool* state, int level, int hysteresis);
		float calculateDistance(int reading);

		// read speed for poll() and readSwitch()
		int msBetweenReads = 100;

	private:
		int ledPin;
		int anaPin;
		RollingAverage<int> averager;
		unsigned long usTimer;
		bool pollState;
		int ambient;
		bool latchedState;
	};


	// Constructorism
	// readingsToAverage can be 0 (the default) if using it as a switch, see readSwitch()
	IrReflectiveSensor::IrReflectiveSensor(int ledOutPin, int analogueInPin, 
		int readingsToAverage /*=0*/)
	{
		ledPin = ledOutPin;
		anaPin = analogueInPin;

		usTimer = 0;
		pollState = false;
		latchedState = false;

		pinMode(ledPin, OUTPUT);
		digitalWrite(ledPin, 0);
		pinMode(anaPin, INPUT);

		if (readingsToAverage > 0)
			averager.begin(readingsToAverage);
	}

	// Reads the sensor directly and returns when a reading is ready
	// this blocks for about 800 microseconds
	int IrReflectiveSensor::read()
	{
		// read ambient ir light
		int ambient = analogRead(anaPin);

		// turn on ir led
		digitalWrite(ledPin, 1);

		// a short delay is needed
		// the TCRT5000 is a slow sensor (and mine may be clones)
		delayMicroseconds(400);

		// read ir light with ir led on
		int sensor = analogRead(anaPin);

		// turn off ir led
		digitalWrite(ledPin, 0);
		
		// this delay is not needed if read() is not called continuously
		delayMicroseconds(400);

		// subtract ambient ir light from the reading
		int reading = ambient - sensor;

		// ignore +-1 difference (ADC resolution error)
		if (reading <= 1)
			reading = 0;

		// average the reading
		if (averager.getBufferLength() > 0) {
			averager.addSample(reading);
			reading = averager.getAverage();
		}
		return reading;
	}

	// Reads the sensor without delays
	// this is a non-blocking function that returns immediately
	// it returns 'true' when a new reading is available
	// takes a reading every 'msBetweenReads' milliseconds if called continuously
	bool IrReflectiveSensor::poll(int* reading)
	{
		// delay between actions
		unsigned long us = micros();
		if (usTimer > us)
			return false;
		pollState = !pollState;

		// read ambient light with IR LED off
		// then turn on the IR LED
		// the actual reading is taken on subsequent call after the delay
		if (pollState) {
			usTimer = us + 400; // delay in microseconds until we read the sensor
			ambient = analogRead(anaPin);
			digitalWrite(ledPin, 1);
			return false;
		}
		usTimer = us + (msBetweenReads * 1000L); // milliseconds until next read

		// read IR sensor, turn off IR LED
		int sensor = analogRead(anaPin);
		digitalWrite(ledPin, 0);

		// difference between the two readings
		int r = ambient - sensor;

		// ignore +-1 difference (ADC resolution error)
		if (r <= 1)
			r = 0;

		// average the reading
		if (averager.getBufferLength() > 0) {
			averager.addSample(r);
			r = averager.getAverage();
		}
		*reading = r;
		return true;
	}

	// Reads the IR sensor as a switch, with hysteresis
	// Returns true if *state has changed.
	// The turnOn/turnOff values are integer values from the analogue input 
	// e.g. range 0..1023, find these by experimentation.
	// 'turnOffLevel' must be lower than 'turnOnLevel' to provide hysteresis.
	// Speed/sensitivity is dependent on the polling rate 'msBetweenReads'.
	// >>> Averaging should be turned off when using it as a switch with hysteresis, 
	// readingsToAverage = 0 (the default) <<<
	bool IrReflectiveSensor::readSwitch(bool* state, int turnOnLevel, int turnOffLevel)
	{
		*state = latchedState;

		int reading;
		if (!poll(&reading)) {
			// state unchanged
			return false;
		}

		// active
		if (reading >= turnOnLevel) {
			// state unchanged
			if (latchedState)
				return false;
			// just activated
			latchedState = true;
			*state = latchedState;
			return true;
		}

		// inactive and unchanged
		if (!latchedState)
			return false;

		// was active, now inactive, check for turn off
		if (reading <= turnOffLevel) {
			// latched state now inactive
			latchedState = false;
			*state = latchedState;
			return true;
		}

		// still active, unchanged
		return false;
	}


	// Convert an analogue reading to the distance in mm
	// Use Excel to determine the equation for distance calculation
	// see the voluminous comment at the top
	float IrReflectiveSensor::calculateDistance(int reading)
	{
		// logarithmic trend line curve-fit equation from Excel data
		return -29.72f * logf((float)reading) + 210.68f;
	}

}

#pragma once

// Optimized Rolling Average Filter
// info@muman.ch, 2025.03.04
// All rights reversed

/*
Averages values of any numerical data type. The data type of the sum value SUMTYPE can 
also be defined.

For float and double, SUMTYPE can be omitted, so SUMTYPE is float or double by default.

For integer types, the SUMTYPE must be able to hold 'MAXVALUE * numberOfSamples' without
overflowing or underflowing. e.g. for a 16-bit 'int' you should use a 32-bit 'long', 
else the sum may overflow/underflow. This is not checked by the compiler. 
On a 32-bit machine, 'int' and 'long' are both 32 bits, so you may need to use 'long long' 
(64 bits) or int64_t if the sum is very large.

If you are averaging 10-bit analog input values (0..1023) on an 8 or 16-bit processor
(16-bit integers), then you can use 'unsigned int' for both because it will not overflow
if you don't sum more than 64 values. This is faster than using 'long'. 

But if you use 'float' for averaging integer values you will get a much smoother result. 
This removes the 'step effect' you can get when reading narrow-range analogue inputs.

As usual, all methods must be called from the same thread (i.e. not from an interrupt),
else use some kind of mutex lock.
*/

namespace MattLabs
{
	template <typename T, typename SUMTYPE = T> 
	class RollingAverage
	{
		T* samples = NULL;
		int bufferLength = 0;
		int index = 0;
		bool bufferFull = false;

		// SUMTYPE must be able to hold the sum of all values (max/min) without overflow
		SUMTYPE sum;

	public:
		RollingAverage() { }
		RollingAverage(int numberOfSamples) { begin(numberOfSamples); }
		~RollingAverage();

		void begin(int numberOfSamples);
		void clear();
		void addSample(T sample);
		T getAverage();
		int getNumberOfSamples() { return bufferFull ? bufferLength : index; }
		int getBufferLength() { return bufferLength; }
	};

	// A destructor is not usually needed for embedded code - use static objects
	template <typename T, typename SUMTYPE>
	RollingAverage<T, SUMTYPE>::~RollingAverage()
	{
		if (samples) {
			free(samples);
			samples = NULL;
		}
	}

	template <typename T, typename SUMTYPE>
	void RollingAverage<T, SUMTYPE>::begin(int numberOfSamples)
	{
		if (numberOfSamples <= 0)
			numberOfSamples = 1;
		bufferLength = numberOfSamples;
		if (samples)
			free(samples);
		samples = (T*)malloc(bufferLength * sizeof(T));
		if (!samples) {
			//TODO fatal error handling, out of memory
		}
		bufferLength = bufferLength;
		clear();
	}

	template <typename T, typename SUMTYPE>
	void RollingAverage<T, SUMTYPE>::clear()
	{
		index = 0;
		sum = 0;
		bufferFull = false;
	}

	template <typename T, typename SUMTYPE>
	void RollingAverage<T, SUMTYPE>::addSample(T sample)
	{
		if (bufferFull)
			sum -= samples[index];
		sum += sample;
		samples[index] = sample;
		if (++index == bufferLength) {
			index = 0;
			bufferFull = true;
		}
	}

	template <typename T, typename SUMTYPE>
	T RollingAverage<T, SUMTYPE>::getAverage()
	{
		if (bufferFull)
			return sum / bufferLength;
		if (index == 0)
			return 0;
		return sum / index;
	}

}

#pragma once

// Miscellaneous Useful Stuff
// info@muman.ch, 2025.04.25
// All rights reversed

#include <Arduino.h>

// Debug output and error logging
#define DEBUG  // define this while debugging, comment out for release

#ifdef DEBUG
// Debug output
#define LOG_SERIAL         Serial     // serial port for debug output
#define LOG_BEGIN(a)       LOG_SERIAL.begin(a); delay(500)
#define LOG_PRINT(a)       LOG_SERIAL.print(a)
#define LOG_PRINT2(a, b)   LOG_SERIAL.print(a, b)
#define LOG_PRINTF(a, b)   LOG_SERIAL.printf(a, b)
#define LOG_PRINTLN(a)     LOG_SERIAL.println(a)
extern const char clsPuTTY[];
#define LOG_CLS()          LOG_SERIAL.print(clsPuTTY)
// Error logger or handler
#define LOG_ERROR(s) LOG_PRINT(s); LOG_PRINT(", "); LOG_PRINTLN(__FILE__)

#else
// No debug output if not in debug mode
#define LOG_SERIAL
#define LOG_BEGIN(a)
#define LOG_PRINT(a)
#define LOG_PRINT2(a, b)
#define LOG_PRINTF(a, b)
#define LOG_PRINTLN(a)
#define LOG_CLS()

#define LOG_ERROR(s)
#endif


// NOTE 
// On 8-bit machines, double and float are both 32-bits (both float)
// On 32-bit machines, int and long are both 32 bits

// Linear range conversions
int16_t rescale(int16_t value, int16_t inmin, int16_t inmax, int16_t outmin, int16_t outmax);
uint16_t rescale(uint16_t value, uint16_t inmin, uint16_t inmax, uint16_t outmin, uint16_t outmax);
int32_t rescale(int32_t value, int32_t inmin, int32_t inmax, int32_t outmin, int32_t outmax);
uint32_t rescale(uint32_t value, uint32_t inmin, uint32_t inmax, uint32_t outmin, uint32_t outmax);
float rescale(float value, float inmin, float inmax, float outmin, float outmax);
double rescale(double value, double inmin, double inmax, double outmin, double outmax);

// String conversions
bool floatToString(float value, char* psz, int decimalPlaces);
bool stringToFloat(const char* psz, float* result, char** pszEnd);
bool doubleToString(double value, char* psz, int decimalPlaces);
bool stringToDouble(const char* psz, double* result, char** pszEnd);


// Polled millisecond timer with implicit inline code
// all instances must be static
// call tick() from loop()
class PolledTimer
{
	unsigned long tout;
	unsigned long tstart;

public:
	PolledTimer(unsigned long timeoutms)
	{
		tout = timeoutms;
		tstart = millis();
	}
	
	// Use this to save multiple calls to millis()
	bool tick(unsigned long tnow)
	{
		unsigned long elapsed = tnow - tstart;
		if (elapsed >= tout) {
			// timed out, restart timer and return true
			tstart = tnow;
			return true;
		}
		// not timed out
		return false;
	}
	
	bool tick()
	{
		return tick(millis());
	}
};

// Miscellaneous Useful Stuff
// info@muman.ch, 2025.04.13
// All rights reversed
/*
TODO write an enormous amount of blurb here

Good details of sprintf() format string
https://www.ibm.com/docs/en/zos/3.1.0?topic=programs-sprintf-format-write-data
*/

#include <Arduino.h>
#include <limits.h>
#include <errno.h>

#include "Utils.h"

#ifdef DEBUG
// Escape sequence to clear the screen if you're using PuTTY, see DEBUG_CLS() in Utils.h
const char clsPuTTY[] = "\033\143\033[3J";
#endif


// Rescale from one linear scale to another, e.g. -1.0 .. +1.0 -> 0.0 .. 200.0
// 
// NOTE: For small ranges of integers these can return invalid results (out by +-1)
// due to rounding errors, see https://github.com/arduino/ArduinoCore-API/issues/51
// Solution: use float values or multiply all values by (say) 1000, and round up 
// before converting back to int or dividing by the multiplier.

int16_t rescale(int16_t value, int16_t inmin, int16_t inmax, int16_t outmin, int16_t outmax)
{
	return outmin + ((int32_t)(value - inmin) * (int32_t)(outmax - outmin)) / (inmax - inmin);
}

uint16_t rescale(uint16_t value, uint16_t inmin, uint16_t inmax, uint16_t outmin, uint16_t outmax)
{
	return outmin + ((uint32_t)(value - inmin) * (uint32_t)(outmax - outmin)) / (inmax - inmin);
}

int32_t rescale(int32_t value, int32_t inmin, int32_t inmax, int32_t outmin, int32_t outmax)
{
	return outmin + ((int64_t)(value - inmin) * (int64_t)(outmax - outmin)) / (inmax - inmin);
}

uint32_t rescale(uint32_t value, uint32_t inmin, uint32_t inmax, uint32_t outmin, uint32_t outmax)
{
	return outmin + ((uint64_t)(value - inmin) * (uint64_t)(outmax - outmin)) / (inmax - inmin);
}

float rescale(float value, float inmin, float inmax, float outmin, float outmax)
{
	return outmin + ((value - inmin) * (outmax - outmin)) / (inmax - inmin);
}

double rescale(double value, double inmin, double inmax, double outmin, double outmax)
{
	return outmin + ((value - inmin) * (outmax - outmin)) / (inmax - inmin);
}

// Floating point conversion routines
// NOTE On 8-bit processors double and float are both 32-bits (both float).

/* 
atof() takes 5192 bytes! That's a lot if you only have 32KB flash.

The AVR Arduinos do not have full support for converting floating point 
numbers to ASCII strings. Functions like sprintf() and sscanf() will not 
convert floating point. The following '***ToString' and '***FromString' 
functions are optimized to be as fast as possible, way faster and smaller
than Arduino's dtostrf().

'psz' should should point to a buffer of at least 22 characters (including 
NUL). Buffer overflow is NOT checked! I use 32, just to be safe.

The maximum value accepted for ****ToString() is +-4294967000.0 
(unsigned long minus rounding errors) if the value is larger it returns 
false with "NAN" in *result.

The output format is 'n.n'. Exponent format ('e+04' etc) is not handled.
The max. number of decimal places is 6 for float and 8 for double. 
But note that float holds only 6 or 7 significant digits so the value
may be truncated and rounded up.
*/

#include <stdio.h>
#include <string.h>
#include <math.h>
#include <errno.h>

static const char utils_nan[] = "NAN"; // or "-E-" for 7-segment display
static const char utils_format1[] = "-%lu";
static const char utils_format2[] = "%lu";
static const char utils_format3[] = "-%lu.%0?lu";
static const char utils_format4[] = "%lu.%0?lu";


// Converts a float to a string with the format [-]1234567890[.123456]
// with max. 6 decimal places with 6 or 7 significant digits.
// The allowed range is +-4294967000.999999.
// Exponent format (e.g. "1e7") is not supported.
bool floatToString(float value, char* psz, unsigned int decimalPlaces)
{
        const float MAX_FORMAT_FLOAT = 4294967000.0f;

        bool negative = value < 0.0f;
        if (negative)
                value = -value;

        // no decimal places, integer part only
        if (decimalPlaces <= 0) {
                float v = value + 0.5f;    // round up
                if (v > MAX_FORMAT_FLOAT) {
                        strcpy(psz, utils_nan);
                        return false;
                }
                sprintf(psz, negative ? utils_format1 : utils_format2, (unsigned long)v);
                return true;
        }

        // max 6 decimal places for float
        if (decimalPlaces > 6)
                decimalPlaces = 6;

        // integer part
        if (value > MAX_FORMAT_FLOAT) {
                strcpy(psz, utils_nan);
                return false;
        }
        unsigned long left = (unsigned long)value;

        // decimal part, multiply by 10^decimalPlaces to get an integer
        float v = value - (float)left;
        const float pwr10[6] = 
                { 10.0f, 100.0f, 1000.0f, 10000.0f, 100000.0f, 1000000.0f };
        float pwr = pwr10[decimalPlaces - 1];
        v *= pwr;
        v += 0.5f;        // round up
        if (v > pwr) {    // handle overflow 
                v -= pwr;
                left += 1;
        }
        unsigned long right = (unsigned long)v;

        // format 'left.right' integers according to decimalPlaces
        char dp = '0' + decimalPlaces;
        char format[20];
        if (negative) {
                strcpy(format, utils_format3);
                format[7] = dp;
        }
        else {
                strcpy(format, utils_format4);
                format[6] = dp;
        }
        sprintf(psz, format, left, right);

        return true;
}


// Convert a string to a float
// String format: "[+|-]4294967295.123456"
// Exponents ('e+04' etc) are not handled.
// The integer part can be up to +-4294967295, the decimal part can be 
// up to 6 digits, but note that float holds only 6 or 7 significant 
// digits so it may be rounded/truncated.
// Returns false with *result = NAN if it fails to convert (bad format  
// or overflow).
// *pszEnd points to the next unprocessed character.
bool stringToFloat(const char* psz, float* result, char** pszEnd)
{
        *result = NAN;
        errno = 0;

        // skip leading spaces
        while (*psz == ' ')
                ++psz;

        // sign
        char ch = *psz;
        bool negative = ch == '-';
        if (negative || ch == '+')
                ch = *++psz;

        // must be at least 1 digit
        if (ch < '0' || ch > '9') {
                *pszEnd = (char*)psz;
                return false;
        }

        // integer part
        char* endPtr1;
        unsigned long left = strtoul(psz, &endPtr1, 10);
        if (errno)        // 'errno' is the only way to detect arithmetic overflow
                return false;
        float f = (float)left;
        ch = *endPtr1;
        if (ch != '.') {
                *pszEnd = endPtr1;
        }
        else {
                // decimal part
                ch = *++endPtr1;
                // must be at least 1 digit
                if (ch < '0' || ch > '9') {
                        *pszEnd = endPtr1;
                        return false;
                }
                char* endPtr2;
                unsigned long right = strtoul(endPtr1, &endPtr2, 10);
                *pszEnd = endPtr2;
                if (errno)
                        return false;
                if (right != 0) {
                        unsigned int decimalPlaces = (unsigned int)(endPtr2 - endPtr1);
                        if (decimalPlaces > 6)
                                return false;
                        const float pwr10[6] =
                                { 0.1f, 0.01f, 0.001f, 0.0001f, 0.00001f, 0.000001f };
                        f += (float)right * pwr10[decimalPlaces - 1];
                }
        }
        *result = (negative && f != 0.0f) ? -f : f;
        return true;
}

// Only if you need doubles, float and double are both floats on many MCUs
#if 1

// Converts a double to a string with the format [-]1234567890[.12345678]
// with max. 8 decimal places with 15..17 significant digits.
// The allowed range is +-4294967000.99999999.
// Exponent format (e.g. "1e7") is not supported.
bool doubleToString(double value, char* psz, unsigned int decimalPlaces)
{
        const double MAX_FORMAT_DOUBLE = 4294967000.0;

#ifdef DEBUG
        if (sizeof(double) == 4)
                LOG_ERROR("Double is not supported");
#endif

        bool negative = value < 0.0;
        if (negative)
                value = -value;

        // no decimal places, integer part only
        if (decimalPlaces <= 0) {
                double v = value + 0.5;    // round up
                if (v > MAX_FORMAT_DOUBLE) {
                        strcpy(psz, utils_nan);
                        return false;
                }
                sprintf(psz, negative ? utils_format1 : utils_format1, (unsigned long)v);
                return true;
        }

        // max 8 decimal places for double
        if (decimalPlaces > 8)
                decimalPlaces = 8;

        // integer part
        if (value > MAX_FORMAT_DOUBLE) {
                strcpy(psz, utils_nan);
                return false;
        }
        unsigned long left = (unsigned long)value;

        // decimal part, multiply by 10^decimalPlaces to get an integer
        double v = value - (double)left;
        const double pwr10[8] =
        { 10.0, 100.0, 1000.0, 10000.0, 100000.0, 1000000.0, 10000000.0, 100000000.0 };
        double pwr = pwr10[decimalPlaces - 1];
        v *= pwr;
        v += 0.5f;        // round up
        if (v > pwr) {    // handle overflow 
                v -= pwr;
                left += 1;
        }
        unsigned long right = (unsigned long)v;

        // format 'left.right' integers according to decimalPlaces
        char dp = '0' + decimalPlaces;
        char format[20];
        if (negative) {
                strcpy(format, utils_format3);
                format[7] = dp;
        }
        else {
                strcpy(format, utils_format4);
                format[6] = dp;
        }
        sprintf(psz, format, left, right);

        return true;
}


// Convert a string to a double
// String format: "[+|-]1234567890.12345678"
// Exponents ('e+04' etc) are not handled.
// The integer part can be up to +-4294967295, the decimal part can be
// up to 8 digits, but note that double holds 15..17 significant digits 
// so it may be rounded/truncated.
// Returns false with *result = NAN if it fails to convert (bad format
// or overflow).
// *pszEnd points to the next unprocessed character.
bool stringToDouble(const char* psz, double* result, char** pszEnd)
{
#ifdef DEBUG
        if (sizeof(double) == 4)
                LOG_ERROR("Double is not supported");
#endif

        *result = NAN;
        errno = 0;

        // skip leading spaces
        while (*psz == ' ')
                ++psz;

        // sign
        char ch = *psz;
        bool negative = ch == '-';
        if (negative || ch == '+')
                ch = *++psz;

        // must be at least 1 digit
        if (ch < '0' || ch > '9') {
                *pszEnd = (char*)psz;
                return false;
        }

        // integer part
        char* endPtr1;
        unsigned long left = strtoul(psz, &endPtr1, 10);
        if (errno)        // 'errno' is the only way to detect arithmetic overflow
                return false;
        double d = (double)left;
        ch = *endPtr1;
        if (ch != '.') {
                *pszEnd = endPtr1;
        }
        else {
                // decimal part
                ch = *++endPtr1;
                // must be at least 1 digit
                if (ch < '0' || ch > '9') {
                        *pszEnd = endPtr1;
                        return false;
                }
                char* endPtr2;
                unsigned long right = strtoul(endPtr1, &endPtr2, 10);
                *pszEnd = endPtr2;
                if (errno)
                        return false;
                unsigned int decimalPlaces = (unsigned int)(endPtr2 - endPtr1);
                if (decimalPlaces > 8)
                        return false;
                const double pwr10[8] =
                        { 0.1, 0.01, 0.001, 0.0001, 0.00001, 0.000001, 0.0000001, 0.00000001 };
                d += (double)right * pwr10[decimalPlaces - 1];
        }
        *result = (negative && d != 0.0) ? -d : d;
        return true;
}
#endif

#pragma once

// Switch Debouncer
// info@muman.ch, 2025.03.21
// All rights reversed
/*
Use this for debouncing pushbuttons or microswitches.

Microswitches must ALWAYS be debounced! Infrared speed sensors may not need it
if they use a comparator with hysterests like an LM393.

Unlike other debouncers, this code regsisters the switch activation (closing) 
immediately, which is great for emergency switches. It is only the decativation 
(opening) of the switch that is delayed by the debounceCount.

It works with active high or active low switches, accoring to 'activeState'.
The 'debounceCount' is the number of calls for which the state must be unchanged 
before it registers as inactive. For example, if called every 2mS, use '5' for a 
10mS debounce delay.

bool getState(bool* newState) returns true if *newState has changed, false if 
unchanged, so you don't have to check for state changes yourself.
*/

namespace MattLabs
{

	/// Use this for push buttons or end-stop switches.
	class DebouncedSwitch
	{
	private:
		int pin;
		int debCount;
		bool actState;
		
		bool firstCall;
		int debCounter;
		bool lastState;
		bool latchedState;

	public:
		DebouncedSwitch(int switchPin, int debounceCount, bool activeState);
		bool getState(bool* newState);
	};


	// Constructicator
	DebouncedSwitch::DebouncedSwitch(int switchPin, int debounceCount, bool activeState)
	{
		pin = switchPin; 
		pinMode(pin, INPUT);		// or INPUT_PULLUP to save a resistor
		debCount = debounceCount; 
		actState = activeState;
		firstCall = true;
		lastState = false;
		latchedState = false;
	}

	/// Returns true if *newState has changed, false if unchanged
	bool DebouncedSwitch::getState(bool* newState)
	{
		*newState = lastState;

		bool state = digitalRead(pin);
		if (!actState)				// active low
			state = !state;

		// register current state on first call
		if (firstCall) {
			firstCall = false;
			lastState = !state;
			latchedState = lastState;
		}

		// state has changed
		if (state != lastState) {
			// restart the debounce counter
			debCounter = debCount;
			lastState = state;
		}

		// switch closed
		if (state) {
			// if it has just been closed, register it as closed immediately
			// and start the release debounce counter
			if (!latchedState) {
				latchedState = true;
				debCounter = debCount;
				*newState = true;	// switch active
				return true;		// state has changed
			}
		}

		// switch was closed and now it's open
		// it must be open for the debounceCount before registering it as open
		else if (latchedState) {
			if (debCounter > 0)
				--debCounter;
			if (debCounter <= 0) {
				latchedState = false;
				*newState = false;	// switch inactive
				return true;		// state has changed
			}
		}
		return false;				// state unchanged
	}

}