flix/flix/util.h

// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix

// Utility functions

#pragma once

#include <math.h>
#ifdef ESP32
#include <soc/soc.h>
#include <soc/rtc_cntl_reg.h>
#include <ESP32_NOW_Serial.h>
#endif

const float ONE_G = 9.80665;
extern float t;

float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

bool invalid(float x) {
	return !isfinite(x);
}

bool valid(float x) {
	return isfinite(x);
}

bool floatEquals(float a, float b, float epsilon = 0) {
	if (isnan(a) && isnan(b)) return true;
	if (a == b) return true;
	return fabsf(a - b) <= epsilon;
}

// Wrap angle to [-PI, PI)
float wrapAngle(float angle) {
	angle = fmodf(angle, 2 * PI);
	if (angle > PI) {
		angle -= 2 * PI;
	} else if (angle < -PI) {
		angle += 2 * PI;
	}
	return angle;
}

// Trim and split string by spaces
void splitString(String& str, String& token0, String& token1, String& token2) {
	str.trim();
	if (str.isEmpty()) return;
	char chars[str.length() + 1];
	str.toCharArray(chars, str.length() + 1);
	token0 = strtok(chars, " ");
	token1 = strtok(NULL, " ");
	token2 = strtok(NULL, "");
	if (token1.c_str() == NULL) token1 = "";
	if (token2.c_str() == NULL) token2 = "";
}

#ifdef ESP32
// Simplified ESP-NOW Serial without resends
class ESPNOWSerial : public ESP_NOW_Serial_Class {
public:
	int lost = 0;
	using ESP_NOW_Serial_Class::ESP_NOW_Serial_Class;
	void onSent(bool success) override {
		if (!success) lost++;
		ESP_NOW_Serial_Class::onSent(true); // always report success to avoid resends
	}
};
#endif

// Rate limiter
class Rate {
public:
	float rate;
	float last = -INFINITY;
	Rate(float rate) : rate(rate) {}

	operator bool() {
		if (t == last) {
			return true; // the same step
		}
		if (t - last >= 1 / rate) {
			last = t;
			return true;
		}
		return false;
	}
};

// Delay filter for boolean signals - ensures the signal is on for at least 'delay' seconds
class Delay {
public:
	float delay;
	float start = NAN;
	Delay(float delay) : delay(delay) {}

	bool update(bool on) {
		if (!on) {
			start = NAN;
			return false;
		} else if (isnan(start)) {
			start = t;
		}
		return t - start >= delay;
	}
};