flix

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

// Utility functions

#pragma once

#include <math.h>
#include <ESP32_NOW_Serial.h>
#include "flix.h"

const float ONE_G = 9.80665;

inline 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;
}

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

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

// Wrap angle to [-PI, PI)
inline 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
inline 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 = "";
}

// Simplified ESP-NOW Serial without tx buffering and resends
class ESPNOWSerial : public ESP_NOW_Serial_Class {
public:
	using ESP_NOW_Serial_Class::ESP_NOW_Serial_Class;
	void onSent(bool success) override {} // disable resends
	size_t write(const uint8_t *data, size_t len) override {
		return ESP_NOW_Peer::send(data, len); // pure send without buffering
	}
};

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

	operator bool() {
		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;
	}
};