[WIP] General Clean Up and final integration of the Lie Group Solving method

Hi, this is going to be the last PR of the Lie Group first order solver. I will work on the integration soon.