#include <avr/io.h>
#include <avr/interrupt.h>
#include "fpm.h"
#define F_CPU 16000000
#define BAUD 57600
#define BRC ((F_CPU/16/BAUD) - 1)
#define UART_BUFFER_SIZE 512
#include <util/delay.h>

volatile uint8_t *UART_buffer;
uint8_t available = 0;
uint16_t buffer_size = 0;
uint16_t UART_write_index = 0;
uint16_t UART_read_index = 0;
volatile uint32_t milliseconds = 0;

void setup(void);
uint16_t read_packet(uint8_t *bytes, uint16_t len);
void send_packet(uint8_t *bytes, uint16_t len);
uint16_t avail_packet(void);
uint32_t millis(void);

int main(void)
{
    setup();

    FPM finger;
    FPM_System_Params params;
    finger.address = 0xFFFFFFFF;
    finger.password = 0;
    finger.avail_func = avail_packet;
    finger.read_func = read_packet;
    finger.write_func = send_packet;

    if (fpm_begin(&finger, 1000, 2000)) {
        fpm_read_params(&finger, &params);
        printf("Found fingerprint sensor!rn");
        printf("Capacity: %drn", params.capacity);
        printf("Packet length: %drn", fpm_packet_lengths[params.packet_len]);
    }
    else {
        printf("Did not find fingerprint sensor :(rn");
        while (1);
    }
}

void setup(void)
{
    UBRR0H = (BRC >> 8); //Shifts the BRC into the high BAUD register.
    UBRR0L = BRC; //Shifts the BRC into the low BAUD register.

    UCSR0B = (1 << TXEN0); //Sets the TX enable bit.
    UCSR0B = (1 << RXEN0); //Sets the RX enable bit.
    UCSR0C = (1 << UCSZ00) | (1 << UCSZ01); //Sets packet size.

    UART_buffer = malloc(512 * sizeof(uint8_t));
}

uint16_t read_packet(uint8_t *bytes, uint16_t len)
{
    *bytes = UART_buffer;
    for(int i = 0; i < available; i++)
        UART_buffer[i] = '';

    return 0;
}

void send_packet(uint8_t *bytes, uint16_t len)
{
    while(len > 0)
    {
        UDR0 = *bytes++;
        len--;
    }
}

uint16_t avail_packet(void)
{
    return buffer_size;
}

uint32_t millis(void)
{
    return milliseconds;
}

ISR(USART_RX_vect)
{
    UART_buffer[UART_write_index] = UDR0;
    UART_write_index++;
    buffer_size++;

    if(UART_write_index > UART_BUFFER_SIZE)
        UART_write_index = 0;
}

/*ISR(TIMER0_COMPA_vect)
{
    ++milliseconds;
}*/

uint8_t fpm_begin(FPM * fpm, fpm_millis_func _millis_func, fpm_delay_func _delay_func) {
    millis_func = _millis_func;
    delay_func = _delay_func;

    delay_func(1000);            // 500 ms at least according to datasheet

    fpm->buffer[0] = FPM_VERIFYPASSWORD;
    fpm->buffer[1] = (fpm->password >> 24) & 0xff; fpm->buffer[2] = (fpm->password >> 16) & 0xff;
    fpm->buffer[3] = (fpm->password >> 8) & 0xff; fpm->buffer[4] = fpm->password & 0xff;
    write_packet(fpm, FPM_COMMANDPACKET, fpm->buffer, 5);

    uint8_t confirm_code = 0;
    int16_t len = read_ack_get_response(fpm, &confirm_code);

    if (len < 0 || confirm_code != FPM_OK)
        return 0;

    if (fpm_read_params(fpm, NULL) != FPM_OK)
        return 0;

    return 1;
}

static int16_t get_reply(FPM * fpm, uint8_t * replyBuf, uint16_t buflen,
                        uint8_t * pktid, fpm_uart_write_func out_stream) {

    FPM_State state = FPM_STATE_READ_HEADER;

    uint16_t header = 0;
    uint8_t pid = 0;
    uint16_t length = 0;
    uint16_t chksum = 0;
    uint16_t remn = 0;

    uint32_t last_read = millis_func();
    //Below is additional code we don't reach...