Added tests, fixed some indexing bugs and TESTS PASS!