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