Added numerical differentiation and started refactoring bounded trace solver