track-core/include/track_core/model.hpp

#pragma once

#include <algorithm>
#include <chrono>
#include <cstdint>
#include <expected>
#include <limits>
#include <optional>
#include <span>
#include <string>
#include <variant>
#include <vector>

namespace track_core {

template <typename T, typename E>
using expected = std::expected<T, E>;

template <typename E>
using unexpected = std::unexpected<E>;

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<std::uint8_t>((value >> 16U) & 0xFFU)),
          g(static_cast<std::uint8_t>((value >> 8U) & 0xFFU)),
          b(static_cast<std::uint8_t>(value & 0xFFU)) {}

    [[nodiscard]]
    constexpr bool operator==(const Color &) const = default;

    [[nodiscard]]
    constexpr explicit operator std::uint32_t() const {
        return (static_cast<std::uint32_t>(r) << 16U) |
               (static_cast<std::uint32_t>(g) << 8U) |
               static_cast<std::uint32_t>(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<float>(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<TrackReport> 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<Status> 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<SMSegment> 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<TrackPidFineTune> 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<TrackPidSegment, TrackConstantSegment>;

    variant_type segment{TrackConstantSegment{}};

    [[nodiscard]]
    TrackPidStageKind kind() const {
        return std::holds_alternative<TrackPidSegment>(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<TrackPidSpeedSuppression> speed_suppression;
    std::vector<TrackPidSchema> 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<unit, TrackError> 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<TrackPidFineTune> fine_tune;

    [[nodiscard]]
    expected<unit, TrackError> validate() const;
    void apply_to(TrackPidSegment &segment) const;
};

struct TrackPidRuntimeCommand {
    std::variant<unit, TrackPidSetTargetHr, TrackPidSetTuning> 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