Added function templates and some optimization.