#include "log.h"
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <ch.h>
#include "rtc.h"

static BSEMAPHORE_DECL(logsemaphore, 0);

static void write_log(char *buffer)
{
    if (chBSemWaitTimeout(&logsemaphore, MS2ST(10)) == RDY_OK)
    {
        fputs(buffer, stdout);
        chBSemSignal(&logsemaphore);
    }
}

void LOG(const char *format, ...)
{
    va_list va;
    va_start(va, format);
    int size = vsnprintf(NULL, 0, format, va);
    va_end(va);
    
    char *buffer = malloc(size + 31 + 2);
    
    if (buffer == NULL)
        return;
    
    rtc_t time;
    if (!rtc_get(&time))
    {
        snprintf(buffer, 31, "%19d %08x:    ", (int)chTimeNow(), (unsigned int)chThdSelf());
    } else {
        snprintf(buffer, 31, "%04d-%02d-%02dT%02d:%02d:%02d %08x:    ",
                 time.year, time.month, time.day, time.hour, time.minute, time.second, (unsigned int)chThdSelf());
    }
    
    va_start(va, format);
    vsnprintf(buffer + 30, size + 1, format, va);
    va_end(va);
    
    // Trim whitespace from end
    char* end = buffer + strlen(buffer);
    while ((end != buffer) && isspace( *(end-1))) {
            --end;
    }
    *end++ = '\n';
    *end = '\0';
    
    write_log(buffer);
    free(buffer);
}