I've finished the code that could by changing the input commands to get the correct FBUDLR(forward/backward/up/down/left/right) motor outputs:
Use case: how to get the correct motor outputs after someone rewire the hardware
The code is now in the github