#pragma once

#include <cstdint>

namespace Driver::Esc::DShot {

#define DSHOT1200_PWM_MHZ 24
#define DSHOT600_PWM_MHZ 12
#define DSHOT300_PWM_MHZ 6
#define DSHOT150_PWM_MHZ 3

#define DSHOT_BIT_0 7
#define DSHOT_BIT_1 14
#define DSHOT_BIT_LENGTH 20

    inline constexpr std::uint16_t encodePacket(std::uint16_t throttleValue, bool requestTelemetry) {
        //cap value
        if (throttleValue > 2047) {
            throttleValue = 2047;
        }

        //throttleValue = 11;

        //add request telemetry bit
        throttleValue = (throttleValue << 1) | (requestTelemetry ? 1 : 0);

        bool useDshotTelemetry = false;

        // compute checksum
        uint16_t csum = 0;
        uint16_t csum_data = throttleValue;
        for (int i = 0; i < 3; i++) {
            csum ^= csum_data;
            csum_data >>= 4;
        }

        if (useDshotTelemetry) {
            csum = ~csum;
        }

        //append checksum
        throttleValue = (throttleValue << 4) | (csum & 0x0f);

        return throttleValue;
    }

    template <class Word, std::uint8_t ZeroPaddingLength = 1>
    class DmaBuffer {
    public:
        static constexpr std::uint8_t BufferSize = 16 + ZeroPaddingLength;
        std::uint32_t size() const {
            return BufferSize;
        }
        DmaBuffer() {
            for (int i = 0; i < ZeroPaddingLength; ++i) {
                _data[16 + i] = 0;
            }
        }
        void fill(std::uint16_t packet) {
            for (int i = 0; i < 16; ++i) {
                _data[i] = (packet & (0x8000 >> i)) ? DSHOT_BIT_1 : DSHOT_BIT_0;
            }
        }
        Word* data() {
            return _data;
        }

    private:
        Word _data[BufferSize];
    };
} // namespace Driver::Esc::DShot