Initial standalone track core

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<HrSample> 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<unit, TrackError> 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_state> 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<HrSample> 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<TrackPidSchema> &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