#include <rtems.h>
#include <rtems/error.h>
#include <rtems/bspIo.h>
#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <mqueue.h>
#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>

/* Forward declaration */
rtems_task Init(rtems_task_argument ignored);

/* Message queue parameters */
static mqd_t mq;
static const char * const mq_name = "/prio_test";
#define MAX_MSG  10
#define MSG_SIZE 32
#define BIGGER_HIGHER 10

static pthread_t low_tid, high_tid;
static rtems_id sem_low, sem_high;
static volatile int msg_receiver = -1;

static void *receive_thread(void *arg)
{
    int priority = *(int *)arg;
    rtems_id sem = (priority == BIGGER_HIGHER) ? sem_low : sem_high;
    char buf[MSG_SIZE];
    ssize_t ret;

    /* Wait for its own start signal */
    rtems_semaphore_obtain(sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT);

    // Get RTEMS internal priority
    rtems_task_priority rtems_prio;
    rtems_task_set_priority(RTEMS_SELF, RTEMS_CURRENT_PRIORITY, &rtems_prio);
    
    printf("Thread POSIX prio %d: RTEMS prio %d, blocking on mq_receive()...\n", 
           priority, rtems_prio);
    
    ret = mq_receive(mq, buf, MSG_SIZE, NULL);
    if (ret == -1) {
        printf("Thread prio %d: mq_receive error: %s\n", priority, strerror(errno));
        return NULL;
    }
    buf[ret] = '\0';
    printf("Thread prio %d: received message: %s\n", priority, buf);
    msg_receiver = priority;
    return NULL;
}

static void run_test(void)
{
    struct mq_attr attr;
    pthread_attr_t pattr;
    struct sched_param sparam;
    int low_prio = 10, high_prio = 20;    // NEW: POSIX 20 will map to higher RTEMS priority.
    //int low_prio = 20, high_prio = 10;
    int rc;

    /* 1. Create two independent binary semaphores */
    rc = rtems_semaphore_create(
        rtems_build_name('L', 'O', 'W', ' '),
        0,
        RTEMS_BINARY_SEMAPHORE | RTEMS_FIFO | RTEMS_NO_INHERIT_PRIORITY,
        0,
        &sem_low
    );
    if (rc != RTEMS_SUCCESSFUL) {
        printf("Failed to create low semaphore\n");
        return;
    }

    rc = rtems_semaphore_create(
        rtems_build_name('H', 'I', 'G', 'H'),
        0,
        RTEMS_BINARY_SEMAPHORE | RTEMS_FIFO | RTEMS_NO_INHERIT_PRIORITY,
        0,
        &sem_high
    );
    if (rc != RTEMS_SUCCESSFUL) {
        printf("Failed to create high semaphore\n");
        rtems_semaphore_delete(sem_low);
        return;
    }

    /* 2. Create the message queue */
    attr.mq_maxmsg = MAX_MSG;
    attr.mq_msgsize = MSG_SIZE;
    attr.mq_flags = 0;
    mq = mq_open(mq_name, O_CREAT | O_RDWR, 0666, &attr);
    if (mq == (mqd_t)-1) {
        printf("mq_open failed: %s\n", strerror(errno));
        rtems_semaphore_delete(sem_low);
        rtems_semaphore_delete(sem_high);
        return;
    }

    /* 3. Create low-priority thread */
    pthread_attr_init(&pattr);
    pthread_attr_setinheritsched(&pattr, PTHREAD_EXPLICIT_SCHED);
    pthread_attr_setschedpolicy(&pattr, SCHED_FIFO);
    sparam.sched_priority = low_prio;
    pthread_attr_setschedparam(&pattr, &sparam);
    rc = pthread_create(&low_tid, &pattr, receive_thread, &low_prio);
    if (rc != 0) {
        printf("Failed to create low prio thread: %s\n", strerror(rc));
        goto cleanup;
    }
    usleep(10000);

    /* 4. Create high-priority thread */
    pthread_attr_init(&pattr);
    pthread_attr_setinheritsched(&pattr, PTHREAD_EXPLICIT_SCHED);
    pthread_attr_setschedpolicy(&pattr, SCHED_FIFO);
    sparam.sched_priority = high_prio;
    pthread_attr_setschedparam(&pattr, &sparam);
    rc = pthread_create(&high_tid, &pattr, receive_thread, &high_prio);
    if (rc != 0) {
        printf("Failed to create high prio thread: %s\n", strerror(rc));
        goto cleanup;
    }
    usleep(10000);

    /* 5. Release semaphores */
    printf("Release low priority thread\n");
    rtems_semaphore_release(sem_low);
    usleep(200000);  // Increased to 200ms

    printf("Release high priority thread\n");
    rtems_semaphore_release(sem_high);
    usleep(200000);  // Increased to 200ms

    /* 6. Check queue status before sending */
    struct mq_attr cur_attr;
    mq_getattr(mq, &cur_attr);
    printf("Before sending: msgs=%ld\n", cur_attr.mq_curmsgs);


    /* 7. Send first message */
    printf("Send first message: Hello\n");
    rc = mq_send(mq, "Hello", 5, 0);
    if (rc == -1) {
        printf("First mq_send failed: %s\n", strerror(errno));
    }

    /* Wait for message to be received */
    int wait_count = 0;
    while (msg_receiver == -1 && wait_count < 50) {
        usleep(20000);  // 20ms × 50 = 1 second total
        wait_count++;
    }

    /* 8. Check result */
    if (msg_receiver == high_prio) {
        printf("PASS: High priority thread received first message (priority order)\n");
    } else if (msg_receiver == low_prio) {
        printf("FAIL: Low priority thread received first message (FIFO order)\n");
    } else {
        printf("ERROR: No thread received message\n");
    }

    /* 9. Send second message immediately */
    printf("Send second message: World\n");
    rc = mq_send(mq, "World", 5, 0);
    if (rc == -1) {
        printf("Second mq_send failed: %s (errno=%d)\n", strerror(errno), errno);
    } else {
        printf("Second mq_send succeeded immediately\n");
    }

    /* 10. Check queue status after sending */
    mq_getattr(mq, &cur_attr);
    printf("After sending: msgs=%ld\n", cur_attr.mq_curmsgs);

    sleep(2);  // Wait longer for threads to complete

cleanup:
    mq_close(mq);
    mq_unlink(mq_name);
    rtems_semaphore_delete(sem_low);
    rtems_semaphore_delete(sem_high);
    pthread_cancel(low_tid);
    pthread_cancel(high_tid);
}

/* Initialization task */
rtems_task Init(rtems_task_argument ignored)
{
    (void)ignored;

    printf("\n=== POSIX Message Queue Priority Wakeup Test (Modified) ===\n");
    printf("=== Test includes second mq_send verification ===\n");
    run_test();
    printf("=== Test Finished ===\n");
    exit(0);
}

/* System configuration */
#define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
#define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
#define CONFIGURE_UNLIMITED_OBJECTS
#define CONFIGURE_UNIFIED_WORK_AREAS
#define CONFIGURE_MAXIMUM_POSIX_THREADS 4
#define CONFIGURE_MAXIMUM_POSIX_SEMAPHORES 2
#define CONFIGURE_MAXIMUM_POSIX_MESSAGE_QUEUES 2

#define CONFIGURE_RTEMS_INIT_TASKS_TABLE
#define CONFIGURE_INIT_TASK_NAME rtems_build_name('U', 'I', '1', ' ')
#define CONFIGURE_INIT_TASK_STACK_SIZE (32 * 1024)
#define CONFIGURE_INIT_TASK_PRIORITY 100

#define CONFIGURE_INIT
#include <rtems/confdefs.h>