#include "include/encoder.hpp"
#include "include/mathUtils.hpp"
#include "include/spi.hpp"

std::vector<int32_t> Encoder::read_all() {
    std::vector<int32_t> result;
    uint8_t result_bytes[ENCODER_COUNT * 4] = {0};

    Spi::getInstance().read_array(Spi::Register::MOTOR_1_POS_B3, ENCODER_COUNT * 4, result_bytes);

    for (int i = 0; i < ENCODER_COUNT; i++) {
        int32_t encoder_value = mathUtils::int_from_bytes(result_bytes + i * 4, 4);
        result.push_back(encoder_value);
        encoder_cache[i] = encoder_value;
    }

    last_read = std::chrono::system_clock::now();
    return result;
}

std::vector<int32_t> Encoder::read_all_cached() {
    auto last_read_delay_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
            std::chrono::system_clock::now() - last_read
        ).count();

    if (last_read_delay_ms > ENCODER_CACHE_DURATION_MS) {
        return read_all();
    }
    return std::vector<int32_t>(std::begin(encoder_cache), std::end(encoder_cache));
}

Encoder::Encoder() {

}