MX variable as an index to a vector for Frenet coordinate conversion to Cartesian frame

[imanask]

I am working on a motion planning problem where I utilize the Frenet coordinate system. I use a conversion function from Frenet to Cartesian that finds the corresponding Cartesian point by searching through the vector of distance along the road in the Frenet frame.

This is implemented as below:
Given a 4 x 1 MX vector [i_fS, i_fD, heading], which are the horizontal, lateral, and heading in the Frenet coordinates.
First, I search for the correct index by looping through the vector RefRunLength. Here, i_fS is the MX symoblic variable. Then, the variable index is also an MX variable.

for i = 1:length(RefRunLength)-1
index = if_else(i_fS <= RefRunLength(i+1), i+1, 0);
end
Then, the heading angle in the Cartesian frame is given as:

fHeading = atan2((i_faRefY(index)-i_faRefY(index - 1)), ...
(i_faRefX(index)-i_faRefX(index - 1)));

where i_faRefY and i_faRefX are the X and Y vectors in the Cartesian coordinates.

Then, obtaining the segment length for more accurate X and Y values:

fSegmentS = i_fS - RefRunLength(index - 1);

fSegmentX = i_faRefX(index - 1) + fSegmentS * cos(fHeading);
fSegmentY = i_faRefY(index - 1) + fSegmentS * sin(fHeading);

Then, the values are used to find the X and Y locations in Cartesian coordiantes

fPerpendicularHeading = fHeading - pi/2;

o_fX = fSegmentX - i_fD * cos(fPerpendicularHeading);
o_fY = fSegmentY - i_fD * sin(fPerpendicularHeading);

yaw_cart = interp1(RefRunLength, RefYaw, i_fS);
heading_cart =  e_heading + yaw - pi;
x_ cart = [o_fX;o_fY; head_cart];

At the moment, I am stuck with not being able to run the line which computes fHeading due to the usage of MX variable as an index.

I would appreciate any help with setting up this Frenet to Cartesian transformation in CasADi.
Thanks