stepper_base/components/direction_controller/direction_controller.c

/*
 * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <ctype.h>
#include <string.h>
#include "esp_log.h"
#include "stepper_motor.h"
#include "direction_controller.h"

static const char *TAG = "Direction Controller";

/* Default direction list (South = 0°, clockwise positive) */
static const direction_controller_direction_t s_default_directions[] = {
    {"南",   "S",   0.0f},
    {"西南", "SW",  45.0f},
    {"西",   "W",   90.0f},
    {"西北", "NW",  135.0f},
    {"北",   "N",   180.0f},
    {"东北", "NE",  -135.0f},
    {"东",   "E",   -90.0f},
    {"东南", "SE",  -45.0f},
};

static const direction_controller_direction_t *s_directions = s_default_directions;
static size_t s_direction_count = sizeof(s_default_directions) / sizeof(s_default_directions[0]);
static float s_current_angle = 0.0f;
static int s_rotate_speed_us = STEPPER_SPEED_FAST;
static bool s_initialized = false;

static void trimmed_span(const char *input, const char **start_out, size_t *len_out)
{
    const char *start = input;
    const char *end = NULL;

    if (input == NULL) {
        *start_out = NULL;
        *len_out = 0;
        return;
    }

    while (*start != '\0' && isspace((unsigned char)*start)) {
        start++;
    }

    end = start + strlen(start);
    while (end > start && isspace((unsigned char)*(end - 1))) {
        end--;
    }

    *start_out = start;
    *len_out = (size_t)(end - start);
}

static bool equals_trimmed(const char *input, const char *token, bool case_insensitive)
{
    const char *start = NULL;
    size_t len = 0;
    size_t token_len = 0;

    if (input == NULL || token == NULL) {
        return false;
    }

    trimmed_span(input, &start, &len);
    token_len = strlen(token);

    if (len == 0 || len != token_len) {
        return false;
    }

    for (size_t i = 0; i < len; i++) {
        char a = start[i];
        char b = token[i];

        if (case_insensitive) {
            a = (char)tolower((unsigned char)a);
            b = (char)tolower((unsigned char)b);
        }

        if (a != b) {
            return false;
        }
    }

    return true;
}

static float calculate_shortest_rotation(float current, float target)
{
    float diff = target - current;

    while (diff > 180.0f) {
        diff -= 360.0f;
    }
    while (diff < -180.0f) {
        diff += 360.0f;
    }

    return diff;
}

void direction_controller_init(const direction_controller_config_t *config)
{
    const direction_controller_direction_t *directions = s_default_directions;
    size_t direction_count = sizeof(s_default_directions) / sizeof(s_default_directions[0]);
    float initial_angle = 0.0f;
    int rotate_speed_us = STEPPER_SPEED_FAST;
    bool init_stepper_gpio = true;

    if (config != NULL) {
        if (config->directions != NULL && config->direction_count > 0) {
            directions = config->directions;
            direction_count = config->direction_count;
        }
        initial_angle = config->initial_angle;
        if (config->rotate_speed_us > 0) {
            rotate_speed_us = config->rotate_speed_us;
        }
        init_stepper_gpio = config->init_stepper_gpio;
    }

    s_directions = directions;
    s_direction_count = direction_count;
    s_current_angle = initial_angle;
    s_rotate_speed_us = rotate_speed_us;

    if (init_stepper_gpio) {
        stepper_motor_gpio_init();
        stepper_motor_power_off();
    }
    s_initialized = true;

    ESP_LOGI(TAG, "Direction controller initialized (angle=%.1f, speed=%dus)",
             s_current_angle, s_rotate_speed_us);
}

direction_controller_cmd_t direction_controller_parse_command(const char *input,
        const direction_controller_direction_t **out_direction)
{
    if (out_direction != NULL) {
        *out_direction = NULL;
    }

    if (equals_trimmed(input, "help", true) || equals_trimmed(input, "?", false)) {
        return DIRECTION_CONTROLLER_CMD_HELP;
    }

    if (equals_trimmed(input, "pos", true)) {
        return DIRECTION_CONTROLLER_CMD_POSITION;
    }

    const direction_controller_direction_t *direction = direction_controller_find_direction(input);
    if (direction != NULL) {
        if (out_direction != NULL) {
            *out_direction = direction;
        }
        return DIRECTION_CONTROLLER_CMD_DIRECTION;
    }

    return DIRECTION_CONTROLLER_CMD_UNKNOWN;
}

const direction_controller_direction_t *direction_controller_find_direction(const char *input)
{
    if (input == NULL) {
        return NULL;
    }

    for (size_t i = 0; i < s_direction_count; i++) {
        if (equals_trimmed(input, s_directions[i].name_cn, false) ||
            equals_trimmed(input, s_directions[i].name_en, true)) {
            return &s_directions[i];
        }
    }

    return NULL;
}

esp_err_t direction_controller_rotate_to_angle(float target_angle)
{
    if (!s_initialized) {
        direction_controller_init(NULL);
    }

    float rotation = calculate_shortest_rotation(s_current_angle, target_angle);

    if (rotation == 0.0f) {
        ESP_LOGI(TAG, "Already at target position");
        return ESP_OK;
    }

    ESP_LOGI(TAG, "Rotating from %.1f° to %.1f° (rotation: %.1f°)",
             s_current_angle, target_angle, rotation);

    stepper_rotate_angle_with_accel(rotation, s_rotate_speed_us);
    s_current_angle = target_angle;
    stepper_motor_power_off();

    ESP_LOGI(TAG, "Rotation complete, current position: %.1f°", s_current_angle);
    return ESP_OK;
}

esp_err_t direction_controller_rotate_to_direction(const char *input)
{
    const direction_controller_direction_t *direction = direction_controller_find_direction(input);

    if (direction == NULL) {
        return ESP_ERR_NOT_FOUND;
    }

    return direction_controller_rotate_to_angle(direction->angle);
}

float direction_controller_get_current_angle(void)
{
    return s_current_angle;
}

void direction_controller_set_current_angle(float angle)
{
    s_current_angle = angle;
}

const direction_controller_direction_t *direction_controller_get_current_direction(void)
{
    for (size_t i = 0; i < s_direction_count; i++) {
        if (s_directions[i].angle == s_current_angle) {
            return &s_directions[i];
        }
    }

    return NULL;
}

size_t direction_controller_get_direction_count(void)
{
    return s_direction_count;
}

const direction_controller_direction_t *direction_controller_get_direction(size_t index)
{
    if (index >= s_direction_count) {
        return NULL;
    }

    return &s_directions[index];
}

int direction_controller_get_rotate_speed_us(void)
{
    return s_rotate_speed_us;
}

void direction_controller_set_rotate_speed_us(int rotate_speed_us)
{
    if (rotate_speed_us > 0) {
        s_rotate_speed_us = rotate_speed_us;
    }
}