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

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

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

Thread *schedulerNextThread(void) {
    Thread *nextThread = NULL;
    while (threadMaxPriority >= 0) {
        int curPriority = threadMaxPriority;
        CAS(threadQueue[curPriority].current, NULL, threadQueue[curPriority].head);
        nextThread = threadQueue[curPriority].current;
        if (nextThread != NULL) {
            if (CAS(threadQueue[curPriority].current, nextThread, nextThread->nextScheduled)) {
                break;
            }
            nextThread = NULL;
            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 *head = threadQueue[thread->priority].head;
            assert(thread != head);
            thread->nextScheduled = head;
            if (CAS(threadQueue[thread->priority].head, thread->nextScheduled, thread)) {
                //printf("1: head[%i] = %p\n", thread->priority, thread);
                break;
            }
        }
        int maxPriority, newMaxPriority;
        do {
            maxPriority = threadMaxPriority;
            newMaxPriority = (maxPriority >= thread->priority) ? maxPriority : thread->priority;
        } while (!CAS(threadMaxPriority, maxPriority, newMaxPriority));
    }
    assert(thread->locked);
    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].head;
            if (prev == thread) {
                Thread *next = thread->nextScheduled;
                if (CAS(threadQueue[thread->priority].head, thread, next)) {
                    //printf("2: head[%i] = %p\n", thread->priority, next);
                    break;
                } else {
                    continue;
                }
            }
            while (prev->nextScheduled != thread) {
                Thread *next = prev->nextScheduled;
                assert(prev != next);
                prev = next;
            }
            if (CAS(prev->nextScheduled, thread, thread->nextScheduled)) {
                Thread *next = thread->nextScheduled;
                if (CAS(threadQueue[thread->priority].head, thread, next)) {
                    //printf("3: head[%i] = %p\n", thread->priority, next);
                }
                CAS(threadQueue[thread->priority].current, thread, thread->nextScheduled);
                break;
            }
        }
    }
    assert(thread->locked);
    thread->locked = false;
}