#include <libopencm3/stm32/usart.h>
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/cm3/nvic.h>

#include "debug.h"


#define DEBUG_BUFFER_SIZE 128
volatile char debugBuffer[DEBUG_BUFFER_SIZE];

volatile uint16_t readPos, writePos;
volatile uint8_t bufferFull, bufferEmpty, usartActive;

void debug_init()
{
  readPos = 0;
  writePos = 0;
  bufferFull = 0;
  bufferEmpty = 1;
  usartActive = 0;

  // Enable Clock
  rcc_periph_clock_enable(RCC_USART1);

  // enable interrupt
  nvic_enable_irq(NVIC_USART1_IRQ);

  /* Setup USART1 parameters. */
  usart_set_baudrate(USART1, 115200);
  usart_set_databits(USART1, 8);
  usart_set_stopbits(USART1, USART_CR2_STOP_1_0BIT);
  usart_set_mode(USART1, USART_MODE_TX);
  usart_set_parity(USART1, USART_PARITY_NONE);
  usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE);

  /* Enable the USART1 TX Interrupt */
  usart_enable_tx_interrupt(USART1);

  /* Finally enable the USART. */
  usart_enable(USART1);
}


static void debug_transfer(void)
{
  if(bufferEmpty) {
    usart_disable_tx_interrupt(USART1);
    usartActive = 0;
    return;
  }

  usart_send(USART1, debugBuffer[readPos]);

  readPos++;
  if(readPos == DEBUG_BUFFER_SIZE) {
    readPos = 0;
  }

  if(readPos == writePos) {
    bufferEmpty = 1;
  }

  bufferFull = 0;
  usart_enable_tx_interrupt(USART1);
}


void usart1_isr(void)
{
	volatile uint32_t tmp = USART1_ISR;
	(void)tmp;
  if(USART1_ISR & USART_ISR_TXE){
    debug_transfer();
  }
}


void debug_send_string(char *str)
{
  while(*str) {
    if(bufferFull) {
      break;
    }

    debugBuffer[writePos] = *str;

    writePos++;
    if(writePos == DEBUG_BUFFER_SIZE) {
      writePos = 0;
    }

    if(writePos == readPos) {
      bufferFull = 1;
    }

    bufferEmpty = 0;

    str++;
  }

  if(!usartActive) {
    usartActive = 1;
    debug_transfer();
  }
}