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    protected 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    protected 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    }
618
619    /**
620     * Set speed of a throttle.
621     *
622     * @param t     the throttle to set
623     * @param blk   the block used for length details
624     * @param speed the speed to set
625     */
626    public void changeLocoSpeed(DccThrottle t, Block blk, float speed) {
627        if (blk == referenced && Float.compare(speed, desiredSpeedStep) == 0) {
628            //log.debug("Already setting to desired speed step for this block");
629            return;
630        }
631        float blockLength = blk.getLengthMm();
632        if (blk == referenced) {
633            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
634            blockLength = distanceRemaining;
635            //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
636            log.debug("Block passed is the same as we are currently processing");
637        } else {
638            referenced = blk;
639        }
640        changeLocoSpeed(t, blockLength, speed);
641    }
642
643    /**
644     * Set speed of a throttle.
645     *
646     * @param t     the throttle to set
647     * @param sec   the section used for length details
648     * @param speed the speed to set
649     * @param usePercentage the percentage of the block to be used for stopping
650     */
651    @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY",
652        justification = "OK to compare floats, as even tiny differences should trigger update")
653    public void changeLocoSpeed(DccThrottle t, Section sec, float speed, float usePercentage) {
654        if (sec == referenced && speed == desiredSpeedStep) {
655            log.debug("Already setting to desired speed step for this Section");
656            return;
657        }
658        float sectionLength = sec.getActualLength() * usePercentage;
659        if (sec == referenced) {
660            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
661            sectionLength = distanceRemaining;
662            //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
663            log.debug("Block passed is the same as we are currently processing");
664        } else {
665            referenced = sec;
666        }
667        changeLocoSpeed(t, sectionLength, speed);
668    }
669
670    /**
671     * Set speed of a throttle.
672     *
673     * @param t     the throttle to set
674     * @param blk   the block used for length details
675     * @param speed the speed to set
676     * @param usePercentage the percentage of the block to be used for stopping
677     */
678    @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY",
679        justification = "OK to compare floats, as even tiny differences should trigger update")
680    public void changeLocoSpeed(DccThrottle t, Block blk, float speed, float usePercentage) {
681        if (blk == referenced && speed == desiredSpeedStep) {
682            //if(log.isDebugEnabled()) log.debug("Already setting to desired speed step for this block");
683            return;
684        }
685        float blockLength = blk.getLengthMm() * usePercentage;
686        if (blk == referenced) {
687            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
688            blockLength = distanceRemaining;
689            //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
690            log.debug("Block passed is the same as we are currently processing");
691        } else {
692            referenced = blk;
693        }
694        changeLocoSpeed(t, blockLength, speed);
695
696    }
697
698    /**
699     * Set speed of a throttle to a speeed set by a float, using the section for
700     * the length details
701     * Set speed of a throttle.
702     *
703     * @param t     the throttle to set
704     * @param sec   the section used for length details
705     * @param speed the speed to set
706     */
707    //@TODO if a section contains multiple blocks then we could calibrate the change of speed based upon the block status change.
708    public void changeLocoSpeed(DccThrottle t, Section sec, float speed) {
709        if (sec == referenced && Float.compare(speed, desiredSpeedStep) == 0) {
710            log.debug("Already setting to desired speed step for this section");
711            return;
712        }
713        float sectionLength = sec.getActualLength();
714        log.debug("call to change speed via section {}", sec.getDisplayName());
715        if (sec == referenced) {
716            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
717            sectionLength = distanceRemaining;
718        } else {
719            referenced = sec;
720        }
721
722        changeLocoSpeed(t, sectionLength, speed);
723    }
724
725    /**
726     * Set speed of a throttle.
727     *
728     * @param t        the throttle to set
729     * @param distance the distance in meters
730     * @param requestedSpeed    the speed to set
731     */
732    public void changeLocoSpeed(DccThrottle t, float distance, float requestedSpeed) {
733        float speed = 0.0f;
734        log.debug("Call to change speed over specific distance float {} distance {}", requestedSpeed, distance);
735        if (requestedSpeed  > maxOperatingSpeed) {
736            speed = maxOperatingSpeed;
737        } else {
738            speed = requestedSpeed;
739        }
740        if (Float.compare(speed, t.getSpeedSetting()) == 0) {
741            log.debug("Throttle and request speed setting are the same {} {} so will quit", speed, t.getSpeedSetting());
742            //Already at correct speed setting
743            finishChange();
744            return;
745        }
746
747        if (Float.compare(speed, desiredSpeedStep) == 0) {
748            log.debug("Already setting to desired speed step");
749            return;
750        }
751        log.debug("public change speed step by float {}", speed);
752        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed);
753
754        if (stopTimer != null) {
755            log.debug("stop timer valid so will cancel");
756            cancelSpeedChange();
757        }
758        _throttle = t;
759
760        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed);
761        desiredSpeedStep = speed;
762
763        log.debug("calculated current step {} required {} current {}",
764            _throttle.getSpeedSetting(), speed, _throttle.getSpeedSetting());
765        if (_throttle.getSpeedSetting() < speed) {
766            log.debug("Going for acceleration");
767        } else {
768            log.debug("Going for deceleration");
769        }
770
771        float adjSpeed = speed;
772        boolean andStop = false;
773        if (speed <= 0.0 && minReliableOperatingSpeed > 0.0f) {
774            andStop = true;
775        }
776        if (speed < minReliableOperatingSpeed) {
777            adjSpeed = minReliableOperatingSpeed;
778        }
779        log.debug("Speed[{}] adjSpeed[{}] MinSpeed[{}]",
780                speed,adjSpeed, minReliableOperatingSpeed);
781        calculateStepDetails(adjSpeed, distance, andStop);
782    }
783
784    private List<SpeedSetting> testSteps = new ArrayList<>();
785    private boolean profileInTestMode = false;
786
787    void calculateStepDetails(float speedStep, float distance, boolean andStop) {
788
789        float stepIncrement = _throttle.getSpeedIncrement();
790        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speedStep);
791        desiredSpeedStep = speedStep;
792        //int step = Math.round(_throttle.getSpeedSetting()*1000);
793        log.debug("calculated current step {} required {} current {} increment {}", _throttle.getSpeedSetting(), speedStep, _throttle.getSpeedSetting(), stepIncrement);
794        boolean increaseSpeed = false;
795        if (_throttle.getSpeedSetting() < speedStep) {
796            increaseSpeed = true;
797            log.debug("Going for acceleration");
798        } else {
799            log.debug("Going for deceleration");
800        }
801
802        if (distance <= 0) {
803            log.debug("Distance is less than 0 {}", distance);
804            _throttle.setSpeedSetting(speedStep);
805            finishChange();
806            return;
807        }
808
809        float calculatedDistance = distance;
810
811        if (stopTimer != null) {
812            stopTimer.stop();
813            distanceRemaining = distance;
814        } else {
815            calculatedDistance = calculateInitialOverRun(distance);
816            distanceRemaining = calculatedDistance;
817        }
818
819        float calculatingStep = _throttle.getSpeedSetting();
820        if (increaseSpeed) {
821            if (calculatingStep < minReliableOperatingSpeed) {
822                calculatingStep = minReliableOperatingSpeed;
823            }
824        }
825
826        float endspd = 0;
827        if (calculatingStep != 0.0 && desiredSpeedStep > 0) { // current speed
828            if (_throttle.getIsForward()) {
829                endspd = getForwardSpeed(desiredSpeedStep);
830            } else {
831                endspd = getReverseSpeed(desiredSpeedStep);
832            }
833        } else if (desiredSpeedStep != 0.0) {
834            if (_throttle.getIsForward()) {
835                endspd = getForwardSpeed(desiredSpeedStep);
836            } else {
837                endspd = getReverseSpeed(desiredSpeedStep);
838            }
839        }
840
841        boolean calculated = false;
842
843        while (!calculated) {
844            float spd = 0;
845            if (calculatingStep != 0.0) { // current speed
846                if (_throttle.getIsForward()) {
847                    spd = getForwardSpeed(calculatingStep);
848                } else {
849                    spd = getReverseSpeed(calculatingStep);
850                }
851            }
852
853            log.debug("end spd {} spd {}", endspd, spd);
854            double avgSpeed = Math.abs((endspd + spd) * 0.5);
855            log.debug("avg Speed {}", avgSpeed);
856
857            double time = (calculatedDistance / avgSpeed); //in seconds
858            time = time * 1000; //covert it to milli seconds
859            /*if(stopTimer==null){
860             log.debug("time before remove over run " + time);
861             time = calculateInitialOverRun(time);//At the start we will deduct the over run time if configured
862             log.debug("time after remove over run " + time);
863             }*/
864            float speeddiff = calculatingStep - desiredSpeedStep;
865            float noSteps = speeddiff / stepIncrement;
866            log.debug("Speed diff {} number of Steps {} step increment {}", speeddiff, noSteps, stepIncrement);
867
868            int timePerStep = Math.abs((int) (time / noSteps));
869            float calculatedStepInc = stepIncrement;
870            if (calculatingStep > (stepIncrement * 2)) {
871                //We do not get reliable time results if the duration per speed step is less than 500ms
872                //therefore we calculate how many speed steps will fit in to 750ms.
873                if (timePerStep <= 500 && timePerStep > 0) {
874                    //thing tIncrement should be different not sure about this bit
875                    float tmp = (750.0f / timePerStep);
876                    calculatedStepInc = stepIncrement * tmp;
877                    log.debug("time per step was {} no of increments in 750 ms is {} new step increment in {}", timePerStep, tmp, calculatedStepInc);
878
879                    timePerStep = 750;
880                }
881            }
882            log.debug("per interval {}", timePerStep);
883
884            //Calculate the new speed setting
885            if (increaseSpeed) {
886                calculatingStep = calculatingStep + calculatedStepInc;
887                if (calculatingStep > 1.0f) {
888                    calculatingStep = 1.0f;
889                    calculated = true;
890                }
891                if (calculatingStep > desiredSpeedStep) {
892                    calculatingStep = desiredSpeedStep;
893                    calculated = true;
894                }
895            } else {
896                calculatingStep = calculatingStep - calculatedStepInc;
897                if (calculatingStep < _throttle.getSpeedIncrement()) {
898                    calculatingStep = 0.0f;
899                    calculated = true;
900                    timePerStep = 0;
901                }
902                if (calculatingStep < desiredSpeedStep) {
903                    calculatingStep = desiredSpeedStep;
904                    calculated = true;
905                }
906            }
907            log.debug("Speed Step current {} speed to set {}", _throttle.getSpeedSetting(), calculatingStep);
908
909            SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep, andStop);
910            synchronized (this) {
911                stepQueue.addLast(ss);
912                if (profileInTestMode) {
913                    testSteps.add(ss);
914                }
915                if (andStop && calculated) {
916                    ss = new SpeedSetting( 0.0f, 0, andStop);
917                    stepQueue.addLast(ss);
918                    if (profileInTestMode) {
919                        testSteps.add(ss);
920                    }
921                }
922            }
923            if (stopTimer == null) { //If this is the first time round then kick off the speed change
924                setNextStep();
925            }
926
927            // The throttle can disappear during a stop situation
928            if (_throttle != null) {
929                calculatedDistance = calculatedDistance - getDistanceTravelled(_throttle.getIsForward(), calculatingStep, ((float) (timePerStep / 1000.0)));
930            } else {
931                log.warn("Throttle destroyed before zero length[{}] remaining.",calculatedDistance);
932                calculatedDistance = 0;
933            }
934            if (calculatedDistance <= 0 && !calculated) {
935                log.warn("distance remaining is now 0, but we have not reached desired speed setting {} v {}", desiredSpeedStep, calculatingStep);
936                ss = new SpeedSetting(desiredSpeedStep, 10, andStop);
937                synchronized (this) {
938                    stepQueue.addLast(ss);
939                }
940                calculated = true;
941            }
942        }
943    }
944
945    //The bit with the distance is not used
946    float calculateInitialOverRun(float distance) {
947        log.debug("Stop timer not configured so will add overrun {}", distance);
948        if (_throttle.getIsForward()) {
949            float extraAsDouble = (getOverRunTimeForward() + extraDelay) / 1000;
950            if (log.isDebugEnabled()) {
951                log.debug("Over run time to remove (Forward) {} {}", getOverRunTimeForward(), extraAsDouble);
952            }
953            float olddistance = getDistanceTravelled(true, _throttle.getSpeedSetting(), extraAsDouble);
954            distance = distance - olddistance;
955            //time = time-getOverRunTimeForward();
956            //time = time-(extraAsDouble*1000);
957        } else {
958            float extraAsDouble = (getOverRunTimeReverse() + extraDelay) / 1000;
959            if (log.isDebugEnabled()) {
960                log.debug("Over run time to remove (Reverse) {} {}", getOverRunTimeReverse(), extraAsDouble);
961            }
962            float olddistance = getDistanceTravelled(false, _throttle.getSpeedSetting(), extraAsDouble);
963            distance = distance - olddistance;
964            //time = time-getOverRunTimeReverse();
965            //time = time-(extraAsDouble*1000);
966        }
967        log.debug("Distance remaining {}", distance);
968        //log.debug("Time after overrun removed " + time);
969        return distance;
970
971    }
972
973    /**
974     * This method is called to cancel the existing change in speed.
975     */
976    public void cancelSpeedChange() {
977        if (stopTimer != null && stopTimer.isRunning()) {
978            stopTimer.stop();
979        }
980        finishChange();
981    }
982
983    synchronized void setNextStep() {
984        if (stepQueue.isEmpty()) {
985            log.debug("No more results");
986            finishChange();
987            return;
988        }
989        SpeedSetting ss = stepQueue.getFirst();
990        if (ss.getDuration() == 0) {
991            if (ss.getAndStop()) {
992                _throttle.setSpeedSetting(0.0f);
993            } else {
994                _throttle.setSpeedSetting(desiredSpeedStep);
995            }
996            finishChange();
997            return;
998        }
999        if (stopTimer != null) {
1000            //Reduce the distanceRemaining and calculate the distance travelling
1001            float distanceTravelledThisStep = getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (stopTimer.getDelay() / 1000.0)));
1002            distanceTravelled = distanceTravelled + distanceTravelledThisStep;
1003            distanceRemaining = distanceRemaining - distanceTravelledThisStep;
1004        }
1005        stepQueue.removeFirst();
1006        _throttle.setSpeedSetting(ss.getSpeedStep());
1007        stopTimer = new javax.swing.Timer(ss.getDuration(), (java.awt.event.ActionEvent e) -> {
1008            setNextStep();
1009        });
1010        stopTimer.setRepeats(false);
1011        lastTimeTimerStarted = System.nanoTime();
1012        stopTimer.start();
1013
1014    }
1015
1016    private LinkedList<SpeedSetting> stepQueue = new LinkedList<>();
1017
1018    static class SpeedSetting {
1019
1020        private float step = 0.0f;
1021        private int duration = 0;
1022        private boolean andStop;
1023
1024        SpeedSetting(float step, int duration, boolean andStop) {
1025            this.step = step;
1026            this.duration = duration;
1027            this.andStop = andStop;
1028        }
1029
1030        float getSpeedStep() {
1031            return step;
1032        }
1033
1034        int getDuration() {
1035            return duration;
1036        }
1037
1038        boolean getAndStop() {
1039            return andStop;
1040        }
1041    }
1042
1043    /*
1044     * The follow deals with the storage and loading of the speed profile for a roster entry.
1045     */
1046    public void store(Element e) {
1047        Element d = new Element("speedprofile");
1048        d.addContent(new Element("overRunTimeForward").addContent(Float.toString(getOverRunTimeForward())));
1049        d.addContent(new Element("overRunTimeReverse").addContent(Float.toString(getOverRunTimeReverse())));
1050        Element s = new Element("speeds");
1051        speeds.keySet().stream().forEachOrdered( i -> {
1052            Element ss = new Element("speed");
1053            ss.addContent(new Element("step").addContent(Integer.toString(i)));
1054            ss.addContent(new Element("forward").addContent(Float.toString(speeds.get(i).getForwardSpeed())));
1055            ss.addContent(new Element("reverse").addContent(Float.toString(speeds.get(i).getReverseSpeed())));
1056            s.addContent(ss);
1057        });
1058        d.addContent(s);
1059        e.addContent(d);
1060    }
1061
1062    public void load(Element e) {
1063        try {
1064            setOverRunTimeForward(Float.parseFloat(e.getChild("overRunTimeForward").getText()));
1065        } catch (NumberFormatException ex) {
1066            log.error("Over run Error For {}", _re.getId());
1067        }
1068        try {
1069            setOverRunTimeReverse(Float.parseFloat(e.getChild("overRunTimeReverse").getText()));
1070        } catch (NumberFormatException ex) {
1071            log.error("Over Run Error Rev {}", _re.getId());
1072        }
1073        e.getChild("speeds").getChildren("speed").forEach( spd -> {
1074            try {
1075                String step = spd.getChild("step").getText();
1076                String forward = spd.getChild("forward").getText();
1077                String reverse = spd.getChild("reverse").getText();
1078                float forwardSpeed = Float.parseFloat(forward);
1079                if (forwardSpeed > 0.0f) {
1080                    _hasForwardSpeeds = true;
1081                }
1082                float reverseSpeed = Float.parseFloat(reverse);
1083                if (reverseSpeed > 0.0f) {
1084                    _hasReverseSpeeds = true;
1085                }
1086                setSpeed(Integer.parseInt(step), forwardSpeed, reverseSpeed);
1087            } catch (NumberFormatException ex) {
1088                log.error("Not loaded {}", ex.getMessage());
1089            }
1090        });
1091    }
1092
1093    public static class SpeedStep {
1094
1095        private float forward = 0.0f;
1096        private float reverse = 0.0f;
1097
1098        /**
1099         * Create a new SpeedStep, Reverse and Forward speeds are 0.
1100         */
1101        public SpeedStep() {
1102        }
1103
1104        /**
1105         * Set the Forward speed for the step.
1106         * @param speed the forward speed for the Step.
1107         */
1108        public void setForwardSpeed(float speed) {
1109            forward = speed;
1110        }
1111
1112        /**
1113         * Set the Reverse speed for the step.
1114         * @param speed the reverse speed for the Step.
1115         */
1116        public void setReverseSpeed(float speed) {
1117            reverse = speed;
1118        }
1119
1120        /**
1121         * Get the Forward Speed for the Step.
1122         * @return the forward speed.
1123         */
1124        public float getForwardSpeed() {
1125            return forward;
1126        }
1127
1128        /**
1129         * Get the Reverse Speed for the Step.
1130         * @return the reverse speed.
1131         */
1132        public float getReverseSpeed() {
1133            return reverse;
1134        }
1135
1136        @Override
1137        public boolean equals(Object obj) {
1138            if (this == obj) {
1139                return true;
1140            }
1141            if (obj == null || getClass() != obj.getClass()) {
1142                return false;
1143            }
1144            SpeedStep ss = (SpeedStep) obj;
1145            return Float.compare(ss.getForwardSpeed(), forward) == 0
1146                && Float.compare(ss.getReverseSpeed(), reverse) == 0;
1147        }
1148
1149            @Override
1150            public int hashCode() {
1151                int result = 17;
1152                result = 31 * result + Float.floatToIntBits(forward);
1153                result = 31 * result + Float.floatToIntBits(reverse);
1154                return result;
1155        }
1156
1157    }
1158
1159    /**
1160     * Get the number of SpeedSteps.
1161     * If there are too few SpeedSteps, it may be difficult to get reasonable
1162     * distances and speeds over a large range of throttle settings.
1163     * @return the number of Speed Steps in the profile.
1164     */
1165    public int getProfileSize() {
1166        return speeds.size();
1167    }
1168
1169    public TreeMap<Integer, SpeedStep> getProfileSpeeds() {
1170        return speeds;
1171    }
1172
1173    /**
1174     * Get the throttle setting to achieve a track speed
1175     *
1176     * @param speed     desired track speed in mm/sec
1177     * @param isForward direction
1178     * @return throttle setting
1179     */
1180    public float getThrottleSetting(float speed, boolean isForward) {
1181        if ((isForward && !_hasForwardSpeeds) || (!isForward && !_hasReverseSpeeds)) {
1182            return 0.0f;
1183        }
1184        int slowerKey = 0;
1185        float slowerValue = 0;
1186        float fasterKey;
1187        float fasterValue;
1188        Entry<Integer, SpeedStep> entry = speeds.firstEntry();
1189        if (entry == null) {
1190            log.warn("There is no speedprofile entries for [{}]", this.getRosterEntry().getId());
1191            return (0.0f);
1192        }
1193        // search through table until end or the entry is greater than
1194        // what we are looking for. This leaves the previous lower value in key. and slower
1195        // Note there may be zero values interspersed in the tree
1196        if (isForward) {
1197            fasterKey = entry.getKey();
1198            fasterValue = entry.getValue().getForwardSpeed();
1199            while (entry != null && entry.getValue().getForwardSpeed() < speed) {
1200                slowerKey = entry.getKey();
1201                float value = entry.getValue().getForwardSpeed();
1202                if (value > 0.0f) {
1203                    slowerValue = value;
1204                }
1205                entry = speeds.higherEntry(slowerKey);
1206                if (entry != null) {
1207                    fasterKey = entry.getKey();
1208                    value = entry.getValue().getForwardSpeed();
1209                    if (value > 0.0f) {
1210                        fasterValue = value;
1211                    }
1212                }
1213            }
1214        } else {
1215            fasterKey = entry.getKey();
1216            fasterValue = entry.getValue().getReverseSpeed();
1217            while (entry != null && entry.getValue().getReverseSpeed() < speed) {
1218                slowerKey = entry.getKey();
1219                float value = entry.getValue().getReverseSpeed();
1220                if (value > 0.0f) {
1221                    slowerValue = value;
1222                }
1223                entry = speeds.higherEntry(slowerKey);
1224                if (entry != null) {
1225                    fasterKey = entry.getKey();
1226                    value = entry.getValue().getReverseSpeed();
1227                    if (value > 0.0f) {
1228                        fasterValue = value;
1229                    }
1230                }
1231            }
1232        }
1233        log.debug("slowerKey={}, slowerValue={} fasterKey={} fasterValue={} for speed={}",
1234                slowerKey, slowerValue, fasterKey, fasterValue, speed);
1235        if (entry == null) {
1236            // faster does not exists use slower...
1237            if (slowerValue <= 0.0f) { // neither does slower
1238                return (0.0f);
1239            }
1240
1241            // extrapolate
1242            float key = slowerKey * speed / slowerValue;
1243            if (key < 1000.0f) {
1244                return key / 1000.0f;
1245            } else {
1246                return 1.0f;
1247            }
1248        }
1249        if (Float.compare(slowerValue, speed) == 0 || fasterValue <= slowerValue) {
1250            return slowerKey / 1000.0f;
1251        }
1252        if (slowerValue <= 0.0f) {  // no entry had a slower speed, therefore key is invalid
1253            slowerKey = 0;
1254            if (fasterValue <= 0.0f) {  // neither is there a faster speed
1255                return (0.0f);
1256            }
1257        }
1258        // we need to interpolate
1259        float ratio = (speed - slowerValue) / (fasterValue - slowerValue);
1260        return (slowerKey + ((fasterKey - slowerKey) * ratio)) / 1000.0f;
1261    }
1262
1263    /**
1264     * Get track speed in millimeters per second from throttle setting
1265     *
1266     * @param speedStep  throttle setting
1267     * @param isForward  direction
1268     * @return track speed
1269     */
1270    public float getSpeed(float speedStep, boolean isForward) {
1271        if (speedStep < 0.00001f) {
1272            return 0.0f;
1273        }
1274        float speed;
1275        if (isForward) {
1276            speed = getForwardSpeed(speedStep);
1277        } else {
1278            speed = getReverseSpeed(speedStep);
1279        }
1280        return speed;
1281    }
1282
1283    private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(RosterSpeedProfile.class);
1284
1285}