Fixed bug in the second case.
Still needs more testing and code review. Must try to make the code simpler.