#include <Arduino.h>
#include "esp32-hal-ledc.h"
#include "semaphore.h"
#include "driver/ledc.h"

#include "hardware.h"
#include "pusher.h"

using namespace std;

Pusher::Pusher(void) {
  xSemaphore = xSemaphoreCreateMutex();
}

void Pusher::setup(void) {
  int channel = LEDC_TIMER_0;
  int pin = 22;
  for(auto pin : SERVO_PINS) {
    if (xSemaphoreTake(xSemaphore, TIMEOUT) == pdTRUE) {
      ledcSetup(channel, 50, TIMER_WIDTH);
      ledcAttachPin(pin, channel);
      ledcWrite(channel, MAX_PULSE_WIDTH);
      ledcDetachPin(pin);
      xSemaphoreGive(xSemaphore);
    }
  }
}

void Pusher::push(int num) {
  if (xSemaphoreTake(xSemaphore, TIMEOUT) == pdTRUE) {
    int channel = 1;
    int pin = SERVO_PINS[num];
    if (channel >= LEDC_NUM_CHANNELS) {
      return;
    }

    ledcSetup(channel, 50, TIMER_WIDTH);
    ledcAttachPin(pin, channel);
    ledcWrite(channel, MIN_PULSE_WIDTH);
    delay(1000);
    ledcWrite(channel, MAX_PULSE_WIDTH);
    delay(1000);
    ledcDetachPin(pin);
    xSemaphoreGive(xSemaphore);
  } else {
    return;
  }
}