#include "app_config.h"
#include "interface.h"

#include "app_frm_signal.h"

typedef struct
{
    void *signals[MAX_SIGNALS];
    uint32_t head;
    uint32_t tail;
    uint32_t count;
} SignalQueue;

typedef struct
{
    void *signal_id;
    CallbackFunc callbacks[MAX_CALLBACKS];
    uint32_t callback_count;
} Subscriber;

// 优先级队列
static SignalQueue priority_queues[PRIORITY_LEVELS];

// 订阅者表
static Subscriber subscriber_table[MAX_SUBSCRIBERS] = {{NULL, {NULL}, 0}};

// 初始化队列
static void initQueue(SignalQueue *q)
{
    q->head = 0;
    q->tail = 0;
    q->count = 0;
}

// 将信号请求添加到队列中（按优先级）
static int32_t enqueue(SignalQueue *q, void *signal_id)
{
    if (q == NULL)
    {
        printf("Error: enqueue received NULL queue pointer\n");
        return -1;
    }
    if (signal_id == NULL)
    {
        printf("Error: Cannot enqueue NULL signal_id\n");
        return -1;
    }

    irq_state_t saved_state = enter_critical_section();

    if (q->count >= MAX_SIGNALS)
    {
        // 队列已满，移除最前面的信号
        q->signals[q->head] = NULL;
        q->head = (q->head + 1) % MAX_SIGNALS;
        q->count--;
        printf("Error: Signal queue is full, remove the first signal\n");
    }

    // 添加新的信号到队列尾部
    q->signals[q->tail] = signal_id;
    q->tail = (q->tail + 1) % MAX_SIGNALS;
    q->count++;

    exit_critical_section(saved_state);
    return 0;
}


// 从队列中取出信号请求（按优先级）
static int32_t dequeue(SignalQueue *q, void **signal_id)
{
    if (q == NULL)
    {
        printf("Error: dequeue received NULL queue pointer\n");
        return -1;
    }
    if (signal_id == NULL)
    {
        printf("Error: dequeue received NULL signal pointer\n");
        return -1;
    }

    // 仅在修改共享资源时进入临界区
    if (q->count > 0)
    {
        irq_state_t saved_state = enter_critical_section();

        *signal_id = q->signals[q->head];
        q->signals[q->head] = NULL; // 清除已取出的信号
        q->head = (q->head + 1) % MAX_SIGNALS;
        q->count--;

        exit_critical_section(saved_state);
        return 0;
    }
    //    printf("Warning: dequeue attempted to remove signal from an empty queue\n");
    return -1;
}

// 哈希函数
static uint32_t hash(void *ptr)
{
    uintptr_t value = (uintptr_t)ptr;
    uint32_t hash = 0;
    while (value != 0)
    {
        hash += value & 0xFF;
        hash += (hash << 10);
        hash ^= (hash >> 6);
        value >>= 8;
    }
    hash += (hash << 3);
    hash ^= (hash >> 11);
    hash += (hash << 15);
    return hash;
}

// 订阅信号, 给每个信号指定回调函数
int32_t subscribe(void *signal_id, CallbackFunc callback)
{
    if (signal_id == NULL || callback == NULL)
    {
        printf("Error: Invalid signal_id or callback\n");
        return -1;
    }

    irq_state_t saved_state = enter_critical_section();

    uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
    uint32_t original_index = index;

    do
    {
        if (subscriber_table[index].signal_id == NULL ||
            subscriber_table[index].signal_id == signal_id)
        {
            if (subscriber_table[index].signal_id == NULL)
            {
                subscriber_table[index].signal_id = signal_id;
                subscriber_table[index].callback_count = 0;
            }

            if (subscriber_table[index].callback_count < MAX_CALLBACKS)
            {
                subscriber_table[index].callbacks[subscriber_table[index].callback_count++] = callback;

                exit_critical_section(saved_state);
                return 0;
            }
            else
            {
                printf("Error: Maximum callbacks reached for this signal\n");
                exit_critical_section(saved_state);
                return -1;
            }
        }

        index = (index + 1) % MAX_SUBSCRIBERS;
    } while (index != original_index);

    printf("Error: Subscriber table is full\n");
    exit_critical_section(saved_state);
    return -1;
}

// 检查信号是否有订阅者
static unsigned char hasSubscribers(void *signal_id)
{
    irq_state_t saved_state = enter_critical_section();

    uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
    uint32_t original_index = index;

    do
    {
        if (subscriber_table[index].signal_id == signal_id)
        {
            unsigned char result = subscriber_table[index].callback_count > 0;
            exit_critical_section(saved_state);
            return result;
        }
        if (subscriber_table[index].signal_id == NULL)
        {
            exit_critical_section(saved_state);
            return 0; // 没有订阅者
        }
        index = (index + 1) % MAX_SUBSCRIBERS;
    } while (index != original_index);

    exit_critical_section(saved_state);
    return 0; // 没有订阅者
}

// 内部一致性检查
static void internalConsistencyCheck(void)
{
    for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++)
    {
        assert(subscriber_table[i].callback_count <= MAX_CALLBACKS);
    }
}

// 处理队列中的信号, 调用所有匹配的回调函数
static void processSignals(void)
{
    void *signal_id;

    for (uint32_t priority = 0; priority < PRIORITY_LEVELS; priority++)
    {
        SignalQueue *q = &priority_queues[priority];

        while (dequeue(q, &signal_id) == 0)
        {
            // 进入临界区保护 subscriber_table 的读取
            irq_state_t saved_state = enter_critical_section();

            uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
            uint32_t original_index = index;
            unsigned char found = 0;

            do
            {
                if (subscriber_table[index].signal_id == signal_id)
                {
                    CallbackFunc *callbacks = subscriber_table[index].callbacks;
                    uint32_t callback_count = subscriber_table[index].callback_count;

                    // 复制回调函数指针, 避免在临界区外访问共享数据
                    CallbackFunc local_callbacks[MAX_CALLBACKS];
                    memcpy(local_callbacks, callbacks, sizeof(CallbackFunc) * callback_count);

                    exit_critical_section(saved_state);

                    // 在临界区外调用回调函数
                    for (uint32_t i = 0; i < callback_count; i++)
                    {
                        local_callbacks[i](signal_id);
                    }
                    found = 1;
                    break;
                }
                if (subscriber_table[index].signal_id == NULL)
                {
                    exit_critical_section(saved_state);
                    break; // 没有订阅者
                }
                index = (index + 1) % MAX_SUBSCRIBERS;
            } while (index != original_index);

            if (!found)
            {
                printf("Warning: No subscribers found for signal %p\n", signal_id);
            }
        }
    }
    internalConsistencyCheck(); // 处理完所有信号后进行一致性检查
}

// 初始化框架
void initFramework(void)
{
    irq_state_t saved_state = enter_critical_section();

    for (uint32_t i = 0; i < PRIORITY_LEVELS; i++)
    {
        initQueue(&priority_queues[i]);
    }
    memset(subscriber_table, 0, sizeof(subscriber_table));

    exit_critical_section(saved_state);
}

// 将信号请求添加到指定优先级的队列中
int32_t publishMessage(void *signal_id, uint8_t priority)
{
    if ((uint32_t)priority >= PRIORITY_LEVELS)
    {
        printf("Error: Invalid priority\n");
        return -1;
    }

    if (hasSubscribers(signal_id))
    {
        return enqueue(&priority_queues[priority], signal_id);
    }
    else return 1;
}

// 处理所有优先级的信号
void processMessages(void)
{
    processSignals();
}

// 获取当前队列中的信号数量
int32_t getSignalCount(uint32_t priority)
{
    if (priority >= PRIORITY_LEVELS)
    {
        printf("Error: Invalid priority\n");
        return -1;
    }
    return priority_queues[priority].count;
}

// 获取订阅者数量
uint32_t getSubscriberCount(void)
{
    uint32_t count = 0;

    irq_state_t saved_state = enter_critical_section();

    for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++)
    {
        if (subscriber_table[i].signal_id != NULL)
        {
            count++;
        }
    }

    exit_critical_section(saved_state);
    return count;
}