#include "isotp_log_backend.h"
#include "log.h"
#include "isotp.h"

#include <string.h>

/* Static variables */
static bool isotp_backend_registered = false;

static bool streaming_enabled = false;
static log_level_t streaming_level = LOG_LEVEL_NOISY;
static bool buffer_dropped_messages = false;
static bool using_buffer_2 = false;

/* Buffer for log messages */
// The buffer is dynamically split to allow double-buffering while data is being sent
// Our ISO-TP implementation requires that the message data is live in memory until the message is fully sent
// Once streaming is enabled, we swap buffers after each message is sent to ensure that the data is not overwritten
static struct {
    uint32_t write_pos;
    uint32_t write_pos_2;
    uint8_t buffer[ISOTP_LOG_BUFFER_1_BYTES + ISOTP_LOG_BUFFER_2_BYTES];
} message_buffer = {};

/* Functions for the message buffer */
static bool buffer_message(const log_message_t* message) {
    if (message == NULL || message->message_length == 0) {
        return false;
    }
    
    /* Check if we have enough space - select the appropriate buffer */
    uint32_t* write_pos = using_buffer_2 ? &message_buffer.write_pos_2 : &message_buffer.write_pos;
    
    /* Calculate buffer offset - buffer 2 starts at ISOTP_LOG_BUFFER_1_BYTES */
    uint32_t buffer_offset = using_buffer_2 ? ISOTP_LOG_BUFFER_1_BYTES : 0;
    uint32_t max_size = using_buffer_2 ? ISOTP_LOG_BUFFER_2_BYTES : ISOTP_LOG_BUFFER_1_BYTES;  // Each buffer has its own size
    
    /* Check if we have enough space in the current buffer */
    if (*write_pos + message->message_length > max_size) {
        buffer_dropped_messages = true;
        return false; /* Buffer full */
    }
    
    /* Write message bytes */
    for (size_t i = 0; i < message->message_length; i++) {
        message_buffer.buffer[buffer_offset + *write_pos] = message->message[i];
        (*write_pos)++;
    }
    
    return true;
}

/* Swap buffers and prepare for sending */
static bool swap_buffers_for_sending() {
    /* Don't swap if there's nothing to send */
    uint32_t* current_pos = using_buffer_2 ? &message_buffer.write_pos_2 : &message_buffer.write_pos;
    if (*current_pos == 0) {
        return false;
    }
    
    /* Swap to the other buffer for future writes */
    using_buffer_2 = !using_buffer_2;
    
    /* Reset the new current buffer position */
    uint32_t* new_current_pos = using_buffer_2 ? &message_buffer.write_pos_2 : &message_buffer.write_pos;
    *new_current_pos = 0;
    
    return true;
}

/* ISO-TP backend functions */
static void isotp_backend_init(void);
static bool isotp_backend_is_enabled(log_level_t level);
static void isotp_backend_write(const log_message_t* message);
static void isotp_backend_flush(void);
static void isotp_backend_clear(void);

/* ISO-TP backend definition */
static const log_backend_t isotp_backend = {
    .name = "ISO-TP",
    .init = isotp_backend_init,
    .is_enabled = isotp_backend_is_enabled,
    .write = isotp_backend_write,
    .flush = isotp_backend_flush,
    .clear = isotp_backend_clear
};

/* Return the ISO-TP backend definition */
const log_backend_t* isotp_log_get_backend(void) {
    return &isotp_backend;
}

/* Check if the ISO-TP backend is registered */
bool isotp_log_is_backend_registered(void) {
    return isotp_backend_registered;
}

/* Initialize the ISO-TP backend and register it */
void isotp_log_backend_init(void) {
    /* Register the backend */
    isotp_backend_registered = log_register_backend(&isotp_backend);
    
    /* Initialize the ISO-TP stack if registration was successful */
    if (isotp_backend_registered) {
        isotp_init();
    }
}

/* ISO-TP backend initialization */
static void isotp_backend_init(void) {
    /* Reset the message buffer */
    message_buffer.write_pos = 0;
    message_buffer.write_pos_2 = 0;
    buffer_dropped_messages = false;
    using_buffer_2 = false;
}

/* Check if the ISO-TP backend is enabled for the given log level */
static bool isotp_backend_is_enabled(log_level_t level) {
    return streaming_enabled ? 
        (level <= streaming_level) : 
        (level <= ISOTP_LOG_BUFFER_MIN_LEVEL); // log level where messages are buffered before streaming mode is enabled
}

/* Write a message to the ISO-TP backend */
static void isotp_backend_write(const log_message_t *message) {
    if (message == NULL) {
        return;
    }

    /* Buffer the message regardless of streaming state */
    buffer_message(message);
    
    /* If streaming is enabled, schedule the messages for sending */
    if (streaming_enabled) {
        /* Auto-flush if we have data in the buffer and are in streaming mode */
        isotp_backend_flush();
    }
}

/* Process buffered log messages and send them via ISO-TP */
void isotp_log_process(void) {
    /* Process ISO-TP updates to send any pending messages */
    isotp_update();
}

/* Flush the ISO-TP backend */
static void isotp_backend_flush(void) {
    /* Swap buffers if we have data to send and we're ready to send */
    if (streaming_enabled) {
        /* Get the buffer that is currently being written to */
        uint32_t buffer_offset = !using_buffer_2 ? 0 : ISOTP_LOG_BUFFER_1_BYTES;
        uint32_t buffer_size = !using_buffer_2 ? message_buffer.write_pos : message_buffer.write_pos_2;
        
        /* Only send if we have data and a previous send is not in progress */
        if (buffer_size > 0) {
            isotp_status_t status = isotp_add_message(
                ISOTP_LOG_CAN_ID, 
                message_buffer.buffer + buffer_offset, 
                buffer_size
            );
            
            if (status == ISOTP_OK) {
                /* Successfully queued for sending, now swap the buffers */
                swap_buffers_for_sending();
                if (buffer_dropped_messages) {
                    log_warning("Buffered messages were dropped");
                    buffer_dropped_messages = false;
                }
            }
        }
    }
}

/* Clear functionality not really applicable for ISO-TP */
static void isotp_backend_clear(void) {
#if USE_ANSI_ESCAPE_CODES
    /* Clear the console using ANSI escape codes */
    isotp_backend_write(&(const log_message_t){.message = "\033[2J\033[H", .message_length = 7});
#endif
}

/* Public function to check if the backend is enabled for a given level */
bool isotp_log_is_enabled(log_level_t level) {
    return isotp_backend_is_enabled(level);
}

/* Public function to enable streaming of log messages */
void isotp_log_enable_streaming(log_level_t level) {
    streaming_enabled = true;
    streaming_level = level;
    isotp_backend_flush();
    log_info("Now streaming log messages over CAN at level %s", log_level_names[level]);
}
