int scheduler() {
  int next_priority = 0;
  minithread_t next = NULL;
  minithread_t temp = NULL;
   
  while (1) {
    while (runnable_count == 0) {
      set_interrupt_level(ENABLED);
    };
    
    set_interrupt_level(DISABLED);
    //dequeue from runnable threads
    next_priority = choose_priority_level();
    if (multilevel_queue_dequeue(runnable_q,
        next_priority,(void**)(&next)) != -1) {
      runnable_count--;
      temp = current_thread;
      current_thread = next;
      minithread_switch(&(temp->stacktop),&(next->stacktop));
      return 0;
    }
    //if dead/runnable queue is empty, do nothing (idle thread)
  }
  return 0;
}

/*
 *	Atomically release the specified test-and-set lock and
 *	block the calling thread. This is a convenience function that
 *  does not add much (if any) power, but makes for more readable
 *  code and simplifies the possible system states, making it
 *  easier to reason about application correctness.
 */
int minithread_unlock_and_stop(tas_lock_t *lock) {
	interrupt_level_t old_int = set_interrupt_level(DISABLED);
	atomic_clear(lock);
	minithread_stop();
	set_interrupt_level(old_int);
	return 0;
}

int deregister_interrupt(int type) {
	int ret = 0;
	interrupt_queue_t *prev = NULL;
	interrupt_queue_t *interrupt_info = interrupt_queue;
	/* disable interrupts */
	interrupt_level_t old_interrupt_level = set_interrupt_level(DISABLED);

	/* look for an interrupt of the desired type */
	while ( (interrupt_info != NULL) && (interrupt_info->type != type) ) {
		prev = interrupt_info;
		interrupt_info = interrupt_info->next;
	}

	if (interrupt_info != NULL) {
		/* interrupt found, delete */
		if ( prev ) {
			prev->next = interrupt_info->next;
		} else {
			interrupt_queue = interrupt_info->next;
		}
		free(interrupt_info);
	} else {
		/* interrupt not registered, error */
		ret = -1;
	}
	
	set_interrupt_level(old_interrupt_level);
	return ret;
}

void 
minimsg_network_handler(network_interrupt_arg_t* arg)
{
	interrupt_level_t old_level = set_interrupt_level(DISABLED); //disable interrupt

	//Get header and destination port
	mini_header_t receivedHeader;
	memcpy(&receivedHeader, arg->buffer, sizeof(mini_header_t));
	int destPort = (int)unpack_unsigned_short(receivedHeader.destination_port);
	assert(destPort >= UNBOUNDED_PORT_START && destPort <= UNBOUNDED_PORT_END); // sanity checking

	//if the unbounded port has not been initialized, throw away the packet
	if (g_unboundedPortPtrs[destPort] == NULL)
	{
		set_interrupt_level(old_level); //restore interrupt level
		return;
	}

	//queue the packet and V the semaphore
	assert(g_unboundedPortPtrs[destPort]->port_type == 'u' && g_unboundedPortPtrs[destPort]->unbound_port.datagrams_ready != NULL
		&& g_unboundedPortPtrs[destPort]->unbound_port.incoming_data != NULL); 
	int appendSuccess = queue_append(g_unboundedPortPtrs[destPort]->unbound_port.incoming_data, (void*)arg);
	AbortOnCondition(appendSuccess == -1, "Queue_append failed in minimsg_network_handler()");

	semaphore_V(g_unboundedPortPtrs[destPort]->unbound_port.datagrams_ready);

	set_interrupt_level(old_level); //restore interrupt level
}

minithread_t minithread_get_from_ready_queue(int disable_interrupts)
{
	// ISO C90...
	minithread_t tcb;
	interrupt_level_t prev_level;
	int ret;

	// Disable interrupts as we're going to access the ready queue / ready_threads
	if (disable_interrupts == 1)
	{
		prev_level = set_interrupt_level(DISABLED);
	}

	// If there are threads, get the next one
	ret = multilevel_queue_dequeue(ready_queue, current_level, (void**) &tcb);

	// If there was an error, ensure the tcb we return is NULL
	if (ret == -1)
	{
		tcb = NULL;
	}
	 else
	{
		// Otherwise tcb stays as the new thread; decrement ready_threads
		ready_threads--;
	}

	// Restore the interrupt level
	if (disable_interrupts == 1)
		set_interrupt_level(prev_level);

	return tcb;
}

/*
 * Initialize the system to run the first minithread at
 * mainproc(mainarg).  This procedure should be called from your
 * main program with the callback procedure and argument specified
 * as arguments.
 */
void
minithread_system_initialize(proc_t mainproc, arg_t mainarg) {
  runnable_queue = multilevel_queue_new(MAX_LEVELS);
  
  stopped_queue = queue_new();
  scheduler_thread = scheduler_thread_create();
  assert(scheduler_thread);
  running_thread = scheduler_thread;
  int res = network_initialize((network_handler_t) network_handler);
  assert(res == 0);
  alarm_system_initialize();  
  minimsg_initialize();
  minisocket_initialize();
  reaper_thread = minithread_create(clean_stopped_threads, NULL);
  minithread_fork(mainproc, mainarg);
  interrupt_level_t prev_level = set_interrupt_level(ENABLED);
  minithread_clock_init(PERIOD * MILLISECOND, clock_handler);
  while (1) {
    if (!multilevel_queue_is_empty(runnable_queue)) {
      minithread_yield();
    }
  }
  set_interrupt_level(prev_level);
  multilevel_queue_free(runnable_queue);
  queue_free(stopped_queue);
}

/*
 * sleep with timeout in milliseconds. Need to disable interrupts during method call as we
  are not using semaphore for sleeping minithreads.
 */
void minithread_sleep_with_timeout(int delay) {
  interrupt_level_t old_level = set_interrupt_level(DISABLED);
  minithread_t* current_thread = schedule_data->running_thread; 
  register_alarm(delay, wakeup_thread, (void *) current_thread); //registers alarm
  minithread_stop(); //moves to next thread
  set_interrupt_level(old_level);
}

int minithread_add_to_ready_queue(minithread_t tcb, int disable_interrupts)
{
	int ret;
	interrupt_level_t prev_level;

	// Turn off interrupts while we access ready_queue & ready_threads
	if (disable_interrupts == 1)
	{
		prev_level = set_interrupt_level(DISABLED);
	}

	// Add the thread to the ready queue
	ret = multilevel_queue_enqueue(ready_queue, tcb->priority, (void*) tcb);

	// Increment the number of threads on the ready queue
	if (ret == 0)
		ready_threads++;	

	// Restore the interrupt level
	if (disable_interrupts == 1)
	{
		set_interrupt_level(prev_level);
	}

	return ret;
}

/*
 * sleep with timeout in milliseconds
 */
void minithread_sleep_with_timeout(int delay)
{
	// Create the sleep semaphore
	semaphore_t sleep_sem = semaphore_create();
	interrupt_level_t prev_level; // ISO C90...

	// Initialize it to a value of 0 so it can act as a way to signal threads
	semaphore_initialize(sleep_sem, 0);

	// Disable interrupts
	prev_level = set_interrupt_level(DISABLED);

	// Register the alarm
	register_alarm(delay, &minithread_sleep_alarm_wakeup, sleep_sem); 
	
	// If, at this point, interrupts were enabled, we could context switch away
	// the alarm could be triggered afterwards before the semaphore_P below was
	// called (depending on this threads priority level, etc) and then when this
	// thread finally calls semaphore_P(), it will hang forever.
	// Therefore we have interrupts disabled.

	// Wait on the sleep semaphore
	semaphore_P(sleep_sem);

	// Now that we've awoken, free the sleep semaphore
	semaphore_destroy(sleep_sem);

	// Restore the previous interrupt level
	set_interrupt_level(prev_level); // is this necessary? 

}

int miniterm_read(char* buffer, int len) {
	struct kb_line* old_ptr;
	interrupt_level_t old_level;
	int string_len;

	if (len == 0 || buffer == NULL) return 0;

	semaphore_P(new_data);
	old_level = set_interrupt_level(DISABLED);

	assert(kb_head != NULL);
	string_len = strlen(kb_head->buf);
	strncpy(buffer, kb_head->buf, len <= string_len ? len : string_len);
	buffer[len <= string_len ? len-1 : string_len] = 0;

	old_ptr = kb_head;
	kb_head = kb_head->next;
	free(old_ptr);
	if (kb_head == NULL)
		kb_tail = NULL;

	set_interrupt_level(old_level);

	return strlen(buffer);
}

/*
 * sleep with timeout in milliseconds
 */
void 
minithread_sleep_with_timeout(int delay)
{
  interrupt_level_t l = set_interrupt_level(DISABLED);
  register_alarm(delay, get_new_alarm_handler(), minithread_self());
  set_interrupt_level(l);
  minithread_stop();
}

/* Wait for available interrupt */
static void
miniroute_wait_for_intrpt(network_interrupt_arg_t **p_intrpt)
{
    interrupt_level_t oldlevel = set_interrupt_level(DISABLED);
    semaphore_P(intrpt_sig);
    queue_wrap_dequeue(intrpt_buffer, (void**)p_intrpt);
    set_interrupt_level(oldlevel);
}

int minisocket_receive(minisocket_t *socket, char *msg, int max_len, minisocket_error *error) {
  if (socket == NULL || msg == NULL || max_len < 0) {
    *error = SOCKET_INVALIDPARAMS;
    return -1;
  }
  if (socket->state == CLOSED) {
    *error = SOCKET_RECEIVEERROR;
    return -1;
  }
  if (max_len == 0) {
    return 0;
  }
  
  int received = 0;
  while (!received) {
    semaphore_P(socket->datagrams_ready);

    network_interrupt_arg_t *interrupt_message = NULL;
    interrupt_level_t old_level = set_interrupt_level(DISABLED);
    queue_dequeue(socket->incoming_data, (void **) &interrupt_message);
    set_interrupt_level(old_level); 

    if (interrupt_message != NULL) {
      mini_header_reliable_t* received_header = (mini_header_reliable_t *) interrupt_message->buffer; 
      network_address_t temp_address;
      unpack_address(received_header->source_address, temp_address);
      if (socket->remote_port_number == unpack_unsigned_short(received_header->source_port) &&
        network_compare_network_addresses(socket->remote_address, temp_address) != 0 &&
        received_header->message_type == MSG_ACK && interrupt_message->size > sizeof(mini_header_reliable_t)) {
        //same address, same ports, right message
        received = 1; 
        int data_left = interrupt_message->size - sizeof(mini_header_reliable_t) - socket->next_read;
        interrupt_level_t old_level = set_interrupt_level(DISABLED);
        
        if (data_left <= max_len) {
          memcpy(msg, interrupt_message->buffer + sizeof(mini_header_reliable_t) + socket->next_read, data_left);
          socket->next_read = 0;
          free(interrupt_message);
          set_interrupt_level(old_level); // must protect global data field next_read 
          return data_left;
        }
        else {
          memcpy(msg, interrupt_message->buffer + sizeof(mini_header_reliable_t) + socket->next_read, max_len);
          socket->next_read += max_len;
          queue_prepend(socket->incoming_data, interrupt_message); //ack works as well when there is data 
          semaphore_V(socket->datagrams_ready); //another message in queue, V semaphore
          set_interrupt_level(old_level); 
          return max_len;
        }
      }
      else {
        free(interrupt_message);
      }
    }
  }
  return -1;
}

//Destroys minisockets
void minisocket_destroy(minisocket_t minisocket, int FIN) {
	int portNumber;
	int i, threads;
	interrupt_level_t prev_level;
	minisocket_error error;

	if (minisocket == NULL)
		return;

	portNumber = minisocket->port_number;

	semaphore_P(destroy_semaphore);

	minisocket->waiting = TCP_PORT_WAITING_TO_CLOSE;

	if (minisockets[portNumber] == NULL)
		return;

	semaphore_V(minisocket->packet_ready);

	semaphore_P(minisocket->mutex);

	if (minisockets[portNumber] == NULL)
		return;

	if (FIN == 1) {
		transmit_packet(minisocket, minisocket->destination_addr, minisocket->destination_port,
				1, MSG_FIN, 0, NULL, &error);
	}

	minisocket->status = TCP_PORT_CLOSING;

	prev_level = set_interrupt_level(DISABLED);
	threads = minisocket->num_waiting_on_mutex;

	for (i = 0; i < threads; i++)
	{
		semaphore_V(minisocket->mutex);
		i++;
	}
	set_interrupt_level(prev_level);

	minisockets[portNumber] = NULL;

	semaphore_destroy(minisocket->wait_for_ack_semaphore);
	semaphore_destroy(minisocket->mutex);
	semaphore_destroy(minisocket->packet_ready);

	if (minisocket->data_length != 0)
		free(minisocket->data_buffer);

	queue_free(minisocket->waiting_packets);

	free(minisocket);

	semaphore_V(destroy_semaphore);
}

/* Used to wake the sending thread up after a route discovery timeout */
void alarm_wakeup_sem(void* arg)
{
	interrupt_level_t prev_level;
	semaphore_t sem = (semaphore_t) arg;

	// As the network handler touches this, need to disable interrupts...
	prev_level = set_interrupt_level(DISABLED);
	semaphore_V(sem);
	set_interrupt_level(prev_level);
}

void minithread_start(minithread_t *t) {
  t->status = RUNNABLE;

  interrupt_level_t old_level = set_interrupt_level(DISABLED); 
  int response = multilevel_queue_enqueue(schedule_data->multi_run_queue, t->level, t);
  set_interrupt_level(old_level);

  if (response != 0) { 
    exit(1); //enqueue failed
  }
}

/**
 *  Network handler function which gets called whenever packet
 *  arrives. Handler disables interrupts for duration of function.
 *  Puts packet onto pkt_q to be processed later by process_packets
 *  thread.
 */
void network_handler(network_interrupt_arg_t* pkt){
  interrupt_level_t l;
  l = set_interrupt_level(DISABLED);
  if (queue_append(pkt_q, pkt)){
    //queue was not initialized
    set_interrupt_level(l);
    return;
  }
  set_interrupt_level(l);
  semaphore_V(pkt_available_sem); //wake up packet processor
  return;
}

/*
 * Make thread t runnable.
 */
void
minithread_start(minithread_t *t) {
  t->s = RUNNABLE;
  if (runnable_queue) {
    // the runnable queue, being critical section i.e. can be modified
    // inside interrupt handler as well, therefore, interrupts
    // are disabled in this section.
    interrupt_level_t old_level = set_interrupt_level(DISABLED);
    multilevel_queue_enqueue(runnable_queue, t->level, t);
    set_interrupt_level(old_level);
  } 
}

/* Used to wakeup a thread when it's attempting retransmissions */
void wake_up_semaphore(void* arg)
{
	minisocket_t socket = (minisocket_t) arg;
	interrupt_level_t prev_level = set_interrupt_level(DISABLED);

	if (socket != NULL && (socket->waiting == TCP_PORT_WAITING_ACK 
				|| socket->waiting == TCP_PORT_WAITING_SYNACK))
	{
		semaphore_V(socket->wait_for_ack_semaphore);
	}

	set_interrupt_level(prev_level);
}

void semaphore_initialize(semaphore_t *sem, int cnt) {
	//Validate input arguments, abort if invalid argument is seen
	AbortOnCondition(sem == NULL || cnt < 0, "Invalid arguments passed to semaphore_initialize()");

	interrupt_level_t old_level = set_interrupt_level(DISABLED); //disable interrupts

	//critical section
	sem->count = cnt;
	assert(sem->semaWaitQ != NULL); //sanity checks
	assert(sem->count == cnt);

	set_interrupt_level(old_level); //restore interrupts
}