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

struct SemaphoreGuard {
public:
  const SemaphoreHandle_t semaphore;

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

  portBASE_TYPE take(TickType_t ticks_to_wait = portMAX_DELAY) {
    portBASE_TYPE result = xSemaphoreTake(this->semaphore, ticks_to_wait);
    taken = (result == pdTRUE);
    return result;
  }

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

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