From 84598cad20f6eeba7dfd51195b1f48f84b084ffe Mon Sep 17 00:00:00 2001 From: crosstyan Date: Fri, 15 May 2026 16:02:44 +0800 Subject: [PATCH] Initial standalone track core --- .gitignore | 3 + CMakeLists.txt | 40 ++++ README.md | 17 ++ examples/render_demo.cpp | 40 ++++ include/track_core/memory_strip.hpp | 49 ++++ include/track_core/model.hpp | 330 ++++++++++++++++++++++++++ include/track_core/pid_program.hpp | 116 +++++++++ include/track_core/render.hpp | 36 +++ include/track_core/scheme_decoder.hpp | 31 +++ src/memory_strip.cpp | 72 ++++++ src/model.cpp | 104 ++++++++ src/pid_program.cpp | 326 +++++++++++++++++++++++++ src/render.cpp | 242 +++++++++++++++++++ src/scheme_decoder.cpp | 137 +++++++++++ tests/track_core_tests.cpp | 190 +++++++++++++++ 15 files changed, 1733 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 examples/render_demo.cpp create mode 100644 include/track_core/memory_strip.hpp create mode 100644 include/track_core/model.hpp create mode 100644 include/track_core/pid_program.hpp create mode 100644 include/track_core/render.hpp create mode 100644 include/track_core/scheme_decoder.hpp create mode 100644 src/memory_strip.cpp create mode 100644 src/model.cpp create mode 100644 src/pid_program.cpp create mode 100644 src/render.cpp create mode 100644 src/scheme_decoder.cpp create mode 100644 tests/track_core_tests.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..86a300e --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +build/ +.DS_Store +compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9d9190e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,40 @@ +set(TRACK_CORE_SOURCES + src/memory_strip.cpp + src/model.cpp + src/pid_program.cpp + src/render.cpp + src/scheme_decoder.cpp +) + +if(DEFINED IDF_TARGET) + idf_component_register( + SRCS ${TRACK_CORE_SOURCES} + INCLUDE_DIRS include + ) +else() + cmake_minimum_required(VERSION 3.20) + project(track_core VERSION 0.1.0 LANGUAGES CXX) + + add_library(track_core ${TRACK_CORE_SOURCES}) + add_library(track_core::track_core ALIAS track_core) + + target_include_directories(track_core + PUBLIC + $ + $ + ) + target_compile_features(track_core PUBLIC cxx_std_23) + + option(TRACK_CORE_BUILD_TESTS "Build track-core tests" ON) + if(TRACK_CORE_BUILD_TESTS) + enable_testing() + add_executable(track_core_tests tests/track_core_tests.cpp) + target_link_libraries(track_core_tests PRIVATE track_core::track_core) + target_compile_features(track_core_tests PRIVATE cxx_std_23) + add_test(NAME track_core_tests COMMAND track_core_tests) + endif() + + add_executable(track_core_render_demo examples/render_demo.cpp) + target_link_libraries(track_core_render_demo PRIVATE track_core::track_core) + target_compile_features(track_core_render_demo PRIVATE cxx_std_23) +endif() diff --git a/README.md b/README.md new file mode 100644 index 0000000..de7d84e --- /dev/null +++ b/README.md @@ -0,0 +1,17 @@ +# track-core + +Standalone C++ core for TrackBackFwd track simulation and strip rendering. + +This library is intentionally platform-neutral. It does not depend on ESP-IDF, +FreeRTOS, protobuf/nanopb, BLE, or hardware LED drivers. + +## Build + +```bash +cmake -S . -B build +cmake --build build +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. diff --git a/examples/render_demo.cpp b/examples/render_demo.cpp new file mode 100644 index 0000000..96aaac9 --- /dev/null +++ b/examples/render_demo.cpp @@ -0,0 +1,40 @@ +#include + +#include "track_core/memory_strip.hpp" + +int main() { + track_core::TrackConfig config{ + .draw_kind = track_core::TrackDrawKind::circular, + .line_length_m = 10.0F, + .active_line_length_m = 4.0F, + .head_offset_m = 0.0F, + .line_leds_num = 20, + }; + track_core::TrackInfo info{ + .kind = track_core::SchemeKind::speed_input_time_segmented_mileage_free, + .color = track_core::Color::green(), + .id = 1, + .is_running = true, + .num_segments = 1, + }; + track_core::TrackReport report{ + .id = 1, + .state = track_core::TrackState::run, + .mileage_m = 8.5F, + .speed_m_s = 1.0F, + .time_elapsed_ms = 0, + }; + + track_core::MemoryStrip strip(config.line_leds_num); + const auto plan = track_core::make_track_render_plan(config, info, report); + static_cast(track_core::apply_render_plan(strip, plan)); + static_cast(strip.show()); + + std::cout << "frame=" << strip.frame_sequence() << " leds=" << strip.leds_count() << "\n"; + for (std::size_t i = 0; i < plan.span_count; ++i) { + const auto &span = plan.spans[i]; + std::cout << "span start=" << span.start_led + << " count=" << span.led_count + << " color=" << span.color.hex() << "\n"; + } +} diff --git a/include/track_core/memory_strip.hpp b/include/track_core/memory_strip.hpp new file mode 100644 index 0000000..5883cac --- /dev/null +++ b/include/track_core/memory_strip.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +#include "track_core/model.hpp" +#include "track_core/render.hpp" + +namespace track_core { + +class MemoryStrip { +public: + explicit MemoryStrip(std::size_t led_count = 0); + + [[nodiscard]] + TrackError begin(); + + [[nodiscard]] + TrackError clear(); + + [[nodiscard]] + TrackError fill(std::size_t start, std::size_t count, Color color); + + [[nodiscard]] + TrackError show(); + + [[nodiscard]] + TrackError set_leds_count(std::size_t count); + + [[nodiscard]] + std::size_t leds_count() const; + + [[nodiscard]] + std::uint64_t frame_sequence() const; + + [[nodiscard]] + std::span pixels() const; + +private: + std::vector pixels_; + std::uint64_t frame_sequence_{}; + bool begun_{}; +}; + +[[nodiscard]] +TrackError apply_render_plan(MemoryStrip &strip, const TrackRenderPlan &plan); + +} // namespace track_core diff --git a/include/track_core/model.hpp b/include/track_core/model.hpp new file mode 100644 index 0000000..fad4653 --- /dev/null +++ b/include/track_core/model.hpp @@ -0,0 +1,330 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace track_core { + +template +using expected = std::expected; + +template +using unexpected = std::unexpected; + +using unit = std::monostate; + +enum class TrackError : int { + ok = 0, + invalid_arg, + invalid_size, + invalid_state, + not_supported, + range, +}; + +enum class TrackDrawKind : std::uint8_t { + circular = 0, + linear = 1, +}; + +enum class SchemeKind : std::uint8_t { + speed_input_mileage_segmented_time_free = 0, + mileage_input_time_segmented_speed_free = 1, + speed_input_time_segmented_mileage_free = 2, + repeated_speed_input_mileage_segmentation_input_time_segmented = 3, +}; + +enum class AccelerationProfile : std::uint8_t { + instant = 0, + smooth = 1, +}; + +enum class TrackState : std::uint8_t { + stop = 0, + run = 1, + test_rainbow = 2, + test_blink = 3, +}; + +enum class TrackControllerMode : std::uint8_t { + scheme = 0, + pid_hr = 1, +}; + +enum class TrackPidStageKind : std::uint8_t { + none = 0, + constant = 1, + pid = 2, +}; + +struct Color { + std::uint8_t r{}; + std::uint8_t g{}; + std::uint8_t b{}; + + constexpr Color() = default; + constexpr Color(std::uint8_t red, std::uint8_t green, std::uint8_t blue) + : r(red), g(green), b(blue) {} + constexpr explicit Color(std::uint32_t value) + : r(static_cast((value >> 16U) & 0xFFU)), + g(static_cast((value >> 8U) & 0xFFU)), + b(static_cast(value & 0xFFU)) {} + + [[nodiscard]] + constexpr bool operator==(const Color &) const = default; + + [[nodiscard]] + constexpr explicit operator std::uint32_t() const { + return (static_cast(r) << 16U) | + (static_cast(g) << 8U) | + static_cast(b); + } + + [[nodiscard]] + std::string hex() const; + + [[nodiscard]] + static constexpr Color black() { return {0, 0, 0}; } + [[nodiscard]] + static constexpr Color red() { return {255, 0, 0}; } + [[nodiscard]] + static constexpr Color orange() { return {255, 165, 0}; } + [[nodiscard]] + static constexpr Color yellow() { return {255, 255, 0}; } + [[nodiscard]] + static constexpr Color green() { return {0, 255, 0}; } + [[nodiscard]] + static constexpr Color cyan() { return {0, 255, 255}; } + [[nodiscard]] + static constexpr Color blue() { return {0, 0, 255}; } + [[nodiscard]] + static constexpr Color indigo() { return {75, 0, 130}; } + [[nodiscard]] + static constexpr Color violet() { return {238, 130, 238}; } + [[nodiscard]] + static constexpr Color white() { return {255, 255, 255}; } +}; + +struct TrackConfig { + TrackDrawKind draw_kind{TrackDrawKind::circular}; + float line_length_m{0.0F}; + float active_line_length_m{0.0F}; + float head_offset_m{0.0F}; + std::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; + } + + [[nodiscard]] + bool verify() const { + if (line_length_m <= 0.0F || active_line_length_m <= 0.0F) { + return false; + } + if (active_line_length_m > line_length_m) { + return false; + } + if (head_offset_m >= line_length_m) { + return false; + } + return true; + } + + [[nodiscard]] + static TrackConfig default_config() { + return { + .draw_kind = TrackDrawKind::circular, + .line_length_m = 400.0F, + .active_line_length_m = 10.0F, + .head_offset_m = 0.0F, + .line_leds_num = 400, + }; + } +}; + +struct TrackInfo { + SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free}; + Color color{Color::white()}; + std::uint8_t id{}; + bool is_running{}; + std::uint8_t num_segments{}; +}; + +struct TrackReport { + std::uint8_t id{}; + TrackState state{TrackState::stop}; + float mileage_m{}; + float speed_m_s{}; + std::uint32_t time_elapsed_ms{}; +}; + +struct TrackStateReportCollection { + std::vector states; +}; + +struct TrackSchemeMgrRead { + struct Status { + std::uint8_t id{}; + std::uint8_t segment_count{}; + SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free}; + }; + + std::vector scheme_status; +}; + +struct SMSegment { + static constexpr float speed_lsb = 10.0F / 255.0F; + static constexpr std::size_t raw_size = 3; + + float speed_m_s{}; + std::uint16_t mileage_from_start_m{}; +}; + +struct MTSegment { + std::uint16_t mileage_to_travel_this_segment_m{}; + std::uint16_t time_since_start_s{}; +}; + +struct STSegment { + static constexpr float speed_lsb = 10.0F / 255.0F; + static constexpr std::size_t raw_size = 3; + + float speed_m_s{}; + std::uint16_t time_since_start_s{}; +}; + +struct RepeatedSMSegment { + std::vector speed_mileage_segments; + std::uint16_t time_since_start_s{}; +}; + +struct TrackPidFineTune { + std::uint8_t band_plus{}; + std::uint8_t band_minus{}; + float gain_scale{0.0F}; +}; + +struct TrackPidSpeedSuppression { + float ratio_min{0.1F}; + float sigma_m{1.0F}; +}; + +struct TrackPidSegment { + static constexpr float default_kp = 0.0457F; + static constexpr float default_ki = 0.001464F; + static constexpr float default_kd = 0.0F; + static constexpr float default_slew_rate_limit_m_s = 0.225F; + + std::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}; + std::optional fine_tune; + + [[nodiscard]] + static TrackPidSegment default_segment() { + return { + .duration_s = 300, + .min_speed_m_s = 0.0F, + .max_speed_m_s = 4.0F, + .kp = default_kp, + .ki = default_ki, + .kd = default_kd, + .slew_rate_limit = default_slew_rate_limit_m_s, + .fine_tune = std::nullopt, + }; + } +}; + +struct TrackConstantSegment { + std::uint16_t duration_s{0}; + float speed_m_s{0.0F}; +}; + +struct TrackPidSchema { + using variant_type = std::variant; + + variant_type segment{TrackConstantSegment{}}; + + [[nodiscard]] + TrackPidStageKind kind() const { + return std::holds_alternative(segment) + ? TrackPidStageKind::pid + : TrackPidStageKind::constant; + } +}; + +struct TrackPidConfig { + std::uint8_t band_id{0}; + std::uint8_t target_hr_bpm{120}; + std::uint8_t deadzone_bpm{3}; + bool preemptive_pid_activation{false}; + std::optional speed_suppression; + std::vector schemas; + + [[nodiscard]] + bool empty() const { + return schemas.empty(); + } + + [[nodiscard]] + static TrackPidConfig default_config() { + TrackPidConfig config; + config.schemas.push_back(TrackPidSchema{TrackPidSegment::default_segment()}); + return config; + } + + [[nodiscard]] + expected validate(bool allow_empty = false) const; +}; + +struct TrackPidSetTargetHr { + std::uint8_t target_hr_bpm{120}; +}; + +struct TrackPidSetTuning { + 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}; + std::optional fine_tune; + + [[nodiscard]] + expected validate() const; + void apply_to(TrackPidSegment &segment) const; +}; + +struct TrackPidRuntimeCommand { + std::variant command{unit{}}; +}; + +struct TrackPidStatus { + std::uint8_t band_id{}; + bool band_is_active{}; + bool is_heart_rate_valid{}; + std::uint8_t heart_rate_bpm{}; + std::uint16_t step_count{}; + std::uint8_t active_segment_index{}; + TrackPidStageKind active_segment_kind{TrackPidStageKind::none}; + float effective_speed_m_s{}; + std::uint32_t remaining_program_ms{}; + float base_speed_m_s{}; +}; + +using clock = std::chrono::steady_clock; + +} // namespace track_core diff --git a/include/track_core/pid_program.hpp b/include/track_core/pid_program.hpp new file mode 100644 index 0000000..2e1ebdd --- /dev/null +++ b/include/track_core/pid_program.hpp @@ -0,0 +1,116 @@ +#pragma once + +#include "track_core/model.hpp" + +namespace track_core { + +class TrackPidProgramState { +public: + using time_point = clock::time_point; + + struct HrSample { + float heart_rate_bpm{0.0F}; + float sample_interval_s{0.0F}; + }; + + TrackPidProgramState() = delete; + TrackPidProgramState(time_point now, TrackPidConfig config); + + [[nodiscard]] + float next(time_point now, std::optional fresh_hr_sample); + + [[nodiscard]] + bool is_finished(time_point now) const; + + [[nodiscard]] + TrackPidStageKind active_stage_kind() const; + + [[nodiscard]] + std::uint8_t active_stage_index() const; + + [[nodiscard]] + float commanded_speed_m_s() const; + + [[nodiscard]] + std::uint32_t remaining_program_ms(time_point now) const; + + void update_target_hr_bpm(std::uint8_t target_hr_bpm); + + [[nodiscard]] + expected update_tuning(const TrackPidSetTuning &tuning); + +private: + struct pid_state { + struct fine_tune_state { + std::uint8_t band_plus{}; + std::uint8_t band_minus{}; + float gain_scale{1.0F}; + }; + + float kp{0.0F}; + float ki{1.0F}; + float kd{0.0F}; + std::optional fine_tune; + float slew_rate_limit{0.0F}; + float e_t_1{0.0F}; + float e_t_2{0.0F}; + float u_t_1{0.0F}; + float min_speed_m_s{0.0F}; + float max_speed_m_s{0.0F}; + bool should_use_nominal_sample_interval{true}; + + void from_segment(const TrackPidSegment &segment); + void reset_with(float speed_m_s); + + [[nodiscard]] + float effective_gain_scale(float target_hr, float current_hr) const; + + [[nodiscard]] + float clamp_commanded_speed(float speed_m_s) const; + + void apply_tuning(const TrackPidSetTuning &tuning); + }; + + [[nodiscard]] + const TrackPidSchema &schema() const; + + [[nodiscard]] + static std::uint16_t schema_duration_s(const TrackPidSchema &schema); + + [[nodiscard]] + bool sync_schema_for_time(time_point now); + + void advance_to_schema_index(std::size_t target_index); + + [[nodiscard]] + std::size_t resolve_schema_index(time_point now) const; + + [[nodiscard]] + bool maybe_jump_to_final_pid(time_point now, std::optional fresh_hr_sample); + + [[nodiscard]] + bool is_in_deadzone(float heart_rate) const; + + [[nodiscard]] + float do_pid(float current_hr, float sample_interval_s); + + void apply_schema(const TrackPidSchema &schema); + void finish(); + + [[nodiscard]] + static bool validate_preemptive_schema(const std::vector &schemas); + + [[nodiscard]] + static time_point safe_add_seconds(time_point now, std::uint32_t duration_s); + + TrackPidConfig config_; + time_point program_start_timestamp_{}; + time_point program_end_timestamp_{}; + std::size_t schema_index_{}; + pid_state pid_{}; + bool preemptive_enabled_{}; + bool forced_final_pid_{}; + bool finished_{}; +}; + +} // namespace track_core diff --git a/include/track_core/render.hpp b/include/track_core/render.hpp new file mode 100644 index 0000000..ac6ae40 --- /dev/null +++ b/include/track_core/render.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +#include "track_core/model.hpp" + +namespace track_core { + +struct TrackRenderSpan { + std::uint16_t start_led{}; + std::uint16_t led_count{}; + Color color{}; +}; + +struct TrackRenderPlan { + static constexpr std::size_t max_spans = 4; + + void add_fill(std::uint16_t start_led, std::uint16_t led_count, Color color); + + [[nodiscard]] + bool empty() const { + return span_count == 0; + } + + std::array spans{}; + std::size_t span_count{}; +}; + +[[nodiscard]] +TrackRenderPlan make_track_render_plan( + const TrackConfig &config, + const TrackInfo &info, + const TrackReport &report); + +} // namespace track_core diff --git a/include/track_core/scheme_decoder.hpp b/include/track_core/scheme_decoder.hpp new file mode 100644 index 0000000..92f1fe2 --- /dev/null +++ b/include/track_core/scheme_decoder.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include +#include +#include +#include + +#include "track_core/model.hpp" + +namespace track_core { + +struct DecodedScheme { + std::uint8_t id{}; + Color color{Color::white()}; + SchemeKind kind{SchemeKind::speed_input_time_segmented_mileage_free}; + AccelerationProfile acceleration_profile{AccelerationProfile::smooth}; + std::variant< + std::vector, + std::vector, + std::vector, + std::vector> + segments; +}; + +[[nodiscard]] +expected decode_scheme( + std::uint8_t id, + Color color, + std::span binary); + +} // namespace track_core diff --git a/src/memory_strip.cpp b/src/memory_strip.cpp new file mode 100644 index 0000000..ff825b0 --- /dev/null +++ b/src/memory_strip.cpp @@ -0,0 +1,72 @@ +#include "track_core/memory_strip.hpp" + +#include + +namespace track_core { + +MemoryStrip::MemoryStrip(std::size_t led_count) + : pixels_(led_count, Color::black()) {} + +TrackError MemoryStrip::begin() { + if (pixels_.empty()) { + return TrackError::invalid_size; + } + begun_ = true; + return TrackError::ok; +} + +TrackError MemoryStrip::clear() { + std::ranges::fill(pixels_, Color::black()); + return TrackError::ok; +} + +TrackError MemoryStrip::fill(std::size_t start, std::size_t count, Color color) { + if (count == 0) { + return TrackError::ok; + } + if (start >= pixels_.size()) { + return TrackError::range; + } + + const auto available = pixels_.size() - start; + const auto length = std::min(count, available); + std::fill_n(pixels_.begin() + static_cast(start), length, color); + return TrackError::ok; +} + +TrackError MemoryStrip::show() { + ++frame_sequence_; + return TrackError::ok; +} + +TrackError MemoryStrip::set_leds_count(std::size_t count) { + if (count == 0) { + return TrackError::invalid_arg; + } + pixels_.assign(count, Color::black()); + return TrackError::ok; +} + +std::size_t MemoryStrip::leds_count() const { + return pixels_.size(); +} + +std::uint64_t MemoryStrip::frame_sequence() const { + return frame_sequence_; +} + +std::span MemoryStrip::pixels() const { + return pixels_; +} + +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; +} + +} // namespace track_core diff --git a/src/model.cpp b/src/model.cpp new file mode 100644 index 0000000..f5fad56 --- /dev/null +++ b/src/model.cpp @@ -0,0 +1,104 @@ +#include "track_core/model.hpp" + +#include +#include + +namespace track_core { +namespace { +template +struct overloads : Ts... { + using Ts::operator()...; +}; +} // namespace + +std::string Color::hex() const { + char buffer[8]{}; + static_cast(std::snprintf(buffer, sizeof(buffer), "#%02X%02X%02X", r, g, b)); + return buffer; +} + +expected TrackPidConfig::validate(bool allow_empty) 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{TrackError::invalid_arg}; + } + if (schemas.empty()) { + if (allow_empty) { + return {}; + } + return unexpected{TrackError::invalid_arg}; + } + for (const auto &schema : schemas) { + const auto ok = std::visit(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{TrackError::invalid_arg}; + } + } + return {}; +} + +expected TrackPidSetTuning::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{TrackError::invalid_arg}; + } + return {}; +} + +void TrackPidSetTuning::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; +} + +} // namespace track_core diff --git a/src/pid_program.cpp b/src/pid_program.cpp new file mode 100644 index 0000000..ff73322 --- /dev/null +++ b/src/pid_program.cpp @@ -0,0 +1,326 @@ +#include "track_core/pid_program.hpp" + +#include +#include +#include + +namespace track_core { +namespace { +constexpr float default_pid_nominal_sample_interval_s = 2.0F; +constexpr float default_pid_min_sample_interval_s = 1.0F; +constexpr float default_pid_max_sample_interval_s = 3.0F; + +template +struct overloads : Ts... { + using Ts::operator()...; +}; + +float clamp_hr_sample_interval_s(float sample_interval_s) { + if (!std::isfinite(sample_interval_s) || sample_interval_s <= 0.0F) { + return default_pid_nominal_sample_interval_s; + } + return std::clamp(sample_interval_s, + default_pid_min_sample_interval_s, + default_pid_max_sample_interval_s); +} +} // namespace + +void TrackPidProgramState::pid_state::from_segment(const TrackPidSegment &segment) { + kp = segment.kp; + ki = segment.ki; + kd = segment.kd; + slew_rate_limit = segment.slew_rate_limit; + min_speed_m_s = segment.min_speed_m_s; + max_speed_m_s = segment.max_speed_m_s; + u_t_1 = clamp_commanded_speed(u_t_1); + should_use_nominal_sample_interval = true; + if (segment.fine_tune) { + fine_tune = fine_tune_state{ + .band_plus = segment.fine_tune->band_plus, + .band_minus = segment.fine_tune->band_minus, + .gain_scale = segment.fine_tune->gain_scale, + }; + } else { + fine_tune.reset(); + } +} + +void TrackPidProgramState::pid_state::reset_with(float speed_m_s) { + kp = 0.0F; + ki = 1.0F; + kd = 0.0F; + fine_tune.reset(); + slew_rate_limit = 0.0F; + e_t_1 = 0.0F; + e_t_2 = 0.0F; + u_t_1 = std::max(0.0F, speed_m_s); + min_speed_m_s = 0.0F; + max_speed_m_s = u_t_1; + should_use_nominal_sample_interval = true; +} + +float TrackPidProgramState::pid_state::effective_gain_scale(float target_hr, float current_hr) const { + if (!fine_tune) { + return 1.0F; + } + const auto &ft = *fine_tune; + if (current_hr > (target_hr + static_cast(ft.band_plus)) || + current_hr < (target_hr - static_cast(ft.band_minus))) { + return 1.0F; + } + return ft.gain_scale; +} + +float TrackPidProgramState::pid_state::clamp_commanded_speed(float speed_m_s) const { + const auto upper_speed_m_s = std::max(0.0F, max_speed_m_s); + const auto lower_speed_m_s = std::clamp(min_speed_m_s, 0.0F, upper_speed_m_s); + return std::clamp(speed_m_s, lower_speed_m_s, upper_speed_m_s); +} + +void TrackPidProgramState::pid_state::apply_tuning(const TrackPidSetTuning &tuning) { + kp = tuning.kp; + ki = tuning.ki; + kd = tuning.kd; + slew_rate_limit = tuning.slew_rate_limit; + min_speed_m_s = tuning.min_speed_m_s; + max_speed_m_s = tuning.max_speed_m_s; + u_t_1 = clamp_commanded_speed(u_t_1); + if (tuning.fine_tune) { + fine_tune = fine_tune_state{ + .band_plus = tuning.fine_tune->band_plus, + .band_minus = tuning.fine_tune->band_minus, + .gain_scale = tuning.fine_tune->gain_scale, + }; + } else { + fine_tune.reset(); + } +} + +TrackPidProgramState::TrackPidProgramState(time_point now, TrackPidConfig config) + : config_(std::move(config)) { + program_start_timestamp_ = now; + pid_.reset_with(0.0F); + preemptive_enabled_ = config_.preemptive_pid_activation && validate_preemptive_schema(config_.schemas); + if (config_.schemas.empty()) { + finished_ = true; + return; + } + std::uint32_t total_duration_s = 0; + for (const auto &item : config_.schemas) { + total_duration_s += schema_duration_s(item); + } + program_end_timestamp_ = safe_add_seconds(now, total_duration_s); + apply_schema(schema()); +} + +float TrackPidProgramState::next(time_point now, std::optional fresh_hr_sample) { + if (is_finished(now)) { + finish(); + return 0.0F; + } + + static_cast(sync_schema_for_time(now)); + static_cast(maybe_jump_to_final_pid(now, fresh_hr_sample)); + if (is_finished(now)) { + finish(); + return 0.0F; + } + + return std::visit(overloads{ + [&](const TrackPidSegment &) -> float { + if (!fresh_hr_sample) { + return pid_.u_t_1; + } + return do_pid(fresh_hr_sample->heart_rate_bpm, fresh_hr_sample->sample_interval_s); + }, + [&](const TrackConstantSegment &constant) -> float { + pid_.u_t_1 = std::max(0.0F, constant.speed_m_s); + return pid_.u_t_1; + }}, + schema().segment); +} + +bool TrackPidProgramState::is_finished(time_point now) const { + return finished_ || config_.schemas.empty() || now >= program_end_timestamp_; +} + +TrackPidStageKind TrackPidProgramState::active_stage_kind() const { + if (finished_ || config_.schemas.empty()) { + return TrackPidStageKind::none; + } + return schema().kind(); +} + +std::uint8_t TrackPidProgramState::active_stage_index() const { + if (finished_ || config_.schemas.empty()) { + return 0; + } + return static_cast(std::min(schema_index_, static_cast(std::numeric_limits::max()))); +} + +float TrackPidProgramState::commanded_speed_m_s() const { + return finished_ ? 0.0F : pid_.u_t_1; +} + +std::uint32_t TrackPidProgramState::remaining_program_ms(time_point now) const { + if (is_finished(now)) { + return 0; + } + const auto remaining = std::chrono::duration_cast(program_end_timestamp_ - now); + return static_cast(std::max(0, remaining.count())); +} + +void TrackPidProgramState::update_target_hr_bpm(std::uint8_t target_hr_bpm) { + config_.target_hr_bpm = target_hr_bpm; +} + +expected TrackPidProgramState::update_tuning(const TrackPidSetTuning &tuning) { + auto valid = tuning.validate(); + if (!valid) { + return unexpected{valid.error()}; + } + if (config_.schemas.size() != 1 || !std::holds_alternative(config_.schemas.front().segment)) { + return unexpected{TrackError::invalid_state}; + } + auto &segment = std::get(config_.schemas.front().segment); + tuning.apply_to(segment); + pid_.apply_tuning(tuning); + return {}; +} + +const TrackPidSchema &TrackPidProgramState::schema() const { + return config_.schemas[schema_index_]; +} + +std::uint16_t TrackPidProgramState::schema_duration_s(const TrackPidSchema &schema) { + return std::visit([](const auto &segment) { + return segment.duration_s; + }, schema.segment); +} + +bool TrackPidProgramState::sync_schema_for_time(time_point now) { + if (forced_final_pid_ || config_.schemas.empty()) { + return false; + } + const auto target_index = resolve_schema_index(now); + if (target_index == schema_index_) { + return false; + } + advance_to_schema_index(target_index); + return true; +} + +void TrackPidProgramState::advance_to_schema_index(std::size_t target_index) { + assert(target_index < config_.schemas.size() && "target schema index out of range"); + while (schema_index_ < target_index) { + ++schema_index_; + apply_schema(schema()); + } +} + +std::size_t TrackPidProgramState::resolve_schema_index(time_point now) const { + assert(!config_.schemas.empty() && "resolve_schema_index requires a non-empty program"); + auto boundary = program_start_timestamp_; + for (std::size_t i = 0; i < config_.schemas.size(); ++i) { + boundary = safe_add_seconds(boundary, schema_duration_s(config_.schemas[i])); + if (now < boundary) { + return i; + } + } + return config_.schemas.size() - 1; +} + +bool TrackPidProgramState::maybe_jump_to_final_pid(time_point, std::optional fresh_hr_sample) { + if (!preemptive_enabled_ || !fresh_hr_sample) { + return false; + } + if (active_stage_kind() == TrackPidStageKind::pid) { + return false; + } + if (!is_in_deadzone(fresh_hr_sample->heart_rate_bpm)) { + return false; + } + schema_index_ = config_.schemas.size() - 1; + apply_schema(schema()); + preemptive_enabled_ = false; + forced_final_pid_ = true; + return true; +} + +bool TrackPidProgramState::is_in_deadzone(float heart_rate) const { + if (config_.deadzone_bpm == 0) { + return heart_rate == static_cast(config_.target_hr_bpm); + } + return heart_rate <= (config_.target_hr_bpm + config_.deadzone_bpm) && + heart_rate >= (config_.target_hr_bpm - config_.deadzone_bpm); +} + +float TrackPidProgramState::do_pid(float current_hr, float sample_interval_s) { + const auto dt_s = pid_.should_use_nominal_sample_interval + ? default_pid_nominal_sample_interval_s + : clamp_hr_sample_interval_s(sample_interval_s); + pid_.should_use_nominal_sample_interval = false; + if (is_in_deadzone(current_hr)) { + return pid_.u_t_1; + } + const auto ek = static_cast(config_.target_hr_bpm) - current_hr; + const auto gain_scale = pid_.effective_gain_scale(static_cast(config_.target_hr_bpm), current_hr); + const auto kp = pid_.kp * gain_scale; + const auto ki = pid_.ki * gain_scale; + const auto kd = pid_.kd * gain_scale; + auto delta_u = (kp * (ek - pid_.e_t_1)) + + (ki * dt_s * ek) + + ((kd / dt_s) * (ek - (2.0F * pid_.e_t_1) + pid_.e_t_2)); + if (pid_.slew_rate_limit > 0.0F && std::isfinite(pid_.slew_rate_limit)) { + delta_u = std::clamp(delta_u, -pid_.slew_rate_limit, pid_.slew_rate_limit); + } + pid_.e_t_2 = pid_.e_t_1; + pid_.e_t_1 = ek; + pid_.u_t_1 = pid_.clamp_commanded_speed(pid_.u_t_1 + delta_u); + return pid_.u_t_1; +} + +void TrackPidProgramState::apply_schema(const TrackPidSchema &item) { + std::visit(overloads{ + [&](const TrackPidSegment &pid) { + pid_.from_segment(pid); + }, + [&](const TrackConstantSegment &constant) { + pid_.reset_with(constant.speed_m_s); + }}, + item.segment); +} + +void TrackPidProgramState::finish() { + finished_ = true; + pid_.reset_with(0.0F); +} + +bool TrackPidProgramState::validate_preemptive_schema(const std::vector &schemas) { + if (schemas.size() < 2) { + return false; + } + if (!std::holds_alternative(schemas.front().segment)) { + return false; + } + if (!std::holds_alternative(schemas.back().segment)) { + return false; + } + for (std::size_t i = 0; i < schemas.size(); ++i) { + if (std::holds_alternative(schemas[i].segment) && i != schemas.size() - 1) { + return false; + } + } + return true; +} + +TrackPidProgramState::time_point TrackPidProgramState::safe_add_seconds(time_point now, std::uint32_t duration_s) { + const auto delta = std::chrono::seconds(duration_s); + const auto candidate = now + delta; + if (candidate < now) { + return time_point::max(); + } + return candidate; +} + +} // namespace track_core diff --git a/src/render.cpp b/src/render.cpp new file mode 100644 index 0000000..63bacbe --- /dev/null +++ b/src/render.cpp @@ -0,0 +1,242 @@ +#include "track_core/render.hpp" + +#include +#include +#include + +namespace { + +struct CircularLineDrawer { + static CircularLineDrawer from_report(const track_core::TrackConfig &config, const track_core::TrackReport &report); + + [[nodiscard]] + std::uint16_t tail_offset_leds_num() const { + return static_cast(std::round(tail_m / led_distance_m)); + } + + [[nodiscard]] + std::uint16_t trail_tail_to_head_leds_num() const { + return static_cast(std::round(trail_length_m / led_distance_m)); + } + + [[nodiscard]] + std::uint16_t wrapped_trail_start_to_head_leds_num() const { + return static_cast(std::round(wrapped_trail_start_to_head_length_m / led_distance_m)); + } + + float tail_m{}; + float trail_length_m{}; + float wrapped_trail_start_to_head_length_m{}; + float led_distance_m{}; +}; + +struct LinearLineDrawer { + enum Direction { + head_to_tail, + tail_to_head, + }; + + static LinearLineDrawer from_report(const track_core::TrackConfig &config, const track_core::TrackReport &report); + + [[nodiscard]] + std::uint16_t center_offset_leds_num() const { + return static_cast(std::round(regulated_center_m / led_distance_m)); + } + + [[nodiscard]] + std::uint16_t center_ahead_leds_num() const { + return static_cast(std::round(center_ahead_m / led_distance_m)); + } + + [[nodiscard]] + std::uint16_t center_behind_leds_num() const { + return static_cast(std::round(center_behind_m / led_distance_m)); + } + + float head_m{}; + float tail_m{}; + float regulated_center_m{}; + float center_ahead_m{}; + float center_behind_m{}; + float led_distance_m{}; + Direction direction{head_to_tail}; +}; + +CircularLineDrawer CircularLineDrawer::from_report( + const track_core::TrackConfig &config, + const track_core::TrackReport &report) { + assert(config.draw_kind == track_core::TrackDrawKind::circular && "unmatched draw kind; expected circular"); + + float trail_length_m = 0.0F; + float wrapped_length_m = 0.0F; + + constexpr auto calc_tail_m = [](float mileage_m, float line_length_m, float offset_m) -> float { + return std::fmod(mileage_m + offset_m, line_length_m); + }; + + const auto tail_m = calc_tail_m(report.mileage_m, config.line_length_m, config.head_offset_m); + if (tail_m < 0.0F) { + const auto start_part = config.active_line_length_m + tail_m; + if (start_part <= 0.0F) { + return { + .tail_m = 0.0F, + .trail_length_m = 0.0F, + .wrapped_trail_start_to_head_length_m = 0.0F, + .led_distance_m = config.led_distance(), + }; + } + return { + .tail_m = 0.0F, + .trail_length_m = 0.0F, + .wrapped_trail_start_to_head_length_m = start_part, + .led_distance_m = config.led_distance(), + }; + } + + assert(config.line_length_m > config.active_line_length_m && + "line length must be greater than active line length"); + const auto wrap_head = config.line_length_m - config.active_line_length_m; + if (config.active_line_length_m < config.led_distance() || tail_m <= wrap_head) { + trail_length_m = config.active_line_length_m; + wrapped_length_m = 0.0F; + } else { + wrapped_length_m = tail_m - wrap_head; + trail_length_m = std::max(0.0F, config.active_line_length_m - wrapped_length_m); + } + + return { + .tail_m = tail_m, + .trail_length_m = trail_length_m, + .wrapped_trail_start_to_head_length_m = wrapped_length_m, + .led_distance_m = config.led_distance(), + }; +} + +float pingpong(float x, float length) { + const float period = 2.0F * length; + float phase = std::fmod(x, period); + if (phase < 0.0F) { + phase += period; + } + return (phase <= length) ? phase : (period - phase); +} + +LinearLineDrawer LinearLineDrawer::from_report( + const track_core::TrackConfig &config, + const track_core::TrackReport &report) { + assert(config.draw_kind == track_core::TrackDrawKind::linear && "unmatched draw kind; expected linear"); + + const float length = config.line_length_m; + const float active_length = config.active_line_length_m; + + assert(length > 0.0F && "line length must be greater than 0"); + assert(active_length <= length && "active line length must be less than or equal to line length"); + + const float center_raw = pingpong(report.mileage_m, length); + + const float period = 2.0F * length; + float phase = std::fmod(report.mileage_m, period); + if (phase < 0.0F) { + phase += period; + } + + const auto dir = (phase <= length) ? head_to_tail : tail_to_head; + const float head_m = center_raw + 0.5F * active_length; + const float tail_m = center_raw - 0.5F * active_length; + const float center_ahead_m = std::min(0.5F * active_length, length - center_raw); + const float center_behind_m = std::min(0.5F * active_length, center_raw); + + return { + .head_m = head_m, + .tail_m = tail_m, + .regulated_center_m = center_raw, + .center_ahead_m = center_ahead_m, + .center_behind_m = center_behind_m, + .led_distance_m = config.led_distance(), + .direction = dir, + }; +} + +} // namespace + +namespace track_core { + +void TrackRenderPlan::add_fill(std::uint16_t start_led, std::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 TrackReport &report) { + TrackRenderPlan plan{}; + if (!info.is_running) { + return plan; + } + + switch (config.draw_kind) { + case TrackDrawKind::circular: { + const auto drawer = CircularLineDrawer::from_report(config, report); + plan.add_fill(drawer.tail_offset_leds_num(), drawer.trail_tail_to_head_leds_num(), info.color); + plan.add_fill(0, drawer.wrapped_trail_start_to_head_leds_num(), info.color); + break; + } + case TrackDrawKind::linear: { + 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 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); + 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); + }; + 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); + 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); + }; + const auto ahead = drawer.center_ahead_leds_num(); + const auto behind = drawer.center_behind_leds_num(); + switch (drawer.direction) { + case LinearLineDrawer::head_to_tail: + fill_positive_side(ahead, info.color, magic_color_ahead); + fill_negative_side(behind, info.color, magic_color_behind); + break; + case LinearLineDrawer::tail_to_head: + fill_negative_side(ahead, info.color, magic_color_ahead); + fill_positive_side(behind, info.color, magic_color_behind); + break; + } + break; + } + } + + return plan; +} + +} // namespace track_core diff --git a/src/scheme_decoder.cpp b/src/scheme_decoder.cpp new file mode 100644 index 0000000..e9a57b1 --- /dev/null +++ b/src/scheme_decoder.cpp @@ -0,0 +1,137 @@ +#include "track_core/scheme_decoder.hpp" + +#include + +namespace track_core { +namespace { +constexpr std::size_t data_header_size = 3; + +std::uint16_t read_u16_le(const std::uint8_t *data) { + return static_cast(data[0]) | + static_cast(static_cast(data[1]) << 8U); +} +} // namespace + +expected decode_scheme( + std::uint8_t id, + Color color, + std::span binary) { + if (binary.empty() || binary.size() < data_header_size) { + return unexpected{TrackError::invalid_size}; + } + + const auto kind = static_cast(binary[0]); + const auto acceleration_profile = static_cast(binary[1]); + const std::uint8_t segment_count = binary[2]; + const auto segment_data = binary.subspan(data_header_size); + + DecodedScheme scheme{ + .id = id, + .color = color, + .kind = kind, + .acceleration_profile = acceleration_profile, + }; + + switch (kind) { + case SchemeKind::speed_input_mileage_segmented_time_free: { + constexpr std::size_t segment_size = SMSegment::raw_size; + const std::size_t expected_size = segment_count * segment_size; + if (segment_data.size() < expected_size) { + return unexpected{TrackError::invalid_size}; + } + + std::vector segments; + segments.reserve(segment_count); + for (std::uint8_t i = 0; i < segment_count; ++i) { + const auto *raw = segment_data.data() + (i * segment_size); + segments.push_back(SMSegment{ + .speed_m_s = static_cast(raw[0]) * SMSegment::speed_lsb, + .mileage_from_start_m = read_u16_le(raw + 1), + }); + } + scheme.segments = std::move(segments); + return scheme; + } + case SchemeKind::mileage_input_time_segmented_speed_free: { + constexpr std::size_t segment_size = 4; + const std::size_t expected_size = segment_count * segment_size; + if (segment_data.size() < expected_size) { + return unexpected{TrackError::invalid_size}; + } + + std::vector segments; + segments.reserve(segment_count); + for (std::uint8_t i = 0; i < segment_count; ++i) { + const auto *raw = segment_data.data() + (i * segment_size); + segments.push_back(MTSegment{ + .mileage_to_travel_this_segment_m = read_u16_le(raw), + .time_since_start_s = read_u16_le(raw + 2), + }); + } + scheme.segments = std::move(segments); + return scheme; + } + case SchemeKind::speed_input_time_segmented_mileage_free: { + constexpr std::size_t segment_size = STSegment::raw_size; + const std::size_t expected_size = segment_count * segment_size; + if (segment_data.size() < expected_size) { + return unexpected{TrackError::invalid_size}; + } + + std::vector segments; + segments.reserve(segment_count); + for (std::uint8_t i = 0; i < segment_count; ++i) { + const auto *raw = segment_data.data() + (i * segment_size); + segments.push_back(STSegment{ + .speed_m_s = static_cast(raw[0]) * STSegment::speed_lsb, + .time_since_start_s = read_u16_le(raw + 1), + }); + } + scheme.segments = std::move(segments); + return scheme; + } + case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: { + std::vector time_segments; + time_segments.reserve(segment_count); + + std::size_t offset = 0; + for (std::uint8_t i = 0; i < segment_count; ++i) { + if (offset + 3 > segment_data.size()) { + return unexpected{TrackError::invalid_size}; + } + + const auto time_since_start_s = read_u16_le(segment_data.data() + offset); + offset += sizeof(std::uint16_t); + const auto sub_segment_count = segment_data[offset++]; + const std::size_t sub_segments_size = sub_segment_count * SMSegment::raw_size; + + if (offset + sub_segments_size > segment_data.size()) { + return unexpected{TrackError::invalid_size}; + } + + std::vector sub_segments; + sub_segments.reserve(sub_segment_count); + for (std::uint8_t j = 0; j < sub_segment_count; ++j) { + const auto *raw = segment_data.data() + offset; + sub_segments.push_back(SMSegment{ + .speed_m_s = static_cast(raw[0]) * SMSegment::speed_lsb, + .mileage_from_start_m = read_u16_le(raw + 1), + }); + offset += SMSegment::raw_size; + } + + time_segments.push_back(RepeatedSMSegment{ + .speed_mileage_segments = std::move(sub_segments), + .time_since_start_s = time_since_start_s, + }); + } + + scheme.segments = std::move(time_segments); + return scheme; + } + default: + return unexpected{TrackError::not_supported}; + } +} + +} // namespace track_core diff --git a/tests/track_core_tests.cpp b/tests/track_core_tests.cpp new file mode 100644 index 0000000..11b4ca2 --- /dev/null +++ b/tests/track_core_tests.cpp @@ -0,0 +1,190 @@ +#include +#include +#include +#include + +#include "track_core/memory_strip.hpp" +#include "track_core/pid_program.hpp" +#include "track_core/scheme_decoder.hpp" + +namespace { + +void require(bool condition, const char *message) { + if (!condition) { + std::cerr << "FAIL: " << message << '\n'; + std::exit(1); + } +} + +void require_eq_u32(std::uint32_t actual, std::uint32_t expected, const char *message) { + if (actual != expected) { + std::cerr << "FAIL: " << message << ": actual=" << actual << " expected=" << expected << '\n'; + std::exit(1); + } +} + +void require_near(float actual, float expected, float tolerance, const char *message) { + if (std::fabs(actual - expected) > tolerance) { + std::cerr << "FAIL: " << message << ": actual=" << actual << " expected=" << expected << '\n'; + std::exit(1); + } +} + +track_core::TrackInfo running_info(track_core::Color color = track_core::Color::green()) { + return { + .kind = track_core::SchemeKind::speed_input_time_segmented_mileage_free, + .color = color, + .id = 1, + .is_running = true, + .num_segments = 1, + }; +} + +void test_circular_render_wraps() { + track_core::TrackConfig config{ + .draw_kind = track_core::TrackDrawKind::circular, + .line_length_m = 10.0F, + .active_line_length_m = 4.0F, + .head_offset_m = 0.0F, + .line_leds_num = 20, + }; + const track_core::TrackReport report{ + .id = 1, + .state = track_core::TrackState::run, + .mileage_m = 8.5F, + .speed_m_s = 1.0F, + }; + + const auto plan = track_core::make_track_render_plan(config, running_info(), report); + + require_eq_u32(static_cast(plan.span_count), 2, "circular span count"); + require_eq_u32(plan.spans[0].start_led, 17, "circular span 0 start"); + require_eq_u32(plan.spans[0].led_count, 3, "circular span 0 count"); + require(plan.spans[0].color == track_core::Color::green(), "circular span 0 color"); + require_eq_u32(plan.spans[1].start_led, 0, "circular span 1 start"); + require_eq_u32(plan.spans[1].led_count, 5, "circular span 1 count"); +} + +void test_linear_render_forward_and_memory_strip() { + 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 track_core::TrackReport report{ + .id = 2, + .state = track_core::TrackState::run, + .mileage_m = 5.0F, + .speed_m_s = 2.0F, + }; + track_core::MemoryStrip strip(config.line_leds_num); + + const auto plan = track_core::make_track_render_plan(config, running_info(track_core::Color::red()), report); + const auto applied = track_core::apply_render_plan(strip, plan); + + require(applied == track_core::TrackError::ok, "linear plan applies"); + require_eq_u32(static_cast(plan.span_count), 4, "linear span count"); + require_eq_u32(plan.spans[0].start_led, 10, "linear span 0 start"); + require_eq_u32(plan.spans[0].led_count, 2, "linear span 0 count"); + require(plan.spans[0].color == track_core::Color::red(), "linear span 0 color"); + require_eq_u32(plan.spans[1].start_led, 12, "linear span 1 start"); + require(plan.spans[1].color == track_core::Color::blue(), "linear span 1 color"); + require_eq_u32(plan.spans[2].start_led, 6, "linear span 2 start"); + require(plan.spans[2].color == track_core::Color::cyan(), "linear span 2 color"); + require_eq_u32(plan.spans[3].start_led, 8, "linear span 3 start"); + require(plan.spans[3].color == track_core::Color::red(), "linear span 3 color"); +} + +void test_linear_render_reverse() { + track_core::TrackConfig config{ + .draw_kind = track_core::TrackDrawKind::linear, + .line_length_m = 10.0F, + .active_line_length_m = 5.0F, + .head_offset_m = 0.0F, + .line_leds_num = 20, + }; + const track_core::TrackReport report{ + .id = 3, + .state = track_core::TrackState::run, + .mileage_m = 15.0F, + .speed_m_s = 2.0F, + }; + + const auto plan = track_core::make_track_render_plan(config, running_info(track_core::Color::red()), report); + + require_eq_u32(static_cast(plan.span_count), 4, "reverse span count"); + require_eq_u32(plan.spans[0].start_led, 5, "reverse span 0 start"); + require_eq_u32(plan.spans[0].led_count, 2, "reverse span 0 count"); + require(plan.spans[0].color == track_core::Color::blue(), "reverse span 0 color"); + require_eq_u32(plan.spans[1].start_led, 7, "reverse span 1 start"); + require_eq_u32(plan.spans[1].led_count, 3, "reverse span 1 count"); + require(plan.spans[1].color == track_core::Color::red(), "reverse span 1 color"); + require_eq_u32(plan.spans[2].start_led, 10, "reverse span 2 start"); + require_eq_u32(plan.spans[2].led_count, 3, "reverse span 2 count"); + require(plan.spans[2].color == track_core::Color::red(), "reverse span 2 color"); + require_eq_u32(plan.spans[3].start_led, 13, "reverse span 3 start"); + require_eq_u32(plan.spans[3].led_count, 2, "reverse span 3 count"); + require(plan.spans[3].color == track_core::Color::cyan(), "reverse span 3 color"); +} + +void test_memory_strip_bounds() { + track_core::MemoryStrip strip(4); + + require(strip.fill(0, 0, track_core::Color::red()) == track_core::TrackError::ok, "zero fill"); + require(strip.fill(2, 8, track_core::Color::blue()) == track_core::TrackError::ok, "clamped fill"); + require(strip.pixels()[2] == track_core::Color::blue(), "clamped fill pixel 2"); + require(strip.pixels()[3] == track_core::Color::blue(), "clamped fill pixel 3"); + require(strip.fill(4, 1, track_core::Color::red()) == track_core::TrackError::range, "out-of-range fill"); +} + +void test_scheme_decoder() { + const std::vector data{ + static_cast(track_core::SchemeKind::speed_input_time_segmented_mileage_free), + static_cast(track_core::AccelerationProfile::instant), + 2, + 0, 0, 0, + 51, 10, 0, + }; + + const auto decoded = track_core::decode_scheme(7, track_core::Color::white(), data); + + require(decoded.has_value(), "decode ST scheme"); + require(decoded->id == 7, "decoded id"); + require(decoded->kind == track_core::SchemeKind::speed_input_time_segmented_mileage_free, "decoded kind"); + const auto *segments = std::get_if>(&decoded->segments); + require(segments != nullptr, "decoded ST segment vector"); + require_eq_u32(static_cast(segments->size()), 2, "decoded ST segment count"); + require_near((*segments)[1].speed_m_s, 2.0F, 0.01F, "decoded ST speed"); + require_eq_u32((*segments)[1].time_since_start_s, 10, "decoded ST time"); +} + +void test_pid_program_constant() { + track_core::TrackPidConfig config; + config.schemas = { + track_core::TrackPidSchema{track_core::TrackConstantSegment{ + .duration_s = 2, + .speed_m_s = 1.5F, + }}, + }; + const auto start = track_core::clock::time_point(std::chrono::seconds(0)); + track_core::TrackPidProgramState state(start, config); + + require_near(state.next(start + std::chrono::seconds(1), std::nullopt), 1.5F, 0.0001F, "constant PID speed"); + require(!state.is_finished(start + std::chrono::seconds(1)), "constant PID not finished"); + require(state.is_finished(start + std::chrono::seconds(2)), "constant PID finished"); +} + +} // namespace + +int main() { + test_circular_render_wraps(); + test_linear_render_forward_and_memory_strip(); + test_linear_render_reverse(); + test_memory_strip_bounds(); + test_scheme_decoder(); + test_pid_program_constant(); + std::cout << "track-core tests passed\n"; + return 0; +}