#include <cstring>
#include <cerrno>

#include "RTTL/common/RTThread.hxx"

/// This class extends MultiThreadedSyncPrimitive by including m_suspended member.
class MultiThreadedScheduler: public MultiThreadedSyncPrimitive
{
public:

  MultiThreadedScheduler() : m_suspended(false), MultiThreadedSyncPrimitive() {
  }

  ~MultiThreadedScheduler() {
  }

  _INLINE int suspended() const {
    return m_suspended;
  }

  // This function hides MultiThreadedSyncPrimitive::suspend().
  _INLINE void suspend(bool locked = false, int value = true) {
    int code;
    if (locked == false) code = lock();
    m_suspended = value;
    code = MultiThreadedSyncPrimitive::suspend();
    code = unlock();
  }

  _INLINE void resume() {
    m_suspended = false;
    MultiThreadedSyncPrimitive::resume();
  }

private:

  int m_suspended;
};


/// This class is neither visible nor accessible by the application.
class MultiThreadedTaskQueueServer: public MultiThreadedTaskQueue, public MultiThreadedSyncPrimitive
{

protected:

  MultiThreadedTaskQueueServer(int)
    : MultiThreadedTaskQueue(),
      MultiThreadedSyncPrimitive(),
      m_thread(NULL) 
    {
      PRINT(this);
      m_finished_jobs = 0;
      memset(m_waitingClients, 0, sizeof(m_waitingClients));
    }

  ~MultiThreadedTaskQueueServer() {
    clean(false);
  }

  _INLINE void clean(bool force_exit_from_thread_function = true) {
    if (m_threads <= 0) {
      return;
    }

    if (force_exit_from_thread_function) {
      lock();
      // Let all active threads know we're finished.
      int nt = m_threads;
      m_threads = -1;
      // Gently push suspended threads allowing them to discover
      // that they should return from threadFunc.
      for (int i = 0; i < nt; i++) {
        m_scheduler[i].resume();
      }
      unlock();

      // Wait till all threads do just that.
      int i = 0;
      while (i < nt) {
        for (; i < nt; i++) {
          if (m_scheduler[i].suspended() != -1) {
            //_mm_pause();
            sched_yield();
            break;
          }
        }
      }
    }

    if (m_thread)
      delete [] m_thread;
    if (m_client)
      delete [] m_client;
    if (m_scheduler)
      delete [] m_scheduler;
  }

public:

  friend class MultiThreadedTaskQueue;

  /// Without arguments, suspend the server,
  /// With integer argument, suspend i-th task.
  using MultiThreadedSyncPrimitive::suspend;
  _INLINE static void suspend(int i) {
    assert(m_client[i] == NULL);
    m_scheduler[i].suspend();
  }

  /// Without arguments, resume the server.
  /// With integer argument, resume i-th task.
  using MultiThreadedSyncPrimitive::resume;
  using MultiThreadedSyncPrimitive::resumeAll;
  _INLINE static void resume(int i) {
    assert(m_client[i]);
    m_scheduler[i].resume();
  }

  void createThreads(int threads) {
    assert(threads >= 1);

    // Free resources.
    clean();

    typedef MultiThreadedTaskQueue* pMultiThreadedTaskQueue;
    m_threads   = threads;
    m_thread    = new pthread_t[threads];
    m_client    = new pMultiThreadedTaskQueue[threads];
    m_scheduler = new MultiThreadedScheduler[threads];
    memset(m_client, 0, sizeof(pMultiThreadedTaskQueue) * threads);

    int res;
    //cout << "creating " << m_threads << " RENDERER THREADS" << endl;
    for (int i = 0; i < m_threads; i++) {
      if ( (res = pthread_create(&m_thread[i], NULL, &threadFunc, (void*)i)) != 0) {
        cerr << "can't create thread " << i;
        if (res == EAGAIN)
          cerr << " (the system does not have enough resources)" << endl;
        else
          cerr << ". reason : " << res << endl;
        exit(EXIT_FAILURE);
      }
    }

    // Wait till all threads suspend themselves for the first time.
    waitUntillAllThreadsAreSuspended();
  }

  _INLINE bool finished() const { return m_threads == -1; }

private:

  // All other member functions are not accessible by derived classes.

  _INLINE void waitUntillAllThreadsAreSuspended() {
    int ns;
    do {
      //_mm_pause(); // sleep(0);
      sched_yield();
      ns = 0;
      for (int i = 0; i < m_threads; i++)
        ns += m_scheduler[i].suspended();
    } while (ns != m_threads);
  }


  static void* threadFunc(void* _id) {
    long long int id = (long long int)_id;

    _mm_setcsr(_mm_getcsr() | /*FTZ:*/ (1<<15) | /*DAZ:*/ (1<<6));

    // First time...
    m_scheduler[id].suspend();

    int seqn;
    long long int tag = -1;
    while (!m_server.finished()) {
      MultiThreadedTaskQueue* client = MultiThreadedTaskQueue::m_client[id];

      int action = 0;

      assert(client);
      if (tag != client->tag()) {
        tag = client->tag();
        m_server.lock();
        if (client->m_assigned_jobs >= client->m_threads) {
          m_server.unlock();
          goto getnext;
        }
        seqn = client->m_assigned_jobs++;
        m_server.unlock();
      }
      action = client->task(seqn, (int)id);
      if (action == THREAD_EXIT)
        break;

    getnext:
      // deactivateThread also suspends it if there are no more jobs pending.
      seqn = deactivateThread((int)id);
    }

    m_scheduler[id].suspend(false, -1);
    return NULL;
  }


  _INLINE static int deactivateThread(const int threadID) {

    // Clear up threadID job.
    MultiThreadedTaskQueue* client = m_client[threadID];
    assert(m_client[threadID]);
    m_client[threadID] = NULL;

    // Check if this client is finished and
    // schedule a new job for the current thread.

    int code = m_server.lock();
    int seqn = -1;
    //printf("%i", (1 + client->m_finished_jobs));

    // If done, server will be waked up (it was suspended in waitForAllThreads).
    bool done = ++client->m_finished_jobs == client->m_threads;

    // Is there any new job for this thread?
    if (m_server.schedule(threadID)) {
      // Yep, unlock and return to the threadFunc
      // (after resuming the server if we done).
      if (done)
        m_server.resume();
      else
        seqn = client->m_assigned_jobs++;
      code = m_server.unlock();
    } else {
      // Nope, wait here till addClient resumes this thread.
      // (after resuming the server if we done).
      if (done)
        m_server.resume();
      m_scheduler[threadID].lock();
      code = m_server.unlock();
      m_scheduler[threadID].suspend(true);
    }
    return seqn;
  }


  _INLINE int getNextClientIndex() {
    // This function doesn't lock anything; caller should do it.
    int j = m_finished_jobs;
    for (int i = 0; i < queuesize; i++) {
      if (m_waitingClients[j]) {
        return j;
      }
      if (++j == queuesize) j = 0;
    }
    return -1;
  }


  void addClient(MultiThreadedTaskQueue* client) {

  recheck:
    m_server.lock();
    int j = m_finished_jobs;
    for (int i = 0; i < queuesize; i++) {
      if (m_waitingClients[j] == NULL) goto found;
      if (++j == queuesize) j = 0;
    }
    // Should be extremely rare occasion; worth signaling fatal error...
    // But just for shear fun of it...
    m_server.unlock();
    //_mm_pause(); // sleep(0);
    sched_yield();
    goto recheck;

  found:

    m_waitingClients[j] = client;
    m_scheduledJobs[j] = 0;

    m_finished_jobs = j + 1;
    if (m_finished_jobs == queuesize) m_finished_jobs = 0;

    int nt = m_server.m_threads;
    // For all free threads among nt server threads...
    for (int i = 0; i < nt; i++) {
      m_scheduler[i].lock();
      if (m_scheduler[i].suspended()) {
        if (!schedule(i)) {
          m_scheduler[i].unlock();
          break;
        }
        m_scheduler[i].resume();
      }
      m_scheduler[i].unlock();
    }

    m_server.unlock();
  }


  bool schedule(int i) {
    // This function doesn't lock anything; caller should do it.
    if (m_client[i] == NULL) {

      int ji = getNextClientIndex();
      if (ji == -1) {
        // There are no more pending jobs.
        return false;
      }
      m_client[i] = m_waitingClients[ji];

      // Are we done assigning threads for this client?
      if (++m_scheduledJobs[ji] == m_waitingClients[ji]->m_threads)
        m_waitingClients[ji] = NULL; // yep, remove it from the queue

    }
    return true;
  }

  pthread_t*              m_thread;                    // active server threads
  static const int        queuesize = 4;               // queue size
  MultiThreadedTaskQueue* m_waitingClients[queuesize]; // client queue
  int                     m_scheduledJobs[queuesize];  // # of already scheduled jobs per client
};


/// This is a public function.
void MultiThreadedTaskQueue::setMaxNumberOfThreads(int threads) {
  PRINT(threads);
  m_server.createThreads(threads);
}
/// This is a public function.
int MultiThreadedTaskQueue::maxNumberOfThreads() {
  return m_server.numberOfThreads();
}

/// This is a public function.
void MultiThreadedTaskQueue::createThreads(int threads) {
  assert(threads >= 1);
  m_threads = threads;
  m_server.lock();
  if (m_server.m_threads == 0) {
    // Initialize server (once).
    m_server.createThreads(threads);
  }
  m_server.unlock();
}


/// This is a public function.
void MultiThreadedTaskQueue::startThreads() {
  m_finished_jobs = m_assigned_jobs = 0;
  m_server.addClient(this);
}

/// This is a public function.
void MultiThreadedTaskQueue::waitForAllThreads() {
  if (m_finished_jobs == m_threads)
    return;
  int code = m_server.lock();
  //printf("%c", char('a' + m_finished_jobs));

  // Server is suspended if threads are stil running...
  while (m_finished_jobs < m_threads)
    m_server.suspend();
  //printf("\n");

  code = m_server.unlock();
}

/// Initialize static members.
MultiThreadedTaskQueueServer MultiThreadedTaskQueue::m_server(1);
MultiThreadedTaskQueue** MultiThreadedTaskQueue::m_client = NULL;
MultiThreadedScheduler*  MultiThreadedTaskQueue::m_scheduler = NULL;