HAL Component to bicubic interpolation of a matrix

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "hal.h"
#include "rtapi.h"
#include "rtapi_app.h"
#include "rtapi_math.h"

#define MAX_ROWS 240

typedef struct {
    hal_float_t *x_coord;
    hal_float_t *y_coord;
    hal_float_t *z_output;
    float x_values[MAX_ROWS];
    float y_values[MAX_ROWS];
    float z_values[MAX_ROWS];
    int rows;
} z_offset_t;

static z_offset_t *comp_data;
static int comp_id;

// Function to load the CSV file
void load_csv(const char *filename) {
    FILE *file = fopen(filename, "r");
    if (!file) {
        rtapi_print_msg(RTAPI_MSG_ERR, "Error opening CSV file\n");
        return;
    }

    comp_data->rows = 0;
    while (fscanf(file, "%f,%f,%f",
                  &comp_data->x_values[comp_data->rows],
                  &comp_data->y_values[comp_data->rows],
                  &comp_data->z_values[comp_data->rows]) == 3) {
        comp_data->rows++;
        if (comp_data->rows >= MAX_ROWS) {
            break;
        }
    }

    fclose(file);
}

// Helper function to clamp values to a range
int clamp(int value, int min, int max) {
    if (value < min) return min;
    if (value > max) return max;
    return value;
}

// Function to perform cubic interpolation
float cubicInterpolate(float p[4], float x) {
    return p[1] + 0.5 * x * (p[2] - p[0] + x * (2.0 * p[0] - 5.0 * p[1] + 4.0 * p[2] - p[3] + x * (3.0 * (p[1] - p[2]) + p[3] - p[0])));
}

// Function to perform bicubic interpolation
float bicubicInterpolate(float p[4][4], float x, float y) {
    float arr[4];
    arr[0] = cubicInterpolate(p[0], y);
    arr[1] = cubicInterpolate(p[1], y);
    arr[2] = cubicInterpolate(p[2], y);
    arr[3] = cubicInterpolate(p[3], y);
    return cubicInterpolate(arr, x);
}

// Function to interpolate the Z value using bicubic interpolation
float interpolate(float x, float y) {
    int i, j;
    for (i = 0; i < comp_data->rows - 1; i++) {
        if (x < comp_data->x_values[i + 1]) break;
    }
    for (j = 0; j < comp_data->rows - 1; j++) {
        if (y < comp_data->y_values[j + 1]) break;
    }

    // If coordinates are out of bounds, return 0.0
    if (x < comp_data->x_values[0] || x > comp_data->x_values[comp_data->rows - 1] ||
        y < comp_data->y_values[0] || y > comp_data->y_values[comp_data->rows - 1]) {
        return 0.0;
    }

    float p[4][4];
    for (int m = -1; m <= 2; m++) {
        for (int n = -1; n <= 2; n++) {
            int xi = clamp(i + m, 0, comp_data->rows - 1);
            int yj = clamp(j + n, 0, comp_data->rows - 1);
            p[m + 1][n + 1] = comp_data->z_values[xi];
        }
    }

    float x1 = comp_data->x_values;
    float x2 = comp_data->x_values[i + 1];
    float y1 = comp_data->y_values[j];
    float y2 = comp_data->y_values[j + 1];

    float x_frac = (x - x1) / (x2 - x1);
    float y_frac = (y - y1) / (y2 - y1);

    return bicubicInterpolate(p, x_frac, y_frac);
}

// Update function to read the current X and Y coordinates, interpolate the Z value, and set the Z output
void update(void *arg, long period) {
    if (!comp_data || !comp_data->x_coord || !comp_data->y_coord || !comp_data->z_output) {
        rtapi_print_msg(RTAPI_MSG_ERR, "Null pointer in update function\n");
        return;
    }

    float x = *(comp_data->x_coord);
    float y = *(comp_data->y_coord);

    rtapi_print_msg(RTAPI_MSG_INFO, "Update called with x: %f, y: %f\n", x, y);

    *(comp_data->z_output) = interpolate(x, y);

    rtapi_print_msg(RTAPI_MSG_INFO, "Interpolated z value: %f\n", *(comp_data->z_output));
}

// Component initialization
int rtapi_app_main(void) {
    comp_id = hal_init("z_offset");
    if (comp_id < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, "hal_init() failed\n");
        return -1;
    }

    comp_data = hal_malloc(sizeof(z_offset_t));
    if (!comp_data) {
        rtapi_print_msg(RTAPI_MSG_ERR, "hal_malloc() failed\n");
        return -1;
    }

    memset(comp_data, 0, sizeof(z_offset_t)); // Ensure comp_data is zero-initialized

    int res = hal_pin_float_new("z_offset.x_coord", HAL_IN, &comp_data->x_coord), comp_id);
    if (res < 0) return res;

    res = hal_pin_float_new("z_offset.y_coord", HAL_IN, &comp_data->y_coord), comp_id);
    if (res < 0) return res;

    res = hal_pin_float_new("z_offset.z_output", HAL_OUT, &comp_data->z_output), comp_id);
    if (res < 0) return res;

    load_csv("/home/linuxcnc/Desktop/CNC/offset_matrix.csv");

    hal_export_funct("z_offset.update", update, comp_data, 1, 0, comp_id);
    hal_ready(comp_id);

    return 0;
}

// Component cleanup
void rtapi_app_exit(void) {
    hal_exit(comp_id);
}