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