001package jmri.jmrit.logix;
002
003import java.io.File;
004import java.io.IOException;
005import java.io.FileNotFoundException;
006import java.util.ArrayList;
007import java.util.List;
008import java.util.TreeMap;
009import java.util.Map.Entry;
010
011import javax.annotation.CheckForNull;
012
013import jmri.DccLocoAddress;
014import jmri.DccThrottle;
015import jmri.InstanceManager;
016import jmri.LocoAddress;
017import jmri.LocoAddress.Protocol;
018import jmri.implementation.SignalSpeedMap;
019import jmri.jmrit.XmlFile;
020import jmri.jmrit.logix.ThrottleSetting.Command;
021import jmri.jmrit.logix.ThrottleSetting.CommandValue;
022import jmri.jmrit.logix.ThrottleSetting.ValueType;
023import jmri.jmrit.roster.Roster;
024import jmri.jmrit.roster.RosterEntry;
025import jmri.jmrit.roster.RosterSpeedProfile;
026import jmri.jmrit.roster.RosterSpeedProfile.SpeedStep;
027
028import org.jdom2.Attribute;
029import org.jdom2.Element;
030import org.jdom2.JDOMException;
031
032/**
033 * All speed related method transferred from Engineer and Warrant classes.
034 * Until June 2017, the problem of determining the actual track speed of a
035 * model train in millimeters per millisecond (same as meters/sec) from the
036 * throttle setting was usually done with an ad hoc "throttle factor".  When
037 * created, the RosterSpeedProfile provides this needed conversion but
038 * generally is not done by users for each of their locos.
039 *
040 * Methods to dynamically determine a RosterSpeedProfile for each loco are
041 * implemented in this class.
042 *
043 * @author Pete Cressman Copyright (C) 2009, 2010, 2017
044 *
045 */
046public class SpeedUtil {
047
048    private DccLocoAddress _dccAddress;
049    private String _rosterId;        // Roster title for train
050    private RosterEntry _rosterEntry;
051
052    private DccThrottle _throttle;
053    private boolean _isForward = true;
054    private float _rampThrottleIncrement;   // user specified throttle increment for ramping
055    private int _rampTimeIncrement; // user specified time for ramp step increment
056
057    private RosterSpeedProfile _sessionProfile; // speeds measured in the session
058    private final SignalSpeedMap _signalSpeedMap;
059    private int _ma;  // milliseconds needed to increase speed by throttle step amount
060    private int _md;  // milliseconds needed to decrease speed by throttle step amount
061    private ArrayList<BlockSpeedInfo> _speedInfo; // map max speeds and occupation times of each block in route
062
063    // A SCALE_FACTOR of 44.704 divided by _scale, computes a scale speed of 100mph at full throttle.
064    // This is set arbitrarily and can be modified by the Preferences "throttle Factor".
065    // Only used when there is no SpeedProfile.
066    public static final float SCALE_FACTOR = 44.704f; // divided by _scale, gives a rough approximation for track speed
067    public static final float MAX_TGV_SPEED = 88889;   // maximum speed of a Bullet train (320 km/hr) in millimeters/sec
068
069    protected SpeedUtil() {
070        _signalSpeedMap = InstanceManager.getDefault(SignalSpeedMap.class);
071    }
072
073    /**
074     * @return RosterEntry
075     */
076    @CheckForNull
077    public RosterEntry getRosterEntry() {
078        return _rosterEntry;
079    }
080
081    /**
082     * Set the key identifier for the Speed Profile
083     * If a RosterEntry exists, _rosterId is the RosterEntry id
084     * or possibly is the RosterEntrytitle.
085     * Otherwise it may be just the decoder address
086     * @return key to speedProfile
087     */
088    public String getRosterId() {
089        return _rosterId;
090    }
091
092    /**
093     * Set a key to a loco's roster and speed info.
094     * If there is no RosterEntry, the id still locates
095     * a session SpeedProfile for the loco.
096     * Called from:
097     *    SpeedUtil.setDccAdress(String) - main parser
098     *    WarrantFrame.setup() - edit existing warrant
099     *    WarrantManagerXml - load warrant
100     * @param id key to speedProfile
101     * @return true if RosterEntry exists for id
102     */
103    public boolean setRosterId(String id) {
104        log.trace("setRosterId({}) old={}", id, _rosterId);
105        if (id == null || id.isEmpty()) {
106            _rosterEntry = null;
107            _sessionProfile = null;
108            return false;
109        }
110        if (id.equals(_rosterId)) {
111            return true;
112        } else {
113            _sessionProfile = null;
114            RosterEntry re = Roster.getDefault().getEntryForId(id);
115            if (re != null) {
116                _rosterEntry = re;
117                _dccAddress = re.getDccLocoAddress();
118                _rosterId = id;
119                return true;
120            }
121        }
122        return false;
123    }
124
125    public DccLocoAddress getDccAddress() {
126        if (_dccAddress == null) {
127            if (_rosterEntry != null) {
128                _dccAddress = _rosterEntry.getDccLocoAddress();
129            }
130        }
131        return _dccAddress;
132    }
133
134    @CheckForNull
135    protected String getAddress() {
136        if (_dccAddress == null) {
137            _dccAddress = getDccAddress();
138        }
139        if (_dccAddress != null) {
140            return _dccAddress.toString();
141        }
142        return null;
143    }
144
145    /**
146     * Called by:
147     * Warrant.setRunMode() about to run a warrant
148     * WarrantFrame.setup() for an existing warrant
149     * WarrantTableModel.cloneWarrant() when cloning an existing warrant
150     *
151     * @param dccAddr DccLocoAddress
152     */
153    protected void setDccAddress(DccLocoAddress dccAddr) {
154        log.trace("setDccAddress(DccLocoAddress) _dccAddress= {}", _dccAddress);
155        if (dccAddr == null) {
156            _sessionProfile = null;
157            _rosterId = null;
158            _rosterEntry = null;
159            _dccAddress = null;
160            return;
161        }
162        if (!dccAddr.equals(_dccAddress)) {
163            _sessionProfile = null;
164            _dccAddress = dccAddr;
165        }
166    }
167
168    public boolean setDccAddress(int number, String type) {
169        log.trace("setDccAddress({}, {})", number, type);
170        LocoAddress.Protocol protocol;
171        switch (type) {
172            case "L":
173            case "l":
174                protocol = LocoAddress.Protocol.DCC_LONG;
175                break;
176            case "S":
177            case "s":
178                protocol = LocoAddress.Protocol.DCC_SHORT;
179                break;
180            default:
181                try {
182                    protocol = Protocol.getByPeopleName(type);
183                } catch (IllegalArgumentException iae) {
184                    try {
185                        protocol = Protocol.getByShortName(type.toLowerCase());
186                    } catch (IllegalArgumentException e) {
187                        _dccAddress = null;
188                        return false;
189                    }
190                }
191                break;
192        }
193        DccLocoAddress addr = new DccLocoAddress(number, protocol);
194        if (_rosterEntry != null && addr.equals(_rosterEntry.getDccLocoAddress())) {
195            return true;
196        } else {
197            _dccAddress = addr;
198            String numStr = String.valueOf(number);
199            List<RosterEntry> l = Roster.getDefault().matchingList(null, null,
200                    numStr, null, null, null, null);
201            if (!l.isEmpty()) {
202                int size = l.size();
203                if ( size!= 1) {
204                    log.info("{} entries for address {}, {}", l.size(), number, type);
205                }
206                _rosterEntry = l.get(size - 1);
207                setRosterId(_rosterEntry.getId());
208            } else {
209                // DCC address is set, but there is not a Roster entry for it
210                _rosterId = "$"+_dccAddress.toString()+"$";
211                makeRosterEntry(_rosterId);
212                _sessionProfile = null;
213            }
214        }
215        return true;
216    }
217
218    protected RosterEntry makeRosterEntry(String id) {
219        RosterEntry rosterEntry = new RosterEntry();
220        rosterEntry.setId(id);
221        DccLocoAddress dccAddr = getDccAddress();
222        if (dccAddr == null) {
223            return null;
224        }
225        rosterEntry.setDccAddress(String.valueOf(dccAddr.getNumber()));
226        rosterEntry.setProtocol(dccAddr.getProtocol());
227        rosterEntry.ensureFilenameExists();
228        return rosterEntry;
229    }
230
231    /**
232     * Sets dccAddress and key for a speedProfile.  Will fetch RosterEntry if one exists.
233     * If _rosterEntry exists, _rosterId set to RosterEntry Id (which may or not be "id")
234     * else _rosterId set to "id" or decoder address.
235     * Called from:
236     *    DefaultConditional.takeActionIfNeeded() - execute a setDccAddress action
237     *    SpeedUtil.makeSpeedTree() - need to use track speeds
238     *    WarrantFrame.checkTrainId() - about to run, assures address is set
239     *    Warrantroute.getRoster() - selection form _rosterBox
240     *    WarrantRoute.setAddress() - whatever is in _dccNumBox.getText()
241     *    WarrantRoute.setTrainPanel() - whatever in _dccNumBox.getText()
242     *    WarrantTableModel.setValue() - whatever address is put into the ADDRESS_COLUMN
243     * @param id address as a String, either RosterEntryTitle or decoder address
244     * @return true if address found for id
245     */
246    public boolean setAddress(String id) {
247        log.trace("setDccAddress: id= {}, _rosterId= {}", id, _rosterId);
248        if (id == null || id.isEmpty()) {
249            return false;
250        }
251        if (setRosterId(id)) {
252            return true;
253        }
254        int index = - 1;
255        for (int i=0; i<id.length(); i++) {
256            if (!Character.isDigit(id.charAt(i))) {
257                index = i;
258                break;
259            }
260        }
261        String numId;
262        String type;
263        if (index == -1) {
264            numId = id;
265            type = null;
266        } else {
267            int beginIdx;
268            int endIdx;
269            if (id.charAt(index) == '(') {
270                beginIdx = index + 1;
271            } else {
272                beginIdx = index;
273            }
274            if (id.charAt(id.length() - 1) == ')') {
275                endIdx = id.length() - 1;
276            } else {
277                endIdx = id.length();
278            }
279            numId = id.substring(0, index);
280            type = id.substring(beginIdx, endIdx);
281        }
282
283        int num;
284        try {
285            num = Integer.parseInt(numId);
286        } catch (NumberFormatException e) {
287            num = 0;
288        }
289        if (type == null) {
290            if (num > 128) {
291                type = "L";
292            } else {
293                type = "S";
294            }
295        }
296        if (!setDccAddress(num, type)) {
297            log.error("setDccAddress failed for ID= {} number={} type={}", id, num, type);
298            return false;
299        } else if (log.isTraceEnabled()) {
300            log.debug("setDccAddress({}): _rosterId= {}, _dccAddress= {}",
301                    id, _rosterId, _dccAddress.toString());
302        }
303        return true;
304    }
305
306    // Possibly customize these ramping values per warrant or loco later
307    // for now use global values set in WarrantPreferences
308    // user's ramp speed increase amount
309    protected float getRampThrottleIncrement() {
310        if (_rampThrottleIncrement <= 0) {
311            _rampThrottleIncrement = WarrantPreferences.getDefault().getThrottleIncrement();
312        }
313        return _rampThrottleIncrement;
314    }
315
316    protected void setRampThrottleIncrement(float incr) {
317        _rampThrottleIncrement = incr;
318    }
319
320    protected int getRampTimeIncrement() {
321        if (_rampTimeIncrement < 500) {
322            _rampTimeIncrement = WarrantPreferences.getDefault().getTimeIncrement();
323            if (_rampTimeIncrement <= 500) {
324                _rampTimeIncrement = 500;
325            }
326        }
327        return _rampTimeIncrement;
328    }
329
330    protected void setRampTimeIncrement(int incr) {
331        _rampTimeIncrement = incr;
332    }
333
334    /** ms momentum time to change speed for a throttle amount
335     * @param fromSpeed throttle change
336     * @param toSpeed throttle change
337     * @return momentum time
338     */
339    protected float getMomentumTime(float fromSpeed, float toSpeed) {
340        float incr = getThrottleSpeedStepIncrement();  // step amount
341        float time;
342        float delta;
343        if (fromSpeed < toSpeed) {
344            delta = toSpeed - fromSpeed;
345            time = _ma * delta / incr;   // accelerating
346        } else {
347            delta = fromSpeed - toSpeed;
348            time = _md * delta / incr;
349        }
350        // delta / incr ought to be number of speed steps
351        if (time < 2 * delta / incr) {
352            time = 2 * delta / incr;  // Even with CV == 0, there must be some time to change speed
353        }
354        if (log.isTraceEnabled()) {
355            log.debug("getMomentumTime for {}, addr={}. fromSpeed={}, toSpeed= {}, time= {}ms for {} steps",
356                    _rosterId, getAddress(), fromSpeed, toSpeed, time, delta / incr);
357        }
358        return time;
359    }
360
361    /**
362     * throttle's minimum speed change amount
363     * @return speed step amount
364     */
365    protected float getThrottleSpeedStepIncrement() {
366        // JMRI throttles don't seem to get actual values
367        if (_throttle != null) {
368            return _throttle.getSpeedIncrement();
369        }
370        return 1.0f / 126.0f;
371    }
372
373    // treeMap implementation in _mergeProfile is not synchronized
374    protected synchronized RosterSpeedProfile getMergeProfile() {
375        if (_sessionProfile == null) {
376            makeSpeedTree();
377            makeRampParameters();
378        }
379        return _sessionProfile;
380    }
381
382    private synchronized void makeSpeedTree() {
383        log.trace("makeSpeedTree for {}.", _rosterId);
384        WarrantManager manager = InstanceManager.getDefault(WarrantManager.class);
385        _sessionProfile = manager.getMergeProfile(_rosterId);
386        if (_sessionProfile == null) {
387            _rosterEntry = Roster.getDefault().getEntryForId(_rosterId);
388            RosterSpeedProfile profile;
389            if (_rosterEntry == null) {
390                _rosterEntry = makeRosterEntry(_rosterId);
391                profile = new RosterSpeedProfile(_rosterEntry);
392            } else {
393                profile = _rosterEntry.getSpeedProfile();
394                if (profile == null) {
395                    profile = new RosterSpeedProfile(_rosterEntry);
396                    _rosterEntry.setSpeedProfile(profile);
397                }
398            }
399            _sessionProfile = manager.makeProfileCopy(profile, _rosterEntry);
400            manager.setMergeProfile(_rosterId, _sessionProfile);
401        }
402
403        if (log.isTraceEnabled()) {
404            log.debug("SignalSpeedMap: throttle factor= {}, layout scale= {} convesion to mm/s= {}",
405                _signalSpeedMap.getDefaultThrottleFactor(), _signalSpeedMap.getLayoutScale(),
406                _signalSpeedMap.getDefaultThrottleFactor() * _signalSpeedMap.getLayoutScale() / SCALE_FACTOR);
407        }
408    }
409
410    private void makeRampParameters() {
411        _rampTimeIncrement = getRampTimeIncrement();    // get a value if not already set
412        _rampThrottleIncrement = getRampThrottleIncrement();
413        // default cv setting of momentum speed change per 1% of throttle increment
414        _ma = 0;  // time needed to accelerate one throttle speed step
415        _md = 0;  // time needed to decelerate one throttle speed step
416        if (_rosterEntry!=null) {
417            String fileName = Roster.getDefault().getRosterFilesLocation() + _rosterEntry.getFileName();
418            Element elem;
419            XmlFile xmlFile = new XmlFile() {};
420            try {
421                elem = xmlFile.rootFromFile(new File(fileName));
422            } catch (FileNotFoundException npe) {
423                elem = null;
424            } catch (IOException | JDOMException eb) {
425                log.error("Exception while loading warrant preferences",eb);
426                elem = null;
427            }
428            if (elem != null) {
429                elem = elem.getChild("locomotive");
430            }
431            if (elem != null) {
432                elem = elem.getChild("values");
433            }
434            if (elem != null) {
435                List<Element> list = elem.getChildren("CVvalue");
436                int count = 0;
437                for (Element cv : list) {
438                    Attribute attr = cv.getAttribute("name");
439                    if (attr != null) {
440                        if (attr.getValue().equals("3")) {
441                            _ma += getMomentumFactor(cv);
442                           count++;
443                        } else if (attr.getValue().equals("4")) {
444                            _md += getMomentumFactor(cv);
445                            count++;
446                        } else if (attr.getValue().equals("23")) {
447                            _ma += getMomentumAdustment(cv);
448                            count++;
449                        } else if (attr.getValue().equals("24")) {
450                            _md += getMomentumAdustment(cv);
451                            count++;
452                        }
453                    }
454                    if (count > 3) {
455                        break;
456                    }
457                }
458            }
459        }
460        if (log.isDebugEnabled()) {
461            log.debug("makeRampParameters for {}, addr={}. _ma= {}ms/step, _md= {}ms/step. rampThrottleIncr= {} rampTimeIncr= {} throttleStep= {}",
462                    _rosterId, getAddress(), _ma, _md, _rampThrottleIncrement, _rampTimeIncrement, getThrottleSpeedStepIncrement());
463        }
464    }
465
466    // return milliseconds per one speed step
467    private int getMomentumFactor(Element cv) {
468        Attribute attr = cv.getAttribute("value");
469        int num = 0;
470        if (attr != null) {
471            try {
472                 /*  .896sec per (throttle Speed Step Increment) is NMRA spec for each CV value
473                 CV#3
474                 Determines the decoder's acceleration rate. The formula for the acceleration rate shall be equal to (the contents 
475                 of CV#3*.896)/(number of speed steps in use). For example, if the contents of CV#3 =2, then the acceleration 
476                 is 0.064 sec/step for a decoder currently using 28 speed steps. If the content of this parameter equals "0" then 
477                 there is no programmed momentum during acceleration.
478                 Same for CV#24
479                 */
480                num = Integer.parseInt( attr.getValue());
481                // reciprocal of getThrottleSpeedStepIncrement() is number of steps in use
482                num = Math.round(num * 896 * getThrottleSpeedStepIncrement());     // milliseconds per step
483            } catch (NumberFormatException nfe) {
484                num = 0;
485            }
486        }
487        if ( log.isTraceEnabled() ) {
488            log.trace("getMomentumFactor for cv {} {}, num= {}",
489                cv.getAttribute("name"), attr, num);
490        }
491        return num;
492    }
493
494    // return milliseconds per one speed step
495    private int getMomentumAdustment(Element cv) {
496        /*  .896sec per  is NMRA spec for each CV value
497        CV#23
498        This Configuration Variable contains additional acceleration rate information that is to be added to or 
499        subtracted from the base value contained in Configuration Variable #3 using the formula (the contents of 
500        CV#23*.896)/(number of speed steps in use). This is a 7 bit value (bits 0-6) with bit 7 being reserved for a 
501        sign bit (0-add, 1-subtract). In case of overflow the maximum acceleration rate shall be used. In case of 
502        160 underflow no acceleration shall be used. The expected use is for changing momentum to simulate differing 
503        train lengths/loads, most often when operating in a consist.
504        Same for CV#24
505        */
506        Attribute attr = cv.getAttribute("value");
507        int num = 0;
508        if (attr != null) {
509            try {
510                int val = Integer.parseInt(attr.getValue());
511                num = val & 0x3F;  //value is 6 bits
512                if ((val & 0x40) != 0) {    // 7th bit sign
513                    num = -num;
514                }
515            } catch (NumberFormatException nfe) {
516                num = 0;
517            }
518        }
519        if ( log.isTraceEnabled()) {
520            log.trace("getMomentumAdustment for cv {} {},  num= {}",
521                cv.getAttribute("name"), attr, num);
522        }
523        return num;
524    }
525
526    protected boolean profileHasSpeedInfo() {
527        RosterSpeedProfile speedProfile = getMergeProfile();
528        if (speedProfile == null) {
529            return false;
530        }
531        return (speedProfile.hasForwardSpeeds() || speedProfile.hasReverseSpeeds());
532    }
533/*
534    private void mergeEntries(Entry<Integer, SpeedStep> sEntry, Entry<Integer, SpeedStep> mEntry) {
535        SpeedStep sStep = sEntry.getValue();
536        SpeedStep mStep = mEntry.getValue();
537        float sTrackSpeed = sStep.getForwardSpeed();
538        float mTrackSpeed = mStep.getForwardSpeed();
539        if (sTrackSpeed > 0) {
540            if (mTrackSpeed > 0) {
541                mTrackSpeed = (mTrackSpeed + sTrackSpeed) / 2;
542            } else {
543                mTrackSpeed = sTrackSpeed;
544            }
545            mStep.setForwardSpeed(mTrackSpeed);
546        }
547        sTrackSpeed = sStep.getReverseSpeed();
548        mTrackSpeed = mStep.getReverseSpeed();
549        if (sTrackSpeed > 0) {
550            if (sTrackSpeed > 0) {
551                if (mTrackSpeed > 0) {
552                    mTrackSpeed = (mTrackSpeed + sTrackSpeed) / 2;
553                } else {
554                    mTrackSpeed = sTrackSpeed;
555                }
556            }
557            mStep.setReverseSpeed(mTrackSpeed);
558        }
559    }*/
560
561    protected void setIsForward(boolean direction) {
562        _isForward = direction;
563        if (_throttle != null) {
564            _throttle.setIsForward(direction);
565        }
566    }
567
568    protected boolean getIsForward() {
569        if (_throttle != null) {
570            _isForward = _throttle.getIsForward();
571        }
572        return _isForward;
573    }
574    /************* runtime speed needs - throttle, engineer acquired ***************/
575
576    /**
577     * @param throttle set DccThrottle
578     */
579    protected void setThrottle( DccThrottle throttle) {
580        _throttle = throttle;
581        getMergeProfile();
582        // adjust user's setting to be throttle speed step settings
583        float stepIncrement = _throttle.getSpeedIncrement();
584        _rampThrottleIncrement = stepIncrement * Math.round(getRampThrottleIncrement()/stepIncrement);
585        if (log.isDebugEnabled()) {
586            log.debug("User's Ramp increment modified to {} ({} speed steps)",
587                    _rampThrottleIncrement, Math.round(_rampThrottleIncrement/stepIncrement));
588        }
589    }
590
591    protected DccThrottle getThrottle() {
592        return _throttle;
593    }
594
595    // return true if the speed named 'speed2' is strictly greater than that of 'speed1'
596    protected boolean secondGreaterThanFirst(String speed1, String speed2) {
597        if (speed2 == null) {
598            return false;
599        }
600        if (speed1 == null) {
601            return true;
602        }
603        if (speed1.equals(speed2)) {
604            return false;
605        }
606        float s1 = _signalSpeedMap.getSpeed(speed1);
607        float s2 = _signalSpeedMap.getSpeed(speed2);
608        return (s1 < s2);
609    }
610
611    /**
612     * Modify a throttle setting to match a speed name type
613     * Modification is done according to the interpretation of the speed name
614     * @param tSpeed throttle setting (current)
615     * @param sType speed type name
616     * @return modified throttle setting
617     */
618    protected float modifySpeed(float tSpeed, String sType) {
619        log.trace("modifySpeed speed= {} for SpeedType= \"{}\"", tSpeed, sType);
620        if (sType.equals(Warrant.Stop)) {
621            return 0.0f;
622        }
623        if (sType.equals(Warrant.EStop)) {
624            return -1.0f;
625        }
626        float throttleSpeed = tSpeed;       // throttleSpeed is a throttle setting
627        if (sType.equals(Warrant.Normal)) {
628            return throttleSpeed;
629        }
630        float signalSpeed = _signalSpeedMap.getSpeed(sType);
631
632        switch (_signalSpeedMap.getInterpretation()) {
633            case SignalSpeedMap.PERCENT_NORMAL:
634                throttleSpeed *= signalSpeed / 100;      // ratio of normal
635                break;
636            case SignalSpeedMap.PERCENT_THROTTLE:
637                signalSpeed /= 100;            // ratio of full throttle setting
638                if (signalSpeed < throttleSpeed) {
639                    throttleSpeed = signalSpeed;
640                }
641                break;
642
643            case SignalSpeedMap.SPEED_MPH:          // convert miles per hour to track speed
644                signalSpeed /= _signalSpeedMap.getLayoutScale();
645                signalSpeed /= 2.2369363f;  // layout track speed mph -> mm/ms
646                float trackSpeed = getTrackSpeed(throttleSpeed);
647                if (signalSpeed < trackSpeed) {
648                    throttleSpeed = getThrottleSettingForSpeed(signalSpeed);
649                }
650                break;
651
652            case SignalSpeedMap.SPEED_KMPH:
653                signalSpeed /= _signalSpeedMap.getLayoutScale();
654                signalSpeed /= 3.6f;  // layout track speed mm/ms -> km/hr
655                trackSpeed = getTrackSpeed(throttleSpeed);
656                if (signalSpeed < trackSpeed) {
657                    throttleSpeed = getThrottleSettingForSpeed(signalSpeed);
658                }
659                break;
660            default:
661                log.error("Unknown speed interpretation {}", _signalSpeedMap.getInterpretation());
662                throw new java.lang.IllegalArgumentException(
663                    "Unknown speed interpretation " + _signalSpeedMap.getInterpretation());
664        }
665        if (log.isTraceEnabled()) {
666            log.trace("modifySpeed: from {}, to {}, signalSpeed= {}. interpretation= {}",
667                tSpeed, throttleSpeed, signalSpeed, _signalSpeedMap.getInterpretation());
668        }
669        return throttleSpeed;
670    }
671
672    /**
673     * A a train's speed at a given throttle setting and time would travel a distance.
674     * return the time it would take for the train at another throttle setting to
675     * travel the same distance.
676     * @param speed a given throttle setting
677     * @param time a given time
678     * @param modifiedSpeed a different speed setting
679     * @return the time to travel the same distance at the different setting
680     */
681    protected static long modifyTime(float speed, long time, float modifiedSpeed) {
682        if (Math.abs(speed - modifiedSpeed) > .0001f) {
683            return (long)((speed / modifiedSpeed) * time);
684        } else {
685            return time;
686        }
687    }
688
689    /**
690     * Get the track speed in millimeters per millisecond (= meters/sec)
691     * If SpeedProfile has no speed information an estimate is given using the WarrantPreferences
692     * throttleFactor.
693     * NOTE:  Call profileHasSpeedInfo() first to determine if a reliable speed is known.
694     * for a given throttle setting and direction.
695     * SpeedProfile returns 0 if it has no speed information
696     * @param throttleSetting throttle setting
697     * @return track speed in millimeters/millisecond (not mm/sec)
698     */
699    protected float getTrackSpeed(float throttleSetting) {
700        if (throttleSetting <= 0.0f) {
701            return 0.0f;
702        }
703        if (_dccAddress == null) {
704            return factorSpeed(throttleSetting);
705        }
706        RosterSpeedProfile sessionProfile = getMergeProfile(); 
707        boolean isForward = getIsForward();
708        // Note SpeedProfile uses millimeters per second.
709        float speed = sessionProfile.getSpeed(throttleSetting, isForward) / 1000;
710        if (speed <= 0.0f) {
711            speed = sessionProfile.getSpeed(throttleSetting, !isForward) / 1000;
712        }
713        if (speed <= 0.0f) {
714            return factorSpeed(throttleSetting);
715        }
716        return speed;
717    }
718
719
720    private float factorSpeed(float throttleSetting) {
721        float factor = _signalSpeedMap.getDefaultThrottleFactor() * SCALE_FACTOR / _signalSpeedMap.getLayoutScale();
722        return throttleSetting * factor;
723    }
724    /**
725     * Get the throttle setting needed to achieve a given track speed
726     * track speed is mm/ms.  SpeedProfile wants mm/s
727     * SpeedProfile returns 0 if it has no speed information
728     * @param trackSpeed in millimeters per millisecond (m/s)
729     * @return throttle setting or 0
730     */
731    protected float getThrottleSettingForSpeed(float trackSpeed) {
732        RosterSpeedProfile speedProfile = getMergeProfile();
733        float throttleSpeed;
734        if (speedProfile != null) {
735            throttleSpeed = speedProfile.getThrottleSetting(trackSpeed * 1000, getIsForward());
736        } else {
737            throttleSpeed = 0f;
738        }
739        if (throttleSpeed <= 0.0f) {
740            throttleSpeed =  trackSpeed * _signalSpeedMap.getLayoutScale() /
741                (SCALE_FACTOR *_signalSpeedMap.getDefaultThrottleFactor());
742        }
743        return throttleSpeed;
744    }
745
746    /**
747     * Get distance traveled at a constant speed. If this is called at
748     * a speed change the throttleSetting should be modified to reflect the
749     * average speed over the time interval.
750     * @param speedSetting Recorded (Normal) throttle setting
751     * @param speedtype speed name to modify throttle setting to get modified speed
752     * @param time milliseconds
753     * @return distance in millimeters
754     */
755    protected float getDistanceTraveled(float speedSetting, String speedtype, float time) {
756        if (time <= 0) {
757            return 0;
758        }
759        float throttleSetting = modifySpeed(speedSetting, speedtype);
760        return getTrackSpeed(throttleSetting) * time;
761    }
762
763    /**
764     * Get time needed to travel a distance at a constant speed.
765     * @param throttleSetting Throttle setting
766     * @param distance in millimeters
767     * @return time in milliseconds
768     */
769    protected int getTimeForDistance(float throttleSetting, float distance) {
770        float speed = getTrackSpeed(throttleSetting);
771        if (distance <= 0 || speed <= 0) {
772            return 0;
773        }
774        return Math.round(distance/speed);
775    }
776
777    /*************** Block Speed Info *****************/
778    /**
779     * build map of BlockSpeedInfo's for the route. Map corresponds to list
780     * of BlockOrders of a Warrant
781     * @param commands list of script commands
782     * @param orders list of BlockOrders
783     */
784    protected void getBlockSpeedTimes(List<ThrottleSetting> commands, List<BlockOrder> orders) {
785        _speedInfo = new ArrayList<>();
786        float firstSpeed = 0.0f; // used for entrance
787        float speed = 0.0f;
788        float intStartSpeed = 0.0f;
789        float intEndSpeed = 0.0f;
790        long blkTime = 0;
791        float pathDist = 0;
792        float calcDist = 0;
793        int firstIdx = 0; // for all blocks except first, this is index of NOOP command
794        int blkOrderIdx = 0;
795        ThrottleSetting ts = commands.get(0);
796        OBlock blk = (OBlock)ts.getNamedBeanHandle().getBean();
797        String blkName = blk.getDisplayName();
798        for (int i = 0; i < commands.size(); i++) {
799            ts = commands.get(i);
800            Command command = ts.getCommand();
801            CommandValue cmdVal = ts.getValue();
802            if (command.equals(Command.FORWARD)) {
803                ValueType val = cmdVal.getType();
804                setIsForward(val.equals(ValueType.VAL_TRUE));
805            }
806            long time = ts.getTime();
807            blkTime += time;
808            if (time > 0) {
809                calcDist += getDistanceOfSpeedChange(intStartSpeed, intEndSpeed, time);
810            }
811            if (command.equals(Command.SPEED)) {
812                speed = cmdVal.getFloat();
813                if (speed < 0) {
814                    speed = 0;
815                }
816                intStartSpeed = intEndSpeed;
817                intEndSpeed = speed;
818            }
819            if (command.equals(Command.NOOP)) {
820                // make map entry. First measure distance to end of block
821                if (time > 0) {
822                    calcDist += getDistanceOfSpeedChange(intStartSpeed, intEndSpeed, time);
823                }               
824                float ratio = 1;
825                if (calcDist > 0 && blkOrderIdx > 0 && blkOrderIdx < commands.size() - 1) {
826                    pathDist = orders.get(blkOrderIdx).getPathLength();
827                    ratio = pathDist / calcDist;
828                } else {
829                    pathDist = orders.get(blkOrderIdx).getPathLength() / 2;
830                }
831                _speedInfo.add(new BlockSpeedInfo(blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, i));
832                if (Warrant._trace || log.isDebugEnabled()) {
833                    if (calcDist <= 0 || Math.abs(ratio) > 2.0f || Math.abs(ratio) < 0.5f) {
834                        log.debug("\"{}\" Speeds: enter= {}, exit= {}. time= {}ms, pathDist= {}, calcDist= {}. index {} to {}",
835                                blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, i);
836                    }
837                }
838                blkOrderIdx++;
839                blk = (OBlock)ts.getNamedBeanHandle().getBean();
840                blkName = blk.getDisplayName();
841                blkTime = 0;
842                calcDist = 0;
843                intStartSpeed = intEndSpeed;
844                firstSpeed = speed;
845                firstIdx = i + 1; // first in next block is next index
846            }
847            // set up recording track speeds
848        }
849        _speedInfo.add(new BlockSpeedInfo(blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, commands.size() - 1));
850        if (log.isDebugEnabled()) {
851            log.debug("block: {} speeds: entrance= {}, exit= {}. time= {}ms pathDist= {}, calcDist= {}. index {} to {}",
852                    blkName, firstSpeed, speed, blkTime, pathDist, calcDist, firstIdx, (commands.size() - 1));
853        }
854        clearStats(-1);
855        _intStartSpeed = 0;
856        _intEndSpeed = 0;
857    }
858
859    protected BlockSpeedInfo getBlockSpeedInfo(int idxBlockOrder) {
860        return _speedInfo.get(idxBlockOrder);
861    }
862
863    /**
864     * Get the ramp for a speed change from Throttle settings
865     * @param fromSpeed - starting speed setting
866     * @param toSpeed - ending speed setting
867     * @return ramp data
868     */
869    protected RampData getRampForSpeedChange(float fromSpeed, float toSpeed) {
870        return new RampData(this, getRampThrottleIncrement(), getRampTimeIncrement(), fromSpeed, toSpeed);
871    }
872
873    /**
874     * Get the ramp length for a speed change from Throttle settings
875     * @param fromSpeed - starting speed setting
876     * @param toSpeed - ending speed setting
877     * @return ramp length
878     */
879   protected float getRampLengthForEntry(float fromSpeed, float toSpeed) {
880        RampData ramp = getRampForSpeedChange(fromSpeed, toSpeed);
881        float enterLen = ramp.getRampLength();
882        if (log.isTraceEnabled()) {
883            log.debug("getRampLengthForEntry: from speed={} to speed={}. rampLen={}",
884                    fromSpeed, toSpeed, enterLen);
885        }
886        return enterLen;
887    }
888
889    /**
890     * Return the distance traveled at current speed after a speed change was made.
891     * Takes into account the momentum configured for the decoder to change from
892     * the previous speed to the current speed.  Assumes the velocity change is linear.
893     * Does not return a distance greater than that needed by momentum time.
894     *
895     * @param fromSpeed throttle setting when speed changed to toSpeed
896     * @param toSpeed throttle setting being set
897     * @param speedTime elapsed time from when the speed change was made to now
898     * @return distance traveled
899     */
900    protected float getDistanceOfSpeedChange(float fromSpeed, float toSpeed, long speedTime) {
901        if (toSpeed < 0) {
902            toSpeed = 0;
903        }
904        if (fromSpeed < 0) {
905            fromSpeed = 0;
906        }
907        float momentumTime = getMomentumTime(fromSpeed, toSpeed);
908        float dist;
909        // assume a linear change of speed
910        if (speedTime <= momentumTime ) {
911            // perhaps will be too far since toSpeed may not be attained
912            dist = getTrackSpeed((fromSpeed + toSpeed)/2) * speedTime;
913        } else {
914            dist = getTrackSpeed((fromSpeed + toSpeed)/2) * momentumTime;
915            if (speedTime > momentumTime) { // time remainder at changed speed
916                dist += getTrackSpeed(toSpeed) * (speedTime - momentumTime);
917            }
918        }
919//      log.debug("momentumTime = {}, speedTime= {} moDist= {}", momentumTime, speedTime, dist);
920        return dist;
921    }
922
923    /*************** dynamic calibration ***********************/
924    private long _timeAtSpeed = 0;
925    private float _intStartSpeed = 0.0f;
926    private float _intEndSpeed = 0.0f;
927    private float _distanceTravelled = 0;
928    private float _settingsTravelled = 0;
929    private long _prevChangeTime = -1;
930    private int _numchanges = 0;        // number of time changes within the block
931    private long _entertime = 0;        // entrance time to block
932    private boolean _cantMeasure = false;    // speed has at 0 at some time while in the block
933
934    /**
935     * Just entered a new block at 'toTime'. Do the calculation of speed of the
936     * previous block from when the previous block block was entered.
937     *
938     * Throttle changes within the block will cause different speeds.  We attempt
939     * to accumulate these time and distances to calculate a weighted speed average.
940     * See method speedChange() below.
941     * @param blkIdx BlockOrder index of the block the engine just left. (not train)
942     * The lead engine just entered the next block after blkIdx.
943     */
944    protected void leavingBlock(int blkIdx) {
945        long exitTime = System.currentTimeMillis();
946        BlockSpeedInfo blkInfo = getBlockSpeedInfo(blkIdx);
947        log.debug("BlockInfo: {}", blkInfo);
948
949        if (_cantMeasure) {
950            clearStats(exitTime);
951            _entertime = exitTime;   // entry of next block
952            log.debug("Skip speed measurement");
953            return;
954        }
955        boolean isForward = getIsForward();
956        float throttle = _throttle.getSpeedSetting();   // may not be a multiple of a speed step
957        float length = blkInfo.getPathLen();
958        long elapsedTime = exitTime - _prevChangeTime;
959        if (_numchanges == 0) {
960            _distanceTravelled = getTrackSpeed(throttle) * elapsedTime;
961            _settingsTravelled = throttle * elapsedTime;
962            _timeAtSpeed = elapsedTime;            
963        } else {
964            float dist = getDistanceOfSpeedChange(_intStartSpeed, _intEndSpeed, elapsedTime);
965            if (_intStartSpeed > 0 || _intEndSpeed > 0) {
966                _timeAtSpeed += elapsedTime;
967            }
968            if (log.isDebugEnabled()) {
969                log.debug("speedChange to {}: dist={} in {}ms from speed {} to {}.",
970                        throttle, dist, elapsedTime, _intStartSpeed, _intEndSpeed);
971            }
972            _distanceTravelled += dist;
973            _settingsTravelled += throttle * elapsedTime;
974        }
975
976        float measuredSpeed;
977        float distRatio;
978        if (length <= 0) {
979            // Origin and Destination block lengths immaterial
980            measuredSpeed = _distanceTravelled / _timeAtSpeed;
981            distRatio = 2;    // actual start and end positions unknown
982        } else {
983            measuredSpeed = length / _timeAtSpeed;
984            distRatio = blkInfo.getCalcLen()/_distanceTravelled;
985        }
986        measuredSpeed *= 1000;    // SpeedProfile is mm/sec
987        float aveSettings = _settingsTravelled / _timeAtSpeed;
988        if (log.isDebugEnabled()) {
989            float timeRatio = (exitTime - _entertime) / (float)_timeAtSpeed;
990            log.debug("distRatio= {}, timeRatio= {}, aveSpeed= {}, length= {}, calcLength= {}, elapsedTime= {}", 
991                    distRatio, timeRatio, measuredSpeed, length, _distanceTravelled, (exitTime - _entertime));
992        }
993        if (aveSettings > 1.0 || measuredSpeed > MAX_TGV_SPEED*aveSettings/_signalSpeedMap.getLayoutScale()
994                || distRatio > 1.15f || distRatio < 0.87f) {
995            if (log.isDebugEnabled()) {
996                // We assume bullet train's speed is linear from 0 throttle to max throttle.
997                // we also tolerate distance calculation errors up to 20% longer or shorter
998                log.info("Bad speed measurements data for block {}. aveThrottle= {}, " +
999                    " measuredSpeed= {},(TGVmax= {}), distTravelled= {}, pathLen= {}",
1000                    blkInfo.getBlockDisplayName(), aveSettings,  measuredSpeed,
1001                    MAX_TGV_SPEED*aveSettings/_signalSpeedMap.getLayoutScale(),
1002                    _distanceTravelled, length);
1003            }
1004        } else if (_numchanges < 3) {
1005            setSpeedProfile(_sessionProfile, aveSettings, measuredSpeed, isForward);
1006        }
1007        if (log.isDebugEnabled()) {
1008            log.debug("{} changes in block \'{}\". measuredDist={}, pathLen={}, " +
1009                " measuredThrottle={},  measuredTrkSpd={}, profileTrkSpd={} curThrottle={}.",
1010                _numchanges, blkInfo.getBlockDisplayName(), Math.round(_distanceTravelled), length,
1011                aveSettings, measuredSpeed, getTrackSpeed(aveSettings)*1000, throttle);
1012        }
1013        clearStats(exitTime);
1014        _entertime = exitTime;   // entry of next block
1015    }
1016
1017    // average with existing entry, if possible
1018    private void setSpeedProfile(RosterSpeedProfile profile, float throttle, float measuredSpeed, boolean isForward) {
1019        int keyIncrement = Math.round(getThrottleSpeedStepIncrement() * 1000);
1020        TreeMap<Integer, SpeedStep> speeds = profile.getProfileSpeeds();
1021        int key = Math.round(throttle * 1000);
1022        Entry<Integer, SpeedStep> entry = speeds.floorEntry(key);
1023        if (entry != null && mergeEntry(key, measuredSpeed, entry, keyIncrement, isForward)) {
1024            return;
1025        }
1026        entry = speeds.ceilingEntry(key);
1027        if (entry != null && mergeEntry(key, measuredSpeed, entry, keyIncrement, isForward)) {
1028            return;
1029        }
1030
1031        float speed = profile.getSpeed(throttle, isForward);
1032        if (speed > 0.0f) {
1033            measuredSpeed = (measuredSpeed + speed) / 2;
1034        }
1035        
1036        if (isForward) {
1037            profile.setForwardSpeed(throttle, measuredSpeed, _throttle.getSpeedIncrement());
1038        } else {
1039            profile.setReverseSpeed(throttle, measuredSpeed, _throttle.getSpeedIncrement());
1040        }
1041        log.debug("Put measuredThrottle={} and measuredTrkSpd={} for isForward= {} curThrottle={}.",
1042            throttle, measuredSpeed, isForward, throttle);
1043    }
1044
1045    private boolean mergeEntry(int key, float measuredSpeed, Entry<Integer, SpeedStep> entry, int keyIncrement, boolean isForward) {
1046        Integer sKey = entry.getKey();
1047        if (Math.abs(sKey - key) < keyIncrement) {
1048            SpeedStep sStep = entry.getValue();
1049            float sTrackSpeed;
1050            if (isForward) {
1051                sTrackSpeed = sStep.getForwardSpeed();
1052                if (sTrackSpeed > 0) {
1053                    if (sTrackSpeed > 0) {
1054                        sTrackSpeed = (sTrackSpeed + measuredSpeed) / 2;
1055                    } else {
1056                        sTrackSpeed = measuredSpeed;
1057                    }
1058                    sStep.setForwardSpeed(sTrackSpeed);
1059                }
1060            } else {
1061                sTrackSpeed = sStep.getReverseSpeed();
1062                if (sTrackSpeed > 0) {
1063                    if (sTrackSpeed > 0) {
1064                        sTrackSpeed = (sTrackSpeed + measuredSpeed) / 2;
1065                    } else {
1066                        sTrackSpeed = measuredSpeed;
1067                    }
1068                    sStep.setReverseSpeed(sTrackSpeed);
1069                }
1070            }
1071        }
1072       return false;
1073    }
1074
1075    private void clearStats(long exitTime) {
1076        _timeAtSpeed = 0;
1077        _distanceTravelled = 0.0f;
1078        _settingsTravelled = 0.0f;
1079        _numchanges = 0;
1080        _prevChangeTime = exitTime;
1081        _cantMeasure = false;
1082    }
1083
1084    /**
1085     * The engineer makes this notification before setting a new speed.
1086     * Calculate the distance traveled since the last speed change.
1087     * @param throttleSetting the new Speed of the Throttle.
1088     */
1089    protected synchronized void speedChange(float throttleSetting) {
1090        if (Math.abs(_intEndSpeed - throttleSetting) < 0.00001f) {
1091            _cantMeasure = true;
1092            return;
1093        }
1094        _numchanges++;
1095        long time = System.currentTimeMillis();
1096        if (throttleSetting <= 0) {
1097            throttleSetting = 0;
1098        }
1099        if (_prevChangeTime > 0) {
1100            long elapsedTime = time - _prevChangeTime;
1101            float dist = getDistanceOfSpeedChange(_intStartSpeed, _intEndSpeed, elapsedTime);
1102            if (dist > 0) {
1103                _timeAtSpeed += elapsedTime;
1104            }
1105            if (log.isTraceEnabled()) {
1106                log.debug("speedChange to {}: dist={} in {}ms from speed {} to {}.",
1107                        throttleSetting, dist, elapsedTime, _intStartSpeed, _intEndSpeed);
1108            }
1109            _distanceTravelled += dist;
1110            _settingsTravelled += throttleSetting * elapsedTime;
1111        }
1112        if (_entertime <= 0) {
1113            _entertime = time;  // time of first non-zero speed
1114        }
1115        _prevChangeTime = time;
1116        _intStartSpeed = _intEndSpeed;
1117        _intEndSpeed = throttleSetting;
1118    }
1119
1120    private static final org.slf4j.Logger log = org.slf4j.LoggerFactory.getLogger(SpeedUtil.class);
1121
1122}