//
// Created by MightyPork on 2020/03/11.
//

#include <string.h>
#include <stdint.h>
#include "console/console.h"
#include "console/utils.h"

#if (CONSOLE_HAVE_CSP && !CSP_POSIX) || CONSOLE_TESTING_ALLOC_FUNCS
    #include <csp/arch/csp_malloc.h>
#else
    #include <malloc.h>
#endif

// Based on: https://stackoverflow.com/a/7776146/2180189
void console_hexdump(const void *data, size_t len)
{
    size_t i;
    char asciibuf[17];
    const unsigned char *pc = (const unsigned char *) data;

    // Length checks.

    if (len <= 0) {
        console_print("BAD LENGTH\r\n");
        return;
    }

    // Process every byte in the data.
    for (i = 0; i < len; i++) {
        size_t ai = i % 16;

        // Multiple of 16 means new line (with line offset).
        if (ai == 0) {
            // Don't print ASCII buffer for the "zeroth" line.
            if (i != 0) {
                console_printf(" |%s|\r\n", asciibuf);
            }

            // Output the offset.

            console_printf("%4d :", (int)i);
        }

        // Now the hex code for the specific character.
        console_printf(" %02x", pc[i]);

        // And buffer a printable ASCII character for later.
        if ((pc[i] < 0x20) || (pc[i] > 0x7e)) {
            asciibuf[ai] = '.';
        }
        else {
            asciibuf[ai] = (char) pc[i];
        }

        asciibuf[ai + 1] = '\0';
    }

    // Pad out last line if not exactly 16 characters.
    while ((i % 16) != 0) {
        console_print("   ");
        i++;
    }

    // And print the final ASCII buffer.
    console_printf(" |%s|\r\n", asciibuf);
}

static int hex_char_decode(char x)
{
    if (x >= '0' && x <= '9') {
        return x - '0';
    }
    else if (x >= 'a' && x <= 'f') {
        return 10 + (x - 'a');
    }
    else if (x >= 'A' && x <= 'F') {
        return 10 + (x - 'A');
    }
    else {
        return -1;
    }
}

int console_base16_decode(const char *hex, void *dest, size_t capacity)
{
    if (!hex || !dest) return -1;

    const char *px = (const char *) hex;
    uint8_t *pd = (unsigned char *) dest;
    int hlen = (int) strlen(hex);
    if (hlen % 2) {
        return -2;
    }
    int blen = hlen / 2;
    if (blen > (int) capacity) {
        return -3;
    }

    int v;
    uint8_t byte;
    for (int i = 0; i < blen; i++) {
        v = hex_char_decode(*px++);
        if (v == -1) {
            return -2;
        }

        byte = (v & 0x0F) << 4;

        v = hex_char_decode(*px++);
        if (v == -1) {
            return -2;
        }

        byte |= (v & 0x0F);

        *pd++ = byte;
    }

    return blen;
}

void * __attribute__((malloc)) console_malloc(size_t size) {
    #if (CONSOLE_HAVE_CSP && !CSP_POSIX) || CONSOLE_TESTING_ALLOC_FUNCS
        return csp_malloc(size);
    #else
        return malloc(size);
    #endif
}

void * console_realloc(void *ptr, size_t oldsize, size_t newsize) {
    #if (CONSOLE_HAVE_CSP && !CSP_POSIX) || CONSOLE_TESTING_ALLOC_FUNCS
        // csp / freertos do not provide realloc, simply allocate a new one and copy data over.

        if (oldsize >= newsize) {
            // no realloc needed
            if (!ptr) { // NULL was given, act as malloc
                ptr = csp_malloc(newsize);
            }
            return ptr;
        }

        void *tmp = csp_malloc(newsize);
        if (!tmp) {
            // "If realloc() fails, the original block is left untouched; it is not freed or moved."
            return NULL;
        }
        if (ptr) {
            // copy if there is anything to copy from
            memcpy(tmp, ptr, oldsize); // we know oldsize < newsize, otherwise the check above returns.
            // discard the old buffer
            csp_free(ptr);
        }
        return tmp;
    #else
        (void) oldsize; // unused
        return realloc(ptr, newsize);
    #endif
}

void * __attribute__((malloc,alloc_size(1,2))) console_calloc(size_t nmemb, size_t size) {
    #if (CONSOLE_HAVE_CSP && !CSP_POSIX) || CONSOLE_TESTING_ALLOC_FUNCS
        return csp_calloc(nmemb, size);
    #else
        return calloc(nmemb, size);
    #endif
}

void console_free(void *ptr) {
    #if (CONSOLE_HAVE_CSP && !CSP_POSIX) || CONSOLE_TESTING_ALLOC_FUNCS
        return csp_free(ptr);
    #else
        return free(ptr);
    #endif
}

char * console_strdup(const char *ptr) {
    #if (CONSOLE_HAVE_CSP && !CSP_POSIX) || CONSOLE_TESTING_ALLOC_FUNCS
        if (!ptr) return NULL;
        size_t len = strlen(ptr);
        char *copy = console_malloc(len+1);
        if (!copy) return NULL;
        strncpy(copy, ptr, len);
        copy[len]=0;
        return copy;
    #else
        return strdup(ptr);
    #endif
}

char * console_strndup(const char *ptr, size_t maxlen) {
    #if (CONSOLE_HAVE_CSP && !CSP_POSIX) || CONSOLE_TESTING_ALLOC_FUNCS
        if (!ptr) return NULL;
        size_t len = strnlen(ptr, maxlen);
        char *copy = console_malloc(len+1);
        if (!copy) return NULL;
        strncpy(copy, ptr, len);
        copy[len]=0;
        return copy;
    #else
        return strndup(ptr, maxlen);
    #endif
}