1. Completed trajectory implementation (Artificial points)

2. Cleaned up code