Templating complete, works for ints. Working on doubles