int fputc(int c, FILE *f) {
    UART_SendByte(LPC_UART0, c);
    return (c);}

void UART_SendByte(LPC_UART_TypeDef* UARTx, uint8_t Data)
{
    UARTx->THR = Data & UART_THR_MASKBIT;
}

#ifndef __FIFO_H_
#define __FIFO_H_

#include "type.h"

template <u8 size, typename T = u8> class Fifo{
  T buf[size];
  u8 _count;
  u8 _first;
  u8 _last;
public:
  Fifo(){ _count = _last = _first = 0; }
  void clear(){ _count = _last = _first = 0; }
  u8 count(){ return _count; }
  u8 free(){ return size - _count; }

  void push(T item){
    if(_count == size) return;
    _count++;
    buf[_last++] = item;
    if(_last >= size) _last = 0;
  }

  T pop(){
    if(_count == 0) return 0;
    _count--;
    T item = buf[_first++];
    if(_first == size) _first = 0;
    return item;
  }

  u8 write(T * data, u8 num){
    if (num > (size - _count)) num = size - _count;
    u8 n = num;
    while (num--) push(*data++);
    return n;
  }

  u8 read(T * data, u8 num){
    if (num > _count) num = _count;
    u8 n = num;
    while (num--) *data++ = pop();
    return n;
  }

  T at(s16 index){
    s16 i = index + (index < 0) ? _last : _first;
    while (i < 0) i += size;
    while (i >= size) i -= size;
    return buf[i];
  }
};

#endif /* __FIFO_H_ */