#include "io_base.h"
#include "fatfs/ff.h"
#include "portmacro.h"

static void write_to_clones(const void *ptr, size_t size, FILE *stream, portTickType timeout);

void io_init()
{
#define X(fileno, initfunc, readfunc, writefunc) initfunc();
IO_XMACROS
#undef X
}

size_t fread2(void *ptr, size_t size, FILE *stream, portTickType timeout)
{
    UINT count = 0;
    switch ((int)stream)
    {
        // Handle the built-in streams
#define X(fileno, initfunc, readfunc, writefunc) \
        case (int)fileno: count = readfunc(ptr, size, timeout); break;
IO_XMACROS
#undef X
        
        // Anything else is assumed to be a FatFS file pointer
        default:
            f_read((FIL*)stream, ptr, size, &count);
            break;
    }
    
    write_to_clones(ptr, count, stream, timeout);
    return count;
}

// Do the actual fwrite, but do not write to clone destinations.
// This avoids infinite loops with messed up clone definitions.
static size_t fwrite2_noclones(const void *ptr, size_t size, FILE *stream, portTickType timeout)
{
    UINT count = 0;
    switch ((int)stream)
    {
        // Handle the built-in streams
        case (int)stdin:
        case (int)stdout:
        case (int)stderr:
            return 0;
        
        // Handle special IO streams
#define X(fileno, initfunc, readfunc, writefunc) \
        case (int)fileno: return writefunc(ptr, size, timeout);
IO_XMACROS
#undef X
        
        // Anything else is assumed to be a FatFS file pointer
        default:
            f_write((FIL*)stream, ptr, size, &count);
            return count;
    }
}

size_t fwrite2(const void *ptr, size_t size, FILE *stream, portTickType timeout)
{
    write_to_clones(ptr, size, stream, timeout);
    return fwrite2_noclones(ptr, size, stream, timeout);
}

/* For baselibc stdio.h */
size_t _fread(void *ptr, size_t size, FILE *stream)
{
    return fread2(ptr, size, stream, IO_DEFAULT_TIMEOUT);
}

size_t _fwrite(const void *ptr, size_t size, FILE *stream)
{
    return fwrite2(ptr, size, stream, IO_DEFAULT_TIMEOUT);
}

/* IO cloning: logs data for debugging purposes */

// If monitored is NULL, the entry in the table is empty.
// The unused entries are always last in the array, so scanning
// can be stopped at the first NULL.
struct clone_t {
    FILE *monitored;
    FILE *log;
};

#define CLONE_COUNT 8
static volatile struct clone_t clones[CLONE_COUNT] = {{NULL, NULL}};

bool insert_io_clone(FILE *monitored, FILE *log)
{
    int i;
    portENTER_CRITICAL();
    for (i = 0; i < CLONE_COUNT; i++)
    {
        if (clones[i].monitored == monitored && clones[i].log == log)
        {
            break;
        }
        
        if (clones[i].monitored == NULL)
        {
            clones[i].monitored = monitored;
            clones[i].log = log;
            break;
        }
    }
    portEXIT_CRITICAL();
    
    return i < CLONE_COUNT;
}

bool remove_io_clone(FILE *monitored, FILE *log)
{
    int to_remove, last_entry;
    
    portENTER_CRITICAL();
    // We will move the last entry in place of the entry to remove,
    // and then zero out the last entry.
    for (to_remove = 0; to_remove < CLONE_COUNT; to_remove++)
    {
        if (clones[to_remove].monitored == monitored &&
            clones[to_remove].log == log)
        {
            break;
        }
    }
    
    for (last_entry = to_remove; last_entry < CLONE_COUNT - 1; last_entry++)
    {
        if (clones[last_entry + 1].monitored == NULL)
        {
            break;
        }
    }
    
    if (to_remove < CLONE_COUNT)
    {
        clones[to_remove].monitored = clones[last_entry].monitored;
        clones[to_remove].log = clones[last_entry].log;
        clones[last_entry].monitored = NULL;
    }
    portEXIT_CRITICAL();
    
    return to_remove < CLONE_COUNT;
}

// ptr: data that has been read/written to stream
// size: number of bytes read/written
// timeout: timeout to use for log writes
static void write_to_clones(const void *ptr, size_t size, FILE *stream, portTickType timeout)
{
    FILE *log;
    int i = 0;
    do {
        log = NULL;
        
        // Find the next log destination
        portENTER_CRITICAL();
        while (i < CLONE_COUNT && clones[i].monitored != NULL)
        {    
            if (clones[i].monitored == stream)
            {
                log = clones[i].log;
                i++;
                break;
            }
            else
            {
                i++;
            }
        }
        portEXIT_CRITICAL();
        
        if (log != NULL)
        {
            fwrite2_noclones(ptr, size, log, timeout);
            
            if (log > FILE_FATFS)
            {
                // Always sync log files, makes it easier to debug.
                f_sync((FIL*)log);
            }
        }
    } while (log != NULL);
}