#pragma once
#include "freertos/semphr.h"
#include "freertos/portmacro.h"
#include "esp_log.h"

struct SemaphoreGuard {
public:

  SemaphoreGuard(SemaphoreHandle_t semaphore) : semaphore(semaphore) { }

  portBASE_TYPE take(TickType_t ticks_to_wait = portMAX_DELAY) {
    if (this->semaphore == nullptr) {
      ESP_LOGE("SemaphoreGuard", "Attempted to take a null semaphore");
      return pdFALSE;
    }
    portBASE_TYPE result = xSemaphoreTake(this->semaphore, ticks_to_wait);
    taken = (result == pdTRUE);
    return result;
  }

  ~SemaphoreGuard() {
    if (taken) {
      xSemaphoreGive(this->semaphore);
    }
  }

  // allow move semantics
  SemaphoreGuard(SemaphoreGuard&& other) noexcept
    : semaphore(other.semaphore), taken(other.taken) {
    other.taken = false;
  }
  SemaphoreGuard& operator=(SemaphoreGuard&& other) noexcept {
    if (this != &other) {
      // move from other
      taken = other.taken;
      other.taken = false;
      semaphore = other.semaphore;
      other.semaphore = nullptr;
    }
    return *this;
  }

private:
  // prevent copying
  SemaphoreGuard(const SemaphoreGuard&) = delete;
  SemaphoreGuard& operator=(const SemaphoreGuard&) = delete;
  SemaphoreHandle_t semaphore = nullptr;
  bool taken = false;
};