Restructuring

Merging of everything so far. FindRoute Still has issues