#include "usart.h"
#include <string.h>
#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdlib.h>
#include <math.h>


#ifdef USART_READING

int USART::bufferReadIndex = 0;
int USART::bufferWriteIndex = 0;
int USART::incomingBytes = 0;
uint8_t USART::buffer[USART_BUFFER_SIZE];

ISR(USART_RX_vect){
    USART::fillBuffer(UDR0);
}

void USART::reset(){
    incomingBytes=0;
    bufferWriteIndex = 0;
    bufferReadIndex = 0;
}


int USART::available(){
    return incomingBytes;
}

void USART::fillBuffer(uint8_t byte){
    incomingBytes++;
    buffer[bufferWriteIndex] = byte;
    bufferWriteIndex++;
    if(bufferWriteIndex>=USART_BUFFER_SIZE) bufferWriteIndex=0;
}

uint8_t USART::read(){
    while(!available());
    uint8_t byte = buffer[bufferReadIndex];
    bufferReadIndex++;
    if(bufferReadIndex>=USART_BUFFER_SIZE) bufferReadIndex = 0;
    incomingBytes--;
    return byte;
}
#endif

USART::USART(uint32_t baud_rate) {
    uint32_t UBRR_VALUE = ((uint32_t) F_CPU) / 8 / baud_rate - 1;
    UBRR0H = (uint8_t) (UBRR_VALUE >> 8);
    UBRR0L = (uint8_t) (UBRR_VALUE);

#ifdef USART_READING
    UCSR0B = (1<<TXEN0)|(1<<RXEN0)|(1<<RXCIE0);
#else
    UCSR0B = (1 << TXEN0);
#endif

    UCSR0A |= (1 << U2X0);
    UCSR0C = (1 << USBS0) | (1 << UCSZ01) | (1 << UCSZ00);
}

void USART::waitForBufferBeFree() {
    while (!(UCSR0A & (1 << UDRE0)));
}

void USART::send(uint8_t b) {
    waitForBufferBeFree();
    UDR0 = b;
}

void USART::send(void *ptr, size_t count) {
    for (size_t i = 0; i < count; i++) send(((uint8_t *) ptr)[i]);
}

void USART::send(char *string) {
    send(string, strlen(string));
}

void USART::send_P(const char *string) {
    for (size_t i = 0; i < strlen_P(string); i++) send(pgm_read_byte(string + i));

}


void USART::sendFloat(float f, uint8_t p) {
    int64_t value = f;
    float frac = f-value;
    frac*=pow(10, p);
    char str[64];
    itoa(value, str, 10);
    send(str);

    value = frac;
    if(value > 0){
        send('.');
        itoa(value, str, 10);
        uint8_t d = p - strlen(str);
        for(uint8_t i=0;i<d;i++) send('0');
        send(str);
    }else{
        send('.');
        send('0');
    }
}

void USART::sendInt(int64_t i, int radix) {
    char str[64];
    itoa(i, str, radix);
    send(str);
}