/*
   * Copyright (C) 2014  René Kijewski  <rene.kijewski@fu-berlin.de>
   *
   * This library is free software; you can redistribute it and/or
   * modify it under the terms of the GNU Lesser General Public
   * License as published by the Free Software Foundation; either
   * version 2.1 of the License, or (at your option) any later version.
   *
   * This library is distributed in the hope that it will be useful,
   * but WITHOUT ANY WARRANTY; without even the implied warranty of
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   * Lesser General Public License for more details.
   *
   * You should have received a copy of the GNU Lesser General Public
   * License along with this library; if not, write to the Free Software
   * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
   */
  
  /**
   * @ingroup     pthread
   * @{
   *
   * @file
   * @brief       Implementation of a fair, POSIX conforming reader/writer lock.
   *
   * @author      René Kijewski <rene.kijewski@fu-berlin.de>
   *
   * @}
   */
  
  #include "pthread.h"
  #include "sched.h"
  #include "xtimer.h"
  
  #include <stdint.h>
  #include <string.h>
  
  #define ENABLE_DEBUG (0)
  #include "debug.h"
  
  int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr)
  {
      (void) attr;
  
      if (rwlock == NULL) {
          DEBUG("Thread %" PRIkernel_pid " pthread_rwlock_%s(): rwlock=NULL supplied\n", thread_pid, "init");
          return EINVAL;
      }
  
      memset(rwlock, 0, sizeof (*rwlock));
      return 0;
  }
  
  int pthread_rwlock_destroy(pthread_rwlock_t *rwlock)
  {
      if (rwlock == NULL) {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): rwlock=NULL supplied\n", thread_pid, "destroy");
          return EINVAL;
      }
  
      /* do not unlock the mutex, no need */
      if ((mutex_trylock(&rwlock->mutex) == 0) || (rwlock->readers != 0)) {
          return EBUSY;
      }
  
      return 0;
  }
  
  bool __pthread_rwlock_blocked_readingly(const pthread_rwlock_t *rwlock)
  {
      if (rwlock->readers < 0) {
          /* a writer holds the lock */
          return true;
      }
  
      /* Determine if there is a writer waiting to get this lock who has a higher or the same priority: */
  
      if (rwlock->queue.first == NULL) {
          /* no waiting thread */
          return false;
      }
  
      priority_queue_node_t *qnode = rwlock->queue.first;
      if (qnode->priority > sched_active_thread->priority) {
          /* the waiting thread has a lower priority */
          return false;
      }
  
      /* if the waiting node is a writer, then we cannot enter the critical section (to prevent starving the writer) */
      __pthread_rwlock_waiter_node_t *waiting_node = (__pthread_rwlock_waiter_node_t *) qnode->data;
      return waiting_node->is_writer;
  }
  
  bool __pthread_rwlock_blocked_writingly(const pthread_rwlock_t *rwlock)
  {
      /* if any thread holds the lock, then no writer may enter the critical section */
      return rwlock->readers != 0;
  }
  
  static int pthread_rwlock_lock(pthread_rwlock_t *rwlock,
                                 bool (*is_blocked)(const pthread_rwlock_t *rwlock),
                                 bool is_writer,
                                 int incr_when_held,
                                 bool allow_spurious)
  {
      if (rwlock == NULL) {
          DEBUG("Thread %" PRIkernel_pid": pthread_rwlock_%s(): is_writer=%u, allow_spurious=%u %s\n",
                thread_pid, "lock", is_writer, allow_spurious, "rwlock=NULL");
          return EINVAL;
      }
  
      mutex_lock(&rwlock->mutex);
      if (!is_blocked(rwlock)) {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): is_writer=%u, allow_spurious=%u %s\n",
                thread_pid, "lock", is_writer, allow_spurious, "is open");
          rwlock->readers += incr_when_held;
      }
      else {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): is_writer=%u, allow_spurious=%u %s\n",
                thread_pid, "lock", is_writer, allow_spurious, "is locked");
  
          /* queue for the lock */
          __pthread_rwlock_waiter_node_t waiting_node = {
              .is_writer = is_writer,
              .thread = (thread_t *) sched_active_thread,
              .qnode = {
                  .next = NULL,
                  .data = (uintptr_t) &waiting_node,
                  .priority = sched_active_thread->priority,
              },
              .continue_ = false,
          };
          priority_queue_add(&rwlock->queue, &waiting_node.qnode);
  
          while (1) {
              /* wait to be unlocked, so this thread can try to acquire the lock again */
              mutex_unlock_and_sleep(&rwlock->mutex);
  
              mutex_lock(&rwlock->mutex);
              if (waiting_node.continue_) {
                  /* pthread_rwlock_unlock() already set rwlock->readers */
                  DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): is_writer=%u, allow_spurious=%u %s\n",
                        thread_pid, "lock", is_writer, allow_spurious, "continued");
                  break;
              }
              else if (allow_spurious) {
                  DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): is_writer=%u, allow_spurious=%u %s\n",
                        thread_pid, "lock", is_writer, allow_spurious, "is timed out");
                  priority_queue_remove(&rwlock->queue, &waiting_node.qnode);
                  mutex_unlock(&rwlock->mutex);
                  return ETIMEDOUT;
              }
          }
      }
      mutex_unlock(&rwlock->mutex);
  
      return 0;
  }
  
  static int pthread_rwlock_trylock(pthread_rwlock_t *rwlock,
                                    bool (*is_blocked)(const pthread_rwlock_t *rwlock),
                                    int incr_when_held)
  {
      if (rwlock == NULL) {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): rwlock=NULL supplied\n", thread_pid, "trylock");
          return EINVAL;
      }
      else if (mutex_trylock(&rwlock->mutex) == 0) {
          return EBUSY;
      }
      else if (is_blocked(rwlock)) {
          mutex_unlock(&rwlock->mutex);
          return EBUSY;
      }
  
      rwlock->readers += incr_when_held;
  
      mutex_unlock(&rwlock->mutex);
      return 0;
  }
  
  static int pthread_rwlock_timedlock(pthread_rwlock_t *rwlock,
                                      bool (*is_blocked)(const pthread_rwlock_t *rwlock),
                                      bool is_writer,
                                      int incr_when_held,
                                      const struct timespec *abstime)
  {
      timex_t now, then;
  
      then.seconds = abstime->tv_sec;
      then.microseconds = abstime->tv_nsec / 1000u;
      timex_normalize(&then);
  
      xtimer_now_timex(&now);
  
      if (timex_cmp(then, now) <= 0) {
          return ETIMEDOUT;
      }
      else {
          timex_t reltime = timex_sub(then, now);
  
          xtimer_t timer;
          xtimer_set_wakeup64(&timer, timex_uint64(reltime) , sched_active_pid);
          int result = pthread_rwlock_lock(rwlock, is_blocked, is_writer, incr_when_held, true);
          if (result != ETIMEDOUT) {
              xtimer_remove(&timer);
          }
  
          return result;
      }
  }
  
  int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock)
  {
      return pthread_rwlock_lock(rwlock, __pthread_rwlock_blocked_readingly, false, +1, false);
  }
  
  int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock)
  {
      return pthread_rwlock_lock(rwlock, __pthread_rwlock_blocked_writingly, true, -1, false);
  }
  
  int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock)
  {
      return pthread_rwlock_trylock(rwlock, __pthread_rwlock_blocked_readingly, +1);
  }
  
  int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock)
  {
      return pthread_rwlock_trylock(rwlock, __pthread_rwlock_blocked_writingly, -1);
  }
  
  int pthread_rwlock_timedrdlock(pthread_rwlock_t *rwlock, const struct timespec *abstime)
  {
      return pthread_rwlock_timedlock(rwlock, __pthread_rwlock_blocked_readingly, false, +1, abstime);
  }
  
  int pthread_rwlock_timedwrlock(pthread_rwlock_t *rwlock, const struct timespec *abstime)
  {
      return pthread_rwlock_timedlock(rwlock, __pthread_rwlock_blocked_writingly, true, -1, abstime);
  }
  
  int pthread_rwlock_unlock(pthread_rwlock_t *rwlock)
  {
      if (rwlock == NULL) {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): rwlock=NULL supplied\n", thread_pid, "unlock");
          return EINVAL;
      }
  
      mutex_lock(&rwlock->mutex);
      if (rwlock->readers == 0) {
          /* the lock is open */
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): lock is open\n", thread_pid, "unlock");
          mutex_unlock(&rwlock->mutex);
          return EPERM;
      }
  
      if (rwlock->readers > 0) {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): release %s lock\n", thread_pid, "unlock", "read");
          --rwlock->readers;
      }
      else {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): release %s lock\n", thread_pid, "unlock", "write");
          rwlock->readers = 0;
      }
  
      if (rwlock->readers != 0 || rwlock->queue.first == NULL) {
          /* this thread was not the last reader, or no one is waiting to aquire the lock */
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): no one is waiting\n", thread_pid, "unlock");
          mutex_unlock(&rwlock->mutex);
          return 0;
      }
  
      /* wake up the next thread */
      priority_queue_node_t *qnode = priority_queue_remove_head(&rwlock->queue);
      __pthread_rwlock_waiter_node_t *waiting_node = (__pthread_rwlock_waiter_node_t *) qnode->data;
      waiting_node->continue_ = true;
      uint16_t prio = qnode->priority;
      sched_set_status(waiting_node->thread, STATUS_PENDING);
  
      if (waiting_node->is_writer) {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continue %s %" PRIkernel_pid "\n",
                thread_pid, "unlock", "writer", waiting_node->thread->pid);
          --rwlock->readers;
      }
      else {
          DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continue %s %" PRIkernel_pid "\n",
                thread_pid, "unlock", "reader", waiting_node->thread->pid);
          ++rwlock->readers;
  
          /* wake up further readers */
          while (rwlock->queue.first) {
              waiting_node = (__pthread_rwlock_waiter_node_t *) rwlock->queue.first->data;
              if (waiting_node->is_writer) {
                  /* Not to be unfair to writers, we don't try to wake up readers that came after the first writer. */
                  DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continuing readers blocked by writer %" PRIkernel_pid "\n",
                        thread_pid, "unlock", waiting_node->thread->pid);
                  break;
              }
              waiting_node->continue_ = true;
              DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continue %s %" PRIkernel_pid "\n",
                    thread_pid, "unlock", "reader", waiting_node->thread->pid);
  
              /* wake up this reader */
              qnode = priority_queue_remove_head(&rwlock->queue);
              if (qnode->priority < prio) {
                  prio = qnode->priority;
              }
              sched_set_status(waiting_node->thread, STATUS_PENDING);
  
              ++rwlock->readers;
          }
      }
  
      mutex_unlock(&rwlock->mutex);
  
      /* yield if a woken up thread had a higher priority */
      sched_switch(prio);
      return 0;
  }