/**
 * @file can_tp.c
 * @brief CAN Transport Protocol implementation
 */

#include "can_tp.h"
#include <string.h>

/* CAN TP State */
typedef struct {
    bool initialized;
    CanTpConfig_t config;
    CanTpConnection_t connections[CAN_TP_MAX_CONNECTIONS];
    CanTpMessageReceivedCallback_t rx_callback;
    CanTpMessageSentCallback_t tx_callback;
    CanTpErrorCallback_t error_callback;
    Mutex_t global_mutex;
} CanTpState_t;

static CanTpState_t can_tp_state;

/* Initialize CAN TP */
KernelStatus_t can_tp_init(const CanTpConfig_t* config) {
    if (config == NULL || can_tp_state.initialized) {
        return KERNEL_ERROR;
    }

    /* Copy configuration */
    memcpy(&can_tp_state.config, config, sizeof(CanTpConfig_t));

    /* Initialize connections */
    for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
        CanTpConnection_t* conn = &can_tp_state.connections[i];
        conn->connection_id = i;
        conn->state = CAN_TP_IDLE;
        conn->stmin = config->stmin;
        conn->block_size = config->block_size;

        semaphore_create(&conn->flow_control_semaphore, SEMAPHORE_BINARY, 0, 1);
        semaphore_create(&conn->complete_semaphore, SEMAPHORE_BINARY, 0, 1);
        mutex_create(&conn->connection_mutex, false);
    }

    /* Create global mutex */
    mutex_create(&can_tp_state.global_mutex, false);

    can_tp_state.initialized = true;
    return KERNEL_OK;
}

/* Send CAN TP Message */
KernelStatus_t can_tp_send_message(const CanTpMessage_t* message,
                                   uint32_t timeout_ms) {
    if (!can_tp_state.initialized || message == NULL || message->data == NULL) {
        return KERNEL_ERROR;
    }

    /* Find free connection */
    CanTpConnection_t* conn = NULL;
    for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
        if (can_tp_state.connections[i].state == CAN_TP_IDLE) {
            conn = &can_tp_state.connections[i];
            break;
        }
    }

    if (conn == NULL) {
        return KERNEL_RESOURCE_BUSY;
    }

    /* Lock connection */
    if (mutex_lock(&conn->connection_mutex, timeout_ms) != KERNEL_OK) {
        return KERNEL_TIMEOUT;
    }

    /* Set up connection */
    conn->current_message = *message;
    conn->current_index = 0;
    conn->sequence_number = 0;
    conn->block_counter = 0;
    conn->is_sender = true;
    conn->state = CAN_TP_SEND_IN_PROGRESS;

    /* Send single frame or first frame */
    CanMessage_t can_message;
    memset(&can_message, 0, sizeof(CanMessage_t));

    if (message->length <= 7) {
        /* Single Frame */
        can_message.id.id = message->message_id;
        can_message.id.is_extended = true;
        can_message.length = message->length + 1;
        can_message.data[0] = (CAN_TP_FRAME_SINGLE << 4) | message->length;
        memcpy(&can_message.data[1], message->data, message->length);

        /* Send message */
        if (can_send_message(&can_message, timeout_ms) != KERNEL_OK) {
            conn->state = CAN_TP_ERROR;
            mutex_unlock(&conn->connection_mutex);
            return KERNEL_ERROR;
        }

        conn->state = CAN_TP_IDLE;
        mutex_unlock(&conn->connection_mutex);

        /* Signal completion */
        if (can_tp_state.tx_callback != NULL) {
            can_tp_state.tx_callback(conn->connection_id, true);
        }

        return KERNEL_OK;
    } else {
        /* First Frame */
        can_message.id.id = message->message_id;
        can_message.id.is_extended = true;
        can_message.length = 8;
        can_message.data[0] = (CAN_TP_FRAME_FIRST << 4) | ((message->length >> 8) & 0x0F);
        can_message.data[1] = message->length & 0xFF;
        memcpy(&can_message.data[2], &message->data[0], 6);

        /* Send first frame */
        if (can_send_message(&can_message, timeout_ms) != KERNEL_OK) {
            conn->state = CAN_TP_ERROR;
            mutex_unlock(&conn->connection_mutex);
            return KERNEL_ERROR;
        }

        conn->current_index = 6;
        conn->state = CAN_TP_WAIT_FLOW_CONTROL;
    }

    /* Wait for flow control */
    if (semaphore_take(&conn->flow_control_semaphore, timeout_ms) != KERNEL_OK) {
        conn->state = CAN_TP_TIMEOUT;
        mutex_unlock(&conn->connection_mutex);
        return KERNEL_TIMEOUT;
    }

    /* Send consecutive frames */
    while (conn->current_index < message->length) {
        /* Check block size */
        if (conn->block_counter >= conn->block_size && conn->block_size > 0) {
            /* Wait for another flow control */
            conn->block_counter = 0;
            if (semaphore_take(&conn->flow_control_semaphore, timeout_ms) != KERNEL_OK) {
                conn->state = CAN_TP_TIMEOUT;
                mutex_unlock(&conn->connection_mutex);
                return KERNEL_TIMEOUT;
            }
        }

        /* Send consecutive frame */
        CanMessage_t consecutive_frame;
        consecutive_frame.id.id = message->message_id;
        consecutive_frame.id.is_extended = true;

        uint16_t remaining = message->length - conn->current_index;
        uint8_t frame_length = (remaining > 7) ? 7 : remaining;

        consecutive_frame.length = frame_length + 1;
        consecutive_frame.data[0] = (CAN_TP_FRAME_CONSECUTIVE << 4) |
                                    (conn->sequence_number & 0x0F);
        memcpy(&consecutive_frame.data[1],
               &message->data[conn->current_index], frame_length);

        /* Send consecutive frame */
        if (can_send_message(&consecutive_frame, timeout_ms) != KERNEL_OK) {
            conn->state = CAN_TP_ERROR;
            mutex_unlock(&conn->connection_mutex);
            return KERNEL_ERROR;
        }

        conn->current_index += frame_length;
        conn->sequence_number = (conn->sequence_number + 1) & 0x0F;
        conn->block_counter++;

        /* Wait for STMin */
        if (conn->stmin > 0) {
            kernel_delay(conn->stmin);
        }
    }

    /* Message sent successfully */
    conn->state = CAN_TP_IDLE;
    mutex_unlock(&conn->connection_mutex);

    /* Signal completion */
    if (can_tp_state.tx_callback != NULL) {
        can_tp_state.tx_callback(conn->connection_id, true);
    }

    return KERNEL_OK;
}

/* Process Received CAN Message */
void can_tp_process_rx_indication(const CanMessage_t* can_message) {
    if (!can_tp_state.initialized || can_message == NULL) {
        return;
    }

    /* Parse frame type */
    uint8_t frame_type = (can_message->data[0] >> 4) & 0x0F;

    switch (frame_type) {
        case CAN_TP_FRAME_SINGLE: {
            /* Single frame - complete message */
            uint8_t length = can_message->data[0] & 0x0F;

            CanTpMessage_t tp_message;
            tp_message.message_id = can_message->id.id;
            tp_message.length = length;
            tp_message.data = (uint8_t*)&can_message->data[1];

            /* Call callback */
            if (can_tp_state.rx_callback != NULL) {
                can_tp_state.rx_callback(&tp_message);
            }
            break;
        }

        case CAN_TP_FRAME_FIRST: {
            /* First frame - start receiving multi-frame message */
            uint16_t total_length = ((can_message->data[0] & 0x0F) << 8) |
                                    can_message->data[1];

            /* Find connection for receiving */
            for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
                CanTpConnection_t* conn = &can_tp_state.connections[i];

                if (conn->state == CAN_TP_IDLE) {
                    conn->state = CAN_TP_RECEIVE_IN_PROGRESS;
                    conn->is_sender = false;
                    conn->current_message.message_id = can_message->id.id;
                    conn->current_message.length = total_length;
                    conn->current_message.data = (uint8_t*)malloc(total_length);
                    conn->current_index = 0;
                    conn->sequence_number = 0;
                    conn->block_counter = 0;

                    /* Copy first 6 bytes */
                    memcpy(conn->current_message.data, &can_message->data[2], 6);
                    conn->current_index = 6;

                    /* Send flow control */
                    CanMessage_t fc_message;
                    fc_message.id.id = can_message->id.id;
                    fc_message.id.is_extended = true;
                    fc_message.length = 8;
                    fc_message.data[0] = (CAN_TP_FRAME_FLOW_CONTROL << 4) |
                                         CAN_TP_FC_CONTINUE;
                    fc_message.data[1] = conn->block_size;
                    fc_message.data[2] = conn->stmin;

                    can_send_message(&fc_message, CAN_TP_DEFAULT_TIMEOUT_MS);
                    break;
                }
            }
            break;
        }

        case CAN_TP_FRAME_CONSECUTIVE: {
            /* Consecutive frame - part of multi-frame message */
            uint8_t sequence_number = can_message->data[0] & 0x0F;

            /* Find active receiving connection */
            for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
                CanTpConnection_t* conn = &can_tp_state.connections[i];

                if (conn->state == CAN_TP_RECEIVE_IN_PROGRESS && !conn->is_sender) {
                    if (sequence_number == conn->sequence_number) {
                        /* Copy data */
                        uint8_t frame_length = can_message->length - 1;
                        memcpy(&conn->current_message.data[conn->current_index],
                               &can_message->data[1], frame_length);
                        conn->current_index += frame_length;
                        conn->sequence_number = (conn->sequence_number + 1) & 0x0F;
                        conn->block_counter++;

                        /* Check if complete */
                        if (conn->current_index >= conn->current_message.length) {
                            /* Message complete */
                            if (can_tp_state.rx_callback != NULL) {
                                can_tp_state.rx_callback(&conn->current_message);
                            }

                            /* Free data */
                            free(conn->current_message.data);
                            conn->state = CAN_TP_IDLE;
                        } else if (conn->block_counter >= conn->block_size) {
                            /* Send another flow control */
                            CanMessage_t fc_message;
                            fc_message.id.id = conn->current_message.message_id;
                            fc_message.id.is_extended = true;
                            fc_message.length = 8;
                            fc_message.data[0] = (CAN_TP_FRAME_FLOW_CONTROL << 4) |
                                                 CAN_TP_FC_CONTINUE;
                            fc_message.data[1] = conn->block_size;
                            fc_message.data[2] = conn->stmin;

                            can_send_message(&fc_message, CAN_TP_DEFAULT_TIMEOUT_MS);
                            conn->block_counter = 0;
                        }
                    }
                    break;
                }
            }
            break;
        }

        case CAN_TP_FRAME_FLOW_CONTROL: {
            /* Flow control - update sending connection */
            uint8_t flow_status = can_message->data[0] & 0x0F;

            for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
                CanTpConnection_t* conn = &can_tp_state.connections[i];

                if (conn->state == CAN_TP_WAIT_FLOW_CONTROL && conn->is_sender) {
                    if (flow_status == CAN_TP_FC_CONTINUE) {
                        conn->block_size = can_message->data[1];
                        conn->stmin = can_message->data[2];
                        conn->block_counter = 0;

                        /* Signal flow control received */
                        semaphore_give(&conn->flow_control_semaphore);
                    } else if (flow_status == CAN_TP_FC_OVERFLOW) {
                        conn->state = CAN_TP_ERROR;
                        if (can_tp_state.error_callback != NULL) {
                            can_tp_state.error_callback(conn->connection_id,
                                                       CAN_TP_FC_OVERFLOW);
                        }
                    }
                    break;
                }
            }
            break;
        }
    }
}

/* CAN TP Main Function */
void can_tp_main_function(void) {
    if (!can_tp_state.initialized) {
        return;
    }

    /* Check timeouts */
    uint32_t current_time = kernel_get_tick_count();

    for (int i = 0; i < CAN_TP_MAX_CONNECTIONS; i++) {
        CanTpConnection_t* conn = &can_tp_state.connections[i];

        if (conn->state != CAN_TP_IDLE && conn->state != CAN_TP_ERROR) {
            if ((current_time - conn->timeout_timer) > CAN_TP_DEFAULT_TIMEOUT_MS) {
                /* Timeout occurred */
                conn->state = CAN_TP_TIMEOUT;

                if (conn->current_message.data != NULL && !conn->is_sender) {
                    free(conn->current_message.data);
                }

                if (can_tp_state.error_callback != NULL) {
                    can_tp_state.error_callback(conn->connection_id, CAN_TP_TIMEOUT);
                }
            }
        }
    }
}