Many bug fixes

Tested multiple scenarios and fixed potential bugs.