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    public void setReverseSpeed(float speedStep, float reverse) {
241        if (reverse > 0.0f) {
242            _hasReverseSpeeds = true;
243        } else {
244            return;
245        }
246        int iSpeedStep = Math.round(speedStep * 1000);
247        if (!speeds.containsKey(iSpeedStep)) {
248            speeds.put(iSpeedStep, new SpeedStep());
249        }
250        SpeedStep ss = speeds.get(iSpeedStep);
251        ss.setReverseSpeed(reverse);
252    }
253
254    /**
255     * return the forward speed in milli-meters per second for a given
256     * percentage throttle
257     *
258     * @param speedStep which is actual percentage throttle
259     * @return MilliMetres per second using straight line interpolation for
260     *         missing points
261     */
262    public float getForwardSpeed(float speedStep) {
263        int iSpeedStep = Math.round(speedStep * 1000);
264        if (iSpeedStep <= 0 || !_hasForwardSpeeds) {
265            return 0.0f;
266        }
267        // Note there may be zero values interspersed in the tree
268        if (speeds.containsKey(iSpeedStep)) {
269            float speed = speeds.get(iSpeedStep).getForwardSpeed();
270            if (speed > 0.0f) {
271                return speed;
272            }
273        }
274        log.debug("no exact match forward for {}", iSpeedStep);
275        float lower = 0.0f;
276        float higher = 0.0f;
277        int highStep = iSpeedStep;
278        int lowStep = iSpeedStep;
279
280        Entry<Integer, SpeedStep> entry = speeds.higherEntry(highStep);
281        while (entry != null && higher <= 0.0f) {
282            highStep = entry.getKey();
283            float value = entry.getValue().getForwardSpeed();
284            if (value > 0.0f) {
285                higher = value;
286            }
287            entry = speeds.higherEntry(highStep);
288        }
289        boolean nothingHigher = (higher <= 0.0f);
290
291        entry = speeds.lowerEntry(lowStep);
292        while (entry != null && lower <= 0.0f) {
293            lowStep = entry.getKey();
294            float value = entry.getValue().getForwardSpeed();
295            if (value > 0.0f) {
296                lower = value;
297            }
298            entry = speeds.lowerEntry(lowStep);
299        }
300        log.debug("lowStep={}, lower={} highStep={} higher={} for iSpeedStep={}",
301                lowStep, lower, highStep, higher, iSpeedStep);
302        if (lower <= 0.0f) {      // nothing lower
303            if (nothingHigher) {
304                log.error("Nothing in speed Profile");
305                return 0.0f;       // no forward speeds at all
306            }
307            return higher * iSpeedStep / highStep;
308        }
309        if (nothingHigher) {
310            return lower * (1.0f + (iSpeedStep - lowStep) / (1000.0f - lowStep));
311        }
312
313        float valperstep = (higher - lower) / (highStep - lowStep);
314
315        float retValue = lower + (valperstep * (iSpeedStep - lowStep));
316        return retValue;
317    }
318
319    /**
320     * return the reverse speed in millimetres per second for a given percentage
321     * throttle
322     *
323     * @param speedStep percentage of throttle 0.nnn
324     * @return millimetres per second
325     */
326    public float getReverseSpeed(float speedStep) {
327        int iSpeedStep = Math.round(speedStep * 1000);
328        if (iSpeedStep <= 0 || !_hasReverseSpeeds) {
329            return 0.0f;
330        }
331        if (speeds.containsKey(iSpeedStep)) {
332            float speed = speeds.get(iSpeedStep).getReverseSpeed();
333            if (speed > 0.0f) {
334                return speed;
335            }
336        }
337        log.debug("no exact match reverse for {}", iSpeedStep);
338        float lower = 0.0f;
339        float higher = 0.0f;
340        int highStep = iSpeedStep;
341        int lowStep = iSpeedStep;
342        // Note there may be zero values interspersed in the tree
343
344        Entry<Integer, SpeedStep> entry = speeds.higherEntry(highStep);
345        while (entry != null && higher <= 0.0f) {
346            highStep = entry.getKey();
347            float value = entry.getValue().getReverseSpeed();
348            if (value > 0.0f) {
349                higher = value;
350            }
351            entry = speeds.higherEntry(highStep);
352        }
353        boolean nothingHigher = (higher <= 0.0f);
354        entry = speeds.lowerEntry(lowStep);
355        while (entry != null && lower <= 0.0f) {
356            lowStep = entry.getKey();
357            float value = entry.getValue().getReverseSpeed();
358            if (value > 0.0f) {
359                lower = value;
360            }
361            entry = speeds.lowerEntry(lowStep);
362        }
363        log.debug("lowStep={}, lower={} highStep={} higher={} for iSpeedStep={}",
364                lowStep, lower, highStep, higher, iSpeedStep);
365        if (lower <= 0.0f) {      // nothing lower
366            if (nothingHigher) {
367                log.error("Nothing in speed Profile");
368                return 0.0f;       // no reverse speeds at all
369            }
370            return higher * iSpeedStep / highStep;
371        }
372        if (nothingHigher) {
373            return lower * (1.0f + (iSpeedStep - lowStep) / (1000.0f - lowStep));
374        }
375
376        float valperstep = (higher - lower) / (highStep - lowStep);
377
378        float retValue = lower + (valperstep * (iSpeedStep - lowStep));
379        return retValue;
380    }
381
382    /**
383     * Get the approximate time a loco may travel a given distance at a given
384     * speed step.
385     *
386     * @param isForward true if loco is running forward; false otherwise
387     * @param speedStep the desired speed step
388     * @param distance  the desired distance in millimeters
389     * @return the approximate time in seconds
390     */
391    public float getDurationOfTravelInSeconds(boolean isForward, float speedStep, int distance) {
392        float spd;
393        if (isForward) {
394            spd = getForwardSpeed(speedStep);
395        } else {
396            spd = getReverseSpeed(speedStep);
397        }
398        if (spd <= 0.0f) {
399            log.error("Speed not available to compute duration of travel");
400            return 0.0f;
401        }
402        return (distance / spd);
403    }
404
405    /**
406     * Get the approximate distance a loco may travel a given duration at a
407     * given speed step.
408     *
409     * @param isForward true if loco is running forward; false otherwise
410     * @param speedStep the desired speed step
411     * @param duration  the desired time in seconds
412     * @return the approximate distance in millimeters
413     */
414    public float getDistanceTravelled(boolean isForward, float speedStep, float duration) {
415        float spd;
416        if (isForward) {
417            spd = getForwardSpeed(speedStep);
418        } else {
419            spd = getReverseSpeed(speedStep);
420        }
421        if (spd <= 0.01f) {
422            log.error("Speed not available to compute distance travelled");
423            return 0.0f;
424        }
425        return Math.abs(spd * duration);
426    }
427
428    float distanceRemaining = 0;
429    float distanceTravelled = 0;
430
431    TreeMap<Integer, SpeedStep> speeds = new TreeMap<>();
432
433    DccThrottle _throttle;
434
435    float desiredSpeedStep = -1;
436
437    float extraDelay = 0.0f;
438
439    NamedBean referenced = null;
440
441    javax.swing.Timer stopTimer = null;
442
443    long lastTimeTimerStarted = 0L;
444
445    /**
446     * reset everything back to default once the change has finished.
447     */
448    void finishChange() {
449        if (stopTimer != null) {
450            stopTimer.stop();
451        }
452        stopTimer = null;
453        _throttle = null;
454        distanceRemaining = 0;
455        desiredSpeedStep = -1;
456        extraDelay = 0.0f;
457        referenced = null;
458        synchronized (this) {
459            distanceTravelled = 0;
460            stepQueue = new LinkedList<>();
461        }
462        _throttle = null;
463    }
464
465    public void setExtraInitialDelay(float eDelay) {
466        extraDelay = eDelay;
467    }
468
469    /**
470     * Set speed of a throttle.
471     *
472     * @param t     the throttle to set
473     * @param blk   the block used for length details
474     * @param speed the speed to set
475     */
476    public void changeLocoSpeed(DccThrottle t, Block blk, float speed) {
477        if (blk == referenced && Float.compare(speed, desiredSpeedStep) == 0) {
478            //log.debug("Already setting to desired speed step for this block");
479            return;
480        }
481        float blockLength = blk.getLengthMm();
482        if (blk == referenced) {
483            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
484            blockLength = distanceRemaining;
485            //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
486            log.debug("Block passed is the same as we are currently processing");
487        } else {
488            referenced = blk;
489        }
490        changeLocoSpeed(t, blockLength, speed);
491    }
492
493    /**
494     * Set speed of a throttle.
495     *
496     * @param t     the throttle to set
497     * @param sec   the section used for length details
498     * @param speed the speed to set
499     * @param usePercentage the percentage of the block to be used for stopping
500     */
501    @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY",
502        justification = "OK to compare floats, as even tiny differences should trigger update")
503    public void changeLocoSpeed(DccThrottle t, Section sec, float speed, float usePercentage) {
504        if (sec == referenced && speed == desiredSpeedStep) {
505            log.debug("Already setting to desired speed step for this Section");
506            return;
507        }
508        float sectionLength = sec.getActualLength() * usePercentage;
509        if (sec == referenced) {
510            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
511            sectionLength = distanceRemaining;
512            //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
513            log.debug("Block passed is the same as we are currently processing");
514        } else {
515            referenced = sec;
516        }
517        changeLocoSpeed(t, sectionLength, speed);
518    }
519
520    /**
521     * Set speed of a throttle.
522     *
523     * @param t     the throttle to set
524     * @param blk   the block used for length details
525     * @param speed the speed to set
526     * @param usePercentage the percentage of the block to be used for stopping
527     */
528    @edu.umd.cs.findbugs.annotations.SuppressFBWarnings(value = "FE_FLOATING_POINT_EQUALITY",
529        justification = "OK to compare floats, as even tiny differences should trigger update")
530    public void changeLocoSpeed(DccThrottle t, Block blk, float speed, float usePercentage) {
531        if (blk == referenced && speed == desiredSpeedStep) {
532            //if(log.isDebugEnabled()) log.debug("Already setting to desired speed step for this block");
533            return;
534        }
535        float blockLength = blk.getLengthMm() * usePercentage;
536        if (blk == referenced) {
537            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
538            blockLength = distanceRemaining;
539            //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
540            log.debug("Block passed is the same as we are currently processing");
541        } else {
542            referenced = blk;
543        }
544        changeLocoSpeed(t, blockLength, speed);
545
546    }
547
548    /**
549     * Set speed of a throttle to a speeed set by a float, using the section for
550     * the length details
551     * Set speed of a throttle.
552     *
553     * @param t     the throttle to set
554     * @param sec   the section used for length details
555     * @param speed the speed to set
556     */
557    //@TODO if a section contains multiple blocks then we could calibrate the change of speed based upon the block status change.
558    public void changeLocoSpeed(DccThrottle t, Section sec, float speed) {
559        if (sec == referenced && Float.compare(speed, desiredSpeedStep) == 0) {
560            log.debug("Already setting to desired speed step for this section");
561            return;
562        }
563        float sectionLength = sec.getActualLength();
564        log.debug("call to change speed via section {}", sec.getDisplayName());
565        if (sec == referenced) {
566            distanceRemaining = distanceRemaining - getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (System.nanoTime() - lastTimeTimerStarted) / 1000000000));
567            sectionLength = distanceRemaining;
568        } else {
569            referenced = sec;
570        }
571
572        changeLocoSpeed(t, sectionLength, speed);
573    }
574
575    /**
576     * Set speed of a throttle.
577     *
578     * @param t        the throttle to set
579     * @param distance the distance in meters
580     * @param speed    the speed to set
581     */
582    public void changeLocoSpeed(DccThrottle t, float distance, float speed) {
583        log.debug("Call to change speed over specific distance float {} distance {}", speed, distance);
584        if (Float.compare(speed, t.getSpeedSetting()) == 0) {
585            log.debug("Throttle and request speed setting are the same {} {} so will quit", speed, t.getSpeedSetting());
586            //Already at correct speed setting
587            finishChange();
588            return;
589        }
590
591        if (Float.compare(speed, desiredSpeedStep) == 0) {
592            log.debug("Already setting to desired speed step");
593            return;
594        }
595        log.debug("public change speed step by float {}", speed);
596        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed);
597
598        if (stopTimer != null) {
599            log.debug("stop timer valid so will cancel");
600            cancelSpeedChange();
601        }
602        _throttle = t;
603
604        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speed);
605        desiredSpeedStep = speed;
606
607        log.debug("calculated current step {} required {} current {}", _throttle.getSpeedSetting(), speed, _throttle.getSpeedSetting());
608        if (_throttle.getSpeedSetting() < speed) {
609            log.debug("Going for acceleration");
610        } else {
611            log.debug("Going for deceleration");
612        }
613
614        calculateStepDetails(speed, distance);
615    }
616
617    int extraTime = 0;
618
619    void calculateStepDetails(float speedStep, float distance) {
620
621        float stepIncrement = _throttle.getSpeedIncrement();
622        log.debug("Desired Speed Step {} asked for {}", desiredSpeedStep, speedStep);
623        desiredSpeedStep = speedStep;
624        //int step = Math.round(_throttle.getSpeedSetting()*1000);
625        log.debug("calculated current step {} required {} current {} increment {}", _throttle.getSpeedSetting(), speedStep, _throttle.getSpeedSetting(), stepIncrement);
626        boolean increaseSpeed = false;
627        if (_throttle.getSpeedSetting() < speedStep) {
628            increaseSpeed = true;
629            log.debug("Going for acceleration");
630        } else {
631            log.debug("Going for deceleration");
632        }
633
634        if (distance <= 0) {
635            log.debug("Distance is less than 0 {}", distance);
636            _throttle.setSpeedSetting(speedStep);
637            finishChange();
638            return;
639        }
640
641        float calculatedDistance = distance;
642
643        if (stopTimer != null) {
644            stopTimer.stop();
645            distanceRemaining = distance;
646        } else {
647            calculatedDistance = calculateInitialOverRun(distance);
648            distanceRemaining = calculatedDistance;
649        }
650
651        float calculatingStep = _throttle.getSpeedSetting();
652
653        float endspd = 0;
654        if (calculatingStep != 0.0 && desiredSpeedStep > 0) { // current speed
655            if (_throttle.getIsForward()) {
656                endspd = getForwardSpeed(desiredSpeedStep);
657            } else {
658                endspd = getReverseSpeed(desiredSpeedStep);
659            }
660        } else if (desiredSpeedStep != 0.0) {
661            if (_throttle.getIsForward()) {
662                endspd = getForwardSpeed(desiredSpeedStep);
663            } else {
664                endspd = getReverseSpeed(desiredSpeedStep);
665            }
666        }
667
668        boolean calculated = false;
669
670        while (!calculated) {
671            float spd = 0;
672            if (calculatingStep != 0.0) { // current speed
673                if (_throttle.getIsForward()) {
674                    spd = getForwardSpeed(calculatingStep);
675                } else {
676                    spd = getReverseSpeed(calculatingStep);
677                }
678            }
679
680            log.debug("end spd {} spd {}", endspd, spd);
681            double avgSpeed = Math.abs((endspd + spd) * 0.5);
682            log.debug("avg Speed {}", avgSpeed);
683
684            double time = (calculatedDistance / avgSpeed); //in seconds
685            time = time * 1000; //covert it to milli seconds
686            /*if(stopTimer==null){
687             log.debug("time before remove over run " + time);
688             time = calculateInitialOverRun(time);//At the start we will deduct the over run time if configured
689             log.debug("time after remove over run " + time);
690             }*/
691            float speeddiff = calculatingStep - desiredSpeedStep;
692            float noSteps = speeddiff / stepIncrement;
693            log.debug("Speed diff {} number of Steps {} step increment {}", speeddiff, noSteps, stepIncrement);
694
695            int timePerStep = Math.abs((int) (time / noSteps));
696            float calculatedStepInc = stepIncrement;
697            if (calculatingStep > (stepIncrement * 2)) {
698                //We do not get reliable time results if the duration per speed step is less than 500ms
699                //therefore we calculate how many speed steps will fit in to 750ms.
700                if (timePerStep <= 500 && timePerStep > 0) {
701                    //thing tIncrement should be different not sure about this bit
702                    float tmp = (750.0f / timePerStep);
703                    calculatedStepInc = stepIncrement * tmp;
704                    log.debug("time per step was {} no of increments in 750 ms is {} new step increment in {}", timePerStep, tmp, calculatedStepInc);
705
706                    timePerStep = 750;
707                }
708            }
709            log.debug("per interval {}", timePerStep);
710
711            //Calculate the new speed setting
712            if (increaseSpeed) {
713                calculatingStep = calculatingStep + calculatedStepInc;
714                if (calculatingStep > 1.0f) {
715                    calculatingStep = 1.0f;
716                    calculated = true;
717                }
718                if (calculatingStep > desiredSpeedStep) {
719                    calculatingStep = desiredSpeedStep;
720                    calculated = true;
721                }
722            } else {
723                calculatingStep = calculatingStep - calculatedStepInc;
724                if (calculatingStep < _throttle.getSpeedIncrement()) {
725                    calculatingStep = 0.0f;
726                    calculated = true;
727                    timePerStep = 0;
728                }
729                if (calculatingStep < desiredSpeedStep) {
730                    calculatingStep = desiredSpeedStep;
731                    calculated = true;
732                }
733            }
734            log.debug("Speed Step current {} speed to set {}", _throttle.getSpeedSetting(), calculatingStep);
735
736            SpeedSetting ss = new SpeedSetting(calculatingStep, timePerStep);
737            synchronized (this) {
738                stepQueue.addLast(ss);
739            }
740            if (stopTimer == null) { //If this is the first time round then kick off the speed change
741                setNextStep();
742            }
743
744            // The throttle can disappear during a stop situation
745            if (_throttle != null) {
746                calculatedDistance = calculatedDistance - getDistanceTravelled(_throttle.getIsForward(), calculatingStep, ((float) (timePerStep / 1000.0)));
747            } else {
748                log.warn("Throttle destroyed before zero length[{}] remaining.",calculatedDistance);
749                calculatedDistance = 0;
750            }
751            if (calculatedDistance < 0 && !calculated) {
752                log.error("distance remaining is now 0, but we have not reached desired speed setting {} v {}", desiredSpeedStep, calculatingStep);
753                ss = new SpeedSetting(desiredSpeedStep, 10);
754                synchronized (this) {
755                    stepQueue.addLast(ss);
756                }
757                calculated = true;
758            }
759        }
760    }
761
762    //The bit with the distance is not used
763    float calculateInitialOverRun(float distance) {
764        log.debug("Stop timer not configured so will add overrun {}", distance);
765        if (_throttle.getIsForward()) {
766            float extraAsDouble = (getOverRunTimeForward() + extraDelay) / 1000;
767            if (log.isDebugEnabled()) {
768                log.debug("Over run time to remove (Forward) {}", getOverRunTimeForward());
769                log.debug("{}", extraAsDouble);
770            }
771            float olddistance = getDistanceTravelled(true, _throttle.getSpeedSetting(), extraAsDouble);
772            distance = distance - olddistance;
773            //time = time-getOverRunTimeForward();
774            //time = time-(extraAsDouble*1000);
775        } else {
776            float extraAsDouble = (getOverRunTimeReverse() + extraDelay) / 1000;
777            if (log.isDebugEnabled()) {
778                log.debug("Over run time to remove (Reverse) {}", getOverRunTimeReverse());
779                log.debug("{}", extraAsDouble);
780            }
781            float olddistance = getDistanceTravelled(false, _throttle.getSpeedSetting(), extraAsDouble);
782            distance = distance - olddistance;
783            //time = time-getOverRunTimeReverse();
784            //time = time-(extraAsDouble*1000);
785        }
786        log.debug("Distance remaining {}", distance);
787        //log.debug("Time after overrun removed " + time);
788        return distance;
789
790    }
791
792    void stopLocoTimeOut(DccThrottle t) {
793        log.debug("Stopping loco");
794        t.setSpeedSetting(0f);
795    }
796
797    /**
798     * This method is called to cancel the existing change in speed.
799     */
800    public void cancelSpeedChange() {
801        if (stopTimer != null && stopTimer.isRunning()) {
802            stopTimer.stop();
803        }
804        finishChange();
805    }
806
807    synchronized void setNextStep() {
808        if (stepQueue.isEmpty()) {
809            log.debug("No more results");
810            finishChange();
811            return;
812        }
813        SpeedSetting ss = stepQueue.getFirst();
814        if (ss.getDuration() == 0) {
815            _throttle.setSpeedSetting(0);
816            finishChange();
817            return;
818        }
819        if (stopTimer != null) {
820            //Reduce the distanceRemaining and calculate the distance travelling
821            float distanceTravelledThisStep = getDistanceTravelled(_throttle.getIsForward(), _throttle.getSpeedSetting(), ((float) (stopTimer.getDelay() / 1000.0)));
822            distanceTravelled = distanceTravelled + distanceTravelledThisStep;
823            distanceRemaining = distanceRemaining - distanceTravelledThisStep;
824        }
825        stepQueue.removeFirst();
826        _throttle.setSpeedSetting(ss.getSpeedStep());
827        stopTimer = new javax.swing.Timer(ss.getDuration(), (java.awt.event.ActionEvent e) -> {
828            setNextStep();
829        });
830        stopTimer.setRepeats(false);
831        lastTimeTimerStarted = System.nanoTime();
832        stopTimer.start();
833
834    }
835
836    LinkedList<SpeedSetting> stepQueue = new LinkedList<>();
837
838    static class SpeedSetting {
839
840        float step = 0.0f;
841        int duration = 0;
842
843        SpeedSetting(float step, int duration) {
844            this.step = step;
845            this.duration = duration;
846        }
847
848        float getSpeedStep() {
849            return step;
850        }
851
852        int getDuration() {
853            return duration;
854        }
855    }
856
857    /*
858     * The follow deals with the storage and loading of the speed profile for a roster entry.
859     */
860    public void store(Element e) {
861        Element d = new Element("speedprofile");
862        d.addContent(new Element("overRunTimeForward").addContent(Float.toString(getOverRunTimeForward())));
863        d.addContent(new Element("overRunTimeReverse").addContent(Float.toString(getOverRunTimeReverse())));
864        Element s = new Element("speeds");
865        speeds.keySet().stream().forEachOrdered((i) -> {
866            Element ss = new Element("speed");
867            ss.addContent(new Element("step").addContent(Integer.toString(i)));
868            ss.addContent(new Element("forward").addContent(Float.toString(speeds.get(i).getForwardSpeed())));
869            ss.addContent(new Element("reverse").addContent(Float.toString(speeds.get(i).getReverseSpeed())));
870            s.addContent(ss);
871        });
872        d.addContent(s);
873        e.addContent(d);
874    }
875
876    public void load(Element e) {
877        try {
878            setOverRunTimeForward(Float.parseFloat(e.getChild("overRunTimeForward").getText()));
879        } catch (NumberFormatException ex) {
880            log.error("Over run Error For {}", _re.getId());
881        }
882        try {
883            setOverRunTimeReverse(Float.parseFloat(e.getChild("overRunTimeReverse").getText()));
884        } catch (NumberFormatException ex) {
885            log.error("Over Run Error Rev {}", _re.getId());
886        }
887        e.getChild("speeds").getChildren("speed").forEach((spd) -> {
888            try {
889                String step = spd.getChild("step").getText();
890                String forward = spd.getChild("forward").getText();
891                String reverse = spd.getChild("reverse").getText();
892                float forwardSpeed = Float.parseFloat(forward);
893                if (forwardSpeed > 0.0f) {
894                    _hasForwardSpeeds = true;
895                }
896                float reverseSpeed = Float.parseFloat(reverse);
897                if (reverseSpeed > 0.0f) {
898                    _hasReverseSpeeds = true;
899                }
900                setSpeed(Integer.parseInt(step), forwardSpeed, reverseSpeed);
901            } catch (NumberFormatException ex) {
902                log.error("Not loaded {}", ex.getMessage());
903            }
904        });
905    }
906
907    static public class SpeedStep {
908
909        float forward = 0.0f;
910        float reverse = 0.0f;
911
912        public SpeedStep() {
913        }
914
915        public void setForwardSpeed(float speed) {
916            forward = speed;
917        }
918
919        public void setReverseSpeed(float speed) {
920            reverse = speed;
921        }
922
923        public float getForwardSpeed() {
924            return forward;
925        }
926
927        public float getReverseSpeed() {
928            return reverse;
929        }
930    }
931
932    /* If there are too few SpeedSteps to get reasonable distances and speeds
933     * over a good range of throttle settings get whatever SpeedSteps exist.
934     */
935    public int getProfileSize() {
936        return speeds.size();
937    }
938
939    public TreeMap<Integer, SpeedStep> getProfileSpeeds() {
940        return speeds;
941    }
942
943    /**
944     * Get the throttle setting to achieve a track speed
945     *
946     * @param speed     desired track speed in mm/sec
947     * @param isForward direction
948     * @return throttle setting
949     */
950    public float getThrottleSetting(float speed, boolean isForward) {
951        if ((isForward && !_hasForwardSpeeds) || (!isForward && !_hasReverseSpeeds)) {
952            return 0.0f;
953        }
954        int slowerKey = 0;
955        float slowerValue = 0;
956        float fasterKey;
957        float fasterValue;
958        Entry<Integer, SpeedStep> entry = speeds.firstEntry();
959        if (entry == null) {
960            log.warn("There is no speedprofile entries for [{}]", this.getRosterEntry().getId());
961            return (0.0f);
962        }
963        // search through table until end or the entry is greater than
964        // what we are looking for. This leaves the previous lower value in key. and slower
965        // Note there may be zero values interspersed in the tree
966        if (isForward) {
967            fasterKey = entry.getKey();
968            fasterValue = entry.getValue().getForwardSpeed();
969            while (entry != null && entry.getValue().getForwardSpeed() < speed) {
970                slowerKey = entry.getKey();
971                float value = entry.getValue().getForwardSpeed();
972                if (value > 0.0f) {
973                    slowerValue = value;
974                }
975                entry = speeds.higherEntry(slowerKey);
976                if (entry != null) {
977                    fasterKey = entry.getKey();
978                    value = entry.getValue().getForwardSpeed();
979                    if (value > 0.0f) {
980                        fasterValue = value;
981                    }
982                }
983            }
984        } else {
985            fasterKey = entry.getKey();
986            fasterValue = entry.getValue().getReverseSpeed();
987            while (entry != null && entry.getValue().getReverseSpeed() < speed) {
988                slowerKey = entry.getKey();
989                float value = entry.getValue().getReverseSpeed();
990                if (value > 0.0f) {
991                    slowerValue = value;
992                }
993                entry = speeds.higherEntry(slowerKey);
994                if (entry != null) {
995                    fasterKey = entry.getKey();
996                    value = entry.getValue().getReverseSpeed();
997                    if (value > 0.0f) {
998                        fasterValue = value;
999                    }
1000                }
1001            }
1002        }
1003        log.debug("slowerKey={}, slowerValue={} fasterKey={} fasterValue={} for speed={}",
1004                slowerKey, slowerValue, fasterKey, fasterValue, speed);
1005        if (entry == null) {
1006            // faster does not exists use slower...
1007            if (slowerValue <= 0.0f) { // neither does slower
1008                return (0.0f);
1009            }
1010            //return slowerKey / 1000;
1011            // extrapolate instead
1012            float key = slowerKey * speed / slowerValue;
1013            if (key < 1000.0f) {
1014                return key / 1000.0f;
1015            } else {
1016                return 1.0f;
1017            }
1018        }
1019        if (Float.compare(slowerValue, speed) == 0 || fasterValue <= slowerValue) {
1020            return slowerKey / 1000.0f;
1021        }
1022        if (slowerValue <= 0.0f) {  // no entry had a slower speed, therefore key is invalid
1023            slowerKey = 0;
1024            if (fasterValue <= 0.0f) {  // neither is there a faster speed
1025                return (0.0f);
1026            }
1027        }
1028        // we need to interpolate
1029        float ratio = (speed - slowerValue) / (fasterValue - slowerValue);
1030        float setting = (slowerKey + ((fasterKey - slowerKey) * ratio)) / 1000.0f;
1031        return setting;
1032    }
1033
1034    /**
1035     * Get track speed in millimeters per second from throttle setting
1036     *
1037     * @param speedStep  throttle setting
1038     * @param isForward  direction
1039     * @return track speed
1040     */
1041    public float getSpeed(float speedStep, boolean isForward) {
1042        if (speedStep < 0.00001f) {
1043            return 0.0f;
1044        }
1045        float speed;
1046        if (isForward) {
1047            speed = getForwardSpeed(speedStep);
1048        } else {
1049            speed = getReverseSpeed(speedStep);
1050        }
1051        return speed;
1052    }
1053
1054    private final static Logger log = LoggerFactory.getLogger(RosterSpeedProfile.class);
1055}