Major rewriting need testing for regression. Fixed #324.