Lathe spindle & C Axis without PID

I am trying to implement C Axis function on my spindle, but.. I know it was solved here many times, but my configuration is different. Driver has 4 wires
STEP
DIR
ENA
Speed/Position mode

My aim is to control spindle via speed mode and than using M1xx code switch into position control mode and use the spindle as C Axis. I tried to set two different step gens and multiplex their outputs (STEP, DIR) But I was not able to multiplex them on mesa 5i25. So next try was configure one stepgen and switch input values(scale, max speed, position&velocity mode..) Everything works almost brilliant But when I switch from spindle to C Axis, spindle returns to the last position of the C Axis. So when is the machine switched to C Axis after 5 minutes of turning at full speed, it goes 5 minutes reverse at full speed to reach the last position of C Axis mode and after that it can be used.
I also tried to use spindle encoder as a feedback for C Axis to eliminate this, but it also didn't work. One encoder on two different Axes -> following error if one of them try to move

Do you have any idea how to solve it? Maybe there is much simpler solution, but I don't see it

Here is M-code for switching parameters:

I suspect the only way to really do this currently is with the stepgen always in velocity mode
This requires a PID comp in the HAL file with FF1 = 1 and P = ~50
That way the mode change is done with muxes and the current position offset can be applied
so there is no movement when changing to position mode

I have implemented this configuration announced by andy , it works, but the regulator can hold the position in area +-1°. When I use the Axis in position mode, it holds position with error +-0.1°. I think that problem is in "slow communication" between PC and spindle. In velocity mode it reads input value each 10ms, in position mode each 50us

PCW good guess FF1 = 1.2, P = 50, I=10 were the highest values when the spindle was stable.

I get an idea - What if I force the C Axis to "rehome" when switching from spindle mode?? It should clear counter value and rotate till index signal, right?

Homing didn't help. It looks like it isn't possible to implement it. Only solution is make specific bit file for 5i25, that would multiplex two stepgens into one STEP, DIR signal. But it is beyond my ability. I can try connect external HW multiplexer between mesa and driver. It isn't nice solution, but it should work for now

The orient comp does seem like the proper solution.
Note that the stepgen is set for velocity mode when orient is used
(and never changed) and a PID component is used for closing the stepgen position loop.

It may be that the drive should always be left in position mode

SW solution did not work. It was resolved by the external HW multiplexer and it works perfectly. There are some oscilation in rapid moves caused by the belt which needs to be more stretched.