#define CAS(var, oldValue, newValue) __sync_bool_compare_and_swap(&(var), oldValue, newValue)

#define THREAD(ptr) ((Thread*)(ptr))

typedef struct ThreadQueue {
    Thread * volatile head;
    Thread * current;
} ThreadQueue;

ThreadQueue threadQueue[SCHEDULER_MAX_THREAD_PRIORITY + 1];
Thread *currentThread;
volatile int threadMaxPriority;

bool schedulerResumeThread(Thread *thread) {
    thread->shouldSuspend = false;
    if (!CAS(thread->locked, false, true)) return false;
    if (thread->suspend == true) {
        if (thread->priority < THREAD_PRIORITY_IDLE) return false;
        ThreadQueue *queue = threadQueue + thread->priority;
        Thread *head;
        do {
            head = queue->head;
            thread->nextScheduled = head;
        } while (!CAS(queue->head, thread->nextScheduled, thread));
        int maxPriority, newMaxPriority;
        do {
            maxPriority = threadMaxPriority;
            newMaxPriority = (maxPriority >= thread->priority) ? maxPriority : thread->priority;
        } while (!CAS(threadMaxPriority, maxPriority, newMaxPriority));
        thread->suspend = false;
        thread->locked = false;
        return true;
    }
    thread->locked = false;
    return false;
}

bool schedulerSuspendThread(Thread *thread) {
    if (!CAS(thread->locked, false, true)) return false;
    thread->shouldSuspend = true;
    thread->locked = false;
    return true;
}

bool schedulerDoSuspendThread(ThreadQueue *queue, Thread *thread) {
    if (!CAS(thread->locked, false, true)) return false;
    if (!CAS(queue->head, thread, thread->nextScheduled)) {
        Thread *prev = queue->head;
        while (prev->nextScheduled != thread) {
            prev = prev->nextScheduled;
        }
        prev->nextScheduled = thread->nextScheduled;
    }
    thread->suspend = true;
    thread->locked = false;
    return true;
}

Thread *schedulerNextThread(void) {
    Thread *nextThread = NULL;
    while (true) {
        while (threadMaxPriority >= 0) {
            int curPriority = threadMaxPriority;
            ThreadQueue *queue = threadQueue + curPriority;
            nextThread = (queue->current != NULL) ? queue->current : queue->head;
            if (nextThread != NULL) {
                bool shouldSuspend = (nextThread->shouldSuspend == true) && (nextThread->suspend == false);
                if (shouldSuspend == true) {
                    Thread *nextCurrent = nextThread->nextScheduled;
                    if (!schedulerDoSuspendThread(queue, nextThread)) continue;
                    queue->current = nextCurrent;
                    if (nextThread->shouldSuspend == false) {
                        schedulerResumeThread(nextThread);
                    }
                    continue;
                } else {
                    queue->current = nextThread->nextScheduled;
                    break;
                }
            }
            queue->current = NULL;
            int nextPriority = curPriority - 1;
            CAS(threadMaxPriority, curPriority, nextPriority);
        }
        break;
    }
    return nextThread;
}