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

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

inline Thread *schedulerNextThread(void) {
    Thread *nextThread = NULL;
    while (threadMaxPriority >= 0) {
        int curPriority = threadMaxPriority;
        if (threadQueue[curPriority] != NULL) {
            nextThread = threadQueue[curPriority];
            if (CAS(threadQueue[curPriority], nextThread, nextThread->nextScheduled)) {
                break;
            }
            continue;
        }
        int nextPriority = curPriority - 1;
        CAS(threadMaxPriority, curPriority, nextPriority);
    }
    return nextThread;
}

void schedulerResumeThread(Thread *thread) {
    if (!CAS(thread->locked, false, true)) return;
    if (thread->suspend == true) {
        if (thread->priority < THREAD_PRIORITY_IDLE) return;
        thread->suspend = false;
        while (true) {
            thread->nextScheduled = threadQueue[thread->priority];
            if (thread->nextScheduled == NULL) {
                thread->nextScheduled = thread;
                if (CAS(threadQueue[thread->priority], NULL, thread)) {
                    break;
                }
            } else {
                if (CAS(threadQueue[thread->priority], thread->nextScheduled, thread)) {
                    break;
                }
            }
        }
        int maxPriority, newMaxPriority;
        do {
            maxPriority = threadMaxPriority;
            newMaxPriority = (maxPriority >= thread->priority) ? maxPriority : thread->priority;
        } while (!CAS(threadMaxPriority, maxPriority, newMaxPriority));
    }
    thread->locked = false;
}

void schedulerSuspendThread(Thread *thread) {
    if (!CAS(thread->locked, false, true)) return;
    if (thread->suspend == false) {
        thread->suspend = true;
        while (true) {
            Thread *prev = threadQueue[thread->priority];
            while (prev->nextScheduled != thread) {
                prev = prev->nextScheduled;
            }
            if (CAS(prev->nextScheduled, thread, thread->nextScheduled)) {
                if (CAS(threadQueue[thread->priority], thread, thread->nextScheduled)) {
                    CAS(threadQueue[thread->priority], thread, NULL);
                }
                break;
            }
        }
    }
    thread->locked = false;
}

void schedulerInitThread(Thread *thread) {
    thread->next = currentThread;
    thread->prev = currentThread->prev;
    thread->next->prev = thread;
    thread->prev->next = thread;
}

void schedulerInitFirstThread(Thread *thread) {
    thread->next = thread;
    thread->prev = thread;
    currentThread = thread;
}