138 lines
5.2 KiB
C++
138 lines
5.2 KiB
C++
#include "track_core/scheme_decoder.hpp"
|
|
|
|
#include <cstring>
|
|
|
|
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<std::uint16_t>(data[0]) |
|
|
static_cast<std::uint16_t>(static_cast<std::uint16_t>(data[1]) << 8U);
|
|
}
|
|
} // namespace
|
|
|
|
expected<DecodedScheme, TrackError> decode_scheme(
|
|
std::uint8_t id,
|
|
Color color,
|
|
std::span<const std::uint8_t> binary) {
|
|
if (binary.empty() || binary.size() < data_header_size) {
|
|
return unexpected<TrackError>{TrackError::invalid_size};
|
|
}
|
|
|
|
const auto kind = static_cast<SchemeKind>(binary[0]);
|
|
const auto acceleration_profile = static_cast<AccelerationProfile>(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>{TrackError::invalid_size};
|
|
}
|
|
|
|
std::vector<SMSegment> 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<float>(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>{TrackError::invalid_size};
|
|
}
|
|
|
|
std::vector<MTSegment> 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>{TrackError::invalid_size};
|
|
}
|
|
|
|
std::vector<STSegment> 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<float>(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<RepeatedSMSegment> 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>{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>{TrackError::invalid_size};
|
|
}
|
|
|
|
std::vector<SMSegment> 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<float>(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>{TrackError::not_supported};
|
|
}
|
|
}
|
|
|
|
} // namespace track_core
|