From 1005e50be08f9a27c868ca40f90484b901253139 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Mon, 18 May 2026 16:15:45 +0800 Subject: [PATCH] feat(track-core): add portable training runtimes Move scheme and PID training runtime behavior into the pure track_core layer and expose render sinks for injected strip application. Add ESP compatibility adapters, Python bindings/test scaffolding, and in-memory render support so app_track_bt can consume core render and runtime logic without duplicating it. Cover circular/linear rendering boundaries, all scheme runtime types, scheme render_to parity, PID sample de-duplication, speed suppression, and live tuning in track-core tests. --- .gitignore | 4 + CMakeLists.txt | 29 +- README.md | 17 + include/app_track_decoder.hpp | 59 ++ include/app_track_drawer.hpp | 40 + include/app_track_model.hpp | 1106 +++++++++++++++++++++++++ include/track_core/memory_strip.hpp | 3 + include/track_core/pid_runtime.hpp | 73 ++ include/track_core/render.hpp | 25 + include/track_core/scheme_runtime.hpp | 108 +++ pyproject.toml | 23 + python/tests/test_bindings.py | 211 +++++ python/track_core/__init__.py | 71 ++ python/track_core/emulator.py | 304 +++++++ src/esp/app_track_decoder.cpp | 85 ++ src/esp/app_track_drawer.cpp | 179 ++++ src/esp/app_track_model.cpp | 109 +++ src/memory_strip.cpp | 52 +- src/pid_runtime.cpp | 207 +++++ src/python/bindings.cpp | 310 +++++++ src/render.cpp | 73 +- src/scheme_decoder.cpp | 1 + src/scheme_runtime.cpp | 763 +++++++++++++++++ tests/track_core_tests.cpp | 332 ++++++++ 24 files changed, 4169 insertions(+), 15 deletions(-) create mode 100644 include/app_track_decoder.hpp create mode 100644 include/app_track_drawer.hpp create mode 100644 include/app_track_model.hpp create mode 100644 include/track_core/pid_runtime.hpp create mode 100644 include/track_core/scheme_runtime.hpp create mode 100644 pyproject.toml create mode 100644 python/tests/test_bindings.py create mode 100644 python/track_core/__init__.py create mode 100644 python/track_core/emulator.py create mode 100644 src/esp/app_track_decoder.cpp create mode 100644 src/esp/app_track_drawer.cpp create mode 100644 src/esp/app_track_model.cpp create mode 100644 src/pid_runtime.cpp create mode 100644 src/python/bindings.cpp create mode 100644 src/scheme_runtime.cpp diff --git a/.gitignore b/.gitignore index 86a300e..2c3b3e5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,7 @@ build/ +.venv/ +*.egg-info/ +__pycache__/ +.pytest_cache/ .DS_Store compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d9190e..56b44eb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,14 +2,27 @@ set(TRACK_CORE_SOURCES src/memory_strip.cpp src/model.cpp src/pid_program.cpp + src/pid_runtime.cpp src/render.cpp src/scheme_decoder.cpp + src/scheme_runtime.cpp ) if(DEFINED IDF_TARGET) + set(TRACK_CORE_IDF_SOURCES + src/esp/app_track_decoder.cpp + src/esp/app_track_drawer.cpp + src/esp/app_track_model.cpp + ) + idf_component_register( - SRCS ${TRACK_CORE_SOURCES} + SRCS ${TRACK_CORE_SOURCES} ${TRACK_CORE_IDF_SOURCES} INCLUDE_DIRS include + REQUIRES + app_proto + app_strip_if + app_utils + app_utils_clock ) else() cmake_minimum_required(VERSION 3.20) @@ -25,7 +38,21 @@ else() ) target_compile_features(track_core PUBLIC cxx_std_23) + option(TRACK_CORE_BUILD_PYTHON "Build Python bindings" OFF) option(TRACK_CORE_BUILD_TESTS "Build track-core tests" ON) + + if(TRACK_CORE_BUILD_PYTHON) + find_package(Python 3.9 COMPONENTS Interpreter Development.Module REQUIRED) + find_package(nanobind CONFIG REQUIRED) + + nanobind_add_module(_core + src/python/bindings.cpp + ) + target_link_libraries(_core PRIVATE track_core::track_core) + target_compile_features(_core PRIVATE cxx_std_23) + install(TARGETS _core LIBRARY DESTINATION track_core) + endif() + if(TRACK_CORE_BUILD_TESTS) enable_testing() add_executable(track_core_tests tests/track_core_tests.cpp) diff --git a/README.md b/README.md index de7d84e..272495f 100644 --- a/README.md +++ b/README.md @@ -15,3 +15,20 @@ ctest --test-dir build --output-on-failure When used inside an ESP-IDF project under `components/track-core`, the same `CMakeLists.txt` registers an IDF component. + +## Python bindings + +The Python package builds a nanobind extension for the emulator-facing API: + +```bash +python3 -m venv .venv +. .venv/bin/activate +python -m pip install -e ".[emulator,test]" +python -m pytest python/tests +python -m track_core.emulator +``` + +The DearPyGui emulator can either render a manual static report for boundary +checks or load an SM, MT, ST, or RSMT scheme and tick the portable training +runtime with explicit frame deltas. Both paths render pixels through the same +C++ planner used by firmware adapters. diff --git a/include/app_track_decoder.hpp b/include/app_track_decoder.hpp new file mode 100644 index 0000000..796832d --- /dev/null +++ b/include/app_track_decoder.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include +#include + +#include "app_track_model.hpp" +#include "track_core/scheme_decoder.hpp" + +namespace app::track { + +struct TrackSchemeDecoder { + using proto_type = track_app_TrackScheme; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackScheme_msg; + + TrackSchemeDecoder() = default; + + [[nodiscard]] + static TrackSchemeDecoder from_proto(const proto_type &proto); + + [[nodiscard]] + expected decode_core() const; + + uint8_t id = 0; + std::vector binary; + Color color; +}; + +struct TrackSchemeMgr { + using proto_type = track_app_TrackSchemeMgr; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgr_msg; + + struct Add { + using proto_type = track_app_TrackSchemeMgrAdd; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgrAdd_msg; + + TrackSchemeDecoder scheme_decoder; + error_t err{ESP_OK}; + + static Add from_proto(const proto_type &proto); + }; + + struct Clear {}; + + using Unknown = std::monostate; + + enum class MessageType : uint8_t { + NONE = 0, + ADD = 1, + CLEAR = 2 + }; + + explicit TrackSchemeMgr() = default; + + static TrackSchemeMgr from_proto(const proto_type &proto); + + std::variant choice{Unknown{}}; +}; + +} // namespace app::track diff --git a/include/app_track_drawer.hpp b/include/app_track_drawer.hpp new file mode 100644 index 0000000..d26dfa7 --- /dev/null +++ b/include/app_track_drawer.hpp @@ -0,0 +1,40 @@ +#ifndef D2AF35D4_5EEF_406F_A3F6_F7E6F2AA24C4 +#define D2AF35D4_5EEF_406F_A3F6_F7E6F2AA24C4 +#include "app_strip_if.hpp" +#include "app_track_model.hpp" +#include "track_core/render.hpp" +#include +#include + +namespace app::track { + +struct TrackRenderSpan { + uint16_t start_led{}; + uint16_t led_count{}; + Color color; +}; + +struct TrackRenderPlan { + static constexpr size_t MAX_SPANS = 4; + + void add_fill(uint16_t start_led, uint16_t led_count, Color color); + + [[nodiscard]] + bool empty() const { + return span_count == 0; + } + + std::array spans{}; + size_t span_count{}; +}; + +[[nodiscard]] +TrackRenderPlan make_track_render_plan(const TrackConfig &config, const TrackInfo &info, const report &rep); + +void apply_render_plan(app::strip::StripView strip, const TrackRenderPlan &plan); + +[[nodiscard]] +track_core::TrackRenderSink make_track_render_sink(app::strip::StripView &strip); +} + +#endif /* D2AF35D4_5EEF_406F_A3F6_F7E6F2AA24C4 */ diff --git a/include/app_track_model.hpp b/include/app_track_model.hpp new file mode 100644 index 0000000..2ad42de --- /dev/null +++ b/include/app_track_model.hpp @@ -0,0 +1,1106 @@ +#ifndef E32F663C_9C5C_4317_882A_E6457E88B576 +#define E32F663C_9C5C_4317_882A_E6457E88B576 +/** + * @brief ESP-IDF/protobuf compatibility model for the Track BLE service + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "app_track.pb.h" +#include "app_const.hpp" +#include "app_color.hpp" +#include "app_utils.hpp" +#include "esp_err.h" +#include "app_clock.hpp" +#include "esp_log_level.h" + +#ifndef APP_TRACK_C_ARRAY_SIZE +/** + * @brief A macro to calculate the size of a C-style array + * @param arr The array to calculate the size of + */ +#define APP_TRACK_C_ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) +#endif + +#ifndef __packed +#if defined(__GNUC__) || defined(__clang__) +#define __packed __attribute__((__packed__)) +#elif defined(_MSC_VER) +#define __packed __declspec(align(1)) +#else +#error "Unsupported compiler for __packed attribute" +#endif // __GNUC__ || __clang__ +#endif // __packed + +namespace app::track { +template +using expected = std::expected; + +template +using unexpected = std::unexpected; + +template +using optional = std::optional; + +using unit = std::monostate; + +using error_t = esp_err_t; + +using SchemeKind = track_app_TrackSchemeKind; +using AccelerationProfile = track_app_TrackAccelerationProfile; + + +struct Color { + using proto_type = track_app_Color; + static inline const pb_msgdesc_t *pb_fields = &track_app_Color_msg; + + + union { + struct { + uint8_t r; + uint8_t g; + uint8_t b; + }; + uint8_t bytes[3]; + } inner{}; + + Color() = default; + constexpr Color(uint8_t r, uint8_t g, uint8_t b) : inner{.r = r, .g = g, .b = b} {} + constexpr Color(uint32_t value) { + inner.r = (value >> 16) & 0xFF; + inner.g = (value >> 8) & 0xFF; + inner.b = value & 0xFF; + } + constexpr Color(std::span bytes) { + inner.r = bytes[0]; + inner.g = bytes[1]; + inner.b = bytes[2]; + } + + constexpr bool operator==(const Color &other) const { + return inner.r == other.inner.r && + inner.g == other.inner.g && + inner.b == other.inner.b; + } + + constexpr bool operator!=(const Color &other) const { + return not(*this == other); + } + + operator uint32_t() const { + return ((uint32_t)inner.r << 16) | ((uint32_t)inner.g << 8) | inner.b; + } + + explicit operator std::string() const { + return std::format("#{:02X}{:02X}{:02X}", + static_cast(inner.r), + static_cast(inner.g), + static_cast(inner.b)); + } + + [[nodiscard]] + std::string string() const { + return static_cast(*this); + } + + static Color from_proto(const proto_type &proto) { + return {proto.r, proto.g, proto.b}; + } + + static Color black() { + return {0, 0, 0}; + } + + static Color green() { + return {0, 255, 0}; + } + + static Color red() { + return {255, 0, 0}; + } + + static Color blue() { + return {0, 0, 255}; + } + + static Color white() { + return {255, 255, 255}; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto{}; + proto.r = inner.r; + proto.g = inner.g; + proto.b = inner.b; + return proto; + } + + [[nodiscard]] + app::utils::Color to_app_color() const { + return app::utils::Color{inner.r, inner.g, inner.b}; + } +}; + +struct TrackInfo { + SchemeKind kind{track_app_TrackSchemeKind_SPEED_INPUT_TIME_SEGMENTED_MILEAGE_FREE}; + Color color{Color::white()}; + uint8_t id{}; + bool is_running{}; + uint8_t num_segments{}; +}; + +struct TrackConfig { + using proto_type = track_app_TrackConfig; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackConfig_msg; + using TrackDrawKind = track_app_TrackDrawKind; + + /** properties */ + TrackDrawKind draw_kind{track_app_TrackDrawKind_CIRCULAR}; + float line_length_m{0.0F}; + float active_line_length_m{0.0F}; + float head_offset_m{0.0F}; + uint16_t line_leds_num{0}; + + [[nodiscard]] + float led_distance() const { + return line_leds_num > 0 ? line_length_m / static_cast(line_leds_num) : 0.0F; + } + + static TrackConfig from_proto(const proto_type &proto) { + TrackConfig config; + config.draw_kind = static_cast(proto.draw_kind); + config.line_length_m = proto.line_length_m; + config.active_line_length_m = proto.active_line_length_m; + config.head_offset_m = proto.head_offset_m; + config.line_leds_num = proto.line_leds_num; + return config; + } + + /** + * @brief a reasonable default configuration + */ + static TrackConfig Default() { + return {track_app_TrackDrawKind_CIRCULAR, + 400.0F, + 10.0F, + 0.0F, + 400}; + } + + [[nodiscard]] + bool verify() const { + bool length_non_zero_ok = line_length_m > 0.0F && active_line_length_m > 0.0F; + if (not length_non_zero_ok) { + return false; + } + // active line length cannot exceed total line length + if (active_line_length_m > line_length_m) { + return false; + } + // head offset must be within the line length + if (head_offset_m >= line_length_m) { + return false; + } + return true; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto{}; + proto.draw_kind = static_cast(draw_kind); + proto.line_length_m = line_length_m; + proto.active_line_length_m = active_line_length_m; + proto.head_offset_m = head_offset_m; + proto.line_leds_num = line_leds_num; + return proto; + } + + void log(const char *tag, esp_log_level_t level) const; +}; + + +struct TrackSchemeMgrRead { + using proto_type = track_app_TrackSchemeMgrRead; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgrRead_msg; + static constexpr size_t MAX_SCHEMES = APP_TRACK_C_ARRAY_SIZE( + track_app_TrackSchemeMgrRead::scheme_status); + + struct Status { + using proto_type = track_app_TrackSchemeMgrStatus; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackSchemeMgrStatus_msg; + + uint8_t id = 0; + uint8_t segment_count = 0; + + static Status from_proto(const proto_type &proto) { + return {proto.id, proto.segment_count}; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto{}; + proto.id = id; + proto.segment_count = segment_count; + return proto; + } + }; + + static TrackSchemeMgrRead from_proto(const proto_type &proto) { + TrackSchemeMgrRead collection; + collection.scheme_status.reserve(proto.scheme_status_count); + for (pb_size_t i = 0; i < proto.scheme_status_count; ++i) { + collection.scheme_status.push_back(Status::from_proto(proto.scheme_status[i])); + } + return collection; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackSchemeMgrRead_init_default; + proto.scheme_status_count = std::min(scheme_status.size(), MAX_SCHEMES); + for (size_t i = 0; i < proto.scheme_status_count; ++i) { + proto.scheme_status[i] = scheme_status[i].to_proto(); + } + return proto; + } + + /** properties */ + std::vector scheme_status; +}; + +using TrackState = track_app_TrackState; +using TrackControllerMode = track_app_TrackControllerMode; +using TrackPidStageKind = track_app_TrackPidStageKind; + +constexpr float PID_COMPAT_OUTPUT_SCALE_MAX_M_S = 10.0F; + +struct TrackPidFineTune { + using proto_type = track_app_TrackPidFineTune; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidFineTune_msg; + + uint8_t band_plus{0}; + uint8_t band_minus{0}; + float gain_scale{0.0F}; + + static TrackPidFineTune from_proto(const proto_type &proto) { + return { + static_cast(proto.band_plus), + static_cast(proto.band_minus), + proto.gain_scale, + }; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidFineTune_init_default; + proto.band_plus = band_plus; + proto.band_minus = band_minus; + proto.gain_scale = gain_scale; + return proto; + } +}; + +struct TrackPidSpeedSuppression { + using proto_type = track_app_TrackPidSpeedSuppression; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSpeedSuppression_msg; + + float ratio_min{0.1F}; + float sigma_m{1.0F}; + + static TrackPidSpeedSuppression from_proto(const proto_type &proto) { + return { + .ratio_min = proto.ratio_min, + .sigma_m = proto.sigma_m, + }; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidSpeedSuppression_init_default; + proto.ratio_min = ratio_min; + proto.sigma_m = sigma_m; + return proto; + } +}; + +struct TrackPidSegment { + using proto_type = track_app_TrackPidSegment; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSegment_msg; + + uint16_t duration_s{0}; + float min_speed_m_s{0.0F}; + float max_speed_m_s{0.0F}; + float kp{0.0F}; + float ki{1.0F}; + float kd{0.0F}; + float slew_rate_limit{0.0F}; + optional fine_tune; + + static TrackPidSegment Default() { + return { + .duration_s = 300, + .min_speed_m_s = 0.0F, + .max_speed_m_s = 4.0F, + .kp = app::global::constants::DEFAULT_PID_Kp, + .ki = app::global::constants::DEFAULT_PID_Ki, + .kd = app::global::constants::DEFAULT_PID_Kd, + .slew_rate_limit = app::global::constants::DEFAULT_PID_SLEW_RATE_LIMIT_M_S, + .fine_tune = std::nullopt, + }; + } + + static TrackPidSegment from_proto(const proto_type &proto) { + TrackPidSegment segment; + segment.duration_s = static_cast(proto.duration_s); + segment.min_speed_m_s = proto.min_speed_m_s; + segment.max_speed_m_s = proto.max_speed_m_s; + segment.kp = proto.kp; + segment.ki = proto.ki; + segment.kd = proto.kd; + segment.slew_rate_limit = proto.slew_rate_limit; + if (proto.has_fine_tune) { + segment.fine_tune = TrackPidFineTune::from_proto(proto.fine_tune); + } + return segment; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidSegment_init_default; + proto.duration_s = duration_s; + proto.min_speed_m_s = min_speed_m_s; + proto.max_speed_m_s = max_speed_m_s; + proto.kp = kp; + proto.ki = ki; + proto.kd = kd; + proto.slew_rate_limit = slew_rate_limit; + if (fine_tune) { + proto.has_fine_tune = true; + proto.fine_tune = fine_tune->to_proto(); + } + return proto; + } +}; + +struct TrackConstantSegment { + using proto_type = track_app_TrackConstantSegment; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackConstantSegment_msg; + + uint16_t duration_s{0}; + float speed_m_s{0.0F}; + + static TrackConstantSegment from_proto(const proto_type &proto) { + return { + .duration_s = static_cast(proto.duration_s), + .speed_m_s = proto.speed_m_s, + }; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackConstantSegment_init_default; + proto.duration_s = duration_s; + proto.speed_m_s = speed_m_s; + return proto; + } +}; + +struct TrackPidSchema { + using proto_type = track_app_TrackPidSchema; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSchema_msg; + using variant_type = std::variant; + + variant_type segment{TrackConstantSegment{}}; + + static expected try_from_proto(const proto_type &proto) { + switch (proto.which_segment) { + case track_app_TrackPidSchema_pid_tag: + return TrackPidSchema{TrackPidSegment::from_proto(proto.segment.pid)}; + case track_app_TrackPidSchema_constant_tag: + return TrackPidSchema{TrackConstantSegment::from_proto(proto.segment.constant)}; + default: + return unexpected{ESP_ERR_INVALID_ARG}; + } + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidSchema_init_default; + std::visit(app::utils::overloads{ + [&](const TrackPidSegment &pid) { + proto.which_segment = track_app_TrackPidSchema_pid_tag; + proto.segment.pid = pid.to_proto(); + }, + [&](const TrackConstantSegment &constant) { + proto.which_segment = track_app_TrackPidSchema_constant_tag; + proto.segment.constant = constant.to_proto(); + }}, + segment); + return proto; + } + + [[nodiscard]] + TrackPidStageKind kind() const { + return std::holds_alternative(segment) + ? track_app_TrackPidStageKind_PID + : track_app_TrackPidStageKind_CONSTANT; + } +}; + +struct TrackPidConfig { + using proto_type = track_app_TrackPidConfig; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidConfig_msg; + + uint8_t band_id{0}; + uint8_t target_hr_bpm{120}; + uint8_t deadzone_bpm{3}; + bool preemptive_pid_activation{false}; + optional speed_suppression; + std::vector schemas{}; + + static TrackPidConfig Default() { + TrackPidConfig config; + config.schemas.push_back(TrackPidSchema{TrackPidSegment::Default()}); + return config; + } + + static TrackPidConfig from_proto(const proto_type &proto) { + auto config = try_from_proto(proto); + if (config) { + return *config; + } + return Default(); + } + + [[nodiscard]] + bool empty() const { + return schemas.empty(); + } + + [[nodiscard]] + static expected try_from_proto(const proto_type &proto) { + return try_from_proto_impl(proto, false); + } + + [[nodiscard]] + static expected try_from_proto_allow_empty(const proto_type &proto) { + return try_from_proto_impl(proto, true); + } + +private: + [[nodiscard]] + static expected try_from_proto_impl(const proto_type &proto, bool allow_empty) { + TrackPidConfig config; + config.band_id = static_cast(proto.band_id); + config.target_hr_bpm = static_cast(proto.target_hr_bpm); + config.deadzone_bpm = static_cast(proto.deadzone_bpm); + config.preemptive_pid_activation = proto.preemptive_pid_activation; + if (proto.has_speed_suppression) { + config.speed_suppression = TrackPidSpeedSuppression::from_proto(proto.speed_suppression); + } + config.schemas.reserve(proto.schemas_count); + for (pb_size_t i = 0; i < proto.schemas_count; ++i) { + auto schema = TrackPidSchema::try_from_proto(proto.schemas[i]); + if (!schema) { + return unexpected{schema.error()}; + } + config.schemas.push_back(*schema); + } + auto validation = config.validate(allow_empty); + if (!validation) { + return unexpected{validation.error()}; + } + return {std::move(config)}; + } + +public: + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidConfig_init_default; + proto.band_id = band_id; + proto.target_hr_bpm = target_hr_bpm; + proto.deadzone_bpm = deadzone_bpm; + proto.preemptive_pid_activation = preemptive_pid_activation; + if (speed_suppression) { + proto.has_speed_suppression = true; + proto.speed_suppression = speed_suppression->to_proto(); + } + proto.schemas_count = std::min(schemas.size(), APP_TRACK_C_ARRAY_SIZE(proto.schemas)); + for (size_t i = 0; i < proto.schemas_count; ++i) { + proto.schemas[i] = schemas[i].to_proto(); + } + return proto; + } + + [[nodiscard]] + expected validate(bool allow_empty = false) const { + const auto speed_suppression_ok = !speed_suppression.has_value() || + (std::isfinite(speed_suppression->ratio_min) && + std::isfinite(speed_suppression->sigma_m) && + speed_suppression->ratio_min > 0.0F && + speed_suppression->ratio_min < 1.0F && + speed_suppression->sigma_m > 0.0F); + if (!speed_suppression_ok) { + return unexpected{ESP_ERR_INVALID_ARG}; + } + if (schemas.empty()) { + if (allow_empty) { + return {}; + } + return unexpected{ESP_ERR_INVALID_ARG}; + } + for (const auto &schema : schemas) { + auto ok = std::visit(app::utils::overloads{ + [](const TrackPidSegment &pid) { + return pid.duration_s > 0 && + std::isfinite(pid.min_speed_m_s) && + std::isfinite(pid.max_speed_m_s) && + std::isfinite(pid.kp) && + std::isfinite(pid.ki) && + std::isfinite(pid.kd) && + std::isfinite(pid.slew_rate_limit) && + (!pid.fine_tune.has_value() || + (std::isfinite(pid.fine_tune->gain_scale) && + pid.fine_tune->gain_scale >= 0.0F)) && + pid.min_speed_m_s >= 0.0F && + pid.max_speed_m_s >= 0.0F && + pid.min_speed_m_s <= pid.max_speed_m_s && + pid.kp >= 0.0F && + pid.ki >= 0.0F && + pid.kd >= 0.0F && + pid.slew_rate_limit >= 0.0F; + }, + [](const TrackConstantSegment &constant) { + return constant.duration_s > 0 && + std::isfinite(constant.speed_m_s) && + constant.speed_m_s >= 0.0F; + }}, + schema.segment); + if (!ok) { + return unexpected{ESP_ERR_INVALID_ARG}; + } + } + return {}; + } +}; + +struct TrackPidSetTargetHr { + using proto_type = track_app_TrackPidSetTargetHr; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSetTargetHr_msg; + + uint8_t target_hr_bpm{120}; + + static TrackPidSetTargetHr from_proto(const proto_type &proto) { + return { + .target_hr_bpm = static_cast(proto.target_hr_bpm), + }; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidSetTargetHr_init_default; + proto.target_hr_bpm = target_hr_bpm; + return proto; + } +}; + +struct TrackPidSetTuning { + using proto_type = track_app_TrackPidSetTuning; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidSetTuning_msg; + + float min_speed_m_s{0.0F}; + float max_speed_m_s{0.0F}; + float kp{0.0F}; + float ki{1.0F}; + float kd{0.0F}; + float slew_rate_limit{0.0F}; + optional fine_tune; + + static TrackPidSetTuning from_proto(const proto_type &proto) { + TrackPidSetTuning tuning; + tuning.min_speed_m_s = proto.min_speed_m_s; + tuning.max_speed_m_s = proto.max_speed_m_s; + tuning.kp = proto.kp; + tuning.ki = proto.ki; + tuning.kd = proto.kd; + tuning.slew_rate_limit = proto.slew_rate_limit; + if (proto.has_fine_tune) { + tuning.fine_tune = TrackPidFineTune::from_proto(proto.fine_tune); + } + return tuning; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidSetTuning_init_default; + proto.min_speed_m_s = min_speed_m_s; + proto.max_speed_m_s = max_speed_m_s; + proto.kp = kp; + proto.ki = ki; + proto.kd = kd; + proto.slew_rate_limit = slew_rate_limit; + if (fine_tune) { + proto.has_fine_tune = true; + proto.fine_tune = fine_tune->to_proto(); + } + return proto; + } + + [[nodiscard]] + expected validate() const { + const auto fine_tune_ok = !fine_tune.has_value() || + (std::isfinite(fine_tune->gain_scale) && fine_tune->gain_scale >= 0.0F); + if (!std::isfinite(min_speed_m_s) || + !std::isfinite(max_speed_m_s) || + !std::isfinite(kp) || + !std::isfinite(ki) || + !std::isfinite(kd) || + !std::isfinite(slew_rate_limit) || + !fine_tune_ok || + min_speed_m_s < 0.0F || + max_speed_m_s < 0.0F || + min_speed_m_s > max_speed_m_s || + kp < 0.0F || + ki < 0.0F || + kd < 0.0F || + slew_rate_limit < 0.0F) { + return unexpected{ESP_ERR_INVALID_ARG}; + } + return {}; + } + + void apply_to(TrackPidSegment &segment) const { + segment.min_speed_m_s = min_speed_m_s; + segment.max_speed_m_s = max_speed_m_s; + segment.kp = kp; + segment.ki = ki; + segment.kd = kd; + segment.slew_rate_limit = slew_rate_limit; + segment.fine_tune = fine_tune; + } +}; + +struct TrackPidRuntimeCommand { + using proto_type = track_app_TrackPidRuntimeCommand; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidRuntimeCommand_msg; + + std::variant command{unit{}}; + + static TrackPidRuntimeCommand from_proto(const proto_type &proto) { + switch (proto.which_command) { + case track_app_TrackPidRuntimeCommand_set_target_hr_tag: + return TrackPidRuntimeCommand{ + .command = TrackPidSetTargetHr::from_proto(proto.command.set_target_hr), + }; + case track_app_TrackPidRuntimeCommand_set_tuning_tag: + return TrackPidRuntimeCommand{ + .command = TrackPidSetTuning::from_proto(proto.command.set_tuning), + }; + default: + return TrackPidRuntimeCommand{}; + } + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidRuntimeCommand_init_default; + std::visit(app::utils::overloads{ + [](unit) { + // do nothing + }, + [&](const TrackPidSetTargetHr &target_hr) { + proto.which_command = track_app_TrackPidRuntimeCommand_set_target_hr_tag; + proto.command.set_target_hr = target_hr.to_proto(); + }, + [&](const TrackPidSetTuning &tuning) { + proto.which_command = track_app_TrackPidRuntimeCommand_set_tuning_tag; + proto.command.set_tuning = tuning.to_proto(); + }}, + command); + return proto; + } +}; + +struct TrackPidStatus { + using proto_type = track_app_TrackPidStatus; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackPidStatus_msg; + + uint8_t band_id{0}; + bool band_is_active{false}; + bool is_heart_rate_valid{false}; + uint8_t heart_rate_bpm{0}; + uint16_t step_count{0}; + uint8_t active_segment_index{0}; + TrackPidStageKind active_segment_kind{track_app_TrackPidStageKind_NONE}; + float effective_speed_m_s{0.0F}; + uint32_t remaining_program_ms{0}; + float base_speed_m_s{0.0F}; + + static TrackPidStatus Default() { + return { + .band_id = 0, + .band_is_active = false, + .is_heart_rate_valid = false, + .heart_rate_bpm = 0, + .step_count = 0, + .active_segment_index = 0, + .active_segment_kind = track_app_TrackPidStageKind_NONE, + .effective_speed_m_s = 0.0F, + .remaining_program_ms = 0, + .base_speed_m_s = 0.0F, + }; + } + + static TrackPidStatus from_proto(const proto_type &proto) { + return { + .band_id = static_cast(proto.band_id), + .band_is_active = proto.band_is_active, + .is_heart_rate_valid = proto.is_heart_rate_valid, + .heart_rate_bpm = static_cast(proto.heart_rate_bpm), + .step_count = static_cast(proto.step_count), + .active_segment_index = static_cast(proto.active_segment_index), + .active_segment_kind = proto.active_segment_kind, + .effective_speed_m_s = proto.effective_speed_m_s, + .remaining_program_ms = proto.remaining_program_ms, + .base_speed_m_s = proto.base_speed_m_s, + }; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackPidStatus_init_default; + proto.band_id = band_id; + proto.band_is_active = band_is_active; + proto.is_heart_rate_valid = is_heart_rate_valid; + proto.heart_rate_bpm = heart_rate_bpm; + proto.step_count = step_count; + proto.active_segment_index = active_segment_index; + proto.active_segment_kind = active_segment_kind; + proto.effective_speed_m_s = effective_speed_m_s; + proto.remaining_program_ms = remaining_program_ms; + proto.base_speed_m_s = base_speed_m_s; + return proto; + } +}; + +struct TrackTestParameters { + using proto_type = track_app_TrackTestParameters; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackTestParameters_msg; + + /** properties */ + Color blink_color{Color::green()}; + uint8_t blink_duty_cycle_percent{128}; + uint32_t blink_period_ms{1'000}; + uint32_t rainbow_move_speed_leds_per_sec{0}; + uint16_t segment_led_start{0}; + uint16_t segment_led_count{0}; + uint16_t highlight_last_n_leds{0}; + + static TrackTestParameters from_proto(const proto_type &proto) { + TrackTestParameters params; + if (proto.has_blink_color) { + params.blink_color = Color::from_proto(proto.blink_color); + } + params.blink_duty_cycle_percent = proto.blink_duty_cycle_percent; + params.blink_period_ms = proto.blink_period_ms; + params.rainbow_move_speed_leds_per_sec = proto.rainbow_move_speed_leds_per_sec; + params.segment_led_start = proto.segment_led_start; + params.segment_led_count = proto.segment_led_count; + params.highlight_last_n_leds = proto.highlight_last_n_leds; + return params; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto{}; + proto.has_blink_color = true; + proto.blink_color = blink_color.to_proto(); + proto.blink_duty_cycle_percent = blink_duty_cycle_percent; + proto.blink_period_ms = blink_period_ms; + proto.rainbow_move_speed_leds_per_sec = rainbow_move_speed_leds_per_sec; + proto.segment_led_start = segment_led_start; + proto.segment_led_count = segment_led_count; + proto.highlight_last_n_leds = highlight_last_n_leds; + return proto; + } + + + void log(const char *tag, esp_log_level_t level) const { + ESP_LOG_LEVEL(level, tag, + "TrackTestParameters{" + ".blink_color=%s, " + ".blink_duty_cycle_percent=%" PRIu8 ", " + ".blink_period_ms=%" PRIu32 ", " + ".rainbow_move_speed_leds_per_sec=%" PRIu32 ", " + ".segment_led_start=%" PRIu16 ", " + ".segment_led_count=%" PRIu16 "}", + blink_color.string().c_str(), + blink_duty_cycle_percent, + blink_period_ms, + rainbow_move_speed_leds_per_sec, + segment_led_start, + segment_led_count); + } +}; + +struct TrackControlMsg { + using proto_type = track_app_TrackControlMsg; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackControlMsg_msg; + + /** properties */ + std::variant state{unit{}}; + + static TrackControlMsg from_proto(const proto_type &proto) { + switch (proto.which_msg) { + case track_app_TrackControlMsg_set_state_tag: + return TrackControlMsg{.state = static_cast(proto.msg.set_state)}; + case track_app_TrackControlMsg_set_test_parameters_tag: + return TrackControlMsg{.state = TrackTestParameters::from_proto(proto.msg.set_test_parameters)}; + case track_app_TrackControlMsg_set_mode_tag: + return TrackControlMsg{.state = static_cast(proto.msg.set_mode)}; + default: + return TrackControlMsg{.state = unit{}}; + } + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackControlMsg_init_default; + std::visit(app::utils::overloads{ + [](unit) { + // do nothing + }, + [&](TrackState track_state) { + proto.which_msg = track_app_TrackControlMsg_set_state_tag; + proto.msg.set_state = static_cast(track_state); + }, + [&](const TrackTestParameters &test_params) { + proto.which_msg = track_app_TrackControlMsg_set_test_parameters_tag; + proto.msg.set_test_parameters = test_params.to_proto(); + }, + [&](TrackControllerMode mode) { + proto.which_msg = track_app_TrackControlMsg_set_mode_tag; + proto.msg.set_mode = static_cast(mode); + }}, + state); + return proto; + } +}; + + +struct TrackStateReportCollection { + using proto_type = track_app_TrackStateReportCollection; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackStateReportCollection_msg; + constexpr static size_t MAX_STATES = APP_TRACK_C_ARRAY_SIZE(proto_type::states); + + struct Report { + using proto_type = track_app_TrackStateReport; + static inline const pb_msgdesc_t *pb_fields = &track_app_TrackStateReport_msg; + + uint8_t id{0}; + TrackState state{TrackState::track_app_TrackState_STOP}; + float mileage_m{0.0F}; + float speed_m_s{0.0F}; + uint32_t time_elapsed_ms{0}; + + static Report from_proto(const proto_type &proto) { + return {proto.id, + static_cast(proto.state), + proto.mileage_m, + proto.speed_m_s, + proto.time_elapsed_ms}; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto{}; + proto.id = id; + proto.state = static_cast(state); + proto.mileage_m = mileage_m; + proto.speed_m_s = speed_m_s; + proto.time_elapsed_ms = time_elapsed_ms; + return proto; + } + }; + + /** properties */ + std::vector states; + + static TrackStateReportCollection from_proto(const proto_type &proto) { + TrackStateReportCollection collection; + collection.states.reserve(proto.states_count); + for (pb_size_t i = 0; i < proto.states_count; ++i) { + collection.states.push_back(Report::from_proto(proto.states[i])); + } + return collection; + } + + [[nodiscard]] + proto_type to_proto() const { + proto_type proto = track_app_TrackStateReportCollection_init_default; + proto.states_count = std::min(states.size(), MAX_STATES); + for (size_t i = 0; i < proto.states_count; ++i) { + proto.states[i] = states[i].to_proto(); + } + return proto; + } + + void log(const char *tag, esp_log_level_t level) const; +}; + +/** + * @brief Speed-Mileage training segment + */ +struct SMSegment { + struct __attribute__((packed)) raw { + static constexpr float SPEED_LSB = 10 / 255.0F; + /** + * @brief Speed in m/s + * @note LSB: ((10m/s) / 255) = 0.039m/s + */ + uint8_t speed_m_s; + uint16_t mileage_from_start_m; + }; + static constexpr size_t RAW_SIZE = sizeof(raw); + + static SMSegment from_raw(const raw &segment) { + return {.speed_m_s = static_cast(segment.speed_m_s) * raw::SPEED_LSB, + .mileage_from_start_m = segment.mileage_from_start_m}; + } + + [[nodiscard]] + raw to_raw() const { + raw segment{ + .speed_m_s = static_cast(std::round(speed_m_s / raw::SPEED_LSB)), + .mileage_from_start_m = mileage_from_start_m}; + return segment; + } + + + /** properties */ + + /** + * @brief speed in m/s + */ + float speed_m_s; + /** + * @brief absolute start mileage since the training (not difference nor the mileage the segment should maintain) + */ + uint16_t mileage_from_start_m; +}; + +/** + * @brief Time-Mileage training segment + */ +struct MTSegment { + /** + * @brief not the absolute mileage from start, but the mileage to travel in this segment + */ + uint16_t mileage_to_travel_this_segment_m; + /** + * @brief absolute time since the start of training (not time difference since last segment) + */ + uint16_t time_since_start_s; +}; + +/** + * @brief Speed-Time training segment + */ +struct STSegment { + struct __attribute__((packed)) raw { + static constexpr float SPEED_LSB = 10 / 255.0F; + /** + * @brief Speed in m/s + * @note LSB: ((10m/s) / 255) = 0.039m/s + */ + uint8_t speed_m_s; + uint16_t time_since_start_s; + }; + static constexpr size_t RAW_SIZE = sizeof(raw); + + static STSegment from_raw(const raw &segment) { + return {.speed_m_s = static_cast(segment.speed_m_s) * raw::SPEED_LSB, + .time_since_start_s = segment.time_since_start_s}; + } + + [[nodiscard]] + raw to_raw() const { + raw segment{ + .speed_m_s = static_cast(std::round(speed_m_s / raw::SPEED_LSB)), + .time_since_start_s = time_since_start_s}; + return segment; + } + + + /** properties */ + + /** + * @brief speed to maintain in m/s + */ + float speed_m_s; + /** + * @brief + */ + uint16_t time_since_start_s; +}; + +struct RepeatedSMSegment { + /** properties */ + std::vector speed_mileage_segments; + uint16_t time_since_start_s; +}; + + +using config_getter = std::function; +using report = TrackStateReportCollection::Report; +/** + * @brief provide `clock::now()` as the method to get the current timestamp + */ +using clock = app::utils::clock_t; + +const char *to_str(AccelerationProfile profile); + +/** + * @brief describes an acceleration process + */ +struct accel_calc_result_t { + float target_speed_m_s; + /** + * @brief acceleration in m/s^2; when 0, no acceleration is applied + * @note calculated at switching stage; note that it's an absolute value + * if the `target_speed_m_s` is higher than the current speed, + * the acceleration is positive, otherwise negative + * + * basically, depending the distance to the next segment, + * an acceleration rule is applied to the track + * (i.e. when to accelerate, and how fast to accelerate) + * it should be linear acceleration though + * + * if this value changed dynamically, it could be used as lerp + */ + float acceleration_m_s_2; +}; + +void set_global_config_getter(config_getter getter); +const TrackConfig &global_config(); + +const char *to_str(TrackState status); +const char *to_str(TrackControllerMode mode); +const char *to_str(TrackPidStageKind kind); +} + +#endif /* E32F663C_9C5C_4317_882A_E6457E88B576 */ diff --git a/include/track_core/memory_strip.hpp b/include/track_core/memory_strip.hpp index 5883cac..170ee33 100644 --- a/include/track_core/memory_strip.hpp +++ b/include/track_core/memory_strip.hpp @@ -46,4 +46,7 @@ private: [[nodiscard]] TrackError apply_render_plan(MemoryStrip &strip, const TrackRenderPlan &plan); +[[nodiscard]] +TrackRenderSink make_memory_strip_sink(MemoryStrip &strip); + } // namespace track_core diff --git a/include/track_core/pid_runtime.hpp b/include/track_core/pid_runtime.hpp new file mode 100644 index 0000000..5e18e3d --- /dev/null +++ b/include/track_core/pid_runtime.hpp @@ -0,0 +1,73 @@ +#pragma once + +#include +#include +#include + +#include "track_core/model.hpp" +#include "track_core/pid_program.hpp" + +namespace track_core { + +struct TrackPidBandSnapshot { + std::uint8_t band_id{}; + std::uint8_t heart_rate{}; + std::uint32_t heart_rate_sample_seq{}; + bool has_heart_rate{}; + bool hr_is_fresh{}; + std::uint16_t step_count{}; + bool has_step_count{}; + bool band_is_active{}; +}; + +class PidHrRuntime { +public: + using time_point = clock::time_point; + + PidHrRuntime(); + explicit PidHrRuntime(TrackPidConfig config); + + void set_pid_config(TrackPidConfig config); + void update_target_hr_bpm(std::uint8_t target_hr_bpm); + + [[nodiscard]] + expected apply_pid_runtime_command( + const TrackPidRuntimeCommand &command, + const TrackConfig *track_config); + + [[nodiscard]] + const TrackPidConfig &pid_config() const; + + void start(time_point now); + void stop(); + void tick(const TrackConfig *track_config, const TrackPidBandSnapshot &band, time_point now); + + [[nodiscard]] + TrackReport state_report(time_point now) const; + + [[nodiscard]] + TrackInfo info() const; + + [[nodiscard]] + TrackPidStatus pid_status(const TrackPidBandSnapshot &band, time_point now) const; + +private: + static constexpr std::uint8_t magic_pid_track_id = 0; + + [[nodiscard]] + float effective_speed_m_s(float base_speed_m_s, const TrackConfig *track_config) const; + + TrackPidConfig config_{TrackPidConfig::default_config()}; + bool running_{false}; + Color color_{Color::white()}; + time_point start_timestamp_{}; + time_point last_tick_timestamp_{}; + std::optional last_consumed_hr_sample_seq_; + time_point last_consumed_hr_sample_time_{}; + float mileage_m_{0.0F}; + float base_speed_m_s_{0.0F}; + float effective_speed_m_s_{0.0F}; + std::unique_ptr program_state_; +}; + +} // namespace track_core diff --git a/include/track_core/render.hpp b/include/track_core/render.hpp index ac6ae40..111cdf3 100644 --- a/include/track_core/render.hpp +++ b/include/track_core/render.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "track_core/model.hpp" @@ -27,10 +28,34 @@ struct TrackRenderPlan { std::size_t span_count{}; }; +struct TrackRenderSink { + using ClearFn = TrackError (*)(void *context); + using FillFn = TrackError (*)( + void *context, + std::uint16_t start_led, + std::uint16_t led_count, + Color color); + using ShowFn = TrackError (*)(void *context); + + void *context{}; + ClearFn clear{}; + FillFn fill{}; + ShowFn show{}; +}; + [[nodiscard]] TrackRenderPlan make_track_render_plan( const TrackConfig &config, const TrackInfo &info, const TrackReport &report); +[[nodiscard]] +TrackError clear_render_sink(TrackRenderSink sink); + +[[nodiscard]] +TrackError apply_render_plan(TrackRenderSink sink, const TrackRenderPlan &plan); + +[[nodiscard]] +TrackError show_render_sink(TrackRenderSink sink); + } // namespace track_core diff --git a/include/track_core/scheme_runtime.hpp b/include/track_core/scheme_runtime.hpp new file mode 100644 index 0000000..01e06b0 --- /dev/null +++ b/include/track_core/scheme_runtime.hpp @@ -0,0 +1,108 @@ +#pragma once + +#include +#include + +#include "track_core/model.hpp" +#include "track_core/render.hpp" +#include "track_core/scheme_decoder.hpp" + +namespace track_core { + +struct SchemeTrackState { + bool is_running{}; + std::size_t primary_segment_index{}; + std::size_t sub_segment_index{}; + float mileage_m{}; + float loop_mileage_m{}; + float speed_m_s{}; + float elapsed_s{}; +}; + +struct SchemeTrackRuntime { + DecodedScheme scheme; + SchemeTrackState state; +}; + +[[nodiscard]] +DecodedScheme make_speed_mileage_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments); + +[[nodiscard]] +DecodedScheme make_mileage_time_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments); + +[[nodiscard]] +DecodedScheme make_speed_time_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments); + +[[nodiscard]] +DecodedScheme make_repeated_speed_mileage_time_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments); + +[[nodiscard]] +expected make_scheme_track_runtime(DecodedScheme scheme); + +[[nodiscard]] +SchemeTrackRuntime start_scheme_track(SchemeTrackRuntime runtime); + +[[nodiscard]] +SchemeTrackRuntime stop_scheme_track(SchemeTrackRuntime runtime); + +[[nodiscard]] +SchemeTrackRuntime tick_scheme_track( + const TrackConfig &config, + SchemeTrackRuntime runtime, + float delta_s); + +[[nodiscard]] +TrackInfo scheme_track_info(const SchemeTrackRuntime &runtime); + +[[nodiscard]] +TrackReport scheme_track_report(const SchemeTrackRuntime &runtime); + +class SchemeTrainingRuntime { +public: + [[nodiscard]] + bool has_program() const noexcept; + + [[nodiscard]] + bool all_stopped() const noexcept; + + [[nodiscard]] + expected add_scheme(DecodedScheme scheme); + + void clear(); + void start(); + void stop(); + void tick(const TrackConfig &config, float delta_s); + + [[nodiscard]] + TrackStateReportCollection state_collection() const; + + [[nodiscard]] + TrackSchemeMgrRead scheme_status() const; + + [[nodiscard]] + expected render_to(const TrackConfig &config, TrackRenderSink sink) const; + + [[nodiscard]] + expected, TrackError> render_pixels(const TrackConfig &config) const; + +private: + std::vector tracks_; +}; + +} // namespace track_core diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..a7d73ad --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,23 @@ +[build-system] +requires = ["scikit-build-core>=0.10", "nanobind>=2.4"] +build-backend = "scikit_build_core.build" + +[project] +name = "track-core" +version = "0.1.0" +description = "Platform-neutral TrackBackFwd track simulation core with Python bindings" +readme = "README.md" +requires-python = ">=3.9" +dependencies = [] + +[project.optional-dependencies] +emulator = ["dearpygui>=1.10,<2"] +test = ["pytest>=8.0"] + +[tool.scikit-build] +minimum-version = "build-system.requires" +wheel.packages = ["python/track_core"] + +[tool.scikit-build.cmake.define] +TRACK_CORE_BUILD_PYTHON = "ON" +TRACK_CORE_BUILD_TESTS = "OFF" diff --git a/python/tests/test_bindings.py b/python/tests/test_bindings.py new file mode 100644 index 0000000..abc21d3 --- /dev/null +++ b/python/tests/test_bindings.py @@ -0,0 +1,211 @@ +import pytest + +import track_core as tc + + +def running_info(color=None): + info = tc.TrackInfo() + info.color = color or tc.Color.green() + info.id = 1 + info.is_running = True + info.num_segments = 1 + return info + + +def report(mileage): + value = tc.TrackReport() + value.id = 1 + value.state = tc.TrackState.run + value.mileage_m = mileage + value.speed_m_s = 1.0 + return value + + +def config(draw_kind): + value = tc.TrackConfig() + value.draw_kind = draw_kind + value.line_length_m = 10.0 + value.active_line_length_m = 4.0 + value.head_offset_m = 0.0 + value.line_leds_num = 20 + return value + + +def test_circular_render_wraps(): + plan = tc.make_render_plan( + config(tc.TrackDrawKind.circular), + running_info(tc.Color.green()), + report(8.5), + ) + + assert plan.span_count == 2 + assert [(span.start_led, span.led_count, span.color) for span in plan.spans] == [ + (17, 3, tc.Color.green()), + (0, 5, tc.Color.green()), + ] + + +def test_linear_render_forward_pixels(): + pixels = tc.render_pixels( + config(tc.TrackDrawKind.linear), + running_info(tc.Color.red()), + report(5.0), + ) + + assert len(pixels) == 20 + assert pixels[6] == tc.Color.cyan() + assert pixels[8] == tc.Color.red() + assert pixels[10] == tc.Color.red() + assert pixels[12] == tc.Color.blue() + assert pixels[0] == tc.Color.black() + + +def test_linear_render_reverse_spans(): + cfg = config(tc.TrackDrawKind.linear) + cfg.active_line_length_m = 5.0 + plan = tc.make_render_plan(cfg, running_info(tc.Color.red()), report(15.0)) + + assert [(span.start_led, span.led_count, span.color) for span in plan.spans] == [ + (5, 2, tc.Color.blue()), + (7, 3, tc.Color.red()), + (10, 3, tc.Color.red()), + (13, 2, tc.Color.cyan()), + ] + + +def test_linear_render_boundary_sweep_does_not_raise(): + cfg = config(tc.TrackDrawKind.linear) + for mileage in ( + -20.0, + -10.1, + -10.0, + -9.9, + -1.0, + -0.1, + 0.0, + 0.1, + 1.0, + 9.9, + 10.0, + 10.1, + 19.9, + 20.0, + 20.1, + 29.9, + 30.0, + 30.1, + 40.0, + ): + pixels = tc.render_pixels(cfg, running_info(tc.Color.red()), report(mileage)) + assert len(pixels) == cfg.line_leds_num + + +def test_not_running_renders_black_pixels(): + info = running_info() + info.is_running = False + + pixels = tc.render_pixels(config(tc.TrackDrawKind.circular), info, report(8.5)) + + assert len(pixels) == 20 + assert all(pixel == tc.Color.black() for pixel in pixels) + + +def test_invalid_config_raises_value_error(): + cfg = config(tc.TrackDrawKind.circular) + cfg.line_leds_num = 0 + + with pytest.raises(ValueError): + tc.render_pixels(cfg, running_info(), report(0.0)) + + +def st_segment(speed, time_s): + segment = tc.STSegment() + segment.speed_m_s = speed + segment.time_since_start_s = time_s + return segment + + +def sm_segment(speed, mileage): + segment = tc.SMSegment() + segment.speed_m_s = speed + segment.mileage_from_start_m = mileage + return segment + + +def mt_segment(mileage, time_s): + segment = tc.MTSegment() + segment.mileage_to_travel_this_segment_m = mileage + segment.time_since_start_s = time_s + return segment + + +def test_pure_speed_time_runtime_ticks(): + scheme = tc.make_speed_time_scheme( + 7, + tc.Color.green(), + tc.AccelerationProfile.instant, + [st_segment(1.0, 0), st_segment(3.0, 5), st_segment(1.0, 10)], + ) + runtime = tc.start_scheme_track(tc.make_scheme_track_runtime(scheme)) + runtime = tc.tick_scheme_track(config(tc.TrackDrawKind.circular), runtime, 6.0) + state = runtime.state + report = runtime.report() + + assert state.primary_segment_index == 1 + assert report.state == tc.TrackState.run + assert report.speed_m_s == pytest.approx(3.0) + assert report.mileage_m == pytest.approx(6.0) + + +def test_training_runtime_accepts_all_scheme_kinds_and_renders(): + runtime = tc.SchemeTrainingRuntime() + + runtime.add_scheme( + tc.make_speed_mileage_scheme( + 1, + tc.Color.red(), + tc.AccelerationProfile.instant, + [sm_segment(1.0, 0), sm_segment(2.0, 5), sm_segment(1.0, 10)], + ) + ) + runtime.add_scheme( + tc.make_mileage_time_scheme( + 2, + tc.Color.green(), + tc.AccelerationProfile.instant, + [mt_segment(10, 0), mt_segment(20, 5), mt_segment(1, 15)], + ) + ) + runtime.add_scheme( + tc.make_speed_time_scheme( + 3, + tc.Color.blue(), + tc.AccelerationProfile.instant, + [st_segment(1.0, 0), st_segment(1.0, 10)], + ) + ) + + repeated = tc.RepeatedSMSegment() + repeated.time_since_start_s = 0 + repeated.speed_mileage_segments = [sm_segment(1.0, 0), sm_segment(2.0, 5)] + repeated_end = tc.RepeatedSMSegment() + repeated_end.time_since_start_s = 20 + repeated_end.speed_mileage_segments = [sm_segment(1.0, 0), sm_segment(1.0, 5)] + runtime.add_scheme( + tc.make_repeated_speed_mileage_time_scheme( + 4, + tc.Color.white(), + tc.AccelerationProfile.instant, + [repeated, repeated_end], + ) + ) + + runtime.start() + runtime.tick(config(tc.TrackDrawKind.circular), 1.0) + pixels = runtime.render_pixels(config(tc.TrackDrawKind.circular)) + + assert runtime.has_program() + assert not runtime.all_stopped() + assert len(runtime.state_collection()) == 4 + assert len(runtime.scheme_status()) == 4 + assert len(pixels) == 20 diff --git a/python/track_core/__init__.py b/python/track_core/__init__.py new file mode 100644 index 0000000..8e572d9 --- /dev/null +++ b/python/track_core/__init__.py @@ -0,0 +1,71 @@ +from ._core import ( + AccelerationProfile, + Color, + DecodedScheme, + MemoryStrip, + MTSegment, + RepeatedSMSegment, + SMSegment, + SchemeKind, + SchemeTrackRuntime, + SchemeTrackState, + SchemeTrainingRuntime, + STSegment, + TrackConfig, + TrackDrawKind, + TrackError, + TrackInfo, + TrackRenderPlan, + TrackRenderSpan, + TrackReport, + TrackSchemeStatus, + TrackState, + make_mileage_time_scheme, + make_render_plan, + make_repeated_speed_mileage_time_scheme, + make_scheme_track_runtime, + make_speed_mileage_scheme, + make_speed_time_scheme, + render_pixels, + scheme_track_info, + scheme_track_report, + start_scheme_track, + stop_scheme_track, + tick_scheme_track, +) + +__all__ = [ + "AccelerationProfile", + "Color", + "DecodedScheme", + "MemoryStrip", + "MTSegment", + "RepeatedSMSegment", + "SMSegment", + "SchemeKind", + "SchemeTrackRuntime", + "SchemeTrackState", + "SchemeTrainingRuntime", + "STSegment", + "TrackConfig", + "TrackDrawKind", + "TrackError", + "TrackInfo", + "TrackRenderPlan", + "TrackRenderSpan", + "TrackReport", + "TrackSchemeStatus", + "TrackState", + "make_mileage_time_scheme", + "make_render_plan", + "make_repeated_speed_mileage_time_scheme", + "make_scheme_track_runtime", + "make_speed_mileage_scheme", + "make_speed_time_scheme", + "render_pixels", + "scheme_track_info", + "scheme_track_report", + "start_scheme_track", + "stop_scheme_track", + "tick_scheme_track", +] diff --git a/python/track_core/emulator.py b/python/track_core/emulator.py new file mode 100644 index 0000000..84010d6 --- /dev/null +++ b/python/track_core/emulator.py @@ -0,0 +1,304 @@ +from __future__ import annotations + +import math +import time + +from . import ( + AccelerationProfile, + Color, + MTSegment, + RepeatedSMSegment, + SMSegment, + STSegment, + SchemeTrainingRuntime, + TrackConfig, + TrackDrawKind, + TrackInfo, + TrackReport, + TrackState, + make_mileage_time_scheme, + make_repeated_speed_mileage_time_scheme, + make_speed_mileage_scheme, + make_speed_time_scheme, + render_pixels, +) + + +DRAWLIST_TAG = "track_core_drawlist" + + +def _load_dearpygui(): + try: + import dearpygui.dearpygui as dpg + except ImportError as exc: + raise SystemExit( + "DearPyGui is not installed. Install the emulator extra with " + '`python -m pip install -e ".[emulator]"`.' + ) from exc + return dpg + + +def _rgba(color: Color) -> tuple[int, int, int, int]: + return (color.r, color.g, color.b, 255) + + +def _sm(speed: float, mileage_m: int) -> SMSegment: + segment = SMSegment() + segment.speed_m_s = speed + segment.mileage_from_start_m = mileage_m + return segment + + +def _mt(mileage_m: int, time_s: int) -> MTSegment: + segment = MTSegment() + segment.mileage_to_travel_this_segment_m = mileage_m + segment.time_since_start_s = time_s + return segment + + +def _st(speed: float, time_s: int) -> STSegment: + segment = STSegment() + segment.speed_m_s = speed + segment.time_since_start_s = time_s + return segment + + +def _rsmt(time_s: int, segments: list[SMSegment]) -> RepeatedSMSegment: + segment = RepeatedSMSegment() + segment.time_since_start_s = time_s + segment.speed_mileage_segments = segments + return segment + + +def _profile_from_ui(dpg) -> AccelerationProfile: + return ( + AccelerationProfile.smooth + if dpg.get_value("accel_profile") == "smooth" + else AccelerationProfile.instant + ) + + +def _color_from_ui(dpg) -> Color: + rgb = dpg.get_value("runner_color") + return Color(int(rgb[0]), int(rgb[1]), int(rgb[2])) + + +def _config_from_ui(dpg) -> TrackConfig: + line_length = float(dpg.get_value("line_length")) + active_length = min(float(dpg.get_value("active_length")), max(0.1, line_length - 0.001)) + + config = TrackConfig.default_config() + config.draw_kind = ( + TrackDrawKind.circular + if dpg.get_value("draw_kind") == "circular" + else TrackDrawKind.linear + ) + config.line_length_m = line_length + config.active_line_length_m = active_length + config.head_offset_m = float(dpg.get_value("head_offset")) + config.line_leds_num = int(dpg.get_value("led_count")) + return config + + +def _manual_state_to_core(dpg) -> tuple[TrackConfig, TrackInfo, TrackReport]: + config = _config_from_ui(dpg) + color = _color_from_ui(dpg) + + info = TrackInfo() + info.color = color + info.id = 1 + info.is_running = dpg.get_value("manual_running") + info.num_segments = 1 + + report = TrackReport() + report.id = 1 + report.state = TrackState.run if info.is_running else TrackState.stop + report.mileage_m = dpg.get_value("manual_mileage") + report.speed_m_s = dpg.get_value("manual_speed") + report.time_elapsed_ms = 0 + return config, info, report + + +def _draw_linear(dpg, pixels: list[Color]) -> None: + width = 840 + x0 = 30 + y0 = 180 + gap = 1 if len(pixels) <= 240 else 0 + cell_w = max(1, int((width - gap * max(0, len(pixels) - 1)) / max(1, len(pixels)))) + cell_h = 34 + + for index, color in enumerate(pixels): + x = x0 + index * (cell_w + gap) + dpg.draw_rectangle( + (x, y0), + (x + cell_w, y0 + cell_h), + color=(55, 58, 64, 255), + fill=_rgba(color), + parent=DRAWLIST_TAG, + ) + + dpg.draw_rectangle( + (x0 - 1, y0 - 1), + (x0 + width + 1, y0 + cell_h + 1), + color=(90, 96, 108, 255), + parent=DRAWLIST_TAG, + ) + dpg.draw_text((x0, y0 + 52), f"{len(pixels)} LEDs", color=(190, 195, 205, 255), parent=DRAWLIST_TAG) + + +def _draw_circular(dpg, pixels: list[Color]) -> None: + center = (440, 158) + radius = 122 + led_radius = 5 if len(pixels) <= 160 else 3 + + dpg.draw_circle(center, radius, color=(80, 86, 98, 255), thickness=1, parent=DRAWLIST_TAG) + for index, color in enumerate(pixels): + angle = (2.0 * math.pi * index / max(1, len(pixels))) - (math.pi / 2.0) + pos = ( + center[0] + math.cos(angle) * radius, + center[1] + math.sin(angle) * radius, + ) + dpg.draw_circle( + pos, + led_radius, + color=(42, 45, 52, 255), + fill=_rgba(color), + parent=DRAWLIST_TAG, + ) + + dpg.draw_text((30, 300), f"{len(pixels)} LEDs", color=(190, 195, 205, 255), parent=DRAWLIST_TAG) + + +class TrackRuntimeApp: + def __init__(self) -> None: + self.runtime = SchemeTrainingRuntime() + self.last_frame_s = time.perf_counter() + + def build_scheme(self, dpg): + kind = dpg.get_value("scheme_kind") + color = _color_from_ui(dpg) + profile = _profile_from_ui(dpg) + + if kind == "SM": + return make_speed_mileage_scheme( + 1, + color, + profile, + [_sm(1.0, 0), _sm(2.8, 10), _sm(1.2, 24), _sm(2.0, 40)], + ) + if kind == "MT": + return make_mileage_time_scheme( + 1, + color, + profile, + [_mt(12, 0), _mt(20, 6), _mt(10, 18), _mt(1, 28)], + ) + if kind == "RSMT": + return make_repeated_speed_mileage_time_scheme( + 1, + color, + profile, + [ + _rsmt(0, [_sm(1.0, 0), _sm(3.0, 10), _sm(1.2, 24)]), + _rsmt(18, [_sm(2.2, 0), _sm(4.0, 8), _sm(1.0, 20)]), + _rsmt(42, [_sm(1.4, 0), _sm(2.4, 12), _sm(1.4, 24)]), + ], + ) + return make_speed_time_scheme( + 1, + color, + profile, + [_st(1.0, 0), _st(3.2, 8), _st(1.4, 18), _st(2.4, 30), _st(0.8, 42)], + ) + + def load_scheme(self, dpg) -> None: + self.runtime.clear() + self.runtime.add_scheme(self.build_scheme(dpg)) + self.runtime.start() + self.last_frame_s = time.perf_counter() + + def start(self) -> None: + self.runtime.start() + self.last_frame_s = time.perf_counter() + + def stop(self) -> None: + self.runtime.stop() + + def _runtime_pixels(self, dpg, config: TrackConfig) -> list[Color]: + now_s = time.perf_counter() + delta_s = min(0.1, now_s - self.last_frame_s) + self.last_frame_s = now_s + + if dpg.get_value("source_mode") == "runtime": + self.runtime.tick(config, delta_s * float(dpg.get_value("time_scale"))) + return self.runtime.render_pixels(config) + + manual_config, info, report = _manual_state_to_core(dpg) + return render_pixels(manual_config, info, report) + + def render(self, dpg) -> None: + dpg.delete_item(DRAWLIST_TAG, children_only=True) + config = _config_from_ui(dpg) + pixels = self._runtime_pixels(dpg, config) + if config.draw_kind == TrackDrawKind.circular: + _draw_circular(dpg, pixels) + else: + _draw_linear(dpg, pixels) + + states = self.runtime.state_collection() + if states: + state = states[0] + dpg.draw_text( + (30, 18), + f"id {state.id} {state.time_elapsed_ms / 1000.0:5.1f}s " + f"{state.mileage_m:6.2f}m {state.speed_m_s:4.2f}m/s", + color=(220, 225, 235, 255), + parent=DRAWLIST_TAG, + ) + + +def main() -> None: + dpg = _load_dearpygui() + app = TrackRuntimeApp() + + dpg.create_context() + dpg.create_viewport(title="track-core emulator", width=920, height=560) + + with dpg.window(label="track-core emulator", tag="main_window", width=900, height=540): + with dpg.group(horizontal=True): + dpg.add_combo(("runtime", "manual"), default_value="runtime", label="source", tag="source_mode") + dpg.add_combo(("ST", "SM", "MT", "RSMT"), default_value="ST", label="scheme", tag="scheme_kind", callback=lambda *_: app.load_scheme(dpg)) + dpg.add_combo(("instant", "smooth"), default_value="smooth", label="accel", tag="accel_profile", callback=lambda *_: app.load_scheme(dpg)) + dpg.add_combo(("circular", "linear"), default_value="circular", label="draw", tag="draw_kind") + + with dpg.group(horizontal=True): + dpg.add_button(label="load", callback=lambda *_: app.load_scheme(dpg)) + dpg.add_button(label="start", callback=lambda *_: app.start()) + dpg.add_button(label="stop", callback=lambda *_: app.stop()) + dpg.add_color_edit(default_value=(0, 255, 0, 255), label="color", tag="runner_color", no_alpha=True, callback=lambda *_: app.load_scheme(dpg)) + + dpg.add_slider_float(label="time scale", tag="time_scale", default_value=1.0, min_value=0.0, max_value=8.0) + dpg.add_slider_float(label="line length m", tag="line_length", default_value=40.0, min_value=5.0, max_value=80.0) + dpg.add_slider_float(label="active length m", tag="active_length", default_value=10.0, min_value=0.1, max_value=40.0) + dpg.add_slider_float(label="head offset m", tag="head_offset", default_value=0.0, min_value=-40.0, max_value=40.0) + dpg.add_slider_int(label="LEDs", tag="led_count", default_value=160, min_value=8, max_value=400) + dpg.add_checkbox(label="manual running", default_value=True, tag="manual_running") + dpg.add_slider_float(label="manual mileage m", tag="manual_mileage", default_value=8.5, min_value=-80.0, max_value=120.0) + dpg.add_slider_float(label="manual speed m/s", tag="manual_speed", default_value=1.0, min_value=0.0, max_value=10.0) + + dpg.add_drawlist(width=880, height=330, tag=DRAWLIST_TAG) + + app.load_scheme(dpg) + dpg.setup_dearpygui() + dpg.set_primary_window("main_window", True) + dpg.show_viewport() + + while dpg.is_dearpygui_running(): + app.render(dpg) + dpg.render_dearpygui_frame() + + dpg.destroy_context() + + +if __name__ == "__main__": + main() diff --git a/src/esp/app_track_decoder.cpp b/src/esp/app_track_decoder.cpp new file mode 100644 index 0000000..cc80998 --- /dev/null +++ b/src/esp/app_track_decoder.cpp @@ -0,0 +1,85 @@ +#include "app_track_decoder.hpp" + +#include +#include + +namespace app::track { +namespace { + +track_core::Color to_core(const Color &color) { + return { + color.inner.r, + color.inner.g, + color.inner.b, + }; +} + +error_t from_core(track_core::TrackError error) { + switch (error) { + case track_core::TrackError::ok: + return ESP_OK; + case track_core::TrackError::invalid_arg: + return ESP_ERR_INVALID_ARG; + case track_core::TrackError::invalid_size: + return ESP_ERR_INVALID_SIZE; + case track_core::TrackError::invalid_state: + return ESP_ERR_INVALID_STATE; + case track_core::TrackError::not_supported: + return ESP_ERR_NOT_SUPPORTED; + case track_core::TrackError::range: + return ESP_ERR_INVALID_SIZE; + } + return ESP_FAIL; +} + +} // namespace + +TrackSchemeDecoder TrackSchemeDecoder::from_proto(const proto_type &proto) { + TrackSchemeDecoder scheme; + scheme.id = proto.id; + if (proto.has_color) { + scheme.color = Color::from_proto(proto.color); + } else { + scheme.color = Color::white(); + } + auto data = std::span(proto.data.bytes, proto.data.size); + std::ranges::copy(data, std::back_inserter(scheme.binary)); + return scheme; +} + +expected TrackSchemeDecoder::decode_core() const { + using ue = unexpected; + if (binary.empty()) { + return ue{ESP_ERR_INVALID_ARG}; + } + + const auto decoded = track_core::decode_scheme(id, to_core(color), binary); + if (!decoded) { + return ue{from_core(decoded.error())}; + } + return *decoded; +} + +TrackSchemeMgr::Add TrackSchemeMgr::Add::from_proto(const proto_type &proto) { + Add add; + assert(proto.has_scheme); + add.scheme_decoder = TrackSchemeDecoder::from_proto(proto.scheme); + return add; +} + +TrackSchemeMgr TrackSchemeMgr::from_proto(const proto_type &proto) { + TrackSchemeMgr mgmt; + switch (proto.which_msg) { + case track_app_TrackSchemeMgr_add_tag: + mgmt.choice = Add::from_proto(proto.msg.add); + break; + case track_app_TrackSchemeMgr_clear_tag: + mgmt.choice = Clear{}; + break; + default: + break; + } + return mgmt; +} + +} // namespace app::track diff --git a/src/esp/app_track_drawer.cpp b/src/esp/app_track_drawer.cpp new file mode 100644 index 0000000..fd9744f --- /dev/null +++ b/src/esp/app_track_drawer.cpp @@ -0,0 +1,179 @@ +#include "app_track_drawer.hpp" + +#include +#include + +#include "track_core/render.hpp" + +namespace { + +track_core::Color to_core(const app::track::Color &color) { + return { + color.inner.r, + color.inner.g, + color.inner.b, + }; +} + +app::track::Color from_core(const track_core::Color &color) { + return { + color.r, + color.g, + color.b, + }; +} + +track_core::TrackDrawKind to_core(track_app_TrackDrawKind draw_kind) { + switch (draw_kind) { + case track_app_TrackDrawKind_CIRCULAR: + return track_core::TrackDrawKind::circular; + case track_app_TrackDrawKind_LINEAR: + return track_core::TrackDrawKind::linear; + } + return track_core::TrackDrawKind::circular; +} + +track_core::SchemeKind to_core(app::track::SchemeKind kind) { + switch (kind) { + case track_app_TrackSchemeKind_SPEED_INPUT_MILEAGE_SEGMENTED_TIME_FREE: + return track_core::SchemeKind::speed_input_mileage_segmented_time_free; + case track_app_TrackSchemeKind_MILEAGE_INPUT_TIME_SEGMENTED_SPEED_FREE: + return track_core::SchemeKind::mileage_input_time_segmented_speed_free; + case track_app_TrackSchemeKind_SPEED_INPUT_TIME_SEGMENTED_MILEAGE_FREE: + return track_core::SchemeKind::speed_input_time_segmented_mileage_free; + case track_app_TrackSchemeKind_REPEATED_SPEED_INPUT_MILEAGE_SEGMENTATION_INPUT_TIME_SEGMENTED: + return track_core::SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented; + } + return track_core::SchemeKind::speed_input_time_segmented_mileage_free; +} + +track_core::TrackState to_core(app::track::TrackState state) { + switch (state) { + case track_app_TrackState_STOP: + return track_core::TrackState::stop; + case track_app_TrackState_RUN: + return track_core::TrackState::run; + case track_app_TrackState_TEST_RAINBOW: + return track_core::TrackState::test_rainbow; + case track_app_TrackState_TEST_BLINK: + return track_core::TrackState::test_blink; + } + return track_core::TrackState::stop; +} + +track_core::TrackConfig to_core(const app::track::TrackConfig &config) { + return { + .draw_kind = to_core(config.draw_kind), + .line_length_m = config.line_length_m, + .active_line_length_m = config.active_line_length_m, + .head_offset_m = config.head_offset_m, + .line_leds_num = config.line_leds_num, + }; +} + +track_core::TrackInfo to_core(const app::track::TrackInfo &info) { + return { + .kind = to_core(info.kind), + .color = to_core(info.color), + .id = info.id, + .is_running = info.is_running, + .num_segments = info.num_segments, + }; +} + +track_core::TrackReport to_core(const app::track::report &report) { + return { + .id = report.id, + .state = to_core(report.state), + .mileage_m = report.mileage_m, + .speed_m_s = report.speed_m_s, + .time_elapsed_ms = report.time_elapsed_ms, + }; +} + +track_core::TrackError to_core(error_t error) { + if (error == ESP_OK) { + return track_core::TrackError::ok; + } + return track_core::TrackError::invalid_state; +} + +app::strip::StripView *strip_from_context(void *context) { + return static_cast(context); +} + +track_core::TrackError strip_clear(void *context) { + auto *strip = strip_from_context(context); + if (strip == nullptr) { + return track_core::TrackError::invalid_arg; + } + return to_core((*strip)->clear()); +} + +track_core::TrackError strip_fill( + void *context, + std::uint16_t start_led, + std::uint16_t led_count, + track_core::Color color) { + auto *strip = strip_from_context(context); + if (strip == nullptr) { + return track_core::TrackError::invalid_arg; + } + return to_core((*strip)->fill(start_led, led_count, static_cast(from_core(color)))); +} + +track_core::TrackError strip_show(void *context) { + auto *strip = strip_from_context(context); + if (strip == nullptr) { + return track_core::TrackError::invalid_arg; + } + return to_core((*strip)->show()); +} + +} // namespace + +namespace app::track { + +void TrackRenderPlan::add_fill(uint16_t start_led, uint16_t led_count, Color color) { + if (led_count == 0) { + return; + } + assert(span_count < spans.size() && "TrackRenderPlan capacity exceeded"); + spans[span_count++] = TrackRenderSpan{ + .start_led = start_led, + .led_count = led_count, + .color = color, + }; +} + +TrackRenderPlan make_track_render_plan(const TrackConfig &config, const TrackInfo &info, const report &rep) { + const auto core_plan = track_core::make_track_render_plan( + to_core(config), + to_core(info), + to_core(rep)); + + TrackRenderPlan plan{}; + for (size_t i = 0; i < core_plan.span_count; ++i) { + const auto &span = core_plan.spans[i]; + plan.add_fill(span.start_led, span.led_count, from_core(span.color)); + } + return plan; +} + +void apply_render_plan(app::strip::StripView strip, const TrackRenderPlan &plan) { + for (size_t i = 0; i < plan.span_count; ++i) { + const auto &span = plan.spans[i]; + strip->fill(span.start_led, span.led_count, span.color); + } +} + +track_core::TrackRenderSink make_track_render_sink(app::strip::StripView &strip) { + return track_core::TrackRenderSink{ + .context = &strip, + .clear = strip_clear, + .fill = strip_fill, + .show = strip_show, + }; +} + +} // namespace app::track diff --git a/src/esp/app_track_model.cpp b/src/esp/app_track_model.cpp new file mode 100644 index 0000000..7d395ba --- /dev/null +++ b/src/esp/app_track_model.cpp @@ -0,0 +1,109 @@ +#include "app_track_model.hpp" +#include "app_track.pb.h" +#include "esp_log.h" + +namespace { +app::track::config_getter global_config_getter{nullptr}; +constexpr auto TAG = "app::track"; +} + +namespace app::track { +const char *to_str(TrackState status) { + using enum TrackState; + switch (status) { + case track_app_TrackState_STOP: + return "STOP"; + case track_app_TrackState_RUN: + return "RUN"; + case track_app_TrackState_TEST_RAINBOW: + return "TEST_RAINBOW"; + case track_app_TrackState_TEST_BLINK: + return "TEST_BLINK"; + } + return "UNKNOWN"; +} + +const char *to_str(TrackControllerMode mode) { + switch (mode) { + case track_app_TrackControllerMode_SCHEME: + return "SCHEME"; + case track_app_TrackControllerMode_PID_HR: + return "PID_HR"; + } + return "UNKNOWN"; +} + +const char *to_str(TrackPidStageKind kind) { + switch (kind) { + case track_app_TrackPidStageKind_NONE: + return "NONE"; + case track_app_TrackPidStageKind_CONSTANT: + return "CONSTANT"; + case track_app_TrackPidStageKind_PID: + return "PID"; + } + return "UNKNOWN"; +} + +const char *to_str(track_app_TrackDrawKind draw_kind) { + switch (draw_kind) { + case track_app_TrackDrawKind_CIRCULAR: + return "CIRCULAR"; + case track_app_TrackDrawKind_LINEAR: + return "LINEAR"; + } + return "UNKNOWN"; +} + +const char *to_str(AccelerationProfile profile) { + switch (profile) { + case track_app_TrackAccelerationProfile_SMOOTH: + return "SMOOTH"; + case track_app_TrackAccelerationProfile_INSTANT: + return "INSTANT"; + } + return "UNKNOWN"; +} + +void set_global_config_getter(config_getter getter) { + global_config_getter = std::move(getter); +} + +const TrackConfig &global_config() { + if (not global_config_getter) { + static const auto DEFAULT_CONFIG = TrackConfig::Default(); + ESP_LOGW(TAG, "unset global config getter"); + return DEFAULT_CONFIG; + } + return global_config_getter(); +} + +void TrackConfig::log(const char *tag, esp_log_level_t level) const { + ESP_LOG_LEVEL(level, tag, + "TrackConfig{.draw_kind=%s, " + ".line_length_m=%.2f, " + ".active_line_length_m=%.2f, " + ".head_offset_m=%.2f, " + ".line_leds_num=%" PRIu16 + "}", + to_str(draw_kind), + line_length_m, + active_line_length_m, + head_offset_m, + line_leds_num); +} + +void TrackStateReportCollection::log(const char *tag, esp_log_level_t level) const { + for (size_t i = 0; i < states.size(); ++i) { + const auto &report = states[i]; + ESP_LOG_LEVEL(level, tag, + "[%zu] Report{.id=%" PRIu8 ", .state=%s, .mileage_m=%.2f, .speed_m_s=%.2f, .time_elapsed_ms=%" PRIu32 "}", + i, + report.id, + to_str(report.state), + report.mileage_m, + report.speed_m_s, + report.time_elapsed_ms); + } +} +} diff --git a/src/memory_strip.cpp b/src/memory_strip.cpp index ff825b0..760fc47 100644 --- a/src/memory_strip.cpp +++ b/src/memory_strip.cpp @@ -3,6 +3,41 @@ #include namespace track_core { +namespace { + +MemoryStrip *memory_strip_from_context(void *context) { + return static_cast(context); +} + +TrackError memory_strip_clear(void *context) { + auto *strip = memory_strip_from_context(context); + if (strip == nullptr) { + return TrackError::invalid_arg; + } + return strip->clear(); +} + +TrackError memory_strip_fill( + void *context, + std::uint16_t start_led, + std::uint16_t led_count, + Color color) { + auto *strip = memory_strip_from_context(context); + if (strip == nullptr) { + return TrackError::invalid_arg; + } + return strip->fill(start_led, led_count, color); +} + +TrackError memory_strip_show(void *context) { + auto *strip = memory_strip_from_context(context); + if (strip == nullptr) { + return TrackError::invalid_arg; + } + return strip->show(); +} + +} // namespace MemoryStrip::MemoryStrip(std::size_t led_count) : pixels_(led_count, Color::black()) {} @@ -60,13 +95,16 @@ std::span MemoryStrip::pixels() const { } TrackError apply_render_plan(MemoryStrip &strip, const TrackRenderPlan &plan) { - for (std::size_t i = 0; i < plan.span_count; ++i) { - const auto &span = plan.spans[i]; - if (const auto err = strip.fill(span.start_led, span.led_count, span.color); err != TrackError::ok) { - return err; - } - } - return TrackError::ok; + return apply_render_plan(make_memory_strip_sink(strip), plan); +} + +TrackRenderSink make_memory_strip_sink(MemoryStrip &strip) { + return TrackRenderSink{ + .context = &strip, + .clear = memory_strip_clear, + .fill = memory_strip_fill, + .show = memory_strip_show, + }; } } // namespace track_core diff --git a/src/pid_runtime.cpp b/src/pid_runtime.cpp new file mode 100644 index 0000000..29970bf --- /dev/null +++ b/src/pid_runtime.cpp @@ -0,0 +1,207 @@ +#include "track_core/pid_runtime.hpp" + +#include +#include +#include +#include + +namespace track_core { +namespace { + +constexpr float default_pid_nominal_sample_interval_s = 2.0F; + +template +struct overloads : Ts... { + using Ts::operator()...; +}; + +bool supports_live_pid_tuning(const TrackPidConfig &config) { + return config.schemas.size() == 1 && + std::holds_alternative(config.schemas.front().segment); +} + +} // namespace + +PidHrRuntime::PidHrRuntime() = default; + +PidHrRuntime::PidHrRuntime(TrackPidConfig config) + : config_(std::move(config)) {} + +void PidHrRuntime::set_pid_config(TrackPidConfig config) { + config_ = std::move(config); +} + +void PidHrRuntime::update_target_hr_bpm(std::uint8_t target_hr_bpm) { + config_.target_hr_bpm = target_hr_bpm; + if (program_state_) { + program_state_->update_target_hr_bpm(target_hr_bpm); + } +} + +expected PidHrRuntime::apply_pid_runtime_command( + const TrackPidRuntimeCommand &command, + const TrackConfig *track_config) { + if (!running_ || !program_state_) { + return unexpected{TrackError::invalid_state}; + } + + return std::visit(overloads{ + [](unit) -> expected { + return unexpected{TrackError::invalid_arg}; + }, + [&](const TrackPidSetTargetHr &target_hr) -> expected { + update_target_hr_bpm(target_hr.target_hr_bpm); + return {}; + }, + [&](const TrackPidSetTuning &tuning) -> expected { + if (!supports_live_pid_tuning(config_)) { + return unexpected{TrackError::invalid_state}; + } + auto updated = program_state_->update_tuning(tuning); + if (!updated) { + return unexpected{updated.error()}; + } + auto &segment = std::get(config_.schemas.front().segment); + tuning.apply_to(segment); + base_speed_m_s_ = program_state_->commanded_speed_m_s(); + effective_speed_m_s_ = effective_speed_m_s(base_speed_m_s_, track_config); + return {}; + }}, + command.command); +} + +const TrackPidConfig &PidHrRuntime::pid_config() const { + return config_; +} + +void PidHrRuntime::start(time_point now) { + running_ = true; + mileage_m_ = 0.0F; + base_speed_m_s_ = 0.0F; + effective_speed_m_s_ = 0.0F; + start_timestamp_ = now; + last_tick_timestamp_ = now; + last_consumed_hr_sample_seq_.reset(); + last_consumed_hr_sample_time_ = {}; + program_state_ = std::make_unique(now, config_); +} + +void PidHrRuntime::stop() { + running_ = false; + base_speed_m_s_ = 0.0F; + effective_speed_m_s_ = 0.0F; + last_consumed_hr_sample_seq_.reset(); + last_consumed_hr_sample_time_ = {}; + program_state_.reset(); +} + +void PidHrRuntime::tick( + const TrackConfig *track_config, + const TrackPidBandSnapshot &band, + time_point now) { + if (!running_ || !program_state_) { + return; + } + + const auto dt = std::chrono::duration_cast>(now - last_tick_timestamp_); + last_tick_timestamp_ = now; + if (dt.count() <= 0.0F) { + return; + } + + std::optional fresh_hr; + if (band.hr_is_fresh && band.has_heart_rate && + (!last_consumed_hr_sample_seq_.has_value() || + band.heart_rate_sample_seq != *last_consumed_hr_sample_seq_)) { + auto sample_interval_s = default_pid_nominal_sample_interval_s; + if (last_consumed_hr_sample_seq_.has_value()) { + sample_interval_s = std::chrono::duration_cast>( + now - last_consumed_hr_sample_time_) + .count(); + } + fresh_hr = TrackPidProgramState::HrSample{ + .heart_rate_bpm = static_cast(band.heart_rate), + .sample_interval_s = sample_interval_s, + }; + last_consumed_hr_sample_seq_ = band.heart_rate_sample_seq; + last_consumed_hr_sample_time_ = now; + } + + base_speed_m_s_ = program_state_->next(now, fresh_hr); + effective_speed_m_s_ = effective_speed_m_s(base_speed_m_s_, track_config); + if (program_state_->is_finished(now)) { + base_speed_m_s_ = 0.0F; + effective_speed_m_s_ = 0.0F; + running_ = false; + } + + mileage_m_ += effective_speed_m_s_ * dt.count(); +} + +float PidHrRuntime::effective_speed_m_s(float base_speed_m_s, const TrackConfig *track_config) const { + auto effective_speed = std::max(0.0F, base_speed_m_s); + if (!config_.speed_suppression || effective_speed <= 0.0F || track_config == nullptr) { + return effective_speed; + } + + if (!std::isfinite(track_config->line_length_m) || track_config->line_length_m <= 0.0F) { + return effective_speed; + } + + const auto &suppression = *config_.speed_suppression; + if (!std::isfinite(suppression.sigma_m) || suppression.sigma_m <= 0.0F) { + return effective_speed; + } + + const auto phase_raw_m = std::fmod(mileage_m_, track_config->line_length_m); + const auto phase_m = phase_raw_m < 0.0F ? (phase_raw_m + track_config->line_length_m) : phase_raw_m; + const auto seam_distance_m = std::min(phase_m, track_config->line_length_m - phase_m); + const auto sigma_ratio = seam_distance_m / suppression.sigma_m; + const auto gaussian = std::exp(-0.5F * sigma_ratio * sigma_ratio); + const auto speed_ratio = 1.0F - ((1.0F - suppression.ratio_min) * gaussian); + return effective_speed * speed_ratio; +} + +TrackReport PidHrRuntime::state_report(time_point now) const { + return { + .id = magic_pid_track_id, + .state = running_ ? TrackState::run : TrackState::stop, + .mileage_m = mileage_m_, + .speed_m_s = effective_speed_m_s_, + .time_elapsed_ms = static_cast(std::chrono::duration_cast( + now - start_timestamp_) + .count()), + }; +} + +TrackInfo PidHrRuntime::info() const { + return { + .kind = SchemeKind::speed_input_time_segmented_mileage_free, + .color = color_, + .id = magic_pid_track_id, + .is_running = running_, + .num_segments = static_cast( + std::min(config_.schemas.size(), static_cast(std::numeric_limits::max()))), + }; +} + +TrackPidStatus PidHrRuntime::pid_status(const TrackPidBandSnapshot &band, time_point now) const { + TrackPidStatus status; + status.band_id = config_.band_id; + status.band_is_active = band.band_is_active; + status.is_heart_rate_valid = band.hr_is_fresh; + status.heart_rate_bpm = band.has_heart_rate ? band.heart_rate : static_cast(0); + status.step_count = band.has_step_count ? band.step_count : static_cast(0); + status.effective_speed_m_s = effective_speed_m_s_; + status.base_speed_m_s = base_speed_m_s_; + if (program_state_) { + status.active_segment_index = program_state_->active_stage_index(); + status.active_segment_kind = program_state_->active_stage_kind(); + status.remaining_program_ms = program_state_->remaining_program_ms(now); + } else { + status.active_segment_kind = TrackPidStageKind::none; + } + return status; +} + +} // namespace track_core diff --git a/src/python/bindings.cpp b/src/python/bindings.cpp new file mode 100644 index 0000000..8ee7ff0 --- /dev/null +++ b/src/python/bindings.cpp @@ -0,0 +1,310 @@ +#include +#include +#include + +#include +#include +#include + +#include "track_core/memory_strip.hpp" +#include "track_core/render.hpp" +#include "track_core/scheme_runtime.hpp" + +namespace nb = nanobind; +using namespace nb::literals; + +namespace { + +void validate_render_config(const track_core::TrackConfig &config) { + if (!config.verify() || config.line_leds_num == 0) { + throw nb::value_error("invalid TrackConfig"); + } + if (config.draw_kind == track_core::TrackDrawKind::circular && + config.active_line_length_m >= config.line_length_m) { + throw nb::value_error("circular tracks require active_line_length_m < line_length_m"); + } +} + +nb::list colors_to_list(std::span colors) { + nb::list result; + for (const auto &color : colors) { + result.append(color); + } + return result; +} + +nb::list spans_to_list(const track_core::TrackRenderPlan &plan) { + nb::list result; + for (std::size_t i = 0; i < plan.span_count; ++i) { + result.append(plan.spans[i]); + } + return result; +} + +track_core::TrackRenderPlan make_render_plan_checked( + const track_core::TrackConfig &config, + const track_core::TrackInfo &info, + const track_core::TrackReport &report) { + validate_render_config(config); + return track_core::make_track_render_plan(config, info, report); +} + +track_core::SchemeTrackRuntime make_scheme_track_runtime_checked(track_core::DecodedScheme scheme) { + auto result = track_core::make_scheme_track_runtime(std::move(scheme)); + if (!result) { + throw std::runtime_error("invalid scheme"); + } + return std::move(*result); +} + +void add_scheme_checked(track_core::SchemeTrainingRuntime &runtime, track_core::DecodedScheme scheme) { + const auto result = runtime.add_scheme(std::move(scheme)); + if (!result) { + throw std::runtime_error("invalid scheme"); + } +} + +nb::list runtime_state_collection(const track_core::SchemeTrainingRuntime &runtime) { + nb::list result; + for (const auto &report : runtime.state_collection().states) { + result.append(report); + } + return result; +} + +nb::list runtime_scheme_status(const track_core::SchemeTrainingRuntime &runtime) { + nb::list result; + for (const auto &status : runtime.scheme_status().scheme_status) { + result.append(status); + } + return result; +} + +nb::list runtime_render_pixels( + const track_core::SchemeTrainingRuntime &runtime, + const track_core::TrackConfig &config) { + const auto pixels = runtime.render_pixels(config); + if (!pixels) { + throw std::runtime_error("failed to render runtime pixels"); + } + return colors_to_list(*pixels); +} + +nb::list render_pixels( + const track_core::TrackConfig &config, + const track_core::TrackInfo &info, + const track_core::TrackReport &report) { + validate_render_config(config); + track_core::MemoryStrip strip(config.line_leds_num); + const auto plan = track_core::make_track_render_plan(config, info, report); + const auto err = track_core::apply_render_plan(strip, plan); + if (err != track_core::TrackError::ok) { + throw std::runtime_error("failed to apply render plan"); + } + return colors_to_list(strip.pixels()); +} + +std::string color_repr(const track_core::Color &color) { + return "Color(" + std::to_string(color.r) + ", " + + std::to_string(color.g) + ", " + + std::to_string(color.b) + ")"; +} + +} // namespace + +NB_MODULE(_core, m) { + m.doc() = "Python bindings for the platform-neutral track-core library"; + + nb::enum_(m, "TrackError") + .value("ok", track_core::TrackError::ok) + .value("invalid_arg", track_core::TrackError::invalid_arg) + .value("invalid_size", track_core::TrackError::invalid_size) + .value("invalid_state", track_core::TrackError::invalid_state) + .value("not_supported", track_core::TrackError::not_supported) + .value("range", track_core::TrackError::range); + + nb::enum_(m, "TrackDrawKind") + .value("circular", track_core::TrackDrawKind::circular) + .value("linear", track_core::TrackDrawKind::linear); + + nb::enum_(m, "SchemeKind") + .value("speed_input_mileage_segmented_time_free", + track_core::SchemeKind::speed_input_mileage_segmented_time_free) + .value("mileage_input_time_segmented_speed_free", + track_core::SchemeKind::mileage_input_time_segmented_speed_free) + .value("speed_input_time_segmented_mileage_free", + track_core::SchemeKind::speed_input_time_segmented_mileage_free) + .value("repeated_speed_input_mileage_segmentation_input_time_segmented", + track_core::SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented); + + nb::enum_(m, "AccelerationProfile") + .value("instant", track_core::AccelerationProfile::instant) + .value("smooth", track_core::AccelerationProfile::smooth); + + nb::enum_(m, "TrackState") + .value("stop", track_core::TrackState::stop) + .value("run", track_core::TrackState::run) + .value("test_rainbow", track_core::TrackState::test_rainbow) + .value("test_blink", track_core::TrackState::test_blink); + + nb::class_(m, "Color") + .def(nb::init<>()) + .def(nb::init(), "r"_a, "g"_a, "b"_a) + .def(nb::init(), "value"_a) + .def_rw("r", &track_core::Color::r) + .def_rw("g", &track_core::Color::g) + .def_rw("b", &track_core::Color::b) + .def("hex", &track_core::Color::hex) + .def_prop_ro("value", [](const track_core::Color &color) { + return static_cast(color); + }) + .def_static("black", &track_core::Color::black) + .def_static("red", &track_core::Color::red) + .def_static("orange", &track_core::Color::orange) + .def_static("yellow", &track_core::Color::yellow) + .def_static("green", &track_core::Color::green) + .def_static("cyan", &track_core::Color::cyan) + .def_static("blue", &track_core::Color::blue) + .def_static("indigo", &track_core::Color::indigo) + .def_static("violet", &track_core::Color::violet) + .def_static("white", &track_core::Color::white) + .def("__eq__", [](const track_core::Color &lhs, const track_core::Color &rhs) { + return lhs == rhs; + }) + .def("__repr__", &color_repr); + + nb::class_(m, "TrackConfig") + .def(nb::init<>()) + .def_rw("draw_kind", &track_core::TrackConfig::draw_kind) + .def_rw("line_length_m", &track_core::TrackConfig::line_length_m) + .def_rw("active_line_length_m", &track_core::TrackConfig::active_line_length_m) + .def_rw("head_offset_m", &track_core::TrackConfig::head_offset_m) + .def_rw("line_leds_num", &track_core::TrackConfig::line_leds_num) + .def("led_distance", &track_core::TrackConfig::led_distance) + .def("verify", &track_core::TrackConfig::verify) + .def_static("default_config", &track_core::TrackConfig::default_config); + + nb::class_(m, "TrackInfo") + .def(nb::init<>()) + .def_rw("kind", &track_core::TrackInfo::kind) + .def_rw("color", &track_core::TrackInfo::color) + .def_rw("id", &track_core::TrackInfo::id) + .def_rw("is_running", &track_core::TrackInfo::is_running) + .def_rw("num_segments", &track_core::TrackInfo::num_segments); + + nb::class_(m, "TrackSchemeStatus") + .def(nb::init<>()) + .def_rw("id", &track_core::TrackSchemeMgrRead::Status::id) + .def_rw("segment_count", &track_core::TrackSchemeMgrRead::Status::segment_count) + .def_rw("kind", &track_core::TrackSchemeMgrRead::Status::kind); + + nb::class_(m, "TrackReport") + .def(nb::init<>()) + .def_rw("id", &track_core::TrackReport::id) + .def_rw("state", &track_core::TrackReport::state) + .def_rw("mileage_m", &track_core::TrackReport::mileage_m) + .def_rw("speed_m_s", &track_core::TrackReport::speed_m_s) + .def_rw("time_elapsed_ms", &track_core::TrackReport::time_elapsed_ms); + + nb::class_(m, "TrackRenderSpan") + .def(nb::init<>()) + .def_rw("start_led", &track_core::TrackRenderSpan::start_led) + .def_rw("led_count", &track_core::TrackRenderSpan::led_count) + .def_rw("color", &track_core::TrackRenderSpan::color); + + nb::class_(m, "TrackRenderPlan") + .def(nb::init<>()) + .def("empty", &track_core::TrackRenderPlan::empty) + .def_prop_ro("span_count", [](const track_core::TrackRenderPlan &plan) { + return plan.span_count; + }) + .def_prop_ro("spans", &spans_to_list); + + nb::class_(m, "MemoryStrip") + .def(nb::init(), "led_count"_a = 0) + .def("begin", &track_core::MemoryStrip::begin) + .def("clear", &track_core::MemoryStrip::clear) + .def("fill", &track_core::MemoryStrip::fill, "start"_a, "count"_a, "color"_a) + .def("show", &track_core::MemoryStrip::show) + .def("set_leds_count", &track_core::MemoryStrip::set_leds_count, "count"_a) + .def_prop_ro("leds_count", &track_core::MemoryStrip::leds_count) + .def_prop_ro("frame_sequence", &track_core::MemoryStrip::frame_sequence) + .def_prop_ro("pixels", [](const track_core::MemoryStrip &strip) { + return colors_to_list(strip.pixels()); + }); + + nb::class_(m, "SMSegment") + .def(nb::init<>()) + .def_rw("speed_m_s", &track_core::SMSegment::speed_m_s) + .def_rw("mileage_from_start_m", &track_core::SMSegment::mileage_from_start_m); + + nb::class_(m, "MTSegment") + .def(nb::init<>()) + .def_rw("mileage_to_travel_this_segment_m", &track_core::MTSegment::mileage_to_travel_this_segment_m) + .def_rw("time_since_start_s", &track_core::MTSegment::time_since_start_s); + + nb::class_(m, "STSegment") + .def(nb::init<>()) + .def_rw("speed_m_s", &track_core::STSegment::speed_m_s) + .def_rw("time_since_start_s", &track_core::STSegment::time_since_start_s); + + nb::class_(m, "RepeatedSMSegment") + .def(nb::init<>()) + .def_rw("speed_mileage_segments", &track_core::RepeatedSMSegment::speed_mileage_segments) + .def_rw("time_since_start_s", &track_core::RepeatedSMSegment::time_since_start_s); + + nb::class_(m, "DecodedScheme") + .def_prop_ro("id", [](const track_core::DecodedScheme &scheme) { return scheme.id; }) + .def_prop_ro("color", [](const track_core::DecodedScheme &scheme) { return scheme.color; }) + .def_prop_ro("kind", [](const track_core::DecodedScheme &scheme) { return scheme.kind; }) + .def_prop_ro("acceleration_profile", [](const track_core::DecodedScheme &scheme) { + return scheme.acceleration_profile; + }); + + nb::class_(m, "SchemeTrackState") + .def(nb::init<>()) + .def_rw("is_running", &track_core::SchemeTrackState::is_running) + .def_rw("primary_segment_index", &track_core::SchemeTrackState::primary_segment_index) + .def_rw("sub_segment_index", &track_core::SchemeTrackState::sub_segment_index) + .def_rw("mileage_m", &track_core::SchemeTrackState::mileage_m) + .def_rw("loop_mileage_m", &track_core::SchemeTrackState::loop_mileage_m) + .def_rw("speed_m_s", &track_core::SchemeTrackState::speed_m_s) + .def_rw("elapsed_s", &track_core::SchemeTrackState::elapsed_s); + + nb::class_(m, "SchemeTrackRuntime") + .def_prop_ro("state", [](const track_core::SchemeTrackRuntime &runtime) { + return runtime.state; + }) + .def("info", &track_core::scheme_track_info) + .def("report", &track_core::scheme_track_report); + + nb::class_(m, "SchemeTrainingRuntime") + .def(nb::init<>()) + .def("has_program", &track_core::SchemeTrainingRuntime::has_program) + .def("all_stopped", &track_core::SchemeTrainingRuntime::all_stopped) + .def("add_scheme", &add_scheme_checked, "scheme"_a) + .def("clear", &track_core::SchemeTrainingRuntime::clear) + .def("start", &track_core::SchemeTrainingRuntime::start) + .def("stop", &track_core::SchemeTrainingRuntime::stop) + .def("tick", &track_core::SchemeTrainingRuntime::tick, "config"_a, "delta_s"_a) + .def("state_collection", &runtime_state_collection) + .def("scheme_status", &runtime_scheme_status) + .def("render_pixels", &runtime_render_pixels, "config"_a); + + m.def("make_render_plan", &make_render_plan_checked, "config"_a, "info"_a, "report"_a); + m.def("render_pixels", &render_pixels, "config"_a, "info"_a, "report"_a); + m.def("make_speed_mileage_scheme", &track_core::make_speed_mileage_scheme, + "id"_a, "color"_a, "acceleration_profile"_a, "segments"_a); + m.def("make_mileage_time_scheme", &track_core::make_mileage_time_scheme, + "id"_a, "color"_a, "acceleration_profile"_a, "segments"_a); + m.def("make_speed_time_scheme", &track_core::make_speed_time_scheme, + "id"_a, "color"_a, "acceleration_profile"_a, "segments"_a); + m.def("make_repeated_speed_mileage_time_scheme", &track_core::make_repeated_speed_mileage_time_scheme, + "id"_a, "color"_a, "acceleration_profile"_a, "segments"_a); + m.def("make_scheme_track_runtime", &make_scheme_track_runtime_checked, "scheme"_a); + m.def("start_scheme_track", &track_core::start_scheme_track, "runtime"_a); + m.def("stop_scheme_track", &track_core::stop_scheme_track, "runtime"_a); + m.def("tick_scheme_track", &track_core::tick_scheme_track, "config"_a, "runtime"_a, "delta_s"_a); + m.def("scheme_track_info", &track_core::scheme_track_info, "runtime"_a); + m.def("scheme_track_report", &track_core::scheme_track_report, "runtime"_a); +} diff --git a/src/render.cpp b/src/render.cpp index 63bacbe..d44e0a8 100644 --- a/src/render.cpp +++ b/src/render.cpp @@ -173,6 +173,64 @@ void TrackRenderPlan::add_fill(std::uint16_t start_led, std::uint16_t led_count, }; } +TrackError clear_render_sink(TrackRenderSink sink) { + if (sink.clear == nullptr) { + return TrackError::invalid_arg; + } + return sink.clear(sink.context); +} + +TrackError apply_render_plan(TrackRenderSink sink, const TrackRenderPlan &plan) { + if (plan.empty()) { + return TrackError::ok; + } + if (sink.fill == nullptr) { + return TrackError::invalid_arg; + } + + for (std::size_t i = 0; i < plan.span_count; ++i) { + const auto &span = plan.spans[i]; + if (const auto err = sink.fill(sink.context, span.start_led, span.led_count, span.color); + err != TrackError::ok) { + return err; + } + } + return TrackError::ok; +} + +TrackError show_render_sink(TrackRenderSink sink) { + if (sink.show == nullptr) { + return TrackError::invalid_arg; + } + return sink.show(sink.context); +} + +namespace { + +void add_clipped_fill( + TrackRenderPlan &plan, + int start_led, + int led_count, + int line_leds_num, + Color color) { + if (led_count <= 0 || line_leds_num <= 0) { + return; + } + + const auto clipped_start = std::max(start_led, 0); + const auto clipped_end = std::min(start_led + led_count, line_leds_num); + if (clipped_end <= clipped_start) { + return; + } + + plan.add_fill( + static_cast(clipped_start), + static_cast(clipped_end - clipped_start), + color); +} + +} // namespace + TrackRenderPlan make_track_render_plan( const TrackConfig &config, const TrackInfo &info, @@ -193,32 +251,33 @@ TrackRenderPlan make_track_render_plan( const auto drawer = LinearLineDrawer::from_report(config, report); const auto magic_color_ahead = Color::blue(); const auto magic_color_behind = Color::cyan(); - const auto center_offset = drawer.center_offset_leds_num(); + const auto center_offset = static_cast(drawer.center_offset_leds_num()); + const auto line_leds_num = static_cast(config.line_leds_num); const auto fill_positive_side = [&](std::uint16_t count, Color near_center, Color far_end) { if (count == 0) { return; } if (count == 1) { - plan.add_fill(center_offset, 1, near_center); + add_clipped_fill(plan, center_offset, 1, line_leds_num, near_center); return; } const auto distal_count = static_cast(count / 2); const auto proximal_count = static_cast(count - distal_count); - plan.add_fill(center_offset, proximal_count, near_center); - plan.add_fill(static_cast(center_offset + proximal_count), distal_count, far_end); + add_clipped_fill(plan, center_offset, proximal_count, line_leds_num, near_center); + add_clipped_fill(plan, center_offset + proximal_count, distal_count, line_leds_num, far_end); }; const auto fill_negative_side = [&](std::uint16_t count, Color near_center, Color far_end) { if (count == 0) { return; } if (count == 1) { - plan.add_fill(static_cast(center_offset - 1), 1, near_center); + add_clipped_fill(plan, center_offset - 1, 1, line_leds_num, near_center); return; } const auto distal_count = static_cast(count / 2); const auto proximal_count = static_cast(count - distal_count); - plan.add_fill(static_cast(center_offset - count), distal_count, far_end); - plan.add_fill(static_cast(center_offset - proximal_count), proximal_count, near_center); + add_clipped_fill(plan, center_offset - count, distal_count, line_leds_num, far_end); + add_clipped_fill(plan, center_offset - proximal_count, proximal_count, line_leds_num, near_center); }; const auto ahead = drawer.center_ahead_leds_num(); const auto behind = drawer.center_behind_leds_num(); diff --git a/src/scheme_decoder.cpp b/src/scheme_decoder.cpp index e9a57b1..921a5ae 100644 --- a/src/scheme_decoder.cpp +++ b/src/scheme_decoder.cpp @@ -30,6 +30,7 @@ expected decode_scheme( .color = color, .kind = kind, .acceleration_profile = acceleration_profile, + .segments = std::vector{}, }; switch (kind) { diff --git a/src/scheme_runtime.cpp b/src/scheme_runtime.cpp new file mode 100644 index 0000000..6885c59 --- /dev/null +++ b/src/scheme_runtime.cpp @@ -0,0 +1,763 @@ +#include "track_core/scheme_runtime.hpp" + +#include +#include +#include +#include +#include +#include + +#include "track_core/memory_strip.hpp" +#include "track_core/render.hpp" + +namespace track_core { +namespace { + +struct accel_calc_result { + float target_speed_m_s{}; + float acceleration_m_s_2{}; +}; + +constexpr float max_acceleration_m_s_2 = 5.0F; + +[[nodiscard]] +bool valid_acceleration_profile(AccelerationProfile profile) { + return profile == AccelerationProfile::instant || + profile == AccelerationProfile::smooth; +} + +[[nodiscard]] +bool finite_nonnegative(float value) { + return std::isfinite(value) && value >= 0.0F; +} + +[[nodiscard]] +std::uint8_t clamp_u8(std::size_t value) { + return static_cast( + std::min(value, std::numeric_limits::max())); +} + +[[nodiscard]] +std::uint32_t elapsed_ms(float elapsed_s) { + if (!std::isfinite(elapsed_s) || elapsed_s <= 0.0F) { + return 0; + } + const auto ms = elapsed_s * 1000.0F; + if (ms >= static_cast(std::numeric_limits::max())) { + return std::numeric_limits::max(); + } + return static_cast(ms); +} + +[[nodiscard]] +accel_calc_result speed_mileage_accel( + const SMSegment ¤t, + const SMSegment &next, + float current_mileage_m, + float current_speed_m_s) { + const float target_speed = next.speed_m_s; + float remaining_distance = static_cast(next.mileage_from_start_m) - current_mileage_m; + + if (remaining_distance <= 0.0F) { + return {target_speed, 0.0F}; + } + + float accel = + (target_speed * target_speed - current_speed_m_s * current_speed_m_s) / + (2.0F * remaining_distance); + accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2); + return {target_speed, accel}; +} + +[[nodiscard]] +accel_calc_result speed_time_accel(const STSegment ¤t, const STSegment &next) { + const float target_speed = next.speed_m_s; + const float duration_s = + static_cast(next.time_since_start_s) - + static_cast(current.time_since_start_s); + + if (duration_s <= 0.0F) { + return {target_speed, 0.0F}; + } + + float accel = (target_speed - current.speed_m_s) / duration_s; + accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2); + return {target_speed, accel}; +} + +[[nodiscard]] +accel_calc_result mileage_time_accel( + float elapsed_s, + float next_segment_time_s, + float current_speed_m_s, + float target_speed_m_s) { + const float remaining_time_s = next_segment_time_s - elapsed_s; + if (remaining_time_s <= 0.0F) { + return {target_speed_m_s, 0.0F}; + } + + float accel = (target_speed_m_s - current_speed_m_s) / remaining_time_s; + accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2); + return {target_speed_m_s, accel}; +} + +void apply_acceleration(SchemeTrackState &state, accel_calc_result profile, float delta_s) { + state.speed_m_s += profile.acceleration_m_s_2 * delta_s; + + if (profile.acceleration_m_s_2 > 0.0F) { + state.speed_m_s = std::min(state.speed_m_s, profile.target_speed_m_s); + } else if (profile.acceleration_m_s_2 < 0.0F) { + state.speed_m_s = std::max(state.speed_m_s, profile.target_speed_m_s); + } +} + +[[nodiscard]] +std::optional calc_mts_target_speed( + std::size_t segment_index, + const std::vector &segments) { + if (segments.size() < 2 || segment_index > segments.size() - 2) { + return std::nullopt; + } + + const auto ¤t = segments[segment_index]; + const auto &next = segments[segment_index + 1]; + if (next.time_since_start_s < current.time_since_start_s) { + return std::nullopt; + } + + const auto duration_s = next.time_since_start_s - current.time_since_start_s; + if (duration_s == 0 || current.mileage_to_travel_this_segment_m == 0) { + return std::nullopt; + } + + return static_cast(current.mileage_to_travel_this_segment_m) / + static_cast(duration_s); +} + +[[nodiscard]] +std::optional calc_mts_target_speed(const MTSegment ¤t, const MTSegment &next) { + if (next.time_since_start_s < current.time_since_start_s) { + return std::nullopt; + } + + const auto duration_s = next.time_since_start_s - current.time_since_start_s; + if (duration_s == 0 || current.mileage_to_travel_this_segment_m == 0) { + return std::nullopt; + } + + return static_cast(current.mileage_to_travel_this_segment_m) / + static_cast(duration_s); +} + +template +[[nodiscard]] +bool strictly_sorted_by_time(const std::vector &segments) { + for (std::size_t i = 1; i < segments.size(); ++i) { + if (segments[i].time_since_start_s <= segments[i - 1].time_since_start_s) { + return false; + } + } + return true; +} + +[[nodiscard]] +expected canonicalize_scheme(DecodedScheme scheme) { + if (!valid_acceleration_profile(scheme.acceleration_profile)) { + return unexpected{TrackError::invalid_arg}; + } + + switch (scheme.kind) { + case SchemeKind::speed_input_mileage_segmented_time_free: { + auto *segments = std::get_if>(&scheme.segments); + if (segments == nullptr || segments->empty()) { + return unexpected{TrackError::invalid_arg}; + } + std::ranges::sort(*segments, {}, &SMSegment::mileage_from_start_m); + if (segments->front().mileage_from_start_m != 0) { + return unexpected{TrackError::invalid_arg}; + } + for (const auto &segment : *segments) { + if (!finite_nonnegative(segment.speed_m_s)) { + return unexpected{TrackError::invalid_arg}; + } + } + return scheme; + } + + case SchemeKind::mileage_input_time_segmented_speed_free: { + auto *segments = std::get_if>(&scheme.segments); + if (segments == nullptr || segments->size() < 2) { + return unexpected{TrackError::invalid_arg}; + } + std::ranges::sort(*segments, {}, &MTSegment::time_since_start_s); + if (segments->front().time_since_start_s != 0 || !strictly_sorted_by_time(*segments)) { + return unexpected{TrackError::invalid_arg}; + } + for (std::size_t i = 0; i + 1 < segments->size(); ++i) { + if (!calc_mts_target_speed(i, *segments)) { + return unexpected{TrackError::invalid_arg}; + } + } + return scheme; + } + + case SchemeKind::speed_input_time_segmented_mileage_free: { + auto *segments = std::get_if>(&scheme.segments); + if (segments == nullptr || segments->empty()) { + return unexpected{TrackError::invalid_arg}; + } + std::ranges::sort(*segments, {}, &STSegment::time_since_start_s); + if (segments->front().time_since_start_s != 0) { + return unexpected{TrackError::invalid_arg}; + } + for (const auto &segment : *segments) { + if (!finite_nonnegative(segment.speed_m_s)) { + return unexpected{TrackError::invalid_arg}; + } + } + return scheme; + } + + case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: { + auto *segments = std::get_if>(&scheme.segments); + if (segments == nullptr || segments->empty()) { + return unexpected{TrackError::invalid_arg}; + } + std::ranges::sort(*segments, {}, &RepeatedSMSegment::time_since_start_s); + if (segments->front().time_since_start_s != 0) { + return unexpected{TrackError::invalid_arg}; + } + for (auto &time_segment : *segments) { + auto &sub_segments = time_segment.speed_mileage_segments; + if (sub_segments.empty()) { + return unexpected{TrackError::invalid_arg}; + } + std::ranges::sort(sub_segments, {}, &SMSegment::mileage_from_start_m); + if (sub_segments.front().mileage_from_start_m != 0) { + return unexpected{TrackError::invalid_arg}; + } + for (const auto &sub_segment : sub_segments) { + if (!finite_nonnegative(sub_segment.speed_m_s)) { + return unexpected{TrackError::invalid_arg}; + } + } + } + return scheme; + } + } + + return unexpected{TrackError::not_supported}; +} + +void tick_speed_mileage(SchemeTrackRuntime &runtime, float delta_s) { + auto &state = runtime.state; + const auto &segments = std::get>(runtime.scheme.segments); + + state.elapsed_s += delta_s; + const auto ¤t = segments[state.primary_segment_index]; + + if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) { + accel_calc_result profile{current.speed_m_s, 0.0F}; + if (state.primary_segment_index + 1 < segments.size()) { + profile = speed_mileage_accel( + current, + segments[state.primary_segment_index + 1], + state.mileage_m, + state.speed_m_s); + } + apply_acceleration(state, profile, delta_s); + } else { + state.speed_m_s = current.speed_m_s; + } + + state.mileage_m += state.speed_m_s * delta_s; + + if (static_cast(state.mileage_m) >= segments.back().mileage_from_start_m) { + state.is_running = false; + return; + } + + while (state.primary_segment_index + 1 < segments.size()) { + const auto next_index = state.primary_segment_index + 1; + const auto &next = segments[next_index]; + if (static_cast(state.mileage_m) < next.mileage_from_start_m) { + break; + } + state.primary_segment_index = next_index; + state.speed_m_s = next.speed_m_s; + } +} + +void tick_mileage_time(SchemeTrackRuntime &runtime, float delta_s) { + auto &state = runtime.state; + const auto &segments = std::get>(runtime.scheme.segments); + const float next_elapsed_s = state.elapsed_s + delta_s; + + if (next_elapsed_s >= static_cast(segments.back().time_since_start_s)) { + state.elapsed_s = next_elapsed_s; + state.is_running = false; + return; + } + + state.elapsed_s = next_elapsed_s; + + const auto next_target_speed = calc_mts_target_speed(state.primary_segment_index + 1, segments); + if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth && next_target_speed && + state.primary_segment_index + 1 < segments.size()) { + const float next_segment_time_s = + static_cast(segments[state.primary_segment_index + 1].time_since_start_s); + const auto profile = mileage_time_accel( + state.elapsed_s, + next_segment_time_s, + state.speed_m_s, + *next_target_speed); + apply_acceleration(state, profile, delta_s); + } + + state.mileage_m += state.speed_m_s * delta_s; + + while (state.primary_segment_index + 1 < segments.size()) { + const auto next_index = state.primary_segment_index + 1; + const auto &next = segments[next_index]; + if (state.elapsed_s < static_cast(next.time_since_start_s)) { + break; + } + + const auto ¤t = segments[state.primary_segment_index]; + const float last_speed_m_s = state.speed_m_s; + state.primary_segment_index = next_index; + state.speed_m_s = calc_mts_target_speed(current, next).value_or(last_speed_m_s); + } +} + +void tick_speed_time(SchemeTrackRuntime &runtime, float delta_s) { + auto &state = runtime.state; + const auto &segments = std::get>(runtime.scheme.segments); + const float next_elapsed_s = state.elapsed_s + delta_s; + + if (next_elapsed_s >= static_cast(segments.back().time_since_start_s)) { + state.elapsed_s = next_elapsed_s; + state.is_running = false; + return; + } + + state.elapsed_s = next_elapsed_s; + const auto ¤t = segments[state.primary_segment_index]; + + if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) { + accel_calc_result profile{current.speed_m_s, 0.0F}; + if (state.primary_segment_index + 1 < segments.size()) { + profile = speed_time_accel(current, segments[state.primary_segment_index + 1]); + } + apply_acceleration(state, profile, delta_s); + } else { + state.speed_m_s = current.speed_m_s; + } + + state.mileage_m += state.speed_m_s * delta_s; + + while (state.primary_segment_index + 1 < segments.size()) { + const auto next_index = state.primary_segment_index + 1; + const auto &next = segments[next_index]; + if (state.elapsed_s < static_cast(next.time_since_start_s)) { + break; + } + state.primary_segment_index = next_index; + state.speed_m_s = next.speed_m_s; + } +} + +[[nodiscard]] +float current_rsmt_loop_length(const SchemeTrackRuntime &runtime) { + const auto &segments = std::get>(runtime.scheme.segments); + const auto &sub_segments = segments[runtime.state.primary_segment_index].speed_mileage_segments; + return sub_segments.empty() + ? 0.0F + : static_cast(sub_segments.back().mileage_from_start_m); +} + +[[nodiscard]] +std::optional> +next_rsmt_sub_segment(const SchemeTrackRuntime &runtime) { + const auto &segments = std::get>(runtime.scheme.segments); + const auto &sub_segments = segments[runtime.state.primary_segment_index].speed_mileage_segments; + const auto next_index = runtime.state.sub_segment_index + 1; + + if (next_index < sub_segments.size()) { + return std::pair{next_index, &sub_segments[next_index]}; + } + if (!sub_segments.empty()) { + return std::pair{std::size_t{0}, &sub_segments[0]}; + } + return std::nullopt; +} + +[[nodiscard]] +accel_calc_result rsmt_accel( + const SchemeTrackRuntime &runtime, + const SMSegment &next) { + const auto &state = runtime.state; + const float target_speed = next.speed_m_s; + float remaining_distance = + static_cast(next.mileage_from_start_m) - state.loop_mileage_m; + + if (next.mileage_from_start_m == 0 && state.sub_segment_index > 0) { + remaining_distance = current_rsmt_loop_length(runtime) - state.loop_mileage_m; + } + + if (remaining_distance <= 0.0F) { + return {target_speed, 0.0F}; + } + + float accel = + (target_speed * target_speed - state.speed_m_s * state.speed_m_s) / + (2.0F * remaining_distance); + accel = std::clamp(accel, -max_acceleration_m_s_2, max_acceleration_m_s_2); + return {target_speed, accel}; +} + +void tick_repeated_speed_mileage_time(const TrackConfig &config, SchemeTrackRuntime &runtime, float delta_s) { + auto &state = runtime.state; + const auto &segments = std::get>(runtime.scheme.segments); + state.elapsed_s += delta_s; + + if (state.primary_segment_index + 1 < segments.size()) { + const auto &next_time_segment = segments[state.primary_segment_index + 1]; + if (state.elapsed_s >= static_cast(next_time_segment.time_since_start_s)) { + const float line_length_m = config.line_length_m; + const float err = line_length_m > 0.0F + ? std::fmod(state.mileage_m, line_length_m) + : 0.0F; + const float epsilon = line_length_m * 0.01F; + if (std::abs(err) <= epsilon) { + ++state.primary_segment_index; + state.sub_segment_index = 0; + state.loop_mileage_m = 0.0F; + state.speed_m_s = + segments[state.primary_segment_index].speed_mileage_segments[0].speed_m_s; + } + } + } else if (state.elapsed_s >= static_cast(segments.back().time_since_start_s)) { + state.is_running = false; + return; + } + + const auto &sub_segments = segments[state.primary_segment_index].speed_mileage_segments; + const auto ¤t = sub_segments[state.sub_segment_index]; + + if (runtime.scheme.acceleration_profile == AccelerationProfile::smooth) { + accel_calc_result profile{current.speed_m_s, 0.0F}; + if (auto next = next_rsmt_sub_segment(runtime)) { + profile = rsmt_accel(runtime, *next->second); + } + apply_acceleration(state, profile, delta_s); + } else { + state.speed_m_s = current.speed_m_s; + } + + const float distance_traveled = state.speed_m_s * delta_s; + state.mileage_m += distance_traveled; + state.loop_mileage_m += distance_traveled; + + const float loop_length = current_rsmt_loop_length(runtime); + if (state.loop_mileage_m >= loop_length && loop_length > 0.0F) { + state.loop_mileage_m -= loop_length; + state.sub_segment_index = 0; + return; + } + + while (auto next = next_rsmt_sub_segment(runtime)) { + const auto [next_index, next_segment] = *next; + if (next_index == 0) { + break; + } + if (state.loop_mileage_m < static_cast(next_segment->mileage_from_start_m)) { + break; + } + state.sub_segment_index = next_index; + if (runtime.scheme.acceleration_profile == AccelerationProfile::instant) { + state.speed_m_s = next_segment->speed_m_s; + } + } +} + +} // namespace + +DecodedScheme make_speed_mileage_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments) { + return { + .id = id, + .color = color, + .kind = SchemeKind::speed_input_mileage_segmented_time_free, + .acceleration_profile = acceleration_profile, + .segments = std::move(segments), + }; +} + +DecodedScheme make_mileage_time_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments) { + return { + .id = id, + .color = color, + .kind = SchemeKind::mileage_input_time_segmented_speed_free, + .acceleration_profile = acceleration_profile, + .segments = std::move(segments), + }; +} + +DecodedScheme make_speed_time_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments) { + return { + .id = id, + .color = color, + .kind = SchemeKind::speed_input_time_segmented_mileage_free, + .acceleration_profile = acceleration_profile, + .segments = std::move(segments), + }; +} + +DecodedScheme make_repeated_speed_mileage_time_scheme( + std::uint8_t id, + Color color, + AccelerationProfile acceleration_profile, + std::vector segments) { + return { + .id = id, + .color = color, + .kind = SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented, + .acceleration_profile = acceleration_profile, + .segments = std::move(segments), + }; +} + +expected make_scheme_track_runtime(DecodedScheme scheme) { + auto normalized = canonicalize_scheme(std::move(scheme)); + if (!normalized) { + return unexpected{normalized.error()}; + } + + return SchemeTrackRuntime{ + .scheme = std::move(*normalized), + .state = {}, + }; +} + +SchemeTrackRuntime start_scheme_track(SchemeTrackRuntime runtime) { + runtime.state = {}; + runtime.state.is_running = true; + + switch (runtime.scheme.kind) { + case SchemeKind::speed_input_mileage_segmented_time_free: { + const auto &segments = std::get>(runtime.scheme.segments); + runtime.state.speed_m_s = segments[0].speed_m_s; + break; + } + case SchemeKind::mileage_input_time_segmented_speed_free: { + const auto &segments = std::get>(runtime.scheme.segments); + runtime.state.speed_m_s = calc_mts_target_speed(std::size_t{0}, segments).value_or(0.0F); + break; + } + case SchemeKind::speed_input_time_segmented_mileage_free: { + const auto &segments = std::get>(runtime.scheme.segments); + runtime.state.speed_m_s = segments[0].speed_m_s; + break; + } + case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: { + const auto &segments = std::get>(runtime.scheme.segments); + runtime.state.speed_m_s = segments[0].speed_mileage_segments[0].speed_m_s; + break; + } + } + + return runtime; +} + +SchemeTrackRuntime stop_scheme_track(SchemeTrackRuntime runtime) { + runtime.state.is_running = false; + return runtime; +} + +SchemeTrackRuntime tick_scheme_track( + const TrackConfig &config, + SchemeTrackRuntime runtime, + float delta_s) { + if (!runtime.state.is_running || !std::isfinite(delta_s) || delta_s <= 0.0F) { + return runtime; + } + + switch (runtime.scheme.kind) { + case SchemeKind::speed_input_mileage_segmented_time_free: + tick_speed_mileage(runtime, delta_s); + break; + case SchemeKind::mileage_input_time_segmented_speed_free: + tick_mileage_time(runtime, delta_s); + break; + case SchemeKind::speed_input_time_segmented_mileage_free: + tick_speed_time(runtime, delta_s); + break; + case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: + tick_repeated_speed_mileage_time(config, runtime, delta_s); + break; + } + + return runtime; +} + +TrackInfo scheme_track_info(const SchemeTrackRuntime &runtime) { + std::size_t segment_count = 0; + switch (runtime.scheme.kind) { + case SchemeKind::speed_input_mileage_segmented_time_free: + segment_count = std::get>(runtime.scheme.segments).size(); + break; + case SchemeKind::mileage_input_time_segmented_speed_free: + segment_count = std::get>(runtime.scheme.segments).size(); + break; + case SchemeKind::speed_input_time_segmented_mileage_free: + segment_count = std::get>(runtime.scheme.segments).size(); + break; + case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: { + const auto &segments = std::get>(runtime.scheme.segments); + segment_count = std::accumulate( + segments.begin(), + segments.end(), + std::size_t{0}, + [](std::size_t sum, const RepeatedSMSegment &segment) { + return sum + segment.speed_mileage_segments.size(); + }); + break; + } + } + + return { + .kind = runtime.scheme.kind, + .color = runtime.scheme.color, + .id = runtime.scheme.id, + .is_running = runtime.state.is_running, + .num_segments = clamp_u8(segment_count), + }; +} + +TrackReport scheme_track_report(const SchemeTrackRuntime &runtime) { + return { + .id = runtime.scheme.id, + .state = runtime.state.is_running ? TrackState::run : TrackState::stop, + .mileage_m = runtime.state.mileage_m, + .speed_m_s = runtime.state.speed_m_s, + .time_elapsed_ms = elapsed_ms(runtime.state.elapsed_s), + }; +} + +bool SchemeTrainingRuntime::has_program() const noexcept { + return !tracks_.empty(); +} + +bool SchemeTrainingRuntime::all_stopped() const noexcept { + return std::ranges::all_of(tracks_, [](const SchemeTrackRuntime &track) { + return !track.state.is_running; + }); +} + +expected SchemeTrainingRuntime::add_scheme(DecodedScheme scheme) { + auto runtime = make_scheme_track_runtime(std::move(scheme)); + if (!runtime) { + return unexpected{runtime.error()}; + } + + const auto id = runtime->scheme.id; + const auto existing = std::ranges::find_if(tracks_, [id](const SchemeTrackRuntime &track) { + return track.scheme.id == id; + }); + if (existing != tracks_.end()) { + tracks_.erase(existing); + } + + tracks_.push_back(std::move(*runtime)); + return {}; +} + +void SchemeTrainingRuntime::clear() { + tracks_.clear(); +} + +void SchemeTrainingRuntime::start() { + for (auto &track : tracks_) { + track = start_scheme_track(std::move(track)); + } +} + +void SchemeTrainingRuntime::stop() { + for (auto &track : tracks_) { + track = stop_scheme_track(std::move(track)); + } +} + +void SchemeTrainingRuntime::tick(const TrackConfig &config, float delta_s) { + for (auto &track : tracks_) { + track = tick_scheme_track(config, std::move(track), delta_s); + } +} + +TrackStateReportCollection SchemeTrainingRuntime::state_collection() const { + TrackStateReportCollection collection; + collection.states.reserve(tracks_.size()); + for (const auto &track : tracks_) { + collection.states.push_back(scheme_track_report(track)); + } + return collection; +} + +TrackSchemeMgrRead SchemeTrainingRuntime::scheme_status() const { + TrackSchemeMgrRead status; + status.scheme_status.reserve(tracks_.size()); + for (const auto &track : tracks_) { + const auto info = scheme_track_info(track); + status.scheme_status.push_back(TrackSchemeMgrRead::Status{ + .id = info.id, + .segment_count = info.num_segments, + .kind = info.kind, + }); + } + return status; +} + +expected +SchemeTrainingRuntime::render_to(const TrackConfig &config, TrackRenderSink sink) const { + if (!config.verify() || config.line_leds_num == 0) { + return unexpected{TrackError::invalid_arg}; + } + + for (const auto &track : tracks_) { + const auto plan = make_track_render_plan( + config, + scheme_track_info(track), + scheme_track_report(track)); + const auto err = apply_render_plan(sink, plan); + if (err != TrackError::ok) { + return unexpected{err}; + } + } + + return {}; +} + +expected, TrackError> +SchemeTrainingRuntime::render_pixels(const TrackConfig &config) const { + MemoryStrip strip(config.line_leds_num); + const auto rendered = render_to(config, make_memory_strip_sink(strip)); + if (!rendered) { + return unexpected{rendered.error()}; + } + + return std::vector{strip.pixels().begin(), strip.pixels().end()}; +} + +} // namespace track_core diff --git a/tests/track_core_tests.cpp b/tests/track_core_tests.cpp index 11b4ca2..51884fa 100644 --- a/tests/track_core_tests.cpp +++ b/tests/track_core_tests.cpp @@ -5,7 +5,9 @@ #include "track_core/memory_strip.hpp" #include "track_core/pid_program.hpp" +#include "track_core/pid_runtime.hpp" #include "track_core/scheme_decoder.hpp" +#include "track_core/scheme_runtime.hpp" namespace { @@ -129,6 +131,43 @@ void test_linear_render_reverse() { require(plan.spans[3].color == track_core::Color::cyan(), "reverse span 3 color"); } +void test_linear_render_boundaries_apply() { + track_core::TrackConfig config{ + .draw_kind = track_core::TrackDrawKind::linear, + .line_length_m = 10.0F, + .active_line_length_m = 4.0F, + .head_offset_m = 0.0F, + .line_leds_num = 20, + }; + const auto info = running_info(track_core::Color::red()); + + for (const float mileage : { + -20.0F, -10.1F, -10.0F, -9.9F, -1.0F, -0.1F, + 0.0F, 0.1F, 1.0F, 9.9F, 10.0F, 10.1F, + 19.9F, 20.0F, 20.1F, 29.9F, 30.0F, 30.1F, + 40.0F, + }) { + const track_core::TrackReport report{ + .id = 4, + .state = track_core::TrackState::run, + .mileage_m = mileage, + .speed_m_s = 2.0F, + }; + track_core::MemoryStrip strip(config.line_leds_num); + + const auto plan = track_core::make_track_render_plan(config, info, report); + require(plan.span_count <= track_core::TrackRenderPlan::max_spans, "linear boundary span capacity"); + for (std::size_t i = 0; i < plan.span_count; ++i) { + const auto &span = plan.spans[i]; + require(span.start_led < config.line_leds_num, "linear boundary span start in range"); + require(span.start_led + span.led_count <= config.line_leds_num, "linear boundary span end in range"); + } + require( + track_core::apply_render_plan(strip, plan) == track_core::TrackError::ok, + "linear boundary plan applies"); + } +} + void test_memory_strip_bounds() { track_core::MemoryStrip strip(4); @@ -176,15 +215,308 @@ void test_pid_program_constant() { require(state.is_finished(start + std::chrono::seconds(2)), "constant PID finished"); } +track_core::TrackPidConfig pid_runtime_config() { + track_core::TrackPidConfig config; + config.band_id = 5; + config.target_hr_bpm = 148; + config.deadzone_bpm = 3; + config.schemas = { + track_core::TrackPidSchema{track_core::TrackPidSegment{ + .duration_s = 45, + .min_speed_m_s = 0.0F, + .max_speed_m_s = 4.0F, + .kp = 1.0F, + .ki = 2.0F, + .kd = 0.0F, + .slew_rate_limit = 0.25F, + .fine_tune = std::nullopt, + }}, + }; + return config; +} + +track_core::TrackPidConfig constant_pid_runtime_config(float speed_m_s) { + track_core::TrackPidConfig config; + config.band_id = 5; + config.target_hr_bpm = 148; + config.deadzone_bpm = 3; + config.schemas = { + track_core::TrackPidSchema{track_core::TrackConstantSegment{ + .duration_s = 2, + .speed_m_s = speed_m_s, + }}, + }; + return config; +} + +track_core::TrackPidBandSnapshot band_sample(std::uint32_t seq, std::uint8_t heart_rate = 140) { + return { + .band_id = 5, + .heart_rate = heart_rate, + .heart_rate_sample_seq = seq, + .has_heart_rate = true, + .hr_is_fresh = true, + .step_count = 0, + .has_step_count = false, + .band_is_active = true, + }; +} + +void test_pid_runtime_consumes_each_hr_sample_once() { + track_core::PidHrRuntime runtime(pid_runtime_config()); + const auto start = track_core::clock::time_point(std::chrono::seconds(0)); + runtime.start(start); + + runtime.tick(nullptr, band_sample(1), start + std::chrono::milliseconds(1)); + const auto first_status = runtime.pid_status(band_sample(1), start + std::chrono::milliseconds(1)); + require_near(first_status.effective_speed_m_s, 0.25F, 0.0001F, "pid runtime first effective speed"); + require_near(first_status.base_speed_m_s, 0.25F, 0.0001F, "pid runtime first base speed"); + + runtime.tick(nullptr, band_sample(1), start + std::chrono::milliseconds(2)); + const auto repeated_status = runtime.pid_status(band_sample(1), start + std::chrono::milliseconds(2)); + require_near( + repeated_status.effective_speed_m_s, + first_status.effective_speed_m_s, + 0.0001F, + "pid runtime ignores repeated sample effective speed"); + require_near( + repeated_status.base_speed_m_s, + first_status.base_speed_m_s, + 0.0001F, + "pid runtime ignores repeated sample base speed"); + + runtime.tick(nullptr, band_sample(2), start + std::chrono::milliseconds(3)); + const auto second_status = runtime.pid_status(band_sample(2), start + std::chrono::milliseconds(3)); + require(second_status.effective_speed_m_s > repeated_status.effective_speed_m_s, "pid runtime consumes new sample"); + require(second_status.base_speed_m_s > repeated_status.base_speed_m_s, "pid runtime updates base speed on new sample"); +} + +void test_pid_runtime_suppression_is_explicitly_configured() { + auto config = constant_pid_runtime_config(1.0F); + config.speed_suppression = track_core::TrackPidSpeedSuppression{ + .ratio_min = 0.25F, + .sigma_m = 0.01F, + }; + const track_core::TrackConfig track_config{ + .draw_kind = track_core::TrackDrawKind::circular, + .line_length_m = 0.1F, + .active_line_length_m = 0.025F, + .head_offset_m = 0.0F, + .line_leds_num = 100, + }; + const auto start = track_core::clock::time_point(std::chrono::seconds(0)); + + track_core::PidHrRuntime without_config(config); + without_config.start(start); + without_config.tick(nullptr, {}, start + std::chrono::milliseconds(1)); + auto status = without_config.pid_status({}, start + std::chrono::milliseconds(1)); + require_near(status.effective_speed_m_s, 1.0F, 0.0001F, "pid runtime skips suppression without track config"); + require_near(status.base_speed_m_s, 1.0F, 0.0001F, "pid runtime base speed without track config"); + + track_core::PidHrRuntime with_config(config); + with_config.start(start); + with_config.tick(&track_config, {}, start + std::chrono::milliseconds(1)); + status = with_config.pid_status({}, start + std::chrono::milliseconds(1)); + require(status.active_segment_kind == track_core::TrackPidStageKind::constant, "pid runtime constant stage kind"); + require_near(status.effective_speed_m_s, 0.25F, 0.0001F, "pid runtime suppresses at seam"); + require_near(status.base_speed_m_s, 1.0F, 0.0001F, "pid runtime base speed at seam"); +} + +void test_pid_runtime_applies_live_tuning() { + track_core::PidHrRuntime runtime(pid_runtime_config()); + const auto start = track_core::clock::time_point(std::chrono::seconds(0)); + runtime.start(start); + runtime.tick(nullptr, band_sample(1), start + std::chrono::milliseconds(1)); + + const track_core::TrackPidRuntimeCommand command{ + .command = track_core::TrackPidSetTuning{ + .min_speed_m_s = 1.2F, + .max_speed_m_s = 3.5F, + .kp = 0.5F, + .ki = 0.25F, + .kd = 0.0F, + .slew_rate_limit = 0.25F, + .fine_tune = std::nullopt, + }, + }; + + const auto applied = runtime.apply_pid_runtime_command(command, nullptr); + require(applied.has_value(), "pid runtime applies tuning"); + require_near( + std::get(runtime.pid_config().schemas.front().segment).min_speed_m_s, + 1.2F, + 0.0001F, + "pid runtime config tracks tuning"); + const auto status = runtime.pid_status(band_sample(1), start + std::chrono::milliseconds(1)); + require_near(status.effective_speed_m_s, 1.2F, 0.0001F, "pid runtime tuned effective speed"); + require_near(status.base_speed_m_s, 1.2F, 0.0001F, "pid runtime tuned base speed"); +} + +track_core::TrackConfig runtime_config() { + return { + .draw_kind = track_core::TrackDrawKind::circular, + .line_length_m = 5.0F, + .active_line_length_m = 1.0F, + .head_offset_m = 0.0F, + .line_leds_num = 50, + }; +} + +void test_speed_mileage_runtime_ticks() { + auto runtime = track_core::make_scheme_track_runtime(track_core::make_speed_mileage_scheme( + 11, + track_core::Color::red(), + track_core::AccelerationProfile::instant, + { + track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 0}, + track_core::SMSegment{.speed_m_s = 2.0F, .mileage_from_start_m = 5}, + track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 10}, + })); + + require(runtime.has_value(), "SM runtime builds"); + auto track = track_core::start_scheme_track(std::move(*runtime)); + track = track_core::tick_scheme_track(runtime_config(), std::move(track), 6.0F); + + const auto report = track_core::scheme_track_report(track); + require(report.state == track_core::TrackState::run, "SM still running before end"); + require_near(report.mileage_m, 6.0F, 0.001F, "SM mileage after tick"); + require_near(report.speed_m_s, 2.0F, 0.001F, "SM speed after boundary"); +} + +void test_mileage_time_runtime_ticks() { + auto runtime = track_core::make_scheme_track_runtime(track_core::make_mileage_time_scheme( + 12, + track_core::Color::green(), + track_core::AccelerationProfile::instant, + { + track_core::MTSegment{.mileage_to_travel_this_segment_m = 10, .time_since_start_s = 0}, + track_core::MTSegment{.mileage_to_travel_this_segment_m = 20, .time_since_start_s = 5}, + track_core::MTSegment{.mileage_to_travel_this_segment_m = 1, .time_since_start_s = 15}, + })); + + require(runtime.has_value(), "MT runtime builds"); + auto track = track_core::start_scheme_track(std::move(*runtime)); + track = track_core::tick_scheme_track(runtime_config(), std::move(track), 2.0F); + + auto report = track_core::scheme_track_report(track); + require_near(report.speed_m_s, 2.0F, 0.001F, "MT initial derived speed"); + require_near(report.mileage_m, 4.0F, 0.001F, "MT mileage after first tick"); + + track = track_core::tick_scheme_track(runtime_config(), std::move(track), 4.0F); + report = track_core::scheme_track_report(track); + require_near(report.speed_m_s, 2.0F, 0.001F, "MT speed after first time boundary"); + require_near(report.mileage_m, 12.0F, 0.001F, "MT mileage after boundary tick"); +} + +void test_speed_time_runtime_ticks() { + auto runtime = track_core::make_scheme_track_runtime(track_core::make_speed_time_scheme( + 13, + track_core::Color::blue(), + track_core::AccelerationProfile::instant, + { + track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 0}, + track_core::STSegment{.speed_m_s = 3.0F, .time_since_start_s = 5}, + track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 10}, + })); + + require(runtime.has_value(), "ST runtime builds"); + auto track = track_core::start_scheme_track(std::move(*runtime)); + track = track_core::tick_scheme_track(runtime_config(), std::move(track), 6.0F); + + const auto report = track_core::scheme_track_report(track); + require(report.state == track_core::TrackState::run, "ST still running before end"); + require_near(report.mileage_m, 6.0F, 0.001F, "ST mileage after tick"); + require_near(report.speed_m_s, 3.0F, 0.001F, "ST speed after time boundary"); +} + +void test_repeated_speed_mileage_time_runtime_ticks() { + auto runtime = track_core::make_scheme_track_runtime(track_core::make_repeated_speed_mileage_time_scheme( + 14, + track_core::Color::white(), + track_core::AccelerationProfile::instant, + { + track_core::RepeatedSMSegment{ + .speed_mileage_segments = { + track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 0}, + track_core::SMSegment{.speed_m_s = 2.0F, .mileage_from_start_m = 5}, + }, + .time_since_start_s = 0, + }, + track_core::RepeatedSMSegment{ + .speed_mileage_segments = { + track_core::SMSegment{.speed_m_s = 3.0F, .mileage_from_start_m = 0}, + track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 5}, + }, + .time_since_start_s = 4, + }, + track_core::RepeatedSMSegment{ + .speed_mileage_segments = { + track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 0}, + track_core::SMSegment{.speed_m_s = 1.0F, .mileage_from_start_m = 5}, + }, + .time_since_start_s = 20, + }, + })); + + require(runtime.has_value(), "RSMT runtime builds"); + auto track = track_core::start_scheme_track(std::move(*runtime)); + track = track_core::tick_scheme_track(runtime_config(), std::move(track), 3.0F); + track = track_core::tick_scheme_track(runtime_config(), std::move(track), 2.0F); + require_eq_u32(static_cast(track.state.primary_segment_index), 0, "RSMT waits for line alignment"); + + track = track_core::tick_scheme_track(runtime_config(), std::move(track), 1.0F); + require_eq_u32(static_cast(track.state.primary_segment_index), 1, "RSMT switches on alignment"); + require_near(track.state.speed_m_s, 3.0F, 0.001F, "RSMT switched speed"); +} + +void test_scheme_training_runtime_renders() { + track_core::SchemeTrainingRuntime runtime; + const auto added = runtime.add_scheme(track_core::make_speed_time_scheme( + 15, + track_core::Color::green(), + track_core::AccelerationProfile::instant, + { + track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 0}, + track_core::STSegment{.speed_m_s = 1.0F, .time_since_start_s = 10}, + })); + require(added.has_value(), "training runtime accepts scheme"); + + runtime.start(); + runtime.tick(runtime_config(), 1.0F); + const auto pixels = runtime.render_pixels(runtime_config()); + + require(pixels.has_value(), "training runtime renders pixels"); + require_eq_u32(static_cast(pixels->size()), runtime_config().line_leds_num, "training runtime pixel count"); + + track_core::MemoryStrip strip(runtime_config().line_leds_num); + const auto rendered = runtime.render_to(runtime_config(), track_core::make_memory_strip_sink(strip)); + require(rendered.has_value(), "training runtime renders to sink"); + for (std::size_t i = 0; i < pixels->size(); ++i) { + require((*pixels)[i] == strip.pixels()[i], "render_to matches render_pixels"); + } + + require(!runtime.all_stopped(), "training runtime running"); + require_eq_u32(runtime.state_collection().states[0].id, 15, "training runtime report id"); +} + } // namespace int main() { test_circular_render_wraps(); test_linear_render_forward_and_memory_strip(); test_linear_render_reverse(); + test_linear_render_boundaries_apply(); test_memory_strip_bounds(); test_scheme_decoder(); test_pid_program_constant(); + test_pid_runtime_consumes_each_hr_sample_once(); + test_pid_runtime_suppression_is_explicitly_configured(); + test_pid_runtime_applies_live_tuning(); + test_speed_mileage_runtime_ticks(); + test_mileage_time_runtime_ticks(); + test_speed_time_runtime_ticks(); + test_repeated_speed_mileage_time_runtime_ticks(); + test_scheme_training_runtime_renders(); std::cout << "track-core tests passed\n"; return 0; }