#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