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.
This commit is contained in:
2026-05-18 16:15:45 +08:00
parent 84598cad20
commit 1005e50be0
24 changed files with 4169 additions and 15 deletions
+85
View File
@@ -0,0 +1,85 @@
#include "app_track_decoder.hpp"
#include <algorithm>
#include <cstdint>
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<const uint8_t>(proto.data.bytes, proto.data.size);
std::ranges::copy(data, std::back_inserter(scheme.binary));
return scheme;
}
expected<track_core::DecodedScheme, error_t> TrackSchemeDecoder::decode_core() const {
using ue = unexpected<error_t>;
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
+179
View File
@@ -0,0 +1,179 @@
#include "app_track_drawer.hpp"
#include <cassert>
#include <cstddef>
#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<app::strip::StripView *>(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<std::uint32_t>(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
+109
View File
@@ -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);
}
}
}
+45 -7
View File
@@ -3,6 +3,41 @@
#include <algorithm>
namespace track_core {
namespace {
MemoryStrip *memory_strip_from_context(void *context) {
return static_cast<MemoryStrip *>(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<const Color> 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
+207
View File
@@ -0,0 +1,207 @@
#include "track_core/pid_runtime.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <limits>
namespace track_core {
namespace {
constexpr float default_pid_nominal_sample_interval_s = 2.0F;
template <class... Ts>
struct overloads : Ts... {
using Ts::operator()...;
};
bool supports_live_pid_tuning(const TrackPidConfig &config) {
return config.schemas.size() == 1 &&
std::holds_alternative<TrackPidSegment>(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<unit, TrackError> PidHrRuntime::apply_pid_runtime_command(
const TrackPidRuntimeCommand &command,
const TrackConfig *track_config) {
if (!running_ || !program_state_) {
return unexpected<TrackError>{TrackError::invalid_state};
}
return std::visit(overloads{
[](unit) -> expected<unit, TrackError> {
return unexpected<TrackError>{TrackError::invalid_arg};
},
[&](const TrackPidSetTargetHr &target_hr) -> expected<unit, TrackError> {
update_target_hr_bpm(target_hr.target_hr_bpm);
return {};
},
[&](const TrackPidSetTuning &tuning) -> expected<unit, TrackError> {
if (!supports_live_pid_tuning(config_)) {
return unexpected<TrackError>{TrackError::invalid_state};
}
auto updated = program_state_->update_tuning(tuning);
if (!updated) {
return unexpected<TrackError>{updated.error()};
}
auto &segment = std::get<TrackPidSegment>(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<TrackPidProgramState>(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<std::chrono::duration<float>>(now - last_tick_timestamp_);
last_tick_timestamp_ = now;
if (dt.count() <= 0.0F) {
return;
}
std::optional<TrackPidProgramState::HrSample> 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<std::chrono::duration<float>>(
now - last_consumed_hr_sample_time_)
.count();
}
fresh_hr = TrackPidProgramState::HrSample{
.heart_rate_bpm = static_cast<float>(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::uint32_t>(std::chrono::duration_cast<std::chrono::milliseconds>(
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::uint8_t>(
std::min(config_.schemas.size(), static_cast<std::size_t>(std::numeric_limits<std::uint8_t>::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<std::uint8_t>(0);
status.step_count = band.has_step_count ? band.step_count : static_cast<std::uint16_t>(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
+310
View File
@@ -0,0 +1,310 @@
#include <cstdint>
#include <stdexcept>
#include <string>
#include <nanobind/nanobind.h>
#include <nanobind/stl/string.h>
#include <nanobind/stl/vector.h>
#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<const track_core::Color> 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_<track_core::TrackError>(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_<track_core::TrackDrawKind>(m, "TrackDrawKind")
.value("circular", track_core::TrackDrawKind::circular)
.value("linear", track_core::TrackDrawKind::linear);
nb::enum_<track_core::SchemeKind>(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_<track_core::AccelerationProfile>(m, "AccelerationProfile")
.value("instant", track_core::AccelerationProfile::instant)
.value("smooth", track_core::AccelerationProfile::smooth);
nb::enum_<track_core::TrackState>(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_<track_core::Color>(m, "Color")
.def(nb::init<>())
.def(nb::init<std::uint8_t, std::uint8_t, std::uint8_t>(), "r"_a, "g"_a, "b"_a)
.def(nb::init<std::uint32_t>(), "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<std::uint32_t>(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_<track_core::TrackConfig>(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_<track_core::TrackInfo>(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_<track_core::TrackSchemeMgrRead::Status>(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_<track_core::TrackReport>(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_<track_core::TrackRenderSpan>(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_<track_core::TrackRenderPlan>(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_<track_core::MemoryStrip>(m, "MemoryStrip")
.def(nb::init<std::size_t>(), "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_<track_core::SMSegment>(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_<track_core::MTSegment>(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_<track_core::STSegment>(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_<track_core::RepeatedSMSegment>(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_<track_core::DecodedScheme>(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_<track_core::SchemeTrackState>(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_<track_core::SchemeTrackRuntime>(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_<track_core::SchemeTrainingRuntime>(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);
}
+66 -7
View File
@@ -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<std::uint16_t>(clipped_start),
static_cast<std::uint16_t>(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<int>(drawer.center_offset_leds_num());
const auto line_leds_num = static_cast<int>(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<std::uint16_t>(count / 2);
const auto proximal_count = static_cast<std::uint16_t>(count - distal_count);
plan.add_fill(center_offset, proximal_count, near_center);
plan.add_fill(static_cast<std::uint16_t>(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<std::uint16_t>(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<std::uint16_t>(count / 2);
const auto proximal_count = static_cast<std::uint16_t>(count - distal_count);
plan.add_fill(static_cast<std::uint16_t>(center_offset - count), distal_count, far_end);
plan.add_fill(static_cast<std::uint16_t>(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();
+1
View File
@@ -30,6 +30,7 @@ expected<DecodedScheme, TrackError> decode_scheme(
.color = color,
.kind = kind,
.acceleration_profile = acceleration_profile,
.segments = std::vector<SMSegment>{},
};
switch (kind) {
+763
View File
@@ -0,0 +1,763 @@
#include "track_core/scheme_runtime.hpp"
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <limits>
#include <numeric>
#include <type_traits>
#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::uint8_t>(
std::min<std::size_t>(value, std::numeric_limits<std::uint8_t>::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<float>(std::numeric_limits<std::uint32_t>::max())) {
return std::numeric_limits<std::uint32_t>::max();
}
return static_cast<std::uint32_t>(ms);
}
[[nodiscard]]
accel_calc_result speed_mileage_accel(
const SMSegment &current,
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<float>(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 &current, const STSegment &next) {
const float target_speed = next.speed_m_s;
const float duration_s =
static_cast<float>(next.time_since_start_s) -
static_cast<float>(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<float> calc_mts_target_speed(
std::size_t segment_index,
const std::vector<MTSegment> &segments) {
if (segments.size() < 2 || segment_index > segments.size() - 2) {
return std::nullopt;
}
const auto &current = 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<float>(current.mileage_to_travel_this_segment_m) /
static_cast<float>(duration_s);
}
[[nodiscard]]
std::optional<float> calc_mts_target_speed(const MTSegment &current, 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<float>(current.mileage_to_travel_this_segment_m) /
static_cast<float>(duration_s);
}
template <typename T>
[[nodiscard]]
bool strictly_sorted_by_time(const std::vector<T> &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<DecodedScheme, TrackError> canonicalize_scheme(DecodedScheme scheme) {
if (!valid_acceleration_profile(scheme.acceleration_profile)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
switch (scheme.kind) {
case SchemeKind::speed_input_mileage_segmented_time_free: {
auto *segments = std::get_if<std::vector<SMSegment>>(&scheme.segments);
if (segments == nullptr || segments->empty()) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(*segments, {}, &SMSegment::mileage_from_start_m);
if (segments->front().mileage_from_start_m != 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (const auto &segment : *segments) {
if (!finite_nonnegative(segment.speed_m_s)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
return scheme;
}
case SchemeKind::mileage_input_time_segmented_speed_free: {
auto *segments = std::get_if<std::vector<MTSegment>>(&scheme.segments);
if (segments == nullptr || segments->size() < 2) {
return unexpected<TrackError>{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>{TrackError::invalid_arg};
}
for (std::size_t i = 0; i + 1 < segments->size(); ++i) {
if (!calc_mts_target_speed(i, *segments)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
return scheme;
}
case SchemeKind::speed_input_time_segmented_mileage_free: {
auto *segments = std::get_if<std::vector<STSegment>>(&scheme.segments);
if (segments == nullptr || segments->empty()) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(*segments, {}, &STSegment::time_since_start_s);
if (segments->front().time_since_start_s != 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (const auto &segment : *segments) {
if (!finite_nonnegative(segment.speed_m_s)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
return scheme;
}
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
auto *segments = std::get_if<std::vector<RepeatedSMSegment>>(&scheme.segments);
if (segments == nullptr || segments->empty()) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
std::ranges::sort(*segments, {}, &RepeatedSMSegment::time_since_start_s);
if (segments->front().time_since_start_s != 0) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
for (auto &time_segment : *segments) {
auto &sub_segments = time_segment.speed_mileage_segments;
if (sub_segments.empty()) {
return unexpected<TrackError>{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>{TrackError::invalid_arg};
}
for (const auto &sub_segment : sub_segments) {
if (!finite_nonnegative(sub_segment.speed_m_s)) {
return unexpected<TrackError>{TrackError::invalid_arg};
}
}
}
return scheme;
}
}
return unexpected<TrackError>{TrackError::not_supported};
}
void tick_speed_mileage(SchemeTrackRuntime &runtime, float delta_s) {
auto &state = runtime.state;
const auto &segments = std::get<std::vector<SMSegment>>(runtime.scheme.segments);
state.elapsed_s += delta_s;
const auto &current = 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<std::uint16_t>(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<std::uint16_t>(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<std::vector<MTSegment>>(runtime.scheme.segments);
const float next_elapsed_s = state.elapsed_s + delta_s;
if (next_elapsed_s >= static_cast<float>(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<float>(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<float>(next.time_since_start_s)) {
break;
}
const auto &current = 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<std::vector<STSegment>>(runtime.scheme.segments);
const float next_elapsed_s = state.elapsed_s + delta_s;
if (next_elapsed_s >= static_cast<float>(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 &current = 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<float>(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<std::vector<RepeatedSMSegment>>(runtime.scheme.segments);
const auto &sub_segments = segments[runtime.state.primary_segment_index].speed_mileage_segments;
return sub_segments.empty()
? 0.0F
: static_cast<float>(sub_segments.back().mileage_from_start_m);
}
[[nodiscard]]
std::optional<std::pair<std::size_t, const SMSegment *>>
next_rsmt_sub_segment(const SchemeTrackRuntime &runtime) {
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(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<float>(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<std::vector<RepeatedSMSegment>>(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<float>(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<float>(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 &current = 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<float>(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<SMSegment> 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<MTSegment> 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<STSegment> 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<RepeatedSMSegment> 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<SchemeTrackRuntime, TrackError> make_scheme_track_runtime(DecodedScheme scheme) {
auto normalized = canonicalize_scheme(std::move(scheme));
if (!normalized) {
return unexpected<TrackError>{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<std::vector<SMSegment>>(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<std::vector<MTSegment>>(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<std::vector<STSegment>>(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<std::vector<RepeatedSMSegment>>(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<std::vector<SMSegment>>(runtime.scheme.segments).size();
break;
case SchemeKind::mileage_input_time_segmented_speed_free:
segment_count = std::get<std::vector<MTSegment>>(runtime.scheme.segments).size();
break;
case SchemeKind::speed_input_time_segmented_mileage_free:
segment_count = std::get<std::vector<STSegment>>(runtime.scheme.segments).size();
break;
case SchemeKind::repeated_speed_input_mileage_segmentation_input_time_segmented: {
const auto &segments = std::get<std::vector<RepeatedSMSegment>>(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<unit, TrackError> SchemeTrainingRuntime::add_scheme(DecodedScheme scheme) {
auto runtime = make_scheme_track_runtime(std::move(scheme));
if (!runtime) {
return unexpected<TrackError>{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<unit, TrackError>
SchemeTrainingRuntime::render_to(const TrackConfig &config, TrackRenderSink sink) const {
if (!config.verify() || config.line_leds_num == 0) {
return unexpected<TrackError>{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<TrackError>{err};
}
}
return {};
}
expected<std::vector<Color>, 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<TrackError>{rendered.error()};
}
return std::vector<Color>{strip.pixels().begin(), strip.pixels().end()};
}
} // namespace track_core