updated controller to track each segment, added pure heading control phase, but sometimes fails to detect end of segment