Systems and methods for controlling a vehicle are described. A processor of a longitudinal planning system determines a state of the vehicle. The processor determines a state of a leader vehicle. The processor, based on the determined state of the vehicle and the determined state of the leader vehicle, determines a critical distance for the vehicle. The processor compares a distance between the vehicle and the leader vehicle with the critical distance. The processor, based on the comparison, determines whether the vehicle is too close to or too far from the leader vehicle. The processor, based on the determination, applies one or more of overshoot constraints, undershoot constraints, and critical constraints. After applying the one or more of overshoot constraints, undershoot constraints, and critical constraints, the processor determines a target acceleration for the vehicle. The processor controls the vehicle to track the target acceleration for the vehicle.