Finish code and start tests. Still WIP.