001package jmri.jmrit.roster;
002
003import java.util.ArrayList;
004import java.util.LinkedList;
005import java.util.List;
006import java.util.Locale;
007import java.util.Map.Entry;
008import java.util.TreeMap;
009
010import javax.annotation.CheckForNull;
011
012import jmri.Block;
013import jmri.DccThrottle;
014import jmri.InstanceManager;
015import jmri.NamedBean;
016import jmri.Section;
017import jmri.implementation.SignalSpeedMap;
018
019import org.jdom2.Element;
020
021/**
022 * A class to store a speed profile for a given loco.
023 * The speed steps against the profile are on a scale of 0 to 1000,
024 * this equates to the float speed x 1000.
025 * This allows a single profile to cover different throttle speed step settings.
026 * A profile generated for a loco using 28 steps can be used for a throttle with 126 steps.
027 */
028public class RosterSpeedProfile {
029
030    private RosterEntry _re = null;
031
032    private float overRunTimeReverse = 0.0f;
033    private float overRunTimeForward = 0.0f;
034
035    private boolean _hasForwardSpeeds = false;
036    private boolean _hasReverseSpeeds = false;
037
038    /**
039     * Create a new RosterSpeedProfile.
040     * @param re the Roster Entry associated with the profile.
041     */
042    public RosterSpeedProfile(RosterEntry re) {
043        _re = re;
044    }
045
046    /**
047     * Get the RosterEntry associated with the profile.
048     * @return the RosterEntry.
049     */
050    public RosterEntry getRosterEntry() {
051        return _re;
052    }
053
054    public float getOverRunTimeForward() {
055        return overRunTimeForward;
056    }
057
058    public void setOverRunTimeForward(float dt) {
059        overRunTimeForward = dt;
060    }
061
062    public float getOverRunTimeReverse() {
063        return overRunTimeReverse;
064    }
065
066    public void setOverRunTimeReverse(float dt) {
067        overRunTimeReverse = dt;
068    }
069
070    public void clearCurrentProfile() {
071        speeds = new TreeMap<>();
072    }
073
074    public void deleteStep(Integer step) {
075        speeds.remove(step);
076    }
077
078    /**
079     * Check if the Speed Profile contains Forward Speeds.
080     * @return true if forward speeds are present, else false.
081     */
082    public boolean hasForwardSpeeds() {
083        return _hasForwardSpeeds;
084    }
085
086    /**
087     * Check if the Speed Profile contains Reverse Speeds.
088     * @return true if reverse speeds are present, else false.
089     */
090    public boolean hasReverseSpeeds() {
091        return _hasReverseSpeeds;
092    }
093
094    /**
095     * place / remove SpeedProfile from test mode.
096     * reinitializes speedstep trace array
097     * @param value true/false
098     */
099    public void setTestMode(boolean value) {
100        synchronized (this){
101            profileInTestMode = value;
102        }
103        testSteps = new ArrayList<>();
104    }
105
106    /**
107     * Gets the speed step trace array.
108     * @return speedstep trace array
109     */
110    public List<SpeedSetting> getSpeedStepTrace() {
111        return testSteps;
112    }
113
114    /**
115     * Speed conversion Millimetres per second to Miles per hour.
116     */
117    public static final float MMS_TO_MPH = 0.00223694f;
118
119    /**
120     * Speed conversion Millimetres per second to Kilometres per hour.
121     */
122    public static final float MMS_TO_KPH = 0.0036f;
123
124    /**
125     * Returns the scale speed.
126     * If Warrant preferences are not a speed, value returns unchanged.
127     * @param mms MilliMetres per second.
128     * @param factorFastClock true to factor in the Fast Clock ratio, else false.
129     * @return scale speed in units specified by Warrant Preferences,
130     *         unchanged if Warrant preferences are not a speed.
131     */
132    public float mmsToScaleSpeed(float mms, boolean factorFastClock) {
133        int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation();
134        float scale = InstanceManager.getDefault(SignalSpeedMap.class).getLayoutScale();
135        float fastClockFactor = ( factorFastClock ?
136            (float)InstanceManager.getDefault(jmri.Timebase.class).userGetRate() : 1 );
137
138        switch (interp) {
139            case SignalSpeedMap.SPEED_MPH:
140                return mms * scale * MMS_TO_MPH * fastClockFactor;
141            case SignalSpeedMap.SPEED_KMPH:
142                return mms * scale * MMS_TO_KPH * fastClockFactor;
143            case SignalSpeedMap.PERCENT_THROTTLE:
144            case SignalSpeedMap.PERCENT_NORMAL:
145                return mms;
146            default:
147                log.warn("MMSToScaleSpeed: Signal Speed Map is not in a scale speed, not modifing.");
148                return mms;
149        }
150    }
151
152    /**
153     * Returns the scale speed as a numeric.
154     * If Warrant preferences are not a speed, value returns unchanged.
155     * @param mms MilliMetres per second
156     * @return scale speed in units specified by Warrant Preferences,
157     *         unchanged if Warrant preferences are not a speed.
158     * @deprecated use {@link #mmsToScaleSpeed(float mms)}
159     */
160    @Deprecated (since="5.9.6",forRemoval=true)
161    public float MMSToScaleSpeed(float mms) {
162        jmri.util.LoggingUtil.deprecationWarning(log, "MMSToScaleSpeed");
163        return mmsToScaleSpeed(mms);
164    }
165
166    /**
167     * Returns the scale speed as a numeric.
168     * If Warrant preferences are not a speed, value returns unchanged.
169     * Does not factor Fast Clock ratio.
170     * @param mms MilliMetres per second
171     * @return scale speed in units specified by Warrant Preferences,
172     *         unchanged if Warrant preferences are not a speed.
173     */
174    public float mmsToScaleSpeed(float mms) {
175        return mmsToScaleSpeed(mms, false);
176    }
177
178    /**
179     * Returns the scale speed format as I18N string with the units added given
180     * MilliMetres per Second.
181     * If the warrant preference is a percentage of
182     * normal or throttle will use metres per second.
183     * The Fast Clock Ratio is not used in the calculation.
184     *
185     * @param mms MilliMetres per second
186     * @return a string with scale speed and units
187     */
188    public static String convertMMSToScaleSpeedWithUnits(float mms) {
189        int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation();
190        float scale = InstanceManager.getDefault(SignalSpeedMap.class).getLayoutScale();
191        String formattedWithUnits;
192        switch (interp) {
193            case SignalSpeedMap.SPEED_MPH:
194                String unitsMph = Bundle.getMessage("mph");
195                formattedWithUnits = String.format(Locale.getDefault(), "%.2f %s", mms * scale * MMS_TO_MPH, unitsMph);
196                break;
197            case SignalSpeedMap.SPEED_KMPH:
198                String unitsKph = Bundle.getMessage("kph");
199                formattedWithUnits = String.format(Locale.getDefault(), "%.2f %s", mms * scale * MMS_TO_KPH, unitsKph);
200                break;
201            case SignalSpeedMap.PERCENT_THROTTLE:
202            case SignalSpeedMap.PERCENT_NORMAL:
203                String unitsMms = Bundle.getMessage("mmps");
204                formattedWithUnits = String.format(Locale.getDefault(), "%.2f %s", mms, unitsMms);
205                break;
206            default:
207                log.warn("ScaleSpeedToMMS: Signal Speed Map has no interp, not modifing.");
208                formattedWithUnits = String.format( Locale.getDefault(), "%.2f", mms);
209        }
210        return formattedWithUnits;
211    }
212
213    /**
214     * Returns the scale speed format as a string with the units added given a
215     * throttle setting. and direction.
216     * The Fast Clock Ratio is not used in the calculation.
217     *
218     * @param throttleSetting as percentage of 1.0
219     * @param isForward       true or false
220     * @return a string with scale speed and units
221     */
222    public String convertThrottleSettingToScaleSpeedWithUnits(float throttleSetting, boolean isForward) {
223        return convertMMSToScaleSpeedWithUnits(getSpeed(throttleSetting, isForward));
224    }
225
226    /**
227     * MilliMetres per Second given scale speed.
228     * The Fast Clock Ratio is not used in the calculation.
229     * @param scaleSpeed in MPH or KPH
230     * @return MilliMetres per second
231     */
232    public float convertScaleSpeedToMMS(float scaleSpeed) {
233        int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation();
234        float scale = InstanceManager.getDefault(SignalSpeedMap.class).getLayoutScale();
235        float mmsSpeed;
236        switch (interp) {
237            case SignalSpeedMap.SPEED_MPH:
238                mmsSpeed = scaleSpeed / scale / MMS_TO_MPH;
239                break;
240            case SignalSpeedMap.SPEED_KMPH:
241                mmsSpeed = scaleSpeed / scale / MMS_TO_KPH;
242                break;
243            default:
244                log.warn("ScaleSpeedToMMS: Signal Speed Map is not in a scale speed, not modifing.");
245                mmsSpeed = scaleSpeed;
246        }
247        return mmsSpeed;
248    }
249
250    /**
251     * Converts from signal map speed to a throttle setting.
252     * The Fast Clock Ratio is not used in the calculation.
253     * @param signalMapSpeed value from warrants preferences
254     * @param isForward      direction of travel
255     * @return throttle setting
256     */
257    public float getThrottleSettingFromSignalMapSpeed(float signalMapSpeed, boolean isForward) {
258        int interp = InstanceManager.getDefault(SignalSpeedMap.class).getInterpretation();
259        float throttleSetting = 0.0f;
260        switch (interp) {
261            case SignalSpeedMap.PERCENT_NORMAL:
262            case SignalSpeedMap.PERCENT_THROTTLE:
263                throttleSetting = signalMapSpeed / 100.0f;
264                break;
265            case SignalSpeedMap.SPEED_KMPH:
266            case SignalSpeedMap.SPEED_MPH:
267                throttleSetting = getThrottleSetting(convertScaleSpeedToMMS(signalMapSpeed), isForward);
268                break;
269            default:
270                log.warn("getThrottleSettingFromSignalMapSpeed: Signal Speed Map interp not supported.");
271        }
272        return throttleSetting;
273    }
274
275    /**
276     * Set the speed for the given speed step.
277     *
278     * @param speedStep the speed step to set
279     * @param forward   speed in meters per second for running forward at
280     *                  speedStep
281     * @param reverse   speed in meters per second for running in reverse at
282     *                  speedStep
283     */
284    public void setSpeed(int speedStep, float forward, float reverse) {
285        SpeedStep ss = speeds.computeIfAbsent(speedStep, k -> new SpeedStep());
286        ss.setForwardSpeed(forward);
287        ss.setReverseSpeed(reverse);
288        if (forward > 0.0f) {
289            _hasForwardSpeeds = true;
290        }
291        if (reverse > 0.0f) {
292            _hasReverseSpeeds = true;
293        }
294    }
295
296    public SpeedStep getSpeedStep(float speed) {
297        int iSpeedStep = Math.round(speed * 1000);
298        return speeds.get(iSpeedStep);
299    }
300
301    public void setForwardSpeed(float speedStep, float forward) {
302        if (forward > 0.0f) {
303            _hasForwardSpeeds = true;
304        } else {
305            return;
306        }
307        int iSpeedStep = Math.round(speedStep * 1000);
308        speeds.computeIfAbsent(iSpeedStep, k -> new SpeedStep()).setForwardSpeed(forward);
309    }
310
311    /**
312     * Merge raw throttleSetting value with an existing profile SpeedStep if
313     * key for the throttleSetting is within the speedIncrement of the SpeedStep.
314     * @param throttleSetting raw throttle setting value
315     * @param speed track speed
316     * @param speedIncrement throttle's speed step increment.
317     */
318    public void setForwardSpeed(float throttleSetting, float speed, float speedIncrement) {
319        if (throttleSetting> 0.0f) {
320            _hasForwardSpeeds = true;
321        } else {
322            return;
323        }
324        int key;
325        Entry<Integer, SpeedStep> entry = findEquivalentEntry (throttleSetting, speedIncrement);
326        if (entry != null) {    // close keys. i.e. resolve to same throttle step
327            float value = entry.getValue().getForwardSpeed();
328            speed = (speed + value) / 2;
329            key = entry.getKey();
330        } else {    // nothing close. make new entry
331            key = Math.round(throttleSetting * 1000);
332        }
333        speeds.computeIfAbsent(key, k -> new SpeedStep()).setForwardSpeed(speed);
334    }
335
336    @CheckForNull
337    private Entry<Integer, SpeedStep> findEquivalentEntry (float throttleSetting, float speedIncrement) {
338        // search through table until end for an entry is found whose key / 1000
339        // is within the speedIncrement of the throttleSetting
340        // Note there may be zero values interspersed in the tree
341        Entry<Integer, SpeedStep> entry = speeds.firstEntry();
342        if (entry == null) {
343            return null;
344        }
345        int key = entry.getKey();
346        while (entry != null) {
347            entry = speeds.higherEntry(key);
348            if (entry != null) {
349                float speed = entry.getKey();
350                if (Math.abs(speed/1000.0f - throttleSetting) <= speedIncrement) {
351                    return entry;
352                }
353                key = entry.getKey();
354            }
355        }
356        return null;
357    }
358
359    /**
360     * Merge raw throttleSetting value with an existing profile SpeedStep if
361     * key for the throttleSetting is within the speedIncrement of the SpeedStep.
362     * @param throttleSetting raw throttle setting value
363     * @param speed track speed
364     * @param speedIncrement throttle's speed step increment.
365     */
366    public void setReverseSpeed(float throttleSetting, float speed, float speedIncrement) {
367        if (throttleSetting> 0.0f) {
368            _hasReverseSpeeds = true;
369        } else {
370            return;
371        }
372        int key;
373        Entry<Integer, SpeedStep> entry = findEquivalentEntry (throttleSetting, speedIncrement);
374        if (entry != null) {    // close keys. i.e. resolve to same throttle step
375            float value = entry.getValue().getReverseSpeed();
376            speed = (speed + value) / 2;
377            key = entry.getKey();
378        } else {    // nothing close. make new entry
379            key = Math.round(throttleSetting * 1000);
380        }
381        speeds.computeIfAbsent(key, k -> new SpeedStep()).setReverseSpeed(speed);
382    }
383
384    public void setReverseSpeed(float speedStep, float reverse) {
385        if (reverse > 0.0f) {
386            _hasReverseSpeeds = true;
387        } else {
388            return;
389        }
390        int iSpeedStep = Math.round(speedStep * 1000);
391        speeds.computeIfAbsent(iSpeedStep, k -> new SpeedStep()).setReverseSpeed(reverse);
392    }
393
394    /**
395     * return the forward speed in milli-meters per second for a given
396     * percentage throttle
397     *
398     * @param speedStep which is actual percentage throttle
399     * @return MilliMetres per second using straight line interpolation for
400     *         missing points
401     */
402    public float getForwardSpeed(float speedStep) {
403        int iSpeedStep = Math.round(speedStep * 1000);
404        if (iSpeedStep <= 0 || !_hasForwardSpeeds) {
405            return 0.0f;
406        }
407        // Note there may be zero values interspersed in the tree
408        if (speeds.containsKey(iSpeedStep)) {
409            float speed = speeds.get(iSpeedStep).getForwardSpeed();
410            if (speed > 0.0f) {
411                return speed;
412            }
413        }
414        log.debug("no exact match forward for {}", iSpeedStep);
415        float lower = 0.0f;
416        float higher = 0.0f;
417        int highStep = iSpeedStep;
418        int lowStep = iSpeedStep;
419
420        Entry<Integer, SpeedStep> entry = speeds.higherEntry(highStep);
421        while (entry != null && higher <= 0.0f) {
422            highStep = entry.getKey();
423            float value = entry.getValue().getForwardSpeed();
424            if (value > 0.0f) {
425                higher = value;
426            }
427            entry = speeds.higherEntry(highStep);
428        }
429        boolean nothingHigher = (higher <= 0.0f);
430
431        entry = speeds.lowerEntry(lowStep);
432        while (entry != null && lower <= 0.0f) {
433            lowStep = entry.getKey();
434            float value = entry.getValue().getForwardSpeed();
435            if (value > 0.0f) {
436                lower = value;
437            }
438            entry = speeds.lowerEntry(lowStep);
439        }
440        log.debug("lowStep={}, lower={} highStep={} higher={} for iSpeedStep={}",
441                lowStep, lower, highStep, higher, iSpeedStep);
442        if (lower <= 0.0f) {      // nothing lower
443            if (nothingHigher) {
444                log.error("Nothing in speed Profile");
445                return 0.0f;       // no forward speeds at all
446            }
447            return higher * iSpeedStep / highStep;
448        }
449        if (nothingHigher) {
450//            return lower * (1.0f + (iSpeedStep - lowStep) / (1000.0f - lowStep));
451            return lower + (iSpeedStep - lowStep) * lower / lowStep;
452        }
453
454        float valperstep = (higher - lower) / (highStep - lowStep);
455
456        return lower + (valperstep * (iSpeedStep - lowStep));
457    }
458
459    /**
460     * return the reverse speed in millimetres per second for a given percentage
461     * throttle
462     *
463     * @param speedStep percentage of throttle 0.nnn
464     * @return millimetres per second
465     */
466    public float getReverseSpeed(float speedStep) {
467        int iSpeedStep = Math.round(speedStep * 1000);
468        if (iSpeedStep <= 0 || !_hasReverseSpeeds) {
469            return 0.0f;
470        }
471        if (speeds.containsKey(iSpeedStep)) {
472            float speed = speeds.get(iSpeedStep).getReverseSpeed();
473            if (speed > 0.0f) {
474                return speed;
475            }
476        }
477        log.debug("no exact match reverse for {}", iSpeedStep);
478        float lower = 0.0f;
479        float higher = 0.0f;
480        int highStep = iSpeedStep;
481        int lowStep = iSpeedStep;
482        // Note there may be zero values interspersed in the tree
483
484        Entry<Integer, SpeedStep> entry = speeds.higherEntry(highStep);
485        while (entry != null && higher <= 0.0f) {
486            highStep = entry.getKey();
487            float value = entry.getValue().getReverseSpeed();
488            if (value > 0.0f) {
489                higher = value;
490            }
491            entry = speeds.higherEntry(highStep);
492        }
493        boolean nothingHigher = (higher <= 0.0f);
494        entry = speeds.lowerEntry(lowStep);
495        while (entry != null && lower <= 0.0f) {
496            lowStep = entry.getKey();
497            float value = entry.getValue().getReverseSpeed();
498            if (value > 0.0f) {
499                lower = value;
500            }
501            entry = speeds.lowerEntry(lowStep);
502        }
503        log.debug("lowStep={}, lower={} highStep={} higher={} for iSpeedStep={}",
504                lowStep, lower, highStep, higher, iSpeedStep);
505        if (lower <= 0.0f) {      // nothing lower
506            if (nothingHigher) {
507                log.error("Nothing in speed Profile");
508                return 0.0f;       // no reverse speeds at all
509            }
510            return higher * iSpeedStep / highStep;
511        }
512        if (nothingHigher) {
513            return lower * (1.0f + (iSpeedStep - lowStep) / (1000.0f - lowStep));
514        }
515
516        float valperstep = (higher - lower) / (highStep - lowStep);
517
518        return lower + (valperstep * (iSpeedStep - lowStep));
519    }
520
521    /**
522     * Get the approximate time a loco may travel a given distance at a given
523     * speed step.
524     *
525     * @param isForward true if loco is running forward; false otherwise
526     * @param speedStep the desired speed step
527     * @param distance  the desired distance in millimeters
528     * @return the approximate time in seconds
529     */
530    public float getDurationOfTravelInSeconds(boolean isForward, float speedStep, int distance) {
531        float spd;
532        if (isForward) {
533            spd = getForwardSpeed(speedStep);
534        } else {
535            spd = getReverseSpeed(speedStep);
536        }
537        if (spd < 0.0f) {
538            log.error("Speed not available to compute duration of travel");
539            return 0.0f;
540        }
541        return (distance / spd);
542    }
543
544    /**
545     * Get the approximate distance a loco may travel a given duration at a
546     * given speed step.
547     *
548     * @param isForward true if loco is running forward; false otherwise
549     * @param speedStep the desired speed step
550     * @param duration  the desired time in seconds
551     * @return the approximate distance in millimeters
552     */
553    public float getDistanceTravelled(boolean isForward, float speedStep, float duration) {
554        float spd;
555        if (isForward) {
556            spd = getForwardSpeed(speedStep);
557        } else {
558            spd = getReverseSpeed(speedStep);
559        }
560        if (spd < 0.0f) {
561            log.error("Speed not available to compute distance travelled");
562            return 0.0f;
563        }
564        return Math.abs(spd * duration);
565    }
566
567    private float distanceRemaining = 0;
568    private float distanceTravelled = 0;
569
570    private TreeMap<Integer, SpeedStep> speeds = new TreeMap<>();
571
572    private DccThrottle _throttle;
573
574    private float desiredSpeedStep = -1;
575
576    private float extraDelay = 0.0f;
577
578    private float minReliableOperatingSpeed = 0.0f;
579
580    private float maxOperatingSpeed = 1.0f;
581
582    private NamedBean referenced = null;
583
584    private javax.swing.Timer stopTimer = null;
585
586    private long lastTimeTimerStarted = 0L;
587
588    /**
589     * reset everything back to default once the change has finished.
590     */
591    void finishChange() {
592        if (stopTimer != null) {
593            stopTimer.stop();
594        }
595        stopTimer = null;
596        _throttle = null;
597        distanceRemaining = 0;
598        desiredSpeedStep = -1;
599        extraDelay = 0.0f;
600        minReliableOperatingSpeed = 0.0f;
601        maxOperatingSpeed = 1.0f;
602        referenced = null;
603        synchronized (this) {
604            distanceTravelled = 0;
605            stepQueue = new LinkedList<>();
606        }
607        _throttle = null;
608    }
609
610    public void setExtraInitialDelay(float eDelay) {
611        extraDelay = eDelay;
612    }
613
614    public void setMinMaxLimits(float minReliableOperatingSpeed, float maxOperatingSpeed) {
615        this.minReliableOperatingSpeed = minReliableOperatingSpeed;
616        this.maxOperatingSpeed = maxOperatingSpeed;
617        if (minReliableOperatingSpeed > maxOperatingSpeed) {
618            log.warn("MaxOperatingSpeed [{}] < minReliableOperatingSpeed [{}] setting Max = Min",
619                    minReliableOperatingSpeed, maxOperatingSpeed);
620            this.maxOperatingSpeed = this.minReliableOperatingSpeed;
621        }
622    }
623
624    /**
625     * Set speed of a throttle.
626     *
627     * @param t     the throttle to set
628     * @param blk   the block used for length details
629     * @param speed the speed to set
630     */
631    public void changeLocoSpeed(DccThrottle t, Block blk, float speed) {
632        if (blk == referenced && Float.compare(speed, desiredSpeedStep) == 0) {
633            //log.debug("Already setting to desired speed step for this block");
634            return;
635        }
636        float blockLength = blk.getLengthMm();
637        if (blk == referenced) {
638            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
639            blockLength = distanceRemaining;
640            //Not entirely reliable at this stage as the loco could still be running and not completed the calculation of the distance, this could result in an over run
641            log.debug("Block passed is the same as we are currently processing");
642        } else {
643            referenced = blk;
644        }
645        changeLocoSpeed(t, blockLength, speed);
646    }
647
648    /**
649     * Set speed of a throttle.
650     *
651     * @param t     the throttle to set
652     * @param sec   the section used for length details
653     * @param speed the speed to set
654     * @param usePercentage the percentage of the block to be used for stopping
655     */
656    @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY",
657        justification = "OK to compare floats, as even tiny differences should trigger update")
658    public void changeLocoSpeed(DccThrottle t, Section sec, float speed, float usePercentage) {
659        if (sec == referenced && speed == desiredSpeedStep) {
660            log.debug("Already setting to desired speed step for this Section");
661            return;
662        }
663        float sectionLength = sec.getActualLength() * usePercentage;
664        if (sec == referenced) {
665            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
666            sectionLength = distanceRemaining;
667            //Not entirely reliable at this stage as the loco could still be running and not completed the calculation of the distance, this could result in an over run
668            log.debug("Block passed is the same as we are currently processing");
669        } else {
670            referenced = sec;
671        }
672        changeLocoSpeed(t, sectionLength, speed);
673    }
674
675    /**
676     * Set speed of a throttle.
677     *
678     * @param t     the throttle to set
679     * @param blk   the block used for length details
680     * @param speed the speed to set
681     * @param usePercentage the percentage of the block to be used for stopping
682     */
683    @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY",
684        justification = "OK to compare floats, as even tiny differences should trigger update")
685    public void changeLocoSpeed(DccThrottle t, Block blk, float speed, float usePercentage) {
686        if (blk == referenced && speed == desiredSpeedStep) {
687            //if(log.isDebugEnabled()) log.debug("Already setting to desired speed step for this block");
688            return;
689        }
690        float blockLength = blk.getLengthMm() * usePercentage;
691        if (blk == referenced) {
692            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
693            blockLength = distanceRemaining;
694            //Not entirely reliable at this stage as the loco could still be running and not completed the calculation of the distance, this could result in an over run
695            log.debug("Block passed is the same as we are currently processing");
696        } else {
697            referenced = blk;
698        }
699        changeLocoSpeed(t, blockLength, speed);
700
701    }
702
703    /**
704     * Set speed of a throttle to a speeed set by a float, using the section for
705     * the length details
706     * Set speed of a throttle.
707     *
708     * @param t     the throttle to set
709     * @param sec   the section used for length details
710     * @param speed the speed to set
711     */
712    //@TODO if a section contains multiple blocks then we could calibrate the change of speed based upon the block status change.
713    public void changeLocoSpeed(DccThrottle t, Section sec, float speed) {
714        if (sec == referenced && Float.compare(speed, desiredSpeedStep) == 0) {
715            log.debug("Already setting to desired speed step for this section");
716            return;
717        }
718        float sectionLength = sec.getActualLength();
719        log.debug("call to change speed via section {}", sec.getDisplayName());
720        if (sec == referenced) {
721            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
722            sectionLength = distanceRemaining;
723        } else {
724            referenced = sec;
725        }
726
727        changeLocoSpeed(t, sectionLength, speed);
728    }
729
730    /**
731     * Set speed of a throttle.
732     *
733     * @param t        the throttle to set
734     * @param distance the distance in meters
735     * @param requestedSpeed    the speed to set
736     */
737    public void changeLocoSpeed(DccThrottle t, float distance, float requestedSpeed) {
738        float speed = 0.0f;
739        log.debug("Call to change speed over specific distance float {} distance {}", requestedSpeed, distance);
740        if (requestedSpeed  > maxOperatingSpeed) {
741            speed = maxOperatingSpeed;
742        } else {
743            speed = requestedSpeed;
744        }
745        if (Float.compare(speed, desiredSpeedStep) == 0) {
746            // This requires no checks for min/max.
747            log.debug("Already setting to desired speed step");
748            return;
749        }
750        log.debug("public change speed step by float {}", speed);
751        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed);
752
753        if (stopTimer != null) {
754            log.debug("stop timer valid so will cancel");
755            cancelSpeedChange();
756        }
757        _throttle = t;
758
759        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed);
760        desiredSpeedStep = speed;
761
762        log.debug("calculated current step {} required {} current {}",
763            _throttle.getSpeedSetting(), speed, _throttle.getSpeedSetting());
764        if (_throttle.getSpeedSetting() < speed) {
765            log.debug("Going for acceleration");
766        } else {
767            log.debug("Going for deceleration");
768        }
769
770        float adjSpeed = speed;
771        boolean andStop = false;
772        if (speed <= 0.0) {
773            andStop = true;
774        }
775        if (speed < minReliableOperatingSpeed) {
776            adjSpeed = minReliableOperatingSpeed;
777        }
778        log.debug("Speed[{}] adjSpeed[{}] MinSpeed[{}]",
779                speed,adjSpeed, minReliableOperatingSpeed);
780
781        if (!andStop
782                && (Float.compare(adjSpeed, t.getSpeedSetting()) == 0
783                    || (Math.round(adjSpeed/t.getSpeedIncrement()) ==
784                            Math.round(t.getSpeedSetting()/t.getSpeedIncrement())))) {
785            log.debug("Throttle and request speed setting are the same {} {} so will quit", speed, t.getSpeedSetting());
786            //Already at correct speed setting
787            finishChange();
788            return;
789        }
790        calculateStepDetails(adjSpeed, distance, andStop);
791    }
792
793    private List<SpeedSetting> testSteps = new ArrayList<>();
794    private boolean profileInTestMode = false;
795
796    void calculateStepDetails(float speedStep, float distance, boolean andStop) {
797
798        float stepIncrement = _throttle.getSpeedIncrement();
799        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speedStep);
800        desiredSpeedStep = speedStep;
801        log.debug("calculated current step {} required {} current {} increment {}", _throttle.getSpeedSetting(), speedStep, _throttle.getSpeedSetting(), stepIncrement);
802        boolean increaseSpeed = false;
803        if (_throttle.getSpeedSetting() < speedStep) {
804            increaseSpeed = true;
805            log.debug("Going for acceleration");
806        } else {
807            log.debug("Going for deceleration");
808        }
809
810        if (distance <= 0) {
811            log.debug("Distance is less than 0 {}", distance);
812            _throttle.setSpeedSetting(speedStep);
813            finishChange();
814            return;
815        }
816
817        float calculatedDistance = distance;
818
819        if (stopTimer != null) {
820            stopTimer.stop();
821            distanceRemaining = distance;
822        } else {
823            calculatedDistance = calculateInitialOverRun(distance);
824            distanceRemaining = calculatedDistance;
825        }
826        if (distanceRemaining < 0.0f) {
827            if (andStop) {
828                _throttle.setSpeedSetting(0.0f);
829            } else {
830                _throttle.setSpeedSetting(speedStep);
831            }
832            log.warn("There is insufficient distance [{}] after adjustments, setting speed immediately", distanceRemaining);
833            return;
834        }
835
836        float calculatingStep = _throttle.getSpeedSetting();
837        if (increaseSpeed) {
838            if (calculatingStep < minReliableOperatingSpeed) {
839                calculatingStep = minReliableOperatingSpeed;
840            }
841        }
842
843        float endspd = 0;
844        if (calculatingStep != 0.0 && desiredSpeedStep > 0) { // current speed
845            if (_throttle.getIsForward()) {
846                endspd = getForwardSpeed(desiredSpeedStep);
847            } else {
848                endspd = getReverseSpeed(desiredSpeedStep);
849            }
850        } else if (desiredSpeedStep != 0.0) {
851            if (_throttle.getIsForward()) {
852                endspd = getForwardSpeed(desiredSpeedStep);
853            } else {
854                endspd = getReverseSpeed(desiredSpeedStep);
855            }
856        }
857
858        boolean calculated = false;
859        while (!calculated) {
860            float spd = 0;
861            if (calculatingStep != 0.0) { // current speed
862                if (_throttle.getIsForward()) {
863                    spd = getForwardSpeed(calculatingStep);
864                } else {
865                    spd = getReverseSpeed(calculatingStep);
866                }
867            }
868
869            log.debug("end spd {} spd {}", endspd, spd);
870            double avgSpeed = Math.abs((endspd + spd) * 0.5);
871            log.debug("avg Speed {}", avgSpeed);
872
873            double time = (calculatedDistance / avgSpeed); //in seconds
874            time = time * 1000; //covert it to milli seconds
875            float speeddiff = calculatingStep - desiredSpeedStep;
876            if (increaseSpeed) {
877                speeddiff =  desiredSpeedStep - calculatingStep;
878            }
879            float noSteps = speeddiff / stepIncrement;
880            log.debug("Speed diff {} number of Steps {} step increment {}", speeddiff, noSteps, stepIncrement);
881
882            int timePerStep = (int) (time / noSteps);
883            if (timePerStep < 0) {
884                log.error("Time per speed went to zero or below, setting finale speed immediatly.");
885                if (_throttle != null) {
886                    addSpeedStepItem(calculated,new SpeedSetting(desiredSpeedStep, 10, andStop));
887                    setNextStep();
888                }
889                break;
890            }
891            float calculatedStepInc = stepIncrement;
892            boolean lastStep = false;
893            if (Math.abs(speeddiff) > (stepIncrement * 2)) {
894                //We do not get reliable time results if the duration per speed step is less than 500ms
895                //therefore we calculate how many speed steps will fit in to 750ms.
896                if (timePerStep <= 500 && timePerStep > 0) {
897                    float newTime = 750.0f;
898                    float tmp =(float) Math.floor(newTime / timePerStep);
899                    // To avoid the lack of a stub ensure resultant speed is less than final speed by at least a step.
900                    if (increaseSpeed) {
901                        while (desiredSpeedStep - ( calculatingStep + (stepIncrement * tmp)) <= stepIncrement) {
902                            tmp = tmp - 1;
903                        }
904
905                        if (tmp > 0 && calculatedDistance - getDistanceTravelled(_throttle.getIsForward(),
906                                    calculatingStep + (stepIncrement * tmp),
907                                    ((float) (newTime / 1000.0))) > 0) {
908                            calculatedStepInc = stepIncrement * tmp;
909                            timePerStep = (int)newTime;
910                        }
911                    } else {
912                        while (calculatingStep - (stepIncrement * tmp) - desiredSpeedStep <= stepIncrement) {
913                            tmp = tmp - 1;
914                        }
915                        if ( tmp > 0 && (calculatedDistance
916                                - getDistanceTravelled(_throttle.getIsForward(),
917                                        calculatingStep - (stepIncrement * tmp),
918                                        ((float) (newTime / 1000.0)))) > 0) {
919                            calculatedStepInc = stepIncrement * tmp;
920                            timePerStep = (int)newTime;
921                        }
922                    }
923                    log.debug("time per step was {} no of increments in 750 ms is {} new step increment in {}", timePerStep, tmp, calculatedStepInc);
924                }
925            } else {
926                // last bit calculate duration from distance remaining
927                if (increaseSpeed && calculatingStep == 0) {
928                    calculatingStep+=calculatedStepInc;
929                }
930                timePerStep = Math.round(calculatedDistance/getSpeed(calculatingStep,_throttle.getIsForward())*1000);
931                if (!increaseSpeed) {
932                    calculatedStepInc = calculatingStep - desiredSpeedStep;
933                } else {
934                    calculatedStepInc = desiredSpeedStep - calculatingStep ;
935                }
936                lastStep=true;
937            }
938            calculatedStepInc=Math.abs(calculatedStepInc);
939            log.debug("per interval {}", timePerStep);
940            //Calculate the new speed setting
941            if (increaseSpeed) {
942                //if (calculatingStep + calculatedStepInc == desiredSpeedStep) {
943                if (lastStep) {
944                    // last step(s)
945                    SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop);
946                    addSpeedStepItem(calculated,ss);
947                    calculated = true;
948                    if (!andStop) { calculatingStep = desiredSpeedStep;timePerStep=2;}
949                    else {
950                        calculatingStep = 0.0f;timePerStep=2;
951                    }
952                    ss = new SpeedSetting(calculatingStep, timePerStep, andStop);
953                    addSpeedStepItem(calculated,ss);
954                    if (stopTimer == null) {
955                        setNextStep();
956                    }
957                    break;
958                }
959                calculatingStep = calculatingStep + calculatedStepInc;
960            } else {
961                if (lastStep) {
962                    SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop);
963                    addSpeedStepItem(calculated,ss);
964                    calculated = true;
965                    if (!andStop) { calculatingStep = desiredSpeedStep;timePerStep=2;}
966                    else {
967                        calculatingStep = 0.0f;timePerStep=2;
968                    }
969                    ss = new SpeedSetting(calculatingStep, timePerStep, andStop);
970                    addSpeedStepItem(calculated,ss);
971                    if (stopTimer == null) { //If this is the first time round then kick off the speed change
972                        setNextStep();
973                    }
974                    break;
975                }
976                calculatingStep = calculatingStep - calculatedStepInc;
977            }
978            log.debug("Speed Step current {} speed to set {}", _throttle.getSpeedSetting(), calculatingStep);
979            SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop);
980            addSpeedStepItem(calculated,ss);
981            if (stopTimer == null) { //If this is the first time round then kick off the speed change
982                setNextStep();
983            }
984            if (calculated) {
985               if (andStop) {
986                   ss = new SpeedSetting(0.0f, 10, andStop);
987               } else {
988                   ss = new SpeedSetting(desiredSpeedStep, 10, andStop);
989               }
990               addSpeedStepItem(calculated,ss);            }
991            // The throttle can disappear during a stop situation
992            if (_throttle != null) {
993                calculatedDistance = calculatedDistance - getDistanceTravelled(_throttle.getIsForward(), calculatingStep, ((float) (timePerStep / 1000.0)));
994            } else {
995                log.warn("Throttle destroyed before zero length[{}] remaining.",calculatedDistance);
996                calculatedDistance = 0;
997            }
998
999            if (calculatedDistance <= 0 && !calculated) {
1000                log.warn("distance remaining is now 0, but we have not reached desired speed setting {} v {}", desiredSpeedStep, calculatingStep);
1001                calculated = true;
1002            }
1003        }
1004    }
1005
1006    private void addSpeedStepItem(Boolean calculated, SpeedSetting ss) {
1007        synchronized (this) {
1008            stepQueue.addLast(ss);
1009            if (profileInTestMode) {
1010                testSteps.add(ss);
1011            }
1012            if (ss.andStop && calculated) {
1013                ss = new SpeedSetting( 0.0f, 0, ss.andStop);
1014                stepQueue.addLast(ss);
1015                if (profileInTestMode) {
1016                    testSteps.add(ss);
1017                }
1018            }
1019        }
1020    }
1021
1022    //The bit with the distance is not used
1023    float calculateInitialOverRun(float distance) {
1024        log.debug("Stop timer not configured so will add overrun {}", distance);
1025        if (_throttle.getIsForward()) {
1026            float extraAsDouble = (getOverRunTimeForward() + extraDelay) / 1000;
1027            if (log.isDebugEnabled()) {
1028                log.debug("Over run time to remove (Forward) {} {}", getOverRunTimeForward(), extraAsDouble);
1029            }
1030            float olddistance = getDistanceTravelled(true, _throttle.getSpeedSetting(), extraAsDouble);
1031            distance = distance - olddistance;
1032            //time = time-getOverRunTimeForward();
1033            //time = time-(extraAsDouble*1000);
1034        } else {
1035            float extraAsDouble = (getOverRunTimeReverse() + extraDelay) / 1000;
1036            if (log.isDebugEnabled()) {
1037                log.debug("Over run time to remove (Reverse) {} {}", getOverRunTimeReverse(), extraAsDouble);
1038            }
1039            float olddistance = getDistanceTravelled(false, _throttle.getSpeedSetting(), extraAsDouble);
1040            distance = distance - olddistance;
1041            //time = time-getOverRunTimeReverse();
1042            //time = time-(extraAsDouble*1000);
1043        }
1044        log.debug("Distance remaining {}", distance);
1045        //log.debug("Time after overrun removed " + time);
1046        return distance;
1047
1048    }
1049
1050    /**
1051     * This method is called to cancel the existing change in speed.
1052     */
1053    public void cancelSpeedChange() {
1054        if (stopTimer != null && stopTimer.isRunning()) {
1055            stopTimer.stop();
1056        }
1057        finishChange();
1058    }
1059
1060    synchronized void setNextStep() {
1061        //if (profileInTestMode) {
1062        //    return;
1063        //}
1064        if (stepQueue.isEmpty()) {
1065            log.debug("No more results");
1066            finishChange();
1067            return;
1068        }
1069        SpeedSetting ss = stepQueue.getFirst();
1070        if (ss.getDuration() == 0) {
1071            if (ss.getAndStop()) {
1072                _throttle.setSpeedSetting(0.0f);
1073            } else {
1074                _throttle.setSpeedSetting(desiredSpeedStep);
1075            }
1076            finishChange();
1077            return;
1078        }
1079        if (stopTimer != null) {
1080            //Reduce the distanceRemaining and calculate the distance travelling
1081            float distanceTravelledThisStep = getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (stopTimer.getDelay() / 1000.0)));
1082            distanceTravelled = distanceTravelled + distanceTravelledThisStep;
1083            distanceRemaining = distanceRemaining - distanceTravelledThisStep;
1084        }
1085        stepQueue.removeFirst();
1086        _throttle.setSpeedSetting(ss.getSpeedStep());
1087        stopTimer = new javax.swing.Timer(ss.getDuration(), (java.awt.event.ActionEvent e) -> {
1088            setNextStep();
1089        });
1090        stopTimer.setRepeats(false);
1091        lastTimeTimerStarted = System.nanoTime();
1092        stopTimer.start();
1093
1094    }
1095
1096    private LinkedList<SpeedSetting> stepQueue = new LinkedList<>();
1097
1098    public static class SpeedSetting {
1099
1100        private float step = 0.0f;
1101        private int duration = 0;
1102        private boolean andStop;
1103
1104        public SpeedSetting(float step, int duration, boolean andStop) {
1105            this.step = step;
1106            this.duration = duration;
1107            this.andStop = andStop;
1108        }
1109
1110        public float getSpeedStep() {
1111            return step;
1112        }
1113
1114        public int getDuration() {
1115            return duration;
1116        }
1117
1118        public boolean getAndStop() {
1119            return andStop;
1120        }
1121    }
1122
1123    /*
1124     * The follow deals with the storage and loading of the speed profile for a roster entry.
1125     */
1126    public void store(Element e) {
1127        Element d = new Element("speedprofile");
1128        d.addContent(new Element("overRunTimeForward").addContent(Float.toString(getOverRunTimeForward())));
1129        d.addContent(new Element("overRunTimeReverse").addContent(Float.toString(getOverRunTimeReverse())));
1130        Element s = new Element("speeds");
1131        speeds.keySet().stream().forEachOrdered( i -> {
1132            Element ss = new Element("speed");
1133            ss.addContent(new Element("step").addContent(Integer.toString(i)));
1134            ss.addContent(new Element("forward").addContent(Float.toString(speeds.get(i).getForwardSpeed())));
1135            ss.addContent(new Element("reverse").addContent(Float.toString(speeds.get(i).getReverseSpeed())));
1136            s.addContent(ss);
1137        });
1138        d.addContent(s);
1139        e.addContent(d);
1140    }
1141
1142    public void load(Element e) {
1143        try {
1144            setOverRunTimeForward(Float.parseFloat(e.getChild("overRunTimeForward").getText()));
1145        } catch (NumberFormatException ex) {
1146            log.error("Over run Error For {}", _re.getId());
1147        }
1148        try {
1149            setOverRunTimeReverse(Float.parseFloat(e.getChild("overRunTimeReverse").getText()));
1150        } catch (NumberFormatException ex) {
1151            log.error("Over Run Error Rev {}", _re.getId());
1152        }
1153        e.getChild("speeds").getChildren("speed").forEach( spd -> {
1154            try {
1155                String step = spd.getChild("step").getText();
1156                String forward = spd.getChild("forward").getText();
1157                String reverse = spd.getChild("reverse").getText();
1158                float forwardSpeed = Float.parseFloat(forward);
1159                if (forwardSpeed > 0.0f) {
1160                    _hasForwardSpeeds = true;
1161                }
1162                float reverseSpeed = Float.parseFloat(reverse);
1163                if (reverseSpeed > 0.0f) {
1164                    _hasReverseSpeeds = true;
1165                }
1166                setSpeed(Integer.parseInt(step), forwardSpeed, reverseSpeed);
1167            } catch (NumberFormatException ex) {
1168                log.error("Not loaded {}", ex.getMessage());
1169            }
1170        });
1171    }
1172
1173    public static class SpeedStep {
1174
1175        private float forward = 0.0f;
1176        private float reverse = 0.0f;
1177
1178        /**
1179         * Create a new SpeedStep, Reverse and Forward speeds are 0.
1180         */
1181        public SpeedStep() {
1182        }
1183
1184        /**
1185         * Set the Forward speed for the step.
1186         * @param speed the forward speed for the Step.
1187         */
1188        public void setForwardSpeed(float speed) {
1189            forward = speed;
1190        }
1191
1192        /**
1193         * Set the Reverse speed for the step.
1194         * @param speed the reverse speed for the Step.
1195         */
1196        public void setReverseSpeed(float speed) {
1197            reverse = speed;
1198        }
1199
1200        /**
1201         * Get the Forward Speed for the Step.
1202         * @return the forward speed.
1203         */
1204        public float getForwardSpeed() {
1205            return forward;
1206        }
1207
1208        /**
1209         * Get the Reverse Speed for the Step.
1210         * @return the reverse speed.
1211         */
1212        public float getReverseSpeed() {
1213            return reverse;
1214        }
1215
1216        @Override
1217        public boolean equals(Object obj) {
1218            if (this == obj) {
1219                return true;
1220            }
1221            if (obj == null || getClass() != obj.getClass()) {
1222                return false;
1223            }
1224            SpeedStep ss = (SpeedStep) obj;
1225            return Float.compare(ss.getForwardSpeed(), forward) == 0
1226                && Float.compare(ss.getReverseSpeed(), reverse) == 0;
1227        }
1228
1229            @Override
1230            public int hashCode() {
1231                int result = 17;
1232                result = 31 * result + Float.floatToIntBits(forward);
1233                result = 31 * result + Float.floatToIntBits(reverse);
1234                return result;
1235        }
1236
1237    }
1238
1239    /**
1240     * Get the number of SpeedSteps.
1241     * If there are too few SpeedSteps, it may be difficult to get reasonable
1242     * distances and speeds over a large range of throttle settings.
1243     * @return the number of Speed Steps in the profile.
1244     */
1245    public int getProfileSize() {
1246        return speeds.size();
1247    }
1248
1249    public TreeMap<Integer, SpeedStep> getProfileSpeeds() {
1250        return speeds;
1251    }
1252
1253    /**
1254     * Get the throttle setting to achieve a track speed
1255     *
1256     * @param speed     desired track speed in mm/sec
1257     * @param isForward direction
1258     * @return throttle setting
1259     */
1260    public float getThrottleSetting(float speed, boolean isForward) {
1261        if ((isForward && !_hasForwardSpeeds) || (!isForward && !_hasReverseSpeeds)) {
1262            return 0.0f;
1263        }
1264        int slowerKey = 0;
1265        float slowerValue = 0;
1266        float fasterKey;
1267        float fasterValue;
1268        Entry<Integer, SpeedStep> entry = speeds.firstEntry();
1269        if (entry == null) {
1270            log.warn("There is no speedprofile entries for [{}]", this.getRosterEntry().getId());
1271            return (0.0f);
1272        }
1273        // search through table until end or the entry is greater than
1274        // what we are looking for. This leaves the previous lower value in key. and slower
1275        // Note there may be zero values interspersed in the tree
1276        if (isForward) {
1277            fasterKey = entry.getKey();
1278            fasterValue = entry.getValue().getForwardSpeed();
1279            while (entry != null && entry.getValue().getForwardSpeed() < speed) {
1280                slowerKey = entry.getKey();
1281                float value = entry.getValue().getForwardSpeed();
1282                if (value > 0.0f) {
1283                    slowerValue = value;
1284                }
1285                entry = speeds.higherEntry(slowerKey);
1286                if (entry != null) {
1287                    fasterKey = entry.getKey();
1288                    value = entry.getValue().getForwardSpeed();
1289                    if (value > 0.0f) {
1290                        fasterValue = value;
1291                    }
1292                }
1293            }
1294        } else {
1295            fasterKey = entry.getKey();
1296            fasterValue = entry.getValue().getReverseSpeed();
1297            while (entry != null && entry.getValue().getReverseSpeed() < speed) {
1298                slowerKey = entry.getKey();
1299                float value = entry.getValue().getReverseSpeed();
1300                if (value > 0.0f) {
1301                    slowerValue = value;
1302                }
1303                entry = speeds.higherEntry(slowerKey);
1304                if (entry != null) {
1305                    fasterKey = entry.getKey();
1306                    value = entry.getValue().getReverseSpeed();
1307                    if (value > 0.0f) {
1308                        fasterValue = value;
1309                    }
1310                }
1311            }
1312        }
1313        log.debug("slowerKey={}, slowerValue={} fasterKey={} fasterValue={} for speed={}",
1314                slowerKey, slowerValue, fasterKey, fasterValue, speed);
1315        if (entry == null) {
1316            // faster does not exists use slower...
1317            if (slowerValue <= 0.0f) { // neither does slower
1318                return (0.0f);
1319            }
1320
1321            // extrapolate
1322            float key = slowerKey * speed / slowerValue;
1323            if (key < 1000.0f) {
1324                return key / 1000.0f;
1325            } else {
1326                return 1.0f;
1327            }
1328        }
1329        if (Float.compare(slowerValue, speed) == 0 || fasterValue <= slowerValue) {
1330            return slowerKey / 1000.0f;
1331        }
1332        if (slowerValue <= 0.0f) {  // no entry had a slower speed, therefore key is invalid
1333            slowerKey = 0;
1334            if (fasterValue <= 0.0f) {  // neither is there a faster speed
1335                return (0.0f);
1336            }
1337        }
1338        // we need to interpolate
1339        float ratio = (speed - slowerValue) / (fasterValue - slowerValue);
1340        return (slowerKey + ((fasterKey - slowerKey) * ratio)) / 1000.0f;
1341    }
1342
1343    /**
1344     * Get track speed in millimeters per second from throttle setting
1345     *
1346     * @param speedStep  throttle setting
1347     * @param isForward  direction
1348     * @return track speed
1349     */
1350    public float getSpeed(float speedStep, boolean isForward) {
1351        if (speedStep < 0.00001f) {
1352            return 0.0f;
1353        }
1354        float speed;
1355        if (isForward) {
1356            speed = getForwardSpeed(speedStep);
1357        } else {
1358            speed = getReverseSpeed(speedStep);
1359        }
1360        return speed;
1361    }
1362
1363    private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(RosterSpeedProfile.class);
1364
1365}