/*
 * Copyright (C) 2018-2020 Western Digital Corporation or its affiliates
 * Copyright (C) 2017-2018 Wearable, Inc.
 * Copyright (C) 2000-2012 leJOS Contributors
 * Copyright (C) 2000 Jose H. Solorzano and TinyVM Contributors
 *
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
 * 
 * This Source Code Form is “Incompatible With Secondary Licenses”,
 * as defined by the Mozilla Public License, v. 2.0.
 */

#include "types.h"
#include "trace.h"
#include "platform_hooks.h"
#include "constants.h"
#include "specialsignatures.h"
#include "specialclasses.h"
#include "threads.h"
#include "classes.h"
#include "language.h"
#include "configure.h"
#include "interpreter.h"
#include "memory.h"
#include "exceptions.h"
#include "stack.h"
#include <string.h>
#define NO_OWNER 0x00
#define MAX_ID 0xff


void update_stack_frame (nvm_state_t* pState, StackFrame *stackFrame)
{
    stackFrame->stackTop = ptr2ref(pState->program_stack_top);
    stackFrame->pc = ptr2ref(pState->program_counter);
}

void update_registers (nvm_state_t* pState, StackFrame *stackFrame)
{
    pState->program_counter = (byte*) ref2ptr(stackFrame->pc);
    pState->program_stack_top = (STACKWORD*) ref2ptr(stackFrame->stackTop);
    pState->program_locals_base = (STACKWORD*) ref2ptr(stackFrame->localsBase);
}

#define get_thread_id(sync) (((objSync*) sync)->threadId)
#define set_thread_id(sync,_threadId) (((objSync*) sync)->threadId = (_threadId))
#define inc_monitor_count(sync) (((objSync*) sync)->monitorCount++)
#define set_monitor_count(obj,count) (((objSync*) sync)->monitorCount = (count))

/**
 * Initialise the thread pool and create the bootThread.
 * Note we use a Java object so that we can make
 * the pool available to Java. But to do this we have to take great care
 * because the gc makes use of the thread system! So we must ensure that
 * currentThread is pointing to a valid structure before performing any
 * allocations.
 **/
void init_threads(nvm_state_t* pState)
{
    int i;
    Thread *bootThread;
    // Allocate temporary thread structure for use during system startup
    Thread initThread;
    initThread.threadId = 255;
    initThread.state = SUSPENDED;
    pState->current_thread = &initThread;
    
    // Now create the basic threading system
    // This array must be in the gc heap because it is pinned via pState->thread_queue
    pState->threads = ptr2ref(new_single_array(pState, ALJAVA_LANG_OBJECT, MAX_PRIORITY, true));
    pState->thread_queue = ref_array(ref2obj(pState->threads));
    REFERENCE *pQ = pState->thread_queue;
    for (i = 0; i < MAX_PRIORITY; i++) {
        *pQ++ = ptr2ref(NULL);
    }
    
    // Create the system boot thread
    bootThread = (Thread*)(void*)new_object_for_class(pState, JAVA_LANG_THREAD);
    bootThread->priority = NORM_PRIORITY;
    init_thread(pState, bootThread);
    // Now we have a valid thread use it.
    pState->current_thread = bootThread;
    // set things up ready to run the first program
    empty_stacks(pState);
    execute_program(pState);
    pState->current_thread->state = RUNNING;
    
}

/**
 * Find a free thread id and return it.
 */
static int get_new_thread_id(nvm_state_t* pState)
{
    // Thread ids must be between 1 and 255, so we need to recycle ids if we
    // create more than 254 threads. We do this by using a bitmap to mark
    // each id in use then search this map looking for an unused (i.e. zero)
    // entry.
#define BITSPERWORD (8*sizeof(uint32_t))
    uint32_t inUse[MAX_ID/BITSPERWORD];
    size_t i;
    memset(inUse, 0, sizeof(inUse));
    // scan all threads and mark all in use ids
    for (i=0; i < MAX_PRIORITY; i++)
    {
        Thread *pFirstThread = (Thread*) ref2obj(pState->thread_queue[i]);
        if (pFirstThread)
        {
            Thread *pThread = pFirstThread;
            do {
                inUse[pThread->threadId/BITSPERWORD] |= 1 << (pThread->threadId % BITSPERWORD);
                pThread = (Thread*) ref2obj(pThread->nextThread);
            } while (pThread != pFirstThread);
        }
    }
    // id 0 is always in use...
    *inUse |= 1;
    // find a free id...
    for(i = 0; i < sizeof(inUse)/sizeof(uint32_t); i++)
    {
        uint32_t w = inUse[i];
        if (w != ~UINT32_C(0))
        {
            int j = 0;
            for(j=0; w & 1; j++)
                w >>= 1;
            return i*BITSPERWORD + j;
        }
    }
    // failed to find a free id...
    return NO_OWNER;
}

/**
 * Allocate stack frames
 * Allocate ID
 * Insert into run list
 * Mark thread as STARTED
 */
int init_thread (nvm_state_t* pState, Thread *thread)
{
    /* Create a new thread and allocate the resources for it. When being used
     * with the GC we must take steps to allow the calling instruction to be
     * re-started. So take care of any changes to global state.
     */
    thread->threadId = get_new_thread_id(pState);
    
    if (thread->threadId == NO_OWNER)
        return nvm_throw_new_exception(pState, JAVA_LANG_OUTOFMEMORYERROR);
    
    // Catch the primordial thread
    if (pState->current_thread == NULL)
        thread->priority = NORM_PRIORITY;
    
#if NVM_DEBUG_THREADS
    printf("Setting intial priority to %d\n", thread->priority);
#endif
    
    if (thread->state != NEW)
        return nvm_throw_new_exception(pState, JAVA_LANG_ILLEGALSTATEEXCEPTION);
    // Allocate space for stack frames.
    if (init_stacks(pState, thread) < 0)
    {
        return EXEC_RETRY;
    }
    
#ifdef VERIFY
    assert (nvm_object_is_array (ref2obj(thread->stackFrameArray)), THREADS0);
    assert (nvm_object_is_array (ref2obj(thread->stackArray)), THREADS1);
#endif
    thread->stackFrameIndex = 0;
    thread->state = STARTED;
    
    enqueue_thread(pState, thread);

    if (pState->current_thread != NULL && thread->priority > pState->current_thread->priority) {
        // Force the scheduler to be invoked so the higher priority thread can now run.
        schedule_request (pState, REQUEST_SWITCH_THREAD);
    }
    
    return EXEC_CONTINUE;
}

/**
 * Switches to next thread:
 *
 * do
 *   get next thread
 *   if waiting, grab monitor and run
 *   if sleeping and timer expired, run
 *   if DEAD, clean up and use current thread
 *   if started, initialize and run
 * until selected thread can run
 *
 * @return true iff the gc needs to be run instead of a thread
 */

boolean switch_thread(nvm_state_t* pState, TIMESTAMP now)
{
    Thread *anchorThread, *previousThread, *candidate;
    REFERENCE *pThreadQ;
    boolean nonDaemonRunnable = false;
    StackFrame *stackFrame = NULL;
    short i;
#if NVM_DEBUG_THREADS || NVM_DEBUG_BYTECODE
    printf ("------ switch_thread: currentThread at %d\n", (int) pState->current_thread);
#endif
    
    if (pState->current_thread != NULL) {
        // Only current threads can die. Tidy up dead threads
        if (pState->current_thread->state == DEAD)
        {
#if NVM_DEBUG_THREADS
            printf ("Tidying up DEAD thread %d: %d\n", (int) pState->current_thread, (int)pState->current_thread->threadId);
#endif
#if REMOVE_DEAD_THREADS
            free_stacks(pState, pState->current_thread);
#ifdef SAFE
            pState->current_thread->stackFrameArray = ptr2ref(NULL);
            pState->current_thread->stackArray = ptr2ref(NULL);
#endif // SAFE
#endif // REMOVE_DEAD_THREADS
            
            // Remove thread from queue.
            dequeue_thread(pState, pState->current_thread);
        }
        else { // Save context information
            stackFrame = current_stackframe(pState);
            
#if NVM_DEBUG_THREADS
            printf ("switchThread: current stack frame: %d\n", (int) stackFrame);
#endif
            
            update_stack_frame (pState, stackFrame);
        }
    }
    
    pState->current_thread = NULL;

    byte found_system_waiting_priority = 0;
    
    // Loop until a frame is found that can be made to run.
    for (i=MAX_PRIORITY-1; i >= 0; i--) {
        pThreadQ = &pState->thread_queue[i];
        
        previousThread = anchorThread = (Thread*) ref2obj(*pThreadQ);
        if (!previousThread)
            continue;
        
        do
        {
            candidate = (Thread*) ref2obj(previousThread->nextThread);
            
#if NVM_DEBUG_THREADS
            printf ("Checking state of thread %d(%d)(s=%d,p=%d,i=%d,d=%d)\n",
                    (int)candidate,
                    (int)candidate->threadId,
                    (int)candidate->state,
                    (int)candidate->priority,
                    (int)candidate->interruptState,
                    (int)candidate->daemon
                    );
#endif
            // See if we can move a thread to the running state. Used to not do this if we
            // had already found one, but turns out to be easiest if we are avoiding
            // priority inversion.
            switch (candidate->state)
            {
                case CONDVAR_WAITING:
                    // We are waiting to be notified
                    if (candidate->sleepUntil != 0 && now >= candidate->sleepUntil)
                    {
#if NVM_DEBUG_MONITOR
                        printf ("Waking up waiting thread %d: %d\n", (int) candidate, candidate->threadId);
#endif
                        // We will drop through to mon waiting.
                    }
                    else if (candidate->interruptState == INTERRUPT_CLEARED)
                        break;
                    else
                        candidate->interruptState = INTERRUPT_GRANTED;

                    candidate->sleepUntil = 0;
                    
                    // candidate->state = MON_WAITING;
                    // drop through
                case MON_WAITING:
                {
                    objSync *sync = (objSync*)ref2ptr(candidate->sync);
                    byte threadId = get_thread_id(sync);
                    
                    if (threadId == NO_OWNER)
                    {
                        // This is the highest priority thread that's waiting on the monitor.
                        // At this point, we can clear the hasWaiters bit, and when the loop
                        // continues through other threads, it will eventually find all the other
                        // threads waiting on this object and reset hasWaiters.
                        sync->hasWaiters = 0;

                        // NOW enter the monitor (guaranteed to succeed)
                        enter_monitor(pState, candidate, ref2obj(candidate->waitingOn));
                        
                        // Set the monitor depth to whatever was saved.
                        set_monitor_count(sync, candidate->monitorCount);
                        
                        // Let the thread run.
                        candidate->state = RUNNING;
                        
#ifdef SAFE
                        candidate->waitingOn = ptr2ref(NULL);
                        candidate->sync = ptr2ref(NULL);
#endif
                    } else {
                        sync->hasWaiters = 1;
                    }
                    
                }
                    break;
                case JOIN:
                {
                    Thread *pThread = (Thread *)ref2obj(candidate->waitingOn);
                    if (pThread->state == DEAD ||
                        candidate->interruptState != INTERRUPT_CLEARED ||
                        (candidate->sleepUntil != 0 && now >= candidate->sleepUntil))
                    {
                        candidate->state = RUNNING;
                        candidate->waitingOn = ptr2ref(NULL);
                        candidate->sleepUntil = 0;
                        if (candidate->interruptState != INTERRUPT_CLEARED)
                            candidate->interruptState = INTERRUPT_GRANTED;
                    }
                    break;
                }
                case SLEEPING:
                    if (candidate->interruptState != INTERRUPT_CLEARED
                        || (now >= candidate->sleepUntil))
                    {
#if NVM_DEBUG_THREADS
                        printf ("Waking up sleeping thread %d: %d\n", (int) candidate, candidate->threadId);
#endif
                        candidate->state = RUNNING;
                        if (candidate->interruptState != INTERRUPT_CLEARED)
                            candidate->interruptState = INTERRUPT_GRANTED;
                        candidate->sleepUntil = 0;
                    }
                    break;
                case STARTED:
                    if (pState->current_thread == NULL) {
                        // Put stack ptr at the beginning of the stack so we can push arguments
                        // to entry methods. This assumes set_top_word or set_top_ref will
                        // be called immediately below.
#if NVM_DEBUG_THREADS
                        printf ("Starting thread %d: %d\n", (int) candidate, candidate->threadId);
#endif
                        pState->current_thread = candidate;	// Its just easier this way.
                        
                        empty_stacks(pState);
                        candidate->state = RUNNING;
                        call_run(pState, candidate);
                        // The following is needed because the current stack frame
                        // was just created
                        stackFrame = current_stackframe(pState);
                        update_stack_frame (pState, stackFrame);
                    }
                    break;
                case SYSTEM_WAITING:
                    // Just keep on waiting
                    if (candidate->priority > found_system_waiting_priority) {
                        found_system_waiting_priority = candidate->priority;
                    }
                case RUNNING:
                    // It's running already
                case DEAD:
                    // Dead threads should be handled earlier
                default:
                    // ???
                    break;
            }

            if (candidate->sleepUntil && candidate->sleepUntil < pState->wake_time) {
                pState->wake_time = candidate->sleepUntil;
            }
            
            // Do we now have a thread we want to run?
            // Note we may later decide not to if all non-daemon threads have died
            if (pState->current_thread == NULL && candidate->state == RUNNING) {
                pState->current_thread = candidate;
                // Move thread to end of queue
                *pThreadQ = obj2ref(candidate);
            }
            
            if (!candidate->daemon)
            {
                // May or may not be running but it could do at some point
#if NVM_DEBUG_THREADS
                printf ("Found a non-daemon thread %d: %d(%d)\n", (int) candidate, (int)candidate->threadId, (int) candidate->state);
#endif
                nonDaemonRunnable = true;
            }
            
#if NVM_DEBUG_THREADS
            printf ("switch_thread: done processing thread %d: %d\n", (int) candidate,
                    (int) (candidate->state == RUNNING));
#endif
            
            // Always use the first running thread as the thread
            // Keep looping: cull dead threads, check there's at least one non-daemon thread
            previousThread = candidate;
        } while (candidate != anchorThread);
    } // end for
    
#if NVM_DEBUG_THREADS
    printf ("currentThread=%d, ndr=%d\n", (int) pState->current_thread, (int)nonDaemonRunnable);
#endif
    
#if NVM_DEBUG_THREADS
    printf ("Leaving switch_thread()\n");
#endif
    if (pState->current_thread != NULL) {
        // If we found a running thread and there is at least one
        // non-daemon thread left somewhere in the queue...
#if NVM_DEBUG_THREADS
        printf ("Current thread is %d: %d(%d)\n", (int) pState->current_thread, (int)pState->current_thread->threadId, (int) pState->current_thread->state);
        printf ("getting current stack frame...\n");
#endif
        
        stackFrame = current_stackframe(pState);
        
#if NVM_DEBUG_THREADS
        printf ("updating registers...\n");
#endif
        
        update_registers (pState, stackFrame);
#if NVM_DEBUG_THREADS
        printf ("done updating registers\n");
#endif
        
        if (pState->current_thread->interruptState == INTERRUPT_GRANTED)
            nvm_throw_new_exception(pState, JAVA_LANG_INTERRUPTEDEXCEPTION);
    }
    if (!nonDaemonRunnable)
    {
        shutdown_program(pState);
        return false;
    }

    if (pState->current_thread != NULL &&
        found_system_waiting_priority >= pState->current_thread->priority) {
        // a runnable thread was found, but a thread at the same or higher priority
        // could be runnable if the gc were run to completion
        return true;
    } else if (pState->current_thread == NULL) {
        // no runnable thread, but some thread is blocked on gc
        return (found_system_waiting_priority > 0);
    } else {
        // we found a runnable thread and it's the same or higher priority than
        // any thread that's blocked on the gc
        return false;
    }
}

/*
 * Current thread will wait on the specified object, waiting for a
 * system_notify. Note the thread does not need to own the object,
 * to wait on it. However it will wait to own the monitor for the
 * object once the wait is complete.
 */
void system_wait(nvm_state_t* pState, Object *obj)
{
#if NVM_DEBUG_MONITOR
    printf("system_wait of %d, thread %d(%d)\n",(int)obj, (int)pState->current_thread, pState->current_thread->threadId);
#endif
    // Indicate the we are waiting for a system notify
    pState->current_thread->state = SYSTEM_WAITING;
    
    // Set the monitor count for when we resume (always 1).
    pState->current_thread->monitorCount = 1;
    
    // Save the object who's monitor we will want back
    pState->current_thread->waitingOn = ptr2ref (obj);
    pState->current_thread->sync = ptr2ref(get_sync(pState, obj));
    
    // no time out
    pState->current_thread->sleepUntil = 0;
    
#if NVM_DEBUG_MONITOR
    printf("system_wait of %d, thread %d(%d)\n",(int)obj, (int)pState->current_thread, pState->current_thread->threadId);
#endif
    // Gotta yield
    schedule_request(pState, REQUEST_SWITCH_THREAD);
}


/*
 * wake up any objects waiting on the passed system object.
 * Note unlike ordinary waits, we do not allow system waits to be interrupted.
 */
void system_notify(nvm_state_t* pState, Object *obj, const boolean all)
{
    short i;
    Thread *pThread;
    
#if NVM_DEBUG_MONITOR
    if (pState->current_thread) {
        printf("system_notify_ of %d, thread %d(%d)\n",(int)obj, (int)pState->current_thread, pState->current_thread->threadId);
    } else {
        printf("system_notify_ of %d, thread %d\n",(int)obj, (int)pState->current_thread);
    }
#endif
    // Find a thread waiting on us and move to WAIT state.
    for (i=MAX_PRIORITY-1; i >= 0; i--)
    {
        pThread = (Thread*) ref2obj(pState->thread_queue[i]);
        if (!pThread)
            continue;
        
        do {
            // Remember pState->thread_queue[i] is the last thread on the queue
            pThread = (Thread*) ref2obj(pThread->nextThread);
            if (pThread->state == SYSTEM_WAITING && ref2ptr(pThread->waitingOn) == obj)
            {
                pThread->state = MON_WAITING;
                if (!all)
                    return;
            }
        } while (pThread != (Thread*) ref2obj(pState->thread_queue[i]));
    }
}


/*
 * Current thread owns object's monitor (we hope) and wishes to relinquish
 * it temporarily (by calling Object.wait()).
 */
int monitor_wait(nvm_state_t* pState, Object *obj, const TIMESTAMP time)
{
    objSync *sync = get_sync(pState, obj);
#if NVM_DEBUG_MONITOR
    printf("monitor_wait of %d, thread %d(%d)\n",(int)obj, (int)pState->current_thread, pState->current_thread->threadId);
#endif
    if (pState->current_thread->threadId != get_thread_id (sync))
        return nvm_throw_new_exception(pState, JAVA_LANG_ILLEGALMONITORSTATEEXCEPTION);
    
    // Great. We own the monitor which means we can give it up, but
    // indicate that we are listening for notify's.
    pState->current_thread->state = CONDVAR_WAITING;
    
    // Save monitor depth
    pState->current_thread->monitorCount = get_monitor_count(sync);
    
    // Save the object who's monitor we will want back
    pState->current_thread->waitingOn = ptr2ref (obj);
    pState->current_thread->sync = ptr2ref(sync);
    // Might be an alarm set too.
    if (time != 0)
        pState->current_thread->sleepUntil = atomic_time_get(pState->sys_time) + time;
    else
        pState->current_thread->sleepUntil = 0;
    
#if NVM_DEBUG_MONITOR
    printf("monitor_wait of %d, thread %d(%d) until %ld\n",(int)obj, (int)pState->current_thread, pState->current_thread->threadId, time);
#endif
    
    // Indicate that the object's monitor is now free.
    set_thread_id (sync, NO_OWNER);
    set_monitor_count(sync, 0);
    
    // Gotta yield
    schedule_request(pState, REQUEST_SWITCH_THREAD);
    return EXEC_CONTINUE;
}

/*
 * Current thread owns object's monitor (we hope) and wishes to wake up
 * any other threads waiting on it. (by calling Object.notify()).
 */
int monitor_notify(nvm_state_t* pState, Object *obj, const boolean all)
{
    objSync *sync = get_sync(pState, obj);
#if NVM_DEBUG_MONITOR
    printf("monitor_notify of %d, thread %d(%d)\n",(int)obj, (int)pState->current_thread, pState->current_thread->threadId);
#endif
    if (pState->current_thread->threadId != get_thread_id (sync))
        return nvm_throw_new_exception(pState, JAVA_LANG_ILLEGALMONITORSTATEEXCEPTION);
    
    monitor_notify_unchecked(pState, obj, all);
    return EXEC_CONTINUE;
}

/*
 * wake up any objects waiting on the passed object.
 */
boolean monitor_notify_unchecked(nvm_state_t* pState, Object *obj, const boolean all)
{
    short i;
    Thread *pThread;
    boolean woke = false;
    
#if NVM_DEBUG_MONITOR
    printf("monitor_notify_unchecked of %d, thread %d(%d)\n",(int)obj, (int)pState->current_thread, pState->current_thread->threadId);
#endif
    // Find a thread waiting on us and move to WAIT state.
    for (i=MAX_PRIORITY-1; i >= 0; i--)
    {
        pThread = (Thread*) ref2ptr(pState->thread_queue[i]);
        if (!pThread)
            continue;
        
        do {
            // Remember pState->thread_queue[i] is the last thread on the queue
            pThread = (Thread*) ref2obj(pThread->nextThread);
            if (pThread->state == CONDVAR_WAITING && ref2ptr(pThread->waitingOn) == obj)
            {
                // might have been interrupted while waiting
                if (pThread->interruptState != INTERRUPT_CLEARED)
                    pThread->interruptState = INTERRUPT_GRANTED;
                
                pThread->state = MON_WAITING;
                ref2obj(pThread->waitingOn)->sync.hasWaiters = 1;

                woke = true;

                if (!all) {
                    return woke;
                }
            }
        } while (pThread != (Thread*) ref2obj(pState->thread_queue[i]));
    }
    return woke;
}

/**
 * currentThread enters obj's monitor:
 *
 * if monitor is in use, save object in thread and re-schedule
 * else grab monitor and increment its count.
 *
 * Note that this operation is atomic as far as the program is concerned.
 */
void enter_monitor (nvm_state_t* pState, Thread *pThread, Object* obj)
{
    objSync *sync;
#if NVM_DEBUG_MONITOR
    printf("enter_monitor of %d\n",(int)obj);
#endif
    
    if (obj == NULL) {
        nvm_throw_new_exception(pState, JAVA_LANG_NULLPOINTEREXCEPTION);
        return;
    }
    sync = get_sync(pState, obj);
    if (get_monitor_count (sync) != NO_OWNER &&
        pThread->threadId != get_thread_id (sync)) {
        // There is an owner, but its not us.
        // Make thread wait until the monitor is relinquished.
        pThread->state = MON_WAITING;
        pThread->waitingOn = ptr2ref (obj);
        pThread->sync = ptr2ref(sync);
        pThread->monitorCount = 1;
        sync->hasWaiters = 1;
        // Gotta yield
        schedule_request (pState, REQUEST_SWITCH_THREAD);
        return;
    }
    set_thread_id (sync, pThread->threadId);
    inc_monitor_count (sync);
}

/**
 * Decrement monitor count
 * Release monitor if count reaches zero
 */
void exit_monitor(nvm_state_t* pState, Thread *pThread, Object* obj)
{
    byte newMonitorCount;
    objSync *sync;
    
#if NVM_DEBUG_MONITOR
    printf("exit_monitor of %d\n",(int)obj);
#endif
    
    if (obj == NULL) {
        // Exiting due to a NPE on monitor_enter [FIXME]
        return;
    }
    sync = get_sync(pState, obj);
#ifdef VERIFY
    assert (get_thread_id(sync) == pThread->threadId, THREADS7);
    assert (get_monitor_count(sync) > 0, THREADS8);
#endif
    
    newMonitorCount = get_monitor_count(sync)-1;
    if (newMonitorCount == 0)
        set_thread_id (sync, NO_OWNER);
    set_monitor_count (sync, newMonitorCount);

    if (newMonitorCount == 0 && sync->hasWaiters) {
        // Force the scheduler to be invoked in case a higher priority thread can now run.
        schedule_request(pState, REQUEST_SWITCH_THREAD);
    }
}

/**
 * Current thread waits for thread to die.
 *
 * throws InterruptedException
 */
void join_thread(nvm_state_t* pState, Thread *thread, const TIMESTAMP time)
{
    // Make a note of the thread we are waiting for...
    pState->current_thread->waitingOn = ptr2ref (thread);
    // Might be an alarm set too.
    if (time > 0)
        pState->current_thread->sleepUntil = atomic_time_get(pState->sys_time) + time;
    else
        pState->current_thread->sleepUntil = 0;
    // Change our state
    pState->current_thread->state = JOIN;
    // Gotta yield
    schedule_request (pState, REQUEST_SWITCH_THREAD);
}

void dequeue_thread(nvm_state_t* pState, Thread *thread)
{
    // First take it out of its current queue
    byte cIndex = thread->priority-1;
    REFERENCE *pThreadQ = &pState->thread_queue[cIndex];
    
    // Find the previous thread at the old priority
    Thread *previous = (Thread*) ref2obj(*pThreadQ);
#if NVM_DEBUG_THREADS
    printf("Previous thread %p\n", (previous));
#endif
    while ((Thread*) ref2obj(previous->nextThread) != thread)
        previous = (Thread*) ref2obj(previous->nextThread);
    
#if NVM_DEBUG_THREADS
    printf("Previous thread %p\n", (previous));
#endif
    if (previous == thread)
    {
#if NVM_DEBUG_THREADS
        printf("No more threads of priority %d\n", thread->priority);
#endif
        *pThreadQ = ptr2ref(NULL);
    } else {
        previous->nextThread = thread->nextThread;
        *pThreadQ = obj2ref(previous);
    }  
}

void enqueue_thread(nvm_state_t* pState, Thread *thread)
{
    // Could insert it anywhere. Just insert it at the end.
    byte cIndex = thread->priority-1;
    Thread *previous = (Thread*) ref2obj(pState->thread_queue[cIndex]);
    pState->thread_queue[cIndex] = obj2ref((Object*) thread);
    if (previous == NULL) {
        thread->nextThread = ptr2ref(thread);
    } else {
        Thread *pNext = (Thread*) ref2obj(previous->nextThread);
        thread->nextThread = ptr2ref(pNext);
        previous->nextThread = ptr2ref(thread);
    }
}

/**
 * Set the priority of the passed thread. Insert into new queue, remove
 * from old queue. Overload to remove from all queues if passed priority
 * is zero.
 *
 * Returns the 'previous' thread.
 */
void set_thread_priority(nvm_state_t* pState, Thread *thread, const uint32_t priority)
{
#if NVM_DEBUG_THREADS
    printf("Thread priority set to %ld was %d\n", priority, thread->priority);
#endif
    if (thread->priority == priority)
        return;
    
    if (thread->state == NEW)
    {
        // Not fully initialized
        thread->priority = priority;
        return;
    }
    
    dequeue_thread(pState, thread);
    thread->priority = priority;
    enqueue_thread(pState, thread);

    // Force the scheduler to be invoked in case a higher priority thread can now run.
    schedule_request (pState, REQUEST_SWITCH_THREAD);
}

/**
 * Suspend the specified thread. If thread is null suspend all threads
 * except currentThread.
 */
void suspend_thread(nvm_state_t* pState, Thread *thread)
{
    int i;
    Thread *pThread;
    if (thread)
        thread->state |= SUSPENDED;
    else
    {
        // Suspend all threads
        for (i=MAX_PRIORITY-1; i >= 0; i--)
        {
            pThread = (Thread*) ref2obj(pState->thread_queue[i]);
            if (!pThread)
                continue;
            
            do {
                // Remember pState->thread_queue[i] is the last thread on the queue
                pThread = (Thread*) ref2obj(pThread->nextThread);
                if (pThread != pState->current_thread) pThread->state |= SUSPENDED;
            } while (pThread != (Thread*) ref2obj(pState->thread_queue[i]));
        }
    }
    schedule_request(pState, REQUEST_SWITCH_THREAD);
}

void resume_thread(nvm_state_t* pState, Thread *thread)
{
    int i;
    Thread *pThread;
    if (thread)
    {
        thread->state &= ~SUSPENDED;
        return;
    }
    // Suspend all threads
    for (i=MAX_PRIORITY-1; i >= 0; i--)
    {
        pThread = (Thread*) ref2obj(pState->thread_queue[i]);
        if (!pThread)
            continue;
        
        do {
            // Remember pState->thread_queue[i] is the last thread on the queue
            pThread = (Thread*) ref2obj(pThread->nextThread);
            pThread->state &= ~SUSPENDED;
        } while (pThread != (Thread*) ref2obj(pState->thread_queue[i]));
    }
}


