celrs/celrs/crsf_param.c

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

#include "celrs/crsf_param.h"

/* --------------------------------------------------------------------------- */
/* Helpers for power matching                                                 */
/* --------------------------------------------------------------------------- */

/* Case-insensitive string contains */
static int str_contains_ci(char const* haystack, char const* needle) {
    size_t hlen = strlen(haystack);
    size_t nlen = strlen(needle);
    if (nlen > hlen) return 0;
    for (size_t i = 0; i <= hlen - nlen; i++) {
        int match = 1;
        for (size_t j = 0; j < nlen; j++) {
            if (tolower((unsigned char)haystack[i + j])
                != tolower((unsigned char)needle[j]))
            {
                match = 0;
                break;
            }
        }
        if (match) return 1;
    }
    return 0;
}

/* Check if param name contains "power" (case-insensitive) */
static int is_power_param(cel_crsf_param const* param) {
    return str_contains_ci(param->name, "power");
}

/* Match requested power (mW) against a TEXT_SELECT option string.
 * Returns option index (0-based) or -1 if no match. */
static int match_power_option(cel_crsf_param const* param, int mw) {
    char const* opts = param->options;
    char opt_buf[64];
    int index = 0;

    while (*opts) {
        /* Extract option up to ';' or '\0' */
        size_t i = 0;
        while (*opts && *opts != ';' && i < sizeof(opt_buf) - 1) {
            opt_buf[i++] = *opts++;
        }
        opt_buf[i] = '\0';
        if (*opts == ';') opts++; /* skip separator */

        /* Check if option starts with the requested mW value */
        char req_buf[16];
        sprintf(req_buf, "%d", mw);
        if (str_contains_ci(opt_buf, req_buf)) {
            return index;
        }
        index++;
    }
    return -1;
}

/* --------------------------------------------------------------------------- */
/* Public API                                                                 */
/* --------------------------------------------------------------------------- */

int cel_crsf_param_ping(cel_serial_port* port, float timeout_sec) {
    if (port == NULL) return -1;

    /* Send ping frame */
    uint8_t frame[16];
    size_t len = cel_crsf_build_ping_frame(frame);
    if (len == 0) return -1;

    size_t written = cel_serial_write(port, frame, len);
    if (written != len) return -1;

    /* Wait for DEVICE_INFO response */
    cel_crsf_stream* stream = cel_crsf_stream_create();
    if (stream == NULL) return -1;

    clock_t start = clock();
    clock_t limit = (clock_t)(timeout_sec * CLOCKS_PER_SEC);

    while ((clock() - start) < limit) {
        uint8_t buf[256];
        size_t n = cel_serial_read(port, buf, sizeof(buf));
        if (n > 0) {
            cel_crsf_frame frames[4];
            int count = cel_crsf_stream_feed(stream, buf, n, frames, sizeof(frames) / sizeof(frames[0]));
            if (count > 0) {
                for (int i = 0; i < count; i++) {
                    if (frames[i].type == CEL_CRSF_TYPE_DEVICE_INFO) {
                        cel_crsf_stream_destroy(stream);
                        return 0;
                    }
                }
            }
        }
    }

    cel_crsf_stream_destroy(stream);
    return -1;
}

int cel_crsf_param_read(cel_serial_port* port, uint8_t index,
                         cel_crsf_param* out, float timeout_sec) {
    if (port == NULL || out == NULL) return -1;

    /* Send param read frame */
    uint8_t frame[16];
    size_t len = cel_crsf_build_param_read_frame(frame, index, 0);
    if (len == 0) return -1;

    size_t written = cel_serial_write(port, frame, len);
    if (written != len) return -1;

    /* Wait for PARAM_ENTRY response with matching index */
    cel_crsf_stream* stream = cel_crsf_stream_create();
    if (stream == NULL) return -1;

    clock_t start = clock();
    clock_t limit = (clock_t)(timeout_sec * CLOCKS_PER_SEC);

    while ((clock() - start) < limit) {
        uint8_t buf[256];
        size_t n = cel_serial_read(port, buf, sizeof(buf));
        if (n > 0) {
            cel_crsf_frame frames[4];
            int count = cel_crsf_stream_feed(stream, buf, n, frames, sizeof(frames) / sizeof(frames[0]));
            if (count > 0) {
                for (int i = 0; i < count; i++) {
                    if (frames[i].type == CEL_CRSF_TYPE_PARAM_ENTRY) {
                        if (cel_crsf_param_parse(out, frames[i].payload,
                                                  frames[i].payload_len) == 0)
                        {
                            if (out->index == index) {
                                cel_crsf_stream_destroy(stream);
                                return 0;
                            }
                        }
                    }
                }
            }
        }
    }

    cel_crsf_stream_destroy(stream);
    return -1;
}

int cel_crsf_param_write(cel_serial_port* port, uint8_t index,
                          uint8_t value) {
    if (port == NULL) return -1;

    uint8_t frame[16];
    size_t len = cel_crsf_build_param_write_frame(frame, index, value);
    if (len == 0) return -1;

    size_t written = cel_serial_write(port, frame, len);
    return (written == len) ? 0 : -1;
}

int cel_crsf_param_set_power(cel_serial_port* port, int mw,
                              float timeout_sec) {
    if (port == NULL) return -1;

    /* 1. Ping to verify connection */
    if (cel_crsf_param_ping(port, timeout_sec) != 0) return -1;

    /* 2. Enumerate params until power entry is found */
    for (uint8_t idx = 0; idx < 32; idx++) {
        cel_crsf_param param;
        if (cel_crsf_param_read(port, idx, &param, timeout_sec) != 0) {
            /* No response = end of parameter list */
            break;
        }

        /* Check if this is the power parameter */
        if (is_power_param(&param) && param.type == CEL_PARAM_TEXT_SELECT) {
            /* Find matching option */
            int opt_index = match_power_option(&param, mw);
            if (opt_index >= 0) {
                /* Write the selected option */
                return cel_crsf_param_write(port, idx, (uint8_t)opt_index);
            }
        }
    }

    /* 3. Power param not found */
    return -1;
}

int cel_crsf_param_parse(cel_crsf_param* out, uint8_t const* payload,
                          size_t len) {
    if (out == NULL || payload == NULL) return -1;
    if (len < 6) return -1;

    memset(out, 0, sizeof(*out));

    size_t offset = 0;
    (void)payload[offset++]; /* dest */
    (void)payload[offset++]; /* src */
    out->index = payload[offset++];
    (void)payload[offset++]; /* chunks_remaining */
    (void)payload[offset++]; /* parent */
    uint8_t raw_type = payload[offset++];
    out->hidden = (raw_type & 0x80) ? 1 : 0;
    out->type = raw_type & 0x7F;

    /* Parse name (null-terminated) */
    size_t name_start = offset;
    while (offset < len && payload[offset] != '\0') offset++;
    size_t name_len = offset - name_start;
    if (name_len >= sizeof(out->name)) name_len = sizeof(out->name) - 1;
    memcpy(out->name, payload + name_start, name_len);
    out->name[name_len] = '\0';
    if (offset < len) offset++; /* skip null terminator */

    /* Parse type-specific data */
    switch (out->type) {
        case CEL_PARAM_TEXT_SELECT: {
            /* Options: null-terminated, semicolon-separated string */
            size_t opts_start = offset;
            while (offset < len && payload[offset] != '\0') offset++;
            size_t opts_len = offset - opts_start;
            if (opts_len >= sizeof(out->options)) opts_len = sizeof(out->options) - 1;
            memcpy(out->options, payload + opts_start, opts_len);
            out->options[opts_len] = '\0';
            if (offset < len) offset++; /* skip null terminator */

            /* [value][min][max][default] */
            if (offset + 4 <= len) {
                out->value = payload[offset++];
                out->min_val = payload[offset++];
                out->max_val = payload[offset++];
                out->default_val = payload[offset++];
            }
            break;
        }
        case CEL_PARAM_UINT8:
        case CEL_PARAM_INT8:
            /* [min][max][default][value] */
            if (offset + 4 <= len) {
                out->min_val = payload[offset++];
                out->max_val = payload[offset++];
                out->default_val = payload[offset++];
                out->value = payload[offset++];
            }
            break;
        default:
            /* Other types have no simple value representation */
            break;
    }

    return 0;
}