1: start/end points , |

2: compare with to check whether it needs to go |

backwards, if yes, go backwards and restart from step 1 |

3: solve TPBVP for input levels to get , and |

4: |

5: case 1: apply input levels with durations |

6: |

7: case 2: solve TPBVP for input levels |

and apply them with obtained durations |

8: ** else** (i.e., ) |

9: case 3: solve TPBVP for input levels and apply |

them with obtained durations |

10: |