#include <Arduino.h>
#include "esp32-hal-ledc.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"

static TaskHandle_t _tone_task = NULL;
static QueueHandle_t _tone_queue = NULL;
static int8_t _pin = -1;

typedef enum{
  TONE_START,
  TONE_END
} tone_cmd_t;

typedef struct{
  tone_cmd_t tone_cmd;
  uint8_t pin;
  unsigned int frequency;
  unsigned long duration;
} tone_msg_t;

static void tone_task(void*){
  tone_msg_t tone_msg;
  while(1){
    xQueueReceive(_tone_queue, &tone_msg, portMAX_DELAY);
    switch(tone_msg.tone_cmd){
      case TONE_START:
        log_d("Task received from queue TONE_START: pin=%d, frequency=%u Hz, duration=%lu ms", tone_msg.pin, tone_msg.frequency, tone_msg.duration);

        if (_pin == -1) {
          if (ledcAttach(tone_msg.pin, tone_msg.frequency, 10) == 0) {
              log_e("Tone start failed");
              break;
          }
          _pin = tone_msg.pin;
        }
        ledcWriteTone(tone_msg.pin, tone_msg.frequency);

        if(tone_msg.duration){
          delay(tone_msg.duration);
          ledcWriteTone(tone_msg.pin, 0);
        }
        break;

      case TONE_END:
        log_d("Task received from queue TONE_END: pin=%d", tone_msg.pin);
        ledcWriteTone(tone_msg.pin, 0);
        ledcDetach(tone_msg.pin);
        _pin = -1;
        break;

      default: ; // do nothing
    } // switch
  } // infinite loop
}

static int tone_init(){
  if(_tone_queue == NULL){
    log_v("Creating tone queue");
    _tone_queue = xQueueCreate(128, sizeof(tone_msg_t));
    if(_tone_queue == NULL){
      log_e("Could not create tone queue");
      return 0; // ERR
    }
    log_v("Tone queue created");
  }

  if(_tone_task == NULL){
    log_v("Creating tone task");
    xTaskCreate(
      tone_task, // Function to implement the task
      "toneTask", // Name of the task
      3500,  // Stack size in words
      NULL,  // Task input parameter
      1,  // Priority of the task
      &_tone_task  // Task handle.
      );
    if(_tone_task == NULL){
      log_e("Could not create tone task");
      return 0; // ERR
    }
    log_v("Tone task created");
  }
  return 1; // OK
}

void noTone(uint8_t pin){
  log_d("noTone was called");
  if(_pin == pin) {
    if(tone_init()){
      tone_msg_t tone_msg = {
        .tone_cmd = TONE_END,
        .pin = pin,
        .frequency = 0, // Ignored
        .duration = 0, // Ignored
      };
      xQueueReset(_tone_queue); // clear queue
      xQueueSend(_tone_queue, &tone_msg, portMAX_DELAY);
    }
  }
  else {
    log_e("Tone is not running on given pin %d", pin);
  }
}

// parameters:
// pin - pin number which will output the signal
// frequency - PWM frequency in Hz
// duration - time in ms - how long will the signal be outputted.
//   If not provided, or 0 you must manually call noTone to end output
void tone(uint8_t pin, unsigned int frequency, unsigned long duration){
  log_d("pin=%d, frequency=%u Hz, duration=%lu ms", pin, frequency, duration);
  if(_pin == -1 || _pin == pin) {
    if(tone_init()){
      tone_msg_t tone_msg = {
        .tone_cmd = TONE_START,
        .pin = pin,
        .frequency = frequency,
        .duration = duration,
      };
      xQueueSend(_tone_queue, &tone_msg, portMAX_DELAY);
      return;
    }
  }
  else {
    log_e("Tone is still running on pin %d, call noTone(%d) first!", _pin, _pin);
    return;
  }
}
