#include "FT245.h"
#include <stdint.h>
#include <stdbool.h>
#include "circular_buffer.h"
#include <stdatomic.h>

static void data_out(ft245_t *ft245);
static void data_in(ft245_t *ft245);
static void bit_set(ft245_t *ft245, uint8_t bit);
static void bit_reset(ft245_t *ft245, uint8_t bit);
static bool tx_word(ft245_t *ft245);
static bool rx_word(ft245_t *ft245);

static atomic_flag lock = ATOMIC_FLAG_INIT;

void ft245_init(ft245_t *ft245)
{
    data_in(ft245);

    cb_init(ft245->buffer_rx, FT245_BUFFER_SIZE_RX);
    cb_init(ft245->buffer_tx, FT245_BUFFER_SIZE_TX);

    ft245->clk_state = false;
    ft245->clk_count = 0;
}

static void data_out(ft245_t *ft245)
{
    ft245->DATA0_to_output();
    ft245->DATA1_to_output();
    ft245->DATA2_to_output();
    ft245->DATA3_to_output();
    ft245->DATA4_to_output();
    ft245->DATA5_to_output();
    ft245->DATA6_to_output();
    ft245->DATA7_to_output();
    ft245->DATA8_to_output();
    ft245->DATA9_to_output();
    ft245->DATA10_to_output();
    ft245->DATA11_to_output();
    ft245->DATA12_to_output();
    ft245->DATA13_to_output();
    ft245->DATA14_to_output();
    ft245->DATA15_to_output();
    ft245->DATA16_to_output();
    ft245->DATA17_to_output();
    ft245->DATA18_to_output();
    ft245->DATA19_to_output();
    ft245->DATA20_to_output();
    ft245->DATA21_to_output();
    ft245->DATA22_to_output();
    ft245->DATA23_to_output();
    ft245->DATA24_to_output();
    ft245->DATA25_to_output();
    ft245->DATA26_to_output();
    ft245->DATA27_to_output();
    ft245->DATA28_to_output();
    ft245->DATA29_to_output();
    ft245->DATA30_to_output();
    ft245->DATA31_to_output();
    ft245->BE0_to_output();
    ft245->BE1_to_output();
    ft245->BE2_to_output();
    ft245->BE3_to_output();
}

static void data_in(ft245_t *ft245)
{
    ft245->DATA0_to_input();
    ft245->DATA1_to_input();
    ft245->DATA2_to_input();
    ft245->DATA3_to_input();
    ft245->DATA4_to_input();
    ft245->DATA5_to_input();
    ft245->DATA6_to_input();
    ft245->DATA7_to_input();
    ft245->DATA8_to_input();
    ft245->DATA9_to_input();
    ft245->DATA10_to_input();
    ft245->DATA11_to_input();
    ft245->DATA12_to_input();
    ft245->DATA13_to_input();
    ft245->DATA14_to_input();
    ft245->DATA15_to_input();
    ft245->DATA16_to_input();
    ft245->DATA17_to_input();
    ft245->DATA18_to_input();
    ft245->DATA19_to_input();
    ft245->DATA20_to_input();
    ft245->DATA21_to_input();
    ft245->DATA22_to_input();
    ft245->DATA23_to_input();
    ft245->DATA24_to_input();
    ft245->DATA25_to_input();
    ft245->DATA26_to_input();
    ft245->DATA27_to_input();
    ft245->DATA28_to_input();
    ft245->DATA29_to_input();
    ft245->DATA30_to_input();
    ft245->DATA31_to_input();
    ft245->BE0_to_input();
    ft245->BE1_to_input();
    ft245->BE2_to_input();
    ft245->BE3_to_input();
}

static void bit_set(ft245_t *ft245, uint8_t bit)
{
    switch(bit) {
    case 0:
        ft245->DATA0_set();
        break;
    case 1:
        ft245->DATA1_set();
        break;
    case 2:
        ft245->DATA2_set();
        break;
    case 3:
        ft245->DATA3_set();
        break;
    case 4:
        ft245->DATA4_set();
        break;
    case 5:
        ft245->DATA5_set();
        break;
    case 6:
        ft245->DATA6_set();
        break;
    case 7:
        ft245->DATA7_set();
        break;
    case 8:
        ft245->DATA8_set();
        break;
    case 9:
        ft245->DATA9_set();
        break;
    case 10:
        ft245->DATA10_set();
        break;
    case 11:
        ft245->DATA11_set();
        break;
    case 12:
        ft245->DATA12_set();
        break;
    case 13:
        ft245->DATA13_set();
        break;
    case 14:
        ft245->DATA14_set();
        break;
    case 15:
        ft245->DATA15_set();
        break;
    case 16:
        ft245->DATA16_set();
        break;
    case 17:
        ft245->DATA17_set();
        break;
    case 18:
        ft245->DATA18_set();
        break;
    case 19:
        ft245->DATA19_set();
        break;
    case 20:
        ft245->DATA20_set();
        break;
    case 21:
        ft245->DATA21_set();
        break;
    case 22:
        ft245->DATA22_set();
        break;
    case 23:
        ft245->DATA23_set();
        break;
    case 24:
        ft245->DATA24_set();
        break;
    case 25:
        ft245->DATA25_set();
        break;
    case 26:
        ft245->DATA26_set();
        break;
    case 27:
        ft245->DATA27_set();
        break;
    case 28:
        ft245->DATA28_set();
        break;
    case 29:
        ft245->DATA29_set();
        break;
    case 30:
        ft245->DATA30_set();
        break;
    case 31:
        ft245->DATA31_set();
        break;
    }
}

static void bit_reset(ft245_t *ft245, uint8_t bit)
{
    switch(bit) {
    case 0:
        ft245->DATA0_reset();
        break;
    case 1:
        ft245->DATA1_reset();
        break;
    case 2:
        ft245->DATA2_reset();
        break;
    case 3:
        ft245->DATA3_reset();
        break;
    case 4:
        ft245->DATA4_reset();
        break;
    case 5:
        ft245->DATA5_reset();
        break;
    case 6:
        ft245->DATA6_reset();
        break;
    case 7:
        ft245->DATA7_reset();
        break;
    case 8:
        ft245->DATA8_reset();
        break;
    case 9:
        ft245->DATA9_reset();
        break;
    case 10:
        ft245->DATA10_reset();
        break;
    case 11:
        ft245->DATA11_reset();
        break;
    case 12:
        ft245->DATA12_reset();
        break;
    case 13:
        ft245->DATA13_reset();
        break;
    case 14:
        ft245->DATA14_reset();
        break;
    case 15:
        ft245->DATA15_reset();
        break;
    case 16:
        ft245->DATA16_reset();
        break;
    case 17:
        ft245->DATA17_reset();
        break;
    case 18:
        ft245->DATA18_reset();
        break;
    case 19:
        ft245->DATA19_reset();
        break;
    case 20:
        ft245->DATA20_reset();
        break;
    case 21:
        ft245->DATA21_reset();
        break;
    case 22:
        ft245->DATA22_reset();
        break;
    case 23:
        ft245->DATA23_reset();
        break;
    case 24:
        ft245->DATA24_reset();
        break;
    case 25:
        ft245->DATA25_reset();
        break;
    case 26:
        ft245->DATA26_reset();
        break;
    case 27:
        ft245->DATA27_reset();
        break;
    case 28:
        ft245->DATA28_reset();
        break;
    case 29:
        ft245->DATA29_reset();
        break;
    case 30:
        ft245->DATA30_reset();
        break;
    case 31:
        ft245->DATA31_reset();
        break;
    }
}

static bool byte_enabled_get(ft245_t *ft245, uint8_t byte)
{
    switch(byte) {
    case 0:
        ft245->BE0_to_input();
        return (ft245->BE0_read() != 0);
    case 1:
        ft245->BE1_to_input();
        return (ft245->BE1_read() != 0);
    case 2:
        ft245->BE2_to_input();
        return (ft245->BE2_read() != 0);
    case 3:
        ft245->BE3_to_input();
        return (ft245->BE3_read() != 0);
    }

    return false;
}

static void byte_enabled_set(ft245_t *ft245, uint8_t byte, bool enabled)
{
    switch(byte) {
    case 0:
        ft245->BE0_to_output();
        if(enabled)
            ft245->BE0_set();
        else
            ft245->BE0_reset();
        break;
    case 1:
        ft245->BE1_to_output();
        if(enabled)
            ft245->BE1_set();
        else
            ft245->BE1_reset();
        break;
    case 2:
        ft245->BE2_to_output();
        if(enabled)
            ft245->BE2_set();
        else
            ft245->BE2_reset();
        break;
    case 3:
        ft245->BE3_to_output();
        if(enabled)
            ft245->BE3_set();
        else
            ft245->BE3_reset();
        break;
    }
}

static bool tx_word(ft245_t *ft245)
{
    if(!cb_occupation(ft245->buffer_tx))
        return false;

    if(ft245->full_word_mode && (cb_occupation(ft245->buffer_tx) < 4))
        return false;

    data_out(ft245);

    uint8_t data = 0;
    uint32_t word = 0;

    for(uint8_t i = 0; i < 4; i++) {
        if(cb_occupation(ft245->buffer_tx)) {
            cb_pop(ft245->buffer_tx, data);
            byte_enabled_set(ft245, 3 - i, true);
        } else {
            data = 0;
            byte_enabled_set(ft245, 3 - i, false);
        }
        word |= ((uint32_t) data) << (i * 8);
    }

    for(uint8_t i = 0; i < 32; i++) {
        if((word >> i) & 0b1)
            bit_set(ft245, i);
        else
            bit_reset(ft245, i);
    }

    return true;
}

static bool rx_word(ft245_t *ft245)
{
    data_in(ft245);

    if((cb_size(ft245->buffer_rx) - cb_occupation(ft245->buffer_rx)) < 4)
        return false;

    if(ft245->full_word_mode && !(ft245->BE3_read() && ft245->BE2_read() && ft245->BE1_read() && ft245->BE0_read()))
        return false;

    uint8_t data[4];

    if(ft245->BE3_read()) {
        data[3] = ((((ft245->DATA31_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 7) |
            (((ft245->DATA30_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 6) |
            (((ft245->DATA29_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 5) |
            (((ft245->DATA28_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 4) |
            (((ft245->DATA27_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 3) |
            (((ft245->DATA26_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 2) |
            (((ft245->DATA25_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 1) |
            (((ft245->DATA24_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 0));
        cb_push(ft245->buffer_rx, data[3]);
    } else {
        data[3] = 0;
        cb_push(ft245->buffer_rx, data[3]);
    }

    if(ft245->BE2_read()) {
        data[2] = ((((ft245->DATA23_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 7) |
            (((ft245->DATA22_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 6) |
            (((ft245->DATA21_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 5) |
            (((ft245->DATA20_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 4) |
            (((ft245->DATA19_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 3) |
            (((ft245->DATA18_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 2) |
            (((ft245->DATA17_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 1) |
            (((ft245->DATA16_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 0));
        cb_push(ft245->buffer_rx, data[2]);
    } else {
        data[2] = 0;
        cb_push(ft245->buffer_rx, data[2]);
    }

    if(ft245->BE1_read()) {
        data[1] = ((((ft245->DATA15_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 7) |
            (((ft245->DATA14_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 6) |
            (((ft245->DATA13_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 5) |
            (((ft245->DATA12_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 4) |
            (((ft245->DATA11_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 3) |
            (((ft245->DATA10_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 2) |
            (((ft245->DATA9_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 1) |
            (((ft245->DATA8_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 0));
        cb_push(ft245->buffer_rx, data[1]);
    } else {
        data[1] = 0;
        cb_push(ft245->buffer_rx, data[1]);
    }

    if(ft245->BE0_read()) {
        data[0] = ((((ft245->DATA7_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 7) |
            (((ft245->DATA6_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 6) |
            (((ft245->DATA5_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 5) |
            (((ft245->DATA4_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 4) |
            (((ft245->DATA3_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 3) |
            (((ft245->DATA2_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 2) |
            (((ft245->DATA1_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 1) |
            (((ft245->DATA0_read() == 0) ? ((uint32_t) 0) : ((uint32_t) 1)) << 0));
        cb_push(ft245->buffer_rx, data[0]);
    } else {
        data[0] = 0;
        cb_push(ft245->buffer_rx, data[0]);
    }

    // Check for sync word 0x66665555
    if((data[3] == 0x55) && (data[2] == 0x55) && (data[1] == 0x66) && (data[0] == 0x66))
        cb_reset(ft245->buffer_rx);

    return true;
}

void ft245_periodic(ft245_t *ft245)
{
    static size_t oe_n = 0;
    static size_t rd_n = 0;
    static size_t wr_n = 0;
    static size_t rst_n = 0;
    bool rxf_n = false;
    bool txe_n = false;
    if(ft245->clk_state) {
        ft245->CLK_reset();
        ft245->clk_state = false;
    } else {
        ft245->CLK_set();
        ft245->clk_state = true;

        oe_n = ft245->OE_N_read() ? 0 : (oe_n + 1);
        rd_n = ft245->RD_N_read() ? 0 : (rd_n + 1);
        wr_n = ft245->WR_N_read() ? 0 : (wr_n + 1);
        rst_n = ft245->RST_N_read() ? 0 : (rst_n + 1);

        if(rst_n)
            return;

        rxf_n = (cb_occupation(ft245->buffer_tx) > (ft245->full_word_mode ? 3 : 0));
        txe_n = !cb_full(ft245->buffer_rx);

        if(rxf_n)
            ft245->RXF_N_reset();
        else
            ft245->RXF_N_set();

        if(txe_n)
            ft245->TXE_N_reset();
        else
            ft245->TXE_N_set();

        // OE_N low => Output Enable
        // RD_N low => Read access for master
        // Both required to drive data
        if(rxf_n && oe_n && rd_n) {
            tx_word(ft245);
        } else if(txe_n && wr_n) {
            rx_word(ft245);
        } else {
            data_in(ft245);
        }
    }
}

bool ft245_write_buffered(ft245_t *ft245, uint8_t *data, size_t size)
{
    if(cb_full(ft245->buffer_tx))
        return false;

    if(cb_size(ft245->buffer_tx) - cb_occupation(ft245->buffer_tx) < size)
        return false;

    for(size_t i = 0; i < size; i++)
        cb_push(ft245->buffer_tx, data[i]);

    return true;
}

bool ft245_read_buffered(ft245_t *ft245, uint8_t *data, size_t size)
{
    if(cb_occupation(ft245->buffer_rx) < size)
        return false;

    for(size_t i = 0; i < size; i++)
        cb_pop(ft245->buffer_rx, data[i]);

    return true;
}

bool ft245_write_word_buffered(ft245_t *ft245, uint32_t word)
{
    while(atomic_flag_test_and_set_explicit(&lock, memory_order_acquire));
    if(cb_size(ft245->buffer_tx) - cb_occupation(ft245->buffer_tx) < 4) {
        atomic_flag_clear_explicit(&lock, memory_order_release);
        return false;
    }

    cb_push(ft245->buffer_tx, (word >> 0) & 0xFF);
    cb_push(ft245->buffer_tx, (word >> 8) & 0xFF);
    cb_push(ft245->buffer_tx, (word >> 16) & 0xFF);
    cb_push(ft245->buffer_tx, (word >> 24) & 0xFF);

    atomic_flag_clear_explicit(&lock, memory_order_release);

    return true;
}

bool ft245_read_word_buffered(ft245_t *ft245, uint32_t *word)
{
    while(atomic_flag_test_and_set_explicit(&lock, memory_order_acquire));
    if(cb_occupation(ft245->buffer_rx) < 4) {
        atomic_flag_clear_explicit(&lock, memory_order_release);
        return false;
    }

    uint8_t data = 0;
    cb_pop(ft245->buffer_rx, data);
    *word |= ((uint32_t) data) << 24;
    cb_pop(ft245->buffer_rx, data);
    *word |= ((uint32_t) data) << 16;
    cb_pop(ft245->buffer_rx, data);
    *word |= ((uint32_t) data) << 8;
    cb_pop(ft245->buffer_rx, data);
    *word |= ((uint32_t) data) << 0;

    atomic_flag_clear_explicit(&lock, memory_order_release);

    return true;
}

size_t ft245_available_read(ft245_t *ft245)
{
    return cb_occupation(ft245->buffer_rx);
}

size_t ft245_available_write(ft245_t *ft245)
{
    return (cb_size(ft245->buffer_tx) - cb_occupation(ft245->buffer_tx));
}