Works for straight path - Node class need to be fix

Node class needs to be fixed so it does not rely constructor. For now
this is working for straight paths!!