#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