Mostly working. Some hardware issues with the reverse switch are causing issues though.
Added extra checks for the reverse gear and also made it the last one checked to give park a chance to activate. However, should probably replace the actual switch to really fix the issue