Finished algorithms. Bug fixing to come. Nearly completed