text: /*
   * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
   *
   * SPDX-License-Identifier: CC0-1.0
   */
  
  /*
   * This file contains an implementation of step motor middleware based on rmt peripheral
   */
  
  #include <math.h>
  #include "freertos/FreeRTOS.h"
  #include "freertos/semphr.h"
  #include "esp_log.h"
  #include "esp_check.h"
  #include "driver/rmt.h"
  #include "step_motor.h"
  
  static const char *TAG = "RMT_STEP_MOTOR";
  
  typedef enum {
      STOPPED = 0,
      SMOOTH_SPEED_UP,
      SMOOTH_KEEP_SPEED,
      SMOOTH_SLOW_DOWN,
      UNLIMITED_LOOP,
      LIMITED_LOOP,
  } rmt_step_motor_running_status;
  
  typedef struct {
      step_motor_t base;
      step_motor_driver_io_t *io_driver;
      rmt_channel_t rmt_ch;
      rmt_step_motor_running_status status;
      rmt_item32_t rmt_items_loop;
      uint32_t rmt_items_loop_count;
      rmt_item32_t *rmt_items_speedup;
      rmt_item32_t *rmt_items_speeddown;
      uint32_t rmt_items_smoothstep_count;
      SemaphoreHandle_t notify_semphr;
  } rmt_step_motor_t;
  
  static inline float helper_smootherstep_clamp(float x, float lowerlimit, float upperlimit)
  {
      if (x < lowerlimit) {
          x = lowerlimit;
      }
      if (x > upperlimit) {
          x = upperlimit;
      }
      return x;
  }
  
  // smoothstep formula
  // see https://en.wikipedia.org/wiki/Smoothstep
  static float helper_smootherstep(float edge0, float edge1, float x)
  {
      // Scale, and clamp x to 0..1 range
      x = helper_smootherstep_clamp((x - edge0) / (edge1 - edge0), 0.0, 1.0);
      // Evaluate polynomial
      return x * x * x * (x * (x * 6 - 15) + 10) * (edge1 - edge0) + edge0;
  }
  
  static uint16_t helper_speed_to_duration(uint16_t speed)
  {
      return (uint16_t) round(1.0 * 1000 * 1000 / speed);
  }
  
  static esp_err_t helper_fill_rmt_items(rmt_item32_t *items, uint32_t speed, const step_motor_driver_io_t *io_driver)
  {
      items->duration1 = io_driver->step_triggered_edge ? io_driver->pulse_high_period_us : io_driver->pulse_low_period_us;
      items->level1 = io_driver->step_triggered_edge;
      items->level0 = !io_driver->step_triggered_edge;
      uint32_t delay_period = helper_speed_to_duration(speed);
      if (delay_period <= (io_driver->step_triggered_edge ? io_driver->pulse_low_period_us : io_driver->pulse_high_period_us)) {
          ESP_LOGW(TAG, "maximum rate reached, driver will generate another possible highest rate instead");
          items->duration0 = io_driver->step_triggered_edge ? io_driver->pulse_low_period_us : io_driver->pulse_high_period_us;
      } else {
          items->duration0 = delay_period;
      }
      return ESP_OK;
  }
  
  static esp_err_t rmt_step_motor_init(step_motor_t *motor)
  {
      rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
      step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
      if (io_driver->init) {
          return io_driver->init(io_driver);
      }
      return ESP_OK;
  }
  
  static esp_err_t rmt_step_motor_deinit(step_motor_t *motor)
  {
      rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
      step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
      if (io_driver->deinit) {
          return io_driver->deinit(io_driver);
      }
      return ESP_OK;
  }
  
  // assume n != 0 and speed is within considerable range
  static esp_err_t rmt_step_motor_step_impl(step_motor_t *motor, uint32_t n, uint32_t speed)
  {
      rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
  
      ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, true));
      ESP_ERROR_CHECK(rmt_enable_tx_loop_autostop(rmt_handle->rmt_ch, true));
  
      rmt_handle->rmt_items_loop_count = n;
      if ((rmt_handle->rmt_items_loop_count) > 1023) {
          (rmt_handle->rmt_items_loop_count) -= 1023;
          ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, 1023));
      } else {
          ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, rmt_handle->rmt_items_loop_count));
          rmt_handle->rmt_items_loop_count = 0;
      }
      helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed, rmt_handle->io_driver);
  
      rmt_handle->status = LIMITED_LOOP;
  
      rmt_write_items(rmt_handle->rmt_ch, &rmt_handle->rmt_items_loop, 1, false);
      xSemaphoreTake(rmt_handle->notify_semphr, portMAX_DELAY);
      return ESP_OK;
  }
  
  static esp_err_t rmt_step_motor_step(step_motor_t *handle, uint32_t n, uint32_t speed)
  {
      rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
  
      ESP_ERROR_CHECK(rmt_tx_stop(rmt_handle->rmt_ch));
  
      if (n == UINT32_MAX) {   // forever loop, non-blocking
          ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, 0));
          ESP_ERROR_CHECK(rmt_enable_tx_loop_autostop(rmt_handle->rmt_ch, false));
          ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, true));
          helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed, rmt_handle->io_driver);
          rmt_handle->status = UNLIMITED_LOOP;
          ESP_ERROR_CHECK(rmt_write_items(rmt_handle->rmt_ch, &rmt_handle->rmt_items_loop, 1, false));
          return ESP_OK;
      } else if (n == 0) {     // break the forever loop
          rmt_handle->status = STOPPED;
          ESP_ERROR_CHECK(rmt_tx_stop(rmt_handle->rmt_ch));
          ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, false));
          return ESP_OK;
      } else {                 // normally move n steps
          ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed) > 1, ESP_ERR_INVALID_ARG, TAG,
                              "speed too fast");
          return rmt_step_motor_step_impl(handle, n, speed);
      }
  }
  
  static esp_err_t rmt_step_motor_smoothstep(step_motor_t *handle, uint32_t n, uint32_t speed_steps, uint32_t speed_min,
          uint32_t speed_max)
  {
      esp_err_t ret = ESP_OK;
      ESP_RETURN_ON_FALSE(speed_min <= speed_max, ESP_ERR_INVALID_ARG, TAG, "max speed lower than min speed");
      ESP_RETURN_ON_FALSE(n > speed_steps * 2, ESP_ERR_INVALID_ARG, TAG, "too few steps. consider lower speed_steps");
      ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed_min) < 1 << 15, ESP_ERR_INVALID_ARG, TAG, "min speed too low");
      ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed_max) > 1, ESP_ERR_INVALID_ARG, TAG, "max speed too high");
  
      rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
      rmt_handle->rmt_items_speedup = malloc(sizeof(rmt_item32_t) * speed_steps);
      ESP_RETURN_ON_FALSE(rmt_handle->rmt_items_speedup != NULL, ESP_ERR_NO_MEM, TAG,
                          "failed to allocate rmt_items_speedup");
      rmt_handle->rmt_items_speeddown = malloc(sizeof(rmt_item32_t) * speed_steps);
      ESP_GOTO_ON_FALSE(rmt_handle->rmt_items_speeddown != NULL, ESP_ERR_NO_MEM, err_free_speedup, TAG,
                        "failed to allocate rmt_items_speeddown");
      ESP_GOTO_ON_ERROR(rmt_tx_stop(rmt_handle->rmt_ch), err_free_speeddown, TAG, "failed to stop rmt tx");
  
      // prepare speed tables
      for (int i = 0; i < speed_steps; ++i) {
          helper_fill_rmt_items(&rmt_handle->rmt_items_speedup[i],
                                (uint16_t)helper_smootherstep(
                                    (float)speed_min,
                                    (float)speed_max,
                                    (float)speed_min + ( (float)i / (float)speed_steps) * (float)(speed_max - speed_min))
                                , rmt_handle->io_driver
                               );
      }
      for (int i = 0; i < speed_steps; ++i) {
          helper_fill_rmt_items(&rmt_handle->rmt_items_speeddown[i],
                                speed_max + speed_min - (uint16_t)helper_smootherstep(
                                    (float)speed_min,
                                    (float)speed_max,
                                    (float)speed_min + ((float) i / (float)speed_steps) * (float)(speed_max - speed_min)
                                )
                                , rmt_handle->io_driver
                               );
      }
      rmt_handle->rmt_items_smoothstep_count = speed_steps;
      // prepare continuous phase rmt payload
      helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed_max, rmt_handle->io_driver);
      rmt_handle->rmt_items_loop_count = n - speed_steps * 2;
      // set status to be checked inside ISR
      rmt_handle->status = SMOOTH_SPEED_UP;
      // start transmitting
      ESP_ERROR_CHECK(rmt_write_items(rmt_handle->rmt_ch, rmt_handle->rmt_items_speedup, speed_steps, false));
  
      // waiting for transfer done
      xSemaphoreTake(rmt_handle->notify_semphr, portMAX_DELAY);
  
  err_free_speeddown:
      free(rmt_handle->rmt_items_speeddown);
  err_free_speedup:
      free(rmt_handle->rmt_items_speedup);
      return ret;
  }
  
  static esp_err_t rmt_step_motor_set_step(step_motor_t *handle, uint16_t microstep, bool direction)
  {
      rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
      step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
      if (io_driver->set_direction) {
          ESP_ERROR_CHECK(io_driver->set_direction(io_driver, direction));
      }
      if (io_driver->set_microstep) {
          ESP_ERROR_CHECK(io_driver->set_microstep(io_driver, microstep));
      }
      // at least 200ns delay as described in datasheet
      esp_rom_delay_us(1);
      return ESP_OK;
  }
  
  static IRAM_ATTR void rmt_tx_loop_intr(rmt_channel_t channel, void *args)
  {
      rmt_step_motor_t *rmt_step_motor = (rmt_step_motor_t *) args;
  
      // smoothstep speedup stage finished
      if (rmt_step_motor->status == SMOOTH_SPEED_UP) {
          rmt_step_motor->status = SMOOTH_KEEP_SPEED;
          rmt_set_tx_loop_mode(rmt_step_motor->rmt_ch, true);
          rmt_enable_tx_loop_autostop(rmt_step_motor->rmt_ch, true);
          rmt_set_tx_intr_en(rmt_step_motor->rmt_ch, 0);
          // continue and configure loop count
      }
  
      if (rmt_step_motor->status == SMOOTH_KEEP_SPEED || rmt_step_motor->status == LIMITED_LOOP) {
          // loop count not 0, continuing looping
          if ((rmt_step_motor->rmt_items_loop_count) != 0) {
              if ((rmt_step_motor->rmt_items_loop_count) > 1023) {
                  (rmt_step_motor->rmt_items_loop_count) -= 1023;
                  rmt_set_tx_loop_count(rmt_step_motor->rmt_ch, 1023);
              } else {
                  rmt_set_tx_loop_count(rmt_step_motor->rmt_ch, rmt_step_motor->rmt_items_loop_count);
                  rmt_step_motor->rmt_items_loop_count = 0;
              }
              rmt_write_items(rmt_step_motor->rmt_ch, &rmt_step_motor->rmt_items_loop, 1, false);
              return;
          }
      }
  
      // smoothstep keep speed stage finished
      if (rmt_step_motor->status == SMOOTH_KEEP_SPEED) {
          rmt_step_motor->status = SMOOTH_SLOW_DOWN;
          rmt_set_tx_loop_mode(rmt_step_motor->rmt_ch, false);
          rmt_enable_tx_loop_autostop(rmt_step_motor->rmt_ch, false);
          rmt_set_tx_intr_en(rmt_step_motor->rmt_ch, 1);
          rmt_write_items(rmt_step_motor->rmt_ch, rmt_step_motor->rmt_items_speeddown, rmt_step_motor->rmt_items_smoothstep_count, false);
          return;
      }
  
      if (rmt_step_motor->status == LIMITED_LOOP || rmt_step_motor->status == SMOOTH_SLOW_DOWN) {
          rmt_step_motor->status = STOPPED;
          BaseType_t xHigherPriorityTaskWoken = pdFALSE;
          xSemaphoreGiveFromISR(rmt_step_motor->notify_semphr, &xHigherPriorityTaskWoken);
          if (xHigherPriorityTaskWoken == pdTRUE) {
              portYIELD_FROM_ISR();
          }
      }
  }
  
  esp_err_t step_motor_create_rmt(step_motor_driver_io_t *io_driver, const rmt_config_t *rmt_conf, step_motor_handle_t *ret_handle)
  {
      esp_err_t ret = ESP_OK;
      rmt_step_motor_t *rmt_step_motor = NULL;
  
      ESP_RETURN_ON_ERROR(rmt_config(rmt_conf), TAG, "Failed to configure RMT");
      ESP_RETURN_ON_ERROR(rmt_driver_install(rmt_conf->channel, 0, 0), TAG, "Failed to install RMT driver");
  
      ESP_GOTO_ON_FALSE(io_driver, ESP_ERR_INVALID_ARG, err, TAG, "configuration can't be null");
      ESP_GOTO_ON_FALSE(ret_handle, ESP_ERR_INVALID_ARG, err, TAG, "can't assign handle to null");
  
      rmt_step_motor = calloc(1, sizeof(rmt_step_motor_t));
      ESP_GOTO_ON_FALSE(rmt_step_motor, ESP_ERR_NO_MEM, err, TAG, "allocate context memory failed");
      rmt_step_motor->rmt_ch = rmt_conf->channel;
  
      rmt_step_motor->notify_semphr = xSemaphoreCreateBinary();
      ESP_GOTO_ON_FALSE(rmt_step_motor, ESP_ERR_NO_MEM, err, TAG, "allocate semaphore memory failed");
  
      rmt_step_motor->io_driver = io_driver;
  
      // register tx end callback function, which got invoked when tx loop comes to the end
      rmt_register_tx_end_callback(rmt_tx_loop_intr, rmt_step_motor);
  
      rmt_step_motor->base.init = rmt_step_motor_init;
      rmt_step_motor->base.deinit = rmt_step_motor_deinit;
      rmt_step_motor->base.step = rmt_step_motor_step;
      rmt_step_motor->base.set_step = rmt_step_motor_set_step;
      rmt_step_motor->base.smooth_step = rmt_step_motor_smoothstep;
  
      *ret_handle = &(rmt_step_motor->base);
      return ESP_OK;
  
  err:
      if (rmt_step_motor) {
          if (rmt_step_motor->notify_semphr) {
              vSemaphoreDelete(rmt_step_motor->notify_semphr);
          }
          free(rmt_step_motor);
      }
      return ret;
  }
  
  esp_err_t step_motor_delete_rmt(step_motor_handle_t handle)
  {
      ESP_RETURN_ON_FALSE(handle, ESP_ERR_INVALID_STATE, TAG, "empty handle");
      rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
      ESP_RETURN_ON_ERROR(rmt_driver_uninstall(rmt_handle->rmt_ch), TAG, "Failed to uninstall RMT driver");
      vSemaphoreDelete(rmt_handle->notify_semphr);
      free(rmt_handle);
      return ESP_OK;
  }
  