vision/main/robot_link.c

#include "robot_link.h"

#include <stdio.h>
#include <string.h>

#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"

static const char *TAG = "robot_link";

static SemaphoreHandle_t s_lock;
static uint32_t s_seq;

void robot_link_init(void)
{
    s_lock = xSemaphoreCreateMutex();
    s_seq = 0;
    ESP_LOGI(TAG, "ready (I2C bridge stub — local ACK only)");
}

static uint32_t next_seq(void)
{
    uint32_t seq = 0;
    if (xSemaphoreTake(s_lock, portMAX_DELAY) == pdTRUE) {
        seq = ++s_seq;
        xSemaphoreGive(s_lock);
    }
    return seq;
}

robot_cmd_dir_t robot_link_parse_dir(const char *dir)
{
    if (!dir) {
        return ROBOT_CMD_INVALID;
    }
    if (strcmp(dir, "up") == 0) {
        return ROBOT_CMD_UP;
    }
    if (strcmp(dir, "down") == 0) {
        return ROBOT_CMD_DOWN;
    }
    if (strcmp(dir, "left") == 0) {
        return ROBOT_CMD_LEFT;
    }
    if (strcmp(dir, "right") == 0) {
        return ROBOT_CMD_RIGHT;
    }
    if (strcmp(dir, "stop") == 0) {
        return ROBOT_CMD_STOP;
    }
    return ROBOT_CMD_INVALID;
}

/*
 * Placeholder for future RP2040 I2C write.
 * For now we only acknowledge that the ESP32 received the command.
 */
static esp_err_t forward_to_rp2040_stub(const char *kind, const char *payload)
{
    ESP_LOGI(TAG, "ACK %s '%s' (no I2C yet)", kind, payload ? payload : "");
    return ESP_OK;
}

esp_err_t robot_link_handle_cmd(const char *dir, robot_ack_t *ack)
{
    if (!ack) {
        return ESP_ERR_INVALID_ARG;
    }

    memset(ack, 0, sizeof(*ack));
    ack->type = "cmd";
    ack->seq = next_seq();
    ack->dir = dir ? dir : "";

    if (robot_link_parse_dir(dir) == ROBOT_CMD_INVALID) {
        ack->ok = false;
        ack->fwd = "none";
        ack->error = "bad_dir";
        ESP_LOGW(TAG, "reject cmd seq=%lu dir='%s'", (unsigned long)ack->seq,
                 ack->dir);
        return ESP_ERR_INVALID_ARG;
    }

    esp_err_t err = forward_to_rp2040_stub("cmd", dir);
    if (err != ESP_OK) {
        ack->ok = false;
        ack->fwd = "fail";
        ack->error = "forward_failed";
        return err;
    }

    ack->ok = true;
    ack->fwd = "ack";
    return ESP_OK;
}

esp_err_t robot_link_handle_msg(const char *text, size_t len, robot_ack_t *ack)
{
    if (!ack || !text) {
        return ESP_ERR_INVALID_ARG;
    }

    memset(ack, 0, sizeof(*ack));
    ack->type = "msg";
    ack->seq = next_seq();
    ack->text = text;

    if (len == 0) {
        ack->ok = false;
        ack->fwd = "none";
        ack->error = "empty";
        return ESP_ERR_INVALID_ARG;
    }

    esp_err_t err = forward_to_rp2040_stub("msg", text);
    if (err != ESP_OK) {
        ack->ok = false;
        ack->fwd = "fail";
        ack->error = "forward_failed";
        return err;
    }

    ack->ok = true;
    ack->fwd = "ack";
    return ESP_OK;
}

static void json_escape_into(const char *in, char *out, size_t outlen)
{
    size_t o = 0;
    if (!in || outlen == 0) {
        return;
    }
    for (size_t i = 0; in[i] != '\0' && o + 2 < outlen; i++) {
        char c = in[i];
        if (c == '"' || c == '\\') {
            if (o + 3 >= outlen) {
                break;
            }
            out[o++] = '\\';
            out[o++] = c;
        } else if ((unsigned char)c < 0x20) {
            /* skip control chars */
            continue;
        } else {
            out[o++] = c;
        }
    }
    out[o] = '\0';
}

int robot_link_format_ack(const robot_ack_t *ack, char *buf, size_t buflen)
{
    if (!ack || !buf || buflen < 16) {
        return -1;
    }

    char esc[256];
    if (ack->type && strcmp(ack->type, "msg") == 0) {
        json_escape_into(ack->text ? ack->text : "", esc, sizeof(esc));
        if (ack->ok) {
            return snprintf(buf, buflen,
                            "{\"ok\":true,\"seq\":%lu,\"type\":\"msg\","
                            "\"text\":\"%s\",\"fwd\":\"%s\"}",
                            (unsigned long)ack->seq, esc,
                            ack->fwd ? ack->fwd : "ack");
        }
        return snprintf(buf, buflen,
                        "{\"ok\":false,\"seq\":%lu,\"type\":\"msg\","
                        "\"error\":\"%s\",\"fwd\":\"%s\"}",
                        (unsigned long)ack->seq,
                        ack->error ? ack->error : "error",
                        ack->fwd ? ack->fwd : "none");
    }

    json_escape_into(ack->dir ? ack->dir : "", esc, sizeof(esc));
    if (ack->ok) {
        return snprintf(buf, buflen,
                        "{\"ok\":true,\"seq\":%lu,\"type\":\"cmd\","
                        "\"dir\":\"%s\",\"fwd\":\"%s\"}",
                        (unsigned long)ack->seq, esc,
                        ack->fwd ? ack->fwd : "ack");
    }
    return snprintf(buf, buflen,
                    "{\"ok\":false,\"seq\":%lu,\"type\":\"cmd\","
                    "\"dir\":\"%s\",\"error\":\"%s\",\"fwd\":\"%s\"}",
                    (unsigned long)ack->seq, esc,
                    ack->error ? ack->error : "error",
                    ack->fwd ? ack->fwd : "none");
}