Minor bugfixing and improvements. Keep working on optimization.