Lathe bed mapping - surface inequalities compensation

component lathe_bed;
license "GPL"; // GPL v2 or later
pin in float x_rel_in;  //relatywne wspolrzedne X
pin in float x_motor_in;  //maszynowe polozenie X
pin in float z_in;         //maszynowe polozenie Z
pin in touchoff-h-adjust;   //AKTUALNIE NIE DZIALA - korekta loza w momencie bazowania do materialu
pin out float x_out;
function _;
;;
#include <math.h>
#include <stdlib.h>

float z_pos_array[5] = {-750, -700, -100, -50, 0};
float h_corr_array[5] = { 0.0, 0.0, 0.0, 0.4, 0.0};
float slope_array[5] = {0.0, 0.0, 0.0, -0.008, 0.008};


float slope = 0.0;
float z_range_high = 0.0;
float h_high = 0.0;
float x_offset = 0.0;
float h_offset = 0.0;
float r;
int size = sizeof(z_pos_array)/sizeof(z_pos_array[0]);
float sqrt_val;

FUNCTION(_){

    for(int i=0; i<size; i++){
        float z_limit = z_pos_array[i];
        if(z_in <= z_limit){
            z_range_high = z_limit;
            h_high = h_corr_array[i];
            slope = slope_array[i];
            break;
        }
        else{
			slope = 0;
			h_high = 0;
        }
    }
    r = x_rel_in;
    //Circle equation : x^2 + h^2 = r^2
    //x_offset = r - x
    //x_offset = abs(r) - sqrt(pow(r,2) - pow(h_offset, 2));
    
    h_offset = (z_range_high - z_in)*slope + h_high;
    h_offset = h_offset - touchoff-h-adjust  //AKTUALNIE NIE DZIALA - na biezaco korekta H zoffsetowana ezgledem punktu bazowania do materialu

    sqrt_val = sqrt(abs((pow(r,2) - pow(h_offset, 2))*100000.0f)/100000.0f);
    x_offset = abs(r*1000.0f)/1000.0f - roundf(sqrt_val*1000.0f)/1000.0f;
    x_offset = abs(x_offset * 1000.0f) / 1000.0f;

    if(r>0){
        x_offset = -x_offset;
    }
    else{
         x_offset = x_offset;
    }
    x_out = x_motor_in + x_offset;
}

loadrt lathe_bed
addf lathe-bed.0 servo-thread

net x-pos-rel <= halui.axis.0.pos-relative
net x-pos-rel => lathe-bed.0.x-rel-in
net x-pos-cmd => lathe-bed.0.x-motor-in
net z-pos-cmd => lathe-bed.0.z-in 


#calc base height adjustment offset when setting X axis offset (press "END")  - CURRENTLY NOT WORKING
loadusr axisui -name axisui.0   #???
#net z-pos-cmd  => axisui.machine-z-in   # - HAL DOES NOT RECOGNIZE AXISUI PINS
#net base_height_correction axisui.touchoff-h-adjust => lathe-bed.0.touchoff-h-adjust  

net x-pos-cmd-corr <= lathe-bed.0.x-out
net x-pos-cmd-corr   =>  pid.x.command

def touch_off_system(event=None, new_axis_value = None):
        global system
        if not manual_ok(): return
        if joints_mode(): return
        offset_axis = "xyzabcuvw".index(vars.current_axis.get())
        if new_axis_value is None:
            new_axis_value, system = prompt_touchoff(
                title=_("Touch Off (system)"),
                text=_("Enter %s coordinate relative to %%s:") % vars.current_axis.get().upper(),
                default=0.0,
                tool_only=False,
                system=vars.touch_off_system.get()
                )
        else:
            system = vars.touch_off_system.get()
        if new_axis_value is None: return
        vars.touch_off_system.set(system)
        ensure_mode(linuxcnc.MODE_MDI)
        s.poll()
#FUNCTION CALL
        if offset_axis == "x":
            capture_bed_height_offset()
            
        #rest of the code was skipped...

    def capture_bed_height_offset():

        z_in = comp["machine-z-in"] #pobierz z Pinu aktualny Z
        z_pos_array = [-750, -700, -100, -50, 0]
        h_corr_array = [0.0, 0.0, 0.0, 0.4, 0.0]
        slope_array = [0.0, 0.0, 0.0, -0.008, 0.008]

        for i in range(0, len(z_pos_array)):
            z_limit = z_pos_array[i]
            if z_in <= z_limit:
                z_range_high = z_limit
                h_high = h_corr_array[i]
                slope = slope_array[i]
                break
        h_offset = float((z_range_high - z_in)*slope + h_high)
        comp["touchoff-h-adjust"] = h_offset

if hal_present == 1 :
    comp = hal.component("axisui")
    comp.newpin("jog.x", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.y", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.z", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.a", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.b", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.c", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.u", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.v", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.w", hal.HAL_BIT, hal.HAL_OUT)
    comp.newpin("jog.increment", hal.HAL_FLOAT, hal.HAL_OUT)
    comp.newpin("notifications-clear",hal.HAL_BIT,hal.HAL_IN)
    comp.newpin("notifications-clear-info",hal.HAL_BIT,hal.HAL_IN)
    comp.newpin("notifications-clear-error",hal.HAL_BIT,hal.HAL_IN)
    comp.newpin("resume-inhibit",hal.HAL_BIT,hal.HAL_IN)
    #NEWLY ADDED PINS BELOW
    comp.newpin("machine-z-in",hal.HAL_FLOAT,hal.HAL_IN)
    comp.newpin("touchoff-h-adjust",hal.HAL_FLOAT,hal.HAL_OUT)