#include <memory>
#include <cstdio>
#include <sys/reent.h>

#include "cortexa8.hh"
#include "scheduler.hh"

static void idle_task(void *) {
  static int count = 0;
 
  while(true) {
     ++count;
     if(count%100 == 0) {
       printf(".");
       fflush(stdout);
     }
     __asm__ __volatile__("wfi");
  }
}

Scheduler::Scheduler()  {
  // Create kernel process object
  procs_.emplace_back(new Process());
  kernelProc_ = procs_.back().get();
  // Capture the initial (=current) kernel thread and wrap in in a Thread
  threads_.emplace_back(new Thread(*kernelProc_));
  kernelThread_ = threads_.back().get();
  kernelThread_->setState(ThreadState::running);
  currentThread_ = kernelThread_;
  // Create the idle thread
  threads_.emplace_back(new Thread(*kernelProc_, &idle_task, nullptr));
  idleThread_ = threads_.back().get();
  idleThread_->setPriority(65535);
  
  {
    cortexa8::ScopedSetIF disint{};
    // Schedule kernel and idle threads
    runQueue_.push(idleThread_);
  }
}

Scheduler::~Scheduler() {
}

void Scheduler::timeslice() {
  if(currentThread_->getState() == ThreadState::running)
    currentThread_->setState(ThreadState::ready);
  runQueue_.push(currentThread_);
  currentThread_ = runQueue_.top();
  runQueue_.pop();
  currentThread_->setState(ThreadState::running);
  cortexa8::set_cur_thread(currentThread_->get_tcb());
  _impure_ptr = &currentThread_->get_tcb()->newlibReent;
}

void Scheduler::update() {
  cortexa8::ScopedSetIF disint{};

  // If the current thread has become un-ready, or a higher priority thread has become ready, reschedule
  if(!currentThread_->ready() || (runQueue_.top()->getPriority() < currentThread_->getPriority())) {
    auto newThread_ = runQueue_.top();
    runQueue_.pop();

    if(currentThread_->getState() == ThreadState::running || currentThread_->getState() == ThreadState::yield) {
      currentThread_->setState(ThreadState::ready);
      runQueue_.push(currentThread_);
    }

    currentThread_ = newThread_;
    currentThread_->setState(ThreadState::running);
    cortexa8::set_cur_thread(currentThread_->get_tcb());
    _impure_ptr = &currentThread_->get_tcb()->newlibReent;
  }
}

void Scheduler::yield() {
  cortexa8::ScopedSetIF disint{};

  if(cortexa8::in_handler()) {
    if(currentThread_->getState() == ThreadState::running)
      currentThread_->setState(ThreadState::yield);
  } else {
    cortexa8::yield_svc();
  }
}

void Scheduler::exit() {
  cortexa8::ScopedSetIF disint{};

  if(cortexa8::in_handler()) {
    currentThread_->setState(ThreadState::terminated);
  } else {
    cortexa8::exit_svc();
  }
}