tactility_apps/Apps/McpScreen/main/Source/DisplayTools.c

#include "McpScreen.h"

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

#include <esp_log.h>
#include <tt_lvgl.h>

static const char* TAG = "DisplayTools";

static bool ascii_space(uint8_t value) {
    return value == ' ' || value == '\t' || value == '\r' ||
           value == '\n' || value == '\f' || value == '\v';
}

static bool ascii_digit(uint8_t value) {
    return value >= '0' && value <= '9';
}

static uint16_t read_le16(const uint8_t* value) {
    return (uint16_t)value[0] | ((uint16_t)value[1] << 8);
}

static uint32_t read_le32(const uint8_t* value) {
    return (uint32_t)value[0] |
           ((uint32_t)value[1] << 8) |
           ((uint32_t)value[2] << 16) |
           ((uint32_t)value[3] << 24);
}

static bool display_ready(McpScreenState* state) {
    return state != NULL &&
           state->visible &&
           state->draw_area != NULL &&
           state->framebuffer != NULL &&
           state->draw_width > 0 &&
           state->draw_height > 0;
}

static uint16_t rgb888_to_rgb565(uint8_t red, uint8_t green, uint8_t blue) {
    return (uint16_t)(((red & 0xF8) << 8) |
                      ((green & 0xFC) << 3) |
                      (blue >> 3));
}

static void put_pixel(McpScreenState* state, int x, int y, uint16_t color) {
    if (x >= 0 && y >= 0 && x < state->draw_width && y < state->draw_height) {
        state->framebuffer[(size_t)y * state->draw_width + x] = color;
    }
}

bool mcp_ui_draw_rgb565_be(
    McpScreenState* state,
    const uint8_t* data,
    size_t data_size,
    int x,
    int y,
    int width,
    int height
) {
    if (data == NULL || width <= 0 || height <= 0 ||
        data_size < (size_t)width * height * 2 || !display_ready(state)) {
        return false;
    }

    bool success = false;
    if (tt_lvgl_lock(pdMS_TO_TICKS(2000))) {
        if (display_ready(state)) {
            for (int source_y = 0; source_y < height; ++source_y) {
                int destination_y = y + source_y;
                if (destination_y < 0 || destination_y >= state->draw_height) {
                    continue;
                }
                for (int source_x = 0; source_x < width; ++source_x) {
                    int destination_x = x + source_x;
                    if (destination_x < 0 || destination_x >= state->draw_width) {
                        continue;
                    }
                    size_t offset = ((size_t)source_y * width + source_x) * 2;
                    uint16_t color = ((uint16_t)data[offset] << 8) | data[offset + 1];
                    put_pixel(state, destination_x, destination_y, color);
                }
            }
            lv_obj_invalidate(state->draw_area);
            state->override_active = true;
            success = true;
        }
        tt_lvgl_unlock();
    }
    return success;
}

bool mcp_ui_draw_bmp(
    McpScreenState* state,
    const uint8_t* data,
    size_t data_size,
    int x,
    int y
) {
    if (data == NULL || data_size < 54 || !display_ready(state) ||
        data[0] != 'B' || data[1] != 'M') {
        return false;
    }

    uint32_t pixel_offset = read_le32(data + 10);
    uint32_t dib_size = read_le32(data + 14);
    int32_t width = (int32_t)read_le32(data + 18);
    int32_t signed_height = (int32_t)read_le32(data + 22);
    uint16_t planes = read_le16(data + 26);
    uint16_t bits_per_pixel = read_le16(data + 28);
    uint32_t compression = read_le32(data + 30);
    if (dib_size < 40 || width <= 0 || signed_height == 0 || planes != 1 ||
        (bits_per_pixel != 24 && bits_per_pixel != 32) || compression != 0) {
        return false;
    }

    int height = signed_height < 0 ? -signed_height : signed_height;
    bool top_down = signed_height < 0;
    size_t row_stride = (((size_t)width * bits_per_pixel + 31) / 32) * 4;
    if (pixel_offset > data_size ||
        row_stride > data_size ||
        (size_t)height > (data_size - pixel_offset) / row_stride) {
        return false;
    }

    bool success = false;
    if (tt_lvgl_lock(pdMS_TO_TICKS(2500))) {
        if (display_ready(state)) {
            size_t bytes_per_pixel = bits_per_pixel / 8;
            for (int source_y = 0; source_y < height; ++source_y) {
                int file_y = top_down ? source_y : (height - 1 - source_y);
                const uint8_t* row = data + pixel_offset + (size_t)file_y * row_stride;
                for (int source_x = 0; source_x < width; ++source_x) {
                    const uint8_t* pixel = row + (size_t)source_x * bytes_per_pixel;
                    put_pixel(
                        state,
                        x + source_x,
                        y + source_y,
                        rgb888_to_rgb565(pixel[2], pixel[1], pixel[0])
                    );
                }
            }
            lv_obj_invalidate(state->draw_area);
            state->override_active = true;
            success = true;
        }
        tt_lvgl_unlock();
    }
    return success;
}

static bool pbm_next_number(
    const uint8_t* data,
    size_t data_size,
    size_t* offset,
    int* result
) {
    while (*offset < data_size) {
        if (data[*offset] == '#') {
            while (*offset < data_size && data[*offset] != '\n') {
                (*offset)++;
            }
        } else if (ascii_space(data[*offset])) {
            (*offset)++;
        } else {
            break;
        }
    }
    if (*offset >= data_size || !ascii_digit(data[*offset])) {
        return false;
    }

    int value = 0;
    while (*offset < data_size && ascii_digit(data[*offset])) {
        value = value * 10 + (data[*offset] - '0');
        (*offset)++;
    }
    *result = value;
    return true;
}

bool mcp_ui_draw_pbm(
    McpScreenState* state,
    const uint8_t* data,
    size_t data_size,
    int x,
    int y
) {
    if (data == NULL || data_size < 8 || !display_ready(state) ||
        data[0] != 'P' || data[1] != '4') {
        ESP_LOGE(
            TAG,
            "PBM precondition failed data=%p size=%u ready=%d magic=%02x%02x",
            data,
            (unsigned)data_size,
            display_ready(state),
            data_size > 0 ? data[0] : 0,
            data_size > 1 ? data[1] : 0
        );
        return false;
    }

    size_t offset = 2;
    int width = 0;
    int height = 0;
    if (!pbm_next_number(data, data_size, &offset, &width) ||
        !pbm_next_number(data, data_size, &offset, &height) ||
        width <= 0 || height <= 0) {
        ESP_LOGE(TAG, "PBM dimension parse failed offset=%u w=%d h=%d", (unsigned)offset, width, height);
        return false;
    }
    if (offset >= data_size || !ascii_space(data[offset])) {
        ESP_LOGE(
            TAG,
            "PBM missing header separator offset=%u size=%u byte=%02x",
            (unsigned)offset,
            (unsigned)data_size,
            offset < data_size ? data[offset] : 0
        );
        return false;
    }
    if (data[offset] == '\r' && offset + 1 < data_size && data[offset + 1] == '\n') {
        offset += 2;
    } else {
        offset++;
    }

    size_t row_bytes = ((size_t)width + 7) / 8;
    if (offset > data_size ||
        (size_t)height > (data_size - offset) / row_bytes) {
        ESP_LOGE(
            TAG,
            "PBM payload too small offset=%u size=%u row=%u h=%d",
            (unsigned)offset,
            (unsigned)data_size,
            (unsigned)row_bytes,
            height
        );
        return false;
    }

    bool success = false;
    if (tt_lvgl_lock(pdMS_TO_TICKS(2000))) {
        if (display_ready(state)) {
            for (int source_y = 0; source_y < height; ++source_y) {
                const uint8_t* row = data + offset + (size_t)source_y * row_bytes;
                for (int source_x = 0; source_x < width; ++source_x) {
                    bool black = (row[source_x >> 3] & (0x80 >> (source_x & 7))) != 0;
                    put_pixel(state, x + source_x, y + source_y, black ? 0x0000 : 0xFFFF);
                }
            }
            lv_obj_invalidate(state->draw_area);
            state->override_active = true;
            success = true;
        }
        tt_lvgl_unlock();
    } else {
        ESP_LOGE(TAG, "PBM LVGL lock timed out");
    }
    if (!success) ESP_LOGE(TAG, "PBM draw failed after lock ready=%d", display_ready(state));
    return success;
}

static char* base64_encode(const uint8_t* input, size_t input_size) {
    static const char alphabet[] =
        "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
    size_t output_size = ((input_size + 2) / 3) * 4;
    char* output = malloc(output_size + 1);
    if (output == NULL) {
        return NULL;
    }

    size_t in = 0;
    size_t out = 0;
    while (in < input_size) {
        uint32_t value = (uint32_t)input[in++] << 16;
        bool have_second = in < input_size;
        if (have_second) value |= (uint32_t)input[in++] << 8;
        bool have_third = in < input_size;
        if (have_third) value |= input[in++];

        output[out++] = alphabet[(value >> 18) & 0x3F];
        output[out++] = alphabet[(value >> 12) & 0x3F];
        output[out++] = have_second ? alphabet[(value >> 6) & 0x3F] : '=';
        output[out++] = have_third ? alphabet[value & 0x3F] : '=';
    }
    output[out] = '\0';
    return output;
}

char* mcp_ui_get_screenshot_pbm_base64(McpScreenState* state) {
    if (!display_ready(state)) {
        return NULL;
    }

    size_t row_bytes = ((size_t)state->draw_width + 7) / 8;
    char header[40];
    int header_size = snprintf(
        header,
        sizeof(header),
        "P4\n%u %u\n",
        state->draw_width,
        state->draw_height
    );
    size_t pbm_size = (size_t)header_size + row_bytes * state->draw_height;
    uint8_t* pbm = calloc(1, pbm_size);
    if (pbm == NULL) {
        return NULL;
    }
    memcpy(pbm, header, header_size);

    bool success = false;
    if (tt_lvgl_lock(pdMS_TO_TICKS(2000))) {
        if (display_ready(state)) {
            for (int y = 0; y < state->draw_height; ++y) {
                uint8_t* row = pbm + header_size + (size_t)y * row_bytes;
                for (int x = 0; x < state->draw_width; ++x) {
                    uint16_t color = state->framebuffer[(size_t)y * state->draw_width + x];
                    int red = (color >> 11) & 0x1F;
                    int green = (color >> 5) & 0x3F;
                    int blue = color & 0x1F;
                    int luminance = red * 2 + green * 3 + blue;
                    if (luminance < 128) {
                        row[x >> 3] |= (0x80 >> (x & 7));
                    }
                }
            }
            success = true;
        }
        tt_lvgl_unlock();
    }

    char* encoded = success ? base64_encode(pbm, pbm_size) : NULL;
    free(pbm);
    return encoded;
}