#ifndef STM32U5XX_HAL_QUEUE_HPP
#define STM32U5XX_HAL_QUEUE_HPP

#include "global_variable.h"
#include "stdint.h"
#include "stdlib.h"
#include <stm32u5xx.h>
#include <algorithm>

class queue {
  private:
    uint8_t *buffer; // pointer to buffer
    uint32_t size;   // size of buffer
    uint32_t head;   // index of head
    uint32_t tail;   // index of tail
    uint32_t count;  // number of elements in queue

  public:
    queue() {}

    void clear() { head = tail = count = 0; }

    bool is_empty() { return count == 0; }

    hal_status_e init(uint32_t _size) {
        size = _size;
        buffer = (uint8_t *)malloc(size);
        head = tail = count = 0;
        return buffer != nullptr ? HAL_OK : QUEUE_ERROR_MALLOC_FAILED;
    }

    hal_status_e push(uint8_t data) {
        if (count == size) return QUEUE_ERROR_NO_ENOUGH_SPACE;
        buffer[tail] = data;
        tail = (tail + 1) % size;
        count++;
        return HAL_OK;
    }

    hal_status_e pop(uint8_t *data) {
        if (count == 0) return QUEUE_EMPTY;
        *data = buffer[head];
        head = (head + 1) % size;
        count--;
        return HAL_OK;
    }

    hal_status_e push_block(const uint8_t *data, uint32_t len) {
        if (count + len > size) return QUEUE_ERROR_NO_ENOUGH_SPACE;
        for (uint32_t i = 0; i < len; i++) {
            buffer[tail] = data[i];
            tail = (tail + 1) % size;
        }
        count += len;
        return HAL_OK;
    }

    hal_status_e pop_block(uint8_t *data, uint32_t &len) {
        if (count < len) return QUEUE_ERROR_NO_ENOUGH_DATA;
        if (count == 0) {
            len = 0;
            return QUEUE_EMPTY;
        }
        // if there is no enough buf, pop all buf
        // the buffer_size will be set to the actual length of buf
        __disable_irq();
        len = std::min(len, count);
        for (uint32_t i = 0; i < len; i++) {
            *(data + i) = buffer[head];
            head = (head + 1) % size;
        }
        count -= len;
        __enable_irq();
        return HAL_OK;
    }

    hal_status_e get_line(uint8_t *data, uint32_t &len) {

        if (count == 0) {
            len = 0;
            return QUEUE_EMPTY;
        }
        __disable_irq();
        // find '\n'
        uint32_t tmp_len = 0;
        for (uint32_t i = 0; i < count; i++) {
            if (buffer[(head + i) % size] == '\n') {
                tmp_len = i + 1;
                break;
            }
        }
        if (tmp_len == 0) {
            __enable_irq();
            return QUEUE_ERROR_NO_LINE;
        }
        if (tmp_len > len) {
            __enable_irq();
            return QUEUE_ERROR_BUFFER_OVERFLOW;
        }
        len = tmp_len;
        return this->pop_block(data, len);
    }
};

template <int size, typename T> class queue_stack {
  private:
    T buffer[size]{};
    uint32_t head = 0;  // index of head
    uint32_t tail = 0;  // index of tail
    uint32_t count = 0; // number of elements in queue
  public:
    queue_stack() = default;

    void clear() { head = tail = count = 0; }

    bool is_empty() { return count == 0; }

    hal_status_e push(T data) {
        if (count == size) return QUEUE_ERROR_NO_ENOUGH_SPACE;
        buffer[tail] = data;
        tail = (tail + 1) % size;
        count++;
        return HAL_OK;
    }

    hal_status_e pop(T *data) {
        if (count == 0) return QUEUE_EMPTY;
        *data = buffer[head];
        head = (head + 1) % size;
        count--;
        return HAL_OK;
    }

    hal_status_e push_block(const T *data, uint32_t len) {
        if (count + len > size) return QUEUE_ERROR_NO_ENOUGH_SPACE;
        for (uint32_t i = 0; i < len; i++) {
            buffer[tail] = data[i];
            tail = (tail + 1) % size;
        }
        count += len;
        return HAL_OK;
    }

    hal_status_e pop_block(T *data, uint32_t &len) {
        if (count < len) return QUEUE_ERROR_NO_ENOUGH_DATA;
        if (count == 0) {
            len = 0;
            return QUEUE_EMPTY;
        }
        // if there is no enough buf, pop all buf
        // the buffer_size will be set to the actual length of buf
        __disable_irq();
        len = std::min(len, count);
        for (uint32_t i = 0; i < len; i++) {
            *(data + i) = buffer[head];
            head = (head + 1) % size;
        }
        count -= len;
        __enable_irq();
        return HAL_OK;
    }

    hal_status_e get_line(T *data, uint32_t &len) {

        if (count == 0) {
            len = 0;
            return QUEUE_EMPTY;
        }
        __disable_irq();
        // find '\n'
        uint32_t tmp_len = 0;
        for (uint32_t i = 0; i < count; i++) {
            if (buffer[(head + i) % size] == '\n') {
                tmp_len = i + 1;
                break;
            }
        }
        if (tmp_len == 0) {
            __enable_irq();
            return QUEUE_ERROR_NO_LINE;
        }
        if (tmp_len > len) {
            __enable_irq();
            return QUEUE_ERROR_BUFFER_OVERFLOW;
        }
        len = tmp_len;
        return this->pop_block(data, len);
    }
};

#define queue20u8 queue_stack<20, uint8_t>
#define queue20u32 queue_stack<20, uint32_t>

#endif // STM32U5XX_HAL_QUEUE_HPP