Optimization

Fixed a couple of errors in the path generator as well as commenting code. Created priority queue program as practice.