001package jmri.jmrit.vsdecoder;
002
003import java.awt.geom.*;
004import java.util.ArrayList;
005import java.util.List;
006import jmri.jmrit.display.layoutEditor.*;
007import jmri.util.MathUtil;
008
009import javax.annotation.*;
010
011import org.slf4j.Logger;
012import org.slf4j.LoggerFactory;
013
014/**
015 * Navigation through a LayoutEditor panel to set the sound position.
016 *
017 * Almost all code from George Warner's LENavigator.
018 * ------------------------------------------------
019 * Added direction change feature with new methods
020 * setReturnTrack(T), setReturnLastTrack(T) and
021 * a Block check.
022 *
023 * Concept for direction change, e.g.:
024 *  EndBumper ---- TrackSegment ------ Anchor
025 *  lastTrack      returnTrack     returnLastTrack
026 *
027 * <hr>
028 * This file is part of JMRI.
029 * <p>
030 * JMRI is free software; you can redistribute it and/or modify it under
031 * the terms of version 2 of the GNU General Public License as published
032 * by the Free Software Foundation. See the "COPYING" file for a copy
033 * of this license.
034 * <p>
035 * JMRI is distributed in the hope that it will be useful, but WITHOUT
036 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
037 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
038 * for more details.
039 *
040 * @author Klaus Killinger Copyright (C) 2022, 2023
041 */
042public class VSDNavigation {
043
044    private VSDecoder d;
045
046    private boolean use_blocks = VSDecoderManager.instance().getVSDecoderPreferences().getUseBlocksSetting();
047
048    private int lastTurntablePosition = -1;
049
050    // constructor
051    VSDNavigation(VSDecoder vsd) {
052        d = vsd;
053    }
054
055    // layout track specific methods
056    boolean navigatePositionalPoint() {
057        boolean result = true; // always go to next track
058        PositionablePoint pp = (PositionablePoint) d.getLayoutTrack();
059        PositionablePoint.PointType type = pp.getType();
060        switch (type) {
061            case ANCHOR: {
062                if (pp.getConnect1().equals(d.getLastTrack())) {
063                    d.setLayoutTrack(pp.getConnect2());
064                    d.setReturnTrack(d.getLayoutTrack());
065                } else if (pp.getConnect2().equals(d.getLastTrack())) {
066                    d.setLayoutTrack(pp.getConnect1());
067                    d.setReturnTrack(d.getLayoutTrack());
068                } else { // OOPS! we're lost!
069                    result = false;
070                    break;
071                }
072                d.setLastTrack(pp);
073                break;
074            }
075            default:
076            case END_BUMPER: {
077                d.setReturnTrack(pp.getConnect1());
078                d.distanceOnTrack = d.getReturnDistance();
079                d.setDistance(0);
080                result = false;
081                break;
082            }
083            case EDGE_CONNECTOR: {
084                TrackSegment ts2 = null;
085                if (pp.getLinkedPoint() != null) {
086                    ts2 = pp.getLinkedPoint().getConnect1();
087                    d.setModels(pp.getLinkedEditor()); // change the panel
088                    d.setLayoutTrack(ts2);
089                    d.setReturnTrack(d.getLayoutTrack());
090                    if (pp.getLinkedPoint().equals(ts2.getConnect1())) {
091                        d.setLastTrack(ts2.getConnect1());
092                    } else if (pp.getLinkedPoint().equals(ts2.getConnect2())) {
093                        d.setLastTrack(ts2.getConnect2());
094                    } else {
095                        log.warn(" EdgeConnector lost");
096                    }
097                } else {
098                    log.warn(" EdgeConnector is not linked");
099                    d.setReturnTrack(d.getLastTrack());
100                    d.distanceOnTrack = d.getReturnDistance();
101                    d.setDistance(0);
102                    result = false;
103                }
104                break;
105            }
106        }
107        return result;
108    }
109
110    boolean navigateTrackSegment() {
111        boolean result = false;
112        // LayoutTrack block and reported block must be equal
113        if (use_blocks && ((TrackSegment) d.getLayoutTrack()).getLayoutBlock().getBlock() != VSDecoderManager.instance().currentBlock.get(d)) {
114            // not in the block
115            d.setDistance(0);
116            return result;
117        }
118
119        double distanceOnTrack = d.getDistance() + d.distanceOnTrack;
120        d.nextLayoutTrack = null;
121
122        TrackSegmentView tsv = d.getModels().getTrackSegmentView((TrackSegment) d.getLayoutTrack());
123        if (tsv.isArc()) {
124            // tsv.calculateTrackSegmentAngle(); // ... has protected access in TrackSegmentView
125            // when do we need this? After a panel change?
126            Point2D radius2D = new Point2D.Double(tsv.getCW() / 2, tsv.getCH() / 2);
127            double radius = (radius2D.getX() + radius2D.getY()) / 2;
128            Point2D centre = tsv.getCentre();
129            /*
130             * Note: Angles go CCW from south to east to north to west, etc.
131             * For JMRI angles subtract from 90 to get east, south, west, north
132             */
133            //double startAdjDEG = tsv.getStartAdj(); // klk The value of the local variable startAdjDEG is not really used
134            double tmpAngleDEG = tsv.getTmpAngle();
135
136            double distance = 2 * radius * Math.PI * tmpAngleDEG / 360;
137            d.setReturnDistance(distance);
138            if (distanceOnTrack < distance) { // it's on this track
139                Point2D p1 = d.getModels().getCoords(tsv.getConnect1(), tsv.getType1());
140                Point2D p2 = d.getModels().getCoords(tsv.getConnect2(), tsv.getType2());
141                if (!tsv.isCircle()) {
142                    centre = MathUtil.midPoint(p1, p2);
143                    Point2D centreSeg = tsv.getCentreSeg();
144                    double newX = (centre.getX() < centreSeg.getX()) ? Math.min(p1.getX(), p2.getX()) : Math.max(p1.getX(), p2.getX());
145                    double newY = (centre.getY() < centreSeg.getY()) ? Math.min(p1.getY(), p2.getY()) : Math.max(p1.getY(), p2.getY());
146                    centre = new Point2D.Double(newX, newY);
147                }
148                double angle1DEG = MathUtil.computeAngleDEG(p1, centre) - 90;
149                double angle2DEG = MathUtil.computeAngleDEG(p2, centre) - 90;
150                Point2D centreSeg = tsv.getCentreSeg();
151                double angle3DEG = MathUtil.computeAngleDEG(centreSeg, centre) - 90;
152                double angleDeltaDEG = MathUtil.wrapPM360(2 * (angle3DEG - angle1DEG));
153                double ratio = distanceOnTrack / distance;
154                Point2D delta = new Point2D.Double(radius, 0);
155                double angleDEG = 0;
156                if (tsv.getConnect1().equals(d.getLastTrack())) {
157                    // entering from this end...
158                    d.nextLayoutTrack = tsv.getConnect2();
159                    d.setReturnLastTrack(tsv.getConnect2());
160                    angleDEG = angle1DEG;
161                    angleDeltaDEG = MathUtil.lerp(0, angleDeltaDEG, ratio);
162                } else if (tsv.getConnect2().equals(d.getLastTrack())) {
163                    // entering from the other end...
164                    d.nextLayoutTrack = tsv.getConnect1();
165                    d.setReturnLastTrack(tsv.getConnect1());
166                    //startAdjDEG += tmpAngleDEG; // SpotBugs: Dead store to startAdjDEG
167                    angleDEG = angle2DEG;
168                    angleDeltaDEG = MathUtil.lerp(0, -angleDeltaDEG, ratio);
169                } else { // OOPS! we're lost!
170                    log.info(" lost");
171                    result = false;
172                    angleDeltaDEG = 0;
173                }
174                double dirDeltaDEG = Math.signum(angleDeltaDEG) * -90;
175
176                double newAngleDeg = -(angleDEG + angleDeltaDEG);
177                // Compute location
178                delta = MathUtil.rotateDEG(delta, newAngleDeg);
179                if (!tsv.isCircle()) {
180                    delta = MathUtil.multiply(delta, radius2D.getX() / radius, radius2D.getY() / radius);
181                }
182                d.setLocation(MathUtil.add(centre, delta));
183                d.setDirectionDEG(newAngleDeg + dirDeltaDEG);
184                d.setDistance(0);
185            } else { // it's not on this track
186                d.nextLayoutTrack = tsv.getConnect2();
187                if (tsv.getConnect2().equals(d.getLastTrack())) {
188                    // entering from the other end...
189                    d.nextLayoutTrack = tsv.getConnect1();
190                }
191                d.setDistance(distanceOnTrack - distance);
192                distanceOnTrack = 0;
193                result = true;
194            }
195            d.distanceOnTrack = distanceOnTrack;
196        } else if (tsv.isBezier()) {
197            //Point2D[] points = tsv.getBezierPoints(); // getBezierPoints() has private access in TrackSegmentView!
198            // Alternative
199            Point2D ep1 = d.getModels().getCoords(tsv.getConnect1(), tsv.getType1());
200            Point2D ep2 = d.getModels().getCoords(tsv.getConnect2(), tsv.getType2());
201            int cnt = tsv.getBezierControlPoints().size() + 2;
202            Point2D[] points = new Point2D[cnt];
203            points[0] = ep1;
204            for (int idx = 0; idx < cnt - 2; idx++) {
205                points[idx + 1] = tsv.getBezierControlPoints().get(idx);
206            }
207            points[cnt - 1] = ep2;
208
209            double distance = MathUtil.drawBezier(null, points);
210            d.setReturnDistance(distance);
211            if (distanceOnTrack < distance) { // it's on this track
212                d.nextLayoutTrack = tsv.getConnect2();
213                d.setReturnLastTrack(tsv.getConnect2());
214                // if entering from the other end...
215                if (tsv.getConnect2().equals(d.getLastTrack())) {
216                    points = jmri.util.ArrayUtil.reverse(points);     //..reverse the points
217                    d.nextLayoutTrack = tsv.getConnect1(); // and change the next LayoutTrack
218                    d.setReturnLastTrack(tsv.getConnect1());
219                }
220                GeneralPath path = MathUtil.getBezierPath(points);
221                PathIterator i = path.getPathIterator(null);
222                List<Point2D> pathPoints = new ArrayList<>();
223                while (!i.isDone()) {
224                    float[] data = new float[6];
225                    switch (i.currentSegment(data)) {
226                        case PathIterator.SEG_MOVETO:
227                        case PathIterator.SEG_LINETO: {
228                            pathPoints.add(new Point2D.Double(data[0], data[1]));
229                            break;
230                        }
231                        default: {
232                            log.error("Unknown path segment type: {}.", i.currentSegment(data));
233                            //$FALL-THROUGH$
234                      //  case PathIterator.SEG_QUADTO:
235                      //  case PathIterator.SEG_CUBICTO:
236                      //  case PathIterator.SEG_CLOSE: {
237                            // OOPS! we're lost!
238                            log.info(" bezier lost");
239                            result = false;
240                            break;
241                        }
242                    }
243                    i.next();
244                } // while (!i.isDone())
245                return navigate(pathPoints, d.nextLayoutTrack);
246            } else { // it's not on this track
247                d.nextLayoutTrack = tsv.getConnect2();
248                if (tsv.getConnect2().equals(d.getLastTrack())) {
249                    d.nextLayoutTrack = tsv.getConnect1();
250                }
251                d.setDistance(distanceOnTrack - distance);
252                distanceOnTrack = 0;
253                result = true;
254            }
255            d.distanceOnTrack = distanceOnTrack;
256        } else {
257            Point2D p1 = d.getModels().getCoords(tsv.getConnect1(), tsv.getType1());
258            Point2D p2 = d.getModels().getCoords(tsv.getConnect2(), tsv.getType2());
259            double distance = MathUtil.distance(p1, p2);
260            d.setReturnDistance(distance);
261            if (distanceOnTrack < distance) {
262                // it's on this track
263                if (tsv.getConnect1().equals(d.getLastTrack())) {
264                    d.nextLayoutTrack = tsv.getConnect2();
265                    d.setReturnLastTrack(tsv.getConnect2());
266                } else if (tsv.getConnect2().equals(d.getLastTrack())) {
267                    // if entering from the other end then swap end points
268                    d.nextLayoutTrack = tsv.getConnect1();
269                    d.setReturnLastTrack(tsv.getConnect1());
270                    // swap
271                    Point2D temp = p1;
272                    p1 = p2;
273                    p2 = temp;
274                } else { // OOPS! we're lost!
275                    result = false;
276                }
277                double ratio = distanceOnTrack / distance;
278                d.setLocation(MathUtil.lerp(p1, p2, ratio));
279                d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(p2, p1));
280                d.setDistance(0);
281            } else { // it's not on this track
282                if (tsv.getConnect1().equals(d.getLastTrack())) {
283                    d.nextLayoutTrack = tsv.getConnect2();
284                } else if (tsv.getConnect2().equals(d.getLastTrack())) {
285                    d.nextLayoutTrack = tsv.getConnect1();
286                }
287                d.setDistance(distanceOnTrack - distance);
288                distanceOnTrack = 0;
289                result = true;
290            }
291            d.distanceOnTrack = distanceOnTrack;
292        }
293
294        if (result) { // not on this track
295            // go to next track
296            LayoutTrack last = d.getLayoutTrack();
297            if (d.nextLayoutTrack != null) {
298                d.setLayoutTrack(d.nextLayoutTrack);
299            } else { // OOPS! we're lost!
300                result = false;
301            }
302            if (result) {
303                d.setLastTrack(last);
304                d.setReturnTrack(d.getLayoutTrack());
305                d.setReturnLastTrack(d.getLayoutTrack());
306            }
307        }
308        d.savedSound.setTunnel(tsv.isTunnelSideRight() || tsv.isTunnelSideLeft() || tsv.isTunnelHasEntry() || tsv.isTunnelHasExit() ? true : false); // set the tunnel status
309        return result;
310    }
311
312    boolean navigateLayoutTurnout() {
313        boolean result = false;
314        if (use_blocks && ((LayoutTurnout) d.getLayoutTrack()).getLayoutBlock().getBlock() != VSDecoderManager.instance().currentBlock.get(d)) {
315            // we are not in the block
316            d.setDistance(0);
317            return result;
318        }
319
320        double distanceOnTrack = d.getDistance() + d.distanceOnTrack;
321
322        LayoutTurnoutView tv = d.getModels().getLayoutTurnoutView((LayoutTurnout) d.getLayoutTrack());
323        Point2D pM = tv.getCoordsCenter();
324        Point2D pA = tv.getCoordsA();
325        Point2D pB = tv.getCoordsB();
326        Point2D pC = tv.getCoordsC();
327        Point2D pD = tv.getCoordsD();
328
329        int state = LayoutTurnout.UNKNOWN; // 1
330        if (d.getModels().isAnimating()) {
331            state = tv.getState(); // turnout closed: 2, turnout thrown: 4
332        }
333        if ((state != jmri.Turnout.CLOSED) && (state != jmri.Turnout.THROWN)) {
334            log.info("have to stop - state: {}", state); // state UNKNOWN
335            result = false;
336        }
337
338        d.nextLayoutTrack = null;
339
340        switch (tv.getTurnoutType()) {
341            case RH_TURNOUT:
342            case LH_TURNOUT:
343            case WYE_TURNOUT: {
344                Point2D pStart = null;
345                Point2D pEnd = null;
346
347                if (tv.getConnectA().equals(d.getLastTrack())) {
348                    pStart = pA;
349                    if (state == jmri.Turnout.CLOSED) {
350                        pEnd = pB;
351                        d.nextLayoutTrack = tv.getConnectB();
352                    } else if (state == jmri.Turnout.THROWN) {
353                        pEnd = pC;
354                        d.nextLayoutTrack = tv.getConnectC();
355                    }
356                } else if (tv.getConnectB().equals(d.getLastTrack())) {
357                    if (state == jmri.Turnout.CLOSED) {
358                        pStart = pB;
359                        pEnd = pA;
360                        d.nextLayoutTrack = tv.getConnectA();
361                    }
362                } else if (tv.getConnectC().equals(d.getLastTrack())) {
363                    if (state == jmri.Turnout.THROWN) {
364                        pStart = pC;
365                        pEnd = pA;
366                        d.nextLayoutTrack = tv.getConnectA();
367                    }
368                } else { // OOPS! we're lost!
369                    result = false;
370                }
371                if (d.nextLayoutTrack != null) {
372                    d.setReturnLastTrack(d.nextLayoutTrack);
373                    d.setReturnTrack(d.getLayoutTrack());
374                    d.setDistance(0);
375                }
376
377                if (pStart != null) {
378                    double distanceStart = MathUtil.distance(pStart, pM);
379                    d.setReturnDistance(distanceStart);
380                    if (distanceOnTrack < distanceStart) { // it's on startleg
381                        double ratio = distanceOnTrack / distanceStart;
382                        d.setLocation(MathUtil.lerp(pStart, pM, ratio));
383                        d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(pM, pStart));
384                        d.setDistance(0);
385                    } else if (pEnd != null) { // it's not on startleg
386                        double distanceEnd = MathUtil.distance(pM, pEnd);
387                        d.setReturnDistance(distanceEnd);
388                        if ((distanceOnTrack - distanceStart) < distanceEnd) { // it's on end leg
389                            double ratio = (distanceOnTrack - distanceStart) / distanceEnd;
390                            d.setLocation(MathUtil.lerp(pM, pEnd, ratio));
391                            d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(pEnd, pM));
392                            d.setDistance(0);
393                        } else { // it's not on end leg / this track
394                            d.setDistance(distanceOnTrack - (distanceStart + distanceEnd));
395                            distanceOnTrack = 0;
396                            result = true;
397                        }
398                    } else { // OOPS! we're lost!
399                        log.info(" Turnout has unknown state");
400                        result = false;
401                        distanceOnTrack = distanceStart;
402                        d.setDistance(0);
403                        d.setReturnDistance(0);
404                        d.setReturnTrack(d.getLastTrack());
405                    }
406                } else { // OOPS! we're lost!
407                    log.info(" Turnout caused a stop"); // correct position or change direction
408                    result = false;
409                    distanceOnTrack = 0;
410                    d.setDistance(0);
411                    d.setReturnDistance(0);
412                    d.setReturnTrack(d.getLastTrack());
413                }
414                break;
415            }
416
417            case RH_XOVER:
418            case LH_XOVER:
419            case DOUBLE_XOVER: {
420                List<Point2D> points = new ArrayList<>();
421
422                // middles
423                Point2D pABM = MathUtil.midPoint(pA, pB);
424                Point2D pAM = pABM, pBM = pABM;
425
426                Point2D pCDM = MathUtil.midPoint(pC, pD);
427                Point2D pCM = pCDM, pDM = pCDM;
428
429                if (tv.getTurnoutType() == LayoutTurnout.TurnoutType.DOUBLE_XOVER) {
430                    pAM = MathUtil.lerp(pA, pABM, 5.0 / 8.0);
431                    pBM = MathUtil.lerp(pB, pABM, 5.0 / 8.0);
432                    pCM = MathUtil.lerp(pC, pCDM, 5.0 / 8.0);
433                    pDM = MathUtil.lerp(pD, pCDM, 5.0 / 8.0);
434                }
435
436                if (tv.getConnectA().equals(d.getLastTrack())) {
437                    if (state == jmri.Turnout.CLOSED) {
438                        points.add(pA);
439                        points.add(pB);
440                        d.nextLayoutTrack = tv.getConnectB();
441                    } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.LH_XOVER) && (state == jmri.Turnout.THROWN)) {
442                        points.add(pA);
443                        points.add(pAM);
444                        points.add(pCM);
445                        points.add(pC);
446                        d.nextLayoutTrack = tv.getConnectC();
447                    }
448                } else if (tv.getConnectB().equals(d.getLastTrack())) {
449                    if (state == jmri.Turnout.CLOSED) {
450                        points.add(pB);
451                        points.add(pA);
452                        d.nextLayoutTrack = tv.getConnectA();
453                    } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.RH_XOVER) && (state == jmri.Turnout.THROWN)) {
454                        points.add(pB);
455                        points.add(pBM);
456                        points.add(pDM);
457                        points.add(pD);
458                        d.nextLayoutTrack = tv.getConnectD();
459                    }
460                } else if (tv.getConnectC().equals(d.getLastTrack())) {
461                    if (state == jmri.Turnout.CLOSED) {
462                        points.add(pC);
463                        points.add(pD);
464                        d.nextLayoutTrack = tv.getConnectD();
465                    } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.LH_XOVER) && (state == jmri.Turnout.THROWN)) {
466                        points.add(pC);
467                        points.add(pCM);
468                        points.add(pAM);
469                        points.add(pA);
470                        d.nextLayoutTrack = tv.getConnectA();
471                    }
472                } else if (tv.getConnectD().equals(d.getLastTrack())) {
473                    if (state == jmri.Turnout.CLOSED) {
474                        points.add(pD);
475                        points.add(pC);
476                        d.nextLayoutTrack = tv.getConnectC();
477                    } else if ((tv.getTurnoutType() != LayoutTurnout.TurnoutType.RH_XOVER) && (state == jmri.Turnout.THROWN)) {
478                        points.add(pD);
479                        points.add(pDM);
480                        points.add(pBM);
481                        points.add(pB);
482                        d.nextLayoutTrack = tv.getConnectB();
483                    }
484                } else { // OOPS! we're lost!
485                    result = false;
486                }
487
488                if (d.nextLayoutTrack != null) {
489                    d.setReturnLastTrack(d.nextLayoutTrack);
490                    d.setReturnTrack(d.getLayoutTrack());
491                }
492                return navigate(points, d.nextLayoutTrack);
493            }
494
495            case SINGLE_SLIP:
496            case DOUBLE_SLIP: {
497                log.warn("TurnoutView {}.navigate(...); slips should be being handled by LayoutSlip sub-class", tv.getName());
498                break;
499            }
500            default: { // OOPS! we're lost!
501                result = false;
502                break;
503            }
504        }
505        d.distanceOnTrack = distanceOnTrack;
506
507        if (result) { // not on this track
508            // go to next track
509            LayoutTrack last = d.getLayoutTrack();
510            if (d.nextLayoutTrack != null) {
511                d.setLayoutTrack(d.nextLayoutTrack);
512            } else { // OOPS! we're lost!
513                result = false;
514            }
515            if (result) {
516                d.setLastTrack(last);
517                d.setReturnTrack(d.getLayoutTrack());
518                d.setReturnLastTrack(d.getLayoutTrack());
519            }
520        }
521        return result;
522    }
523
524    // NOTE: LayoutSlip uses the checkForNonContiguousBlocks
525    //      and collectContiguousTracksNamesInBlockNamed methods
526    //      inherited from LayoutTurnout
527    boolean navigateLayoutSlip() {
528        if (use_blocks && ((LayoutSlip) d.getLayoutTrack()).getLayoutBlock().getBlock() != VSDecoderManager.instance().currentBlock.get(d)) {
529            // we are not in the block
530            d.setDistance(0);
531            return false;
532        }
533
534        boolean result = true; // assume success (optimist!)
535
536        LayoutSlipView ltv = d.getModels().getLayoutSlipView((LayoutSlip) d.getLayoutTrack());
537
538        Point2D pA = ltv.getCoordsA();
539        Point2D pB = ltv.getCoordsB();
540        Point2D pC = ltv.getCoordsC();
541        Point2D pD = ltv.getCoordsD();
542
543        d.nextLayoutTrack = null;
544
545        List<Point2D> points = new ArrayList<>();
546
547        // thirds
548        double third = 1.0 / 3.0;
549        Point2D pACT = MathUtil.lerp(pA, pC, third);
550        Point2D pBDT = MathUtil.lerp(pB, pD, third);
551        Point2D pCAT = MathUtil.lerp(pC, pA, third);
552        Point2D pDBT = MathUtil.lerp(pD, pB, third);
553
554        int slipState = ltv.getSlipState();
555
556        boolean slip_lost = false;
557
558        if (ltv.getConnectA().equals(d.getLastTrack())) {
559            if (slipState == LayoutTurnout.STATE_AC) {
560                points.add(pA);
561                points.add(pC);
562                d.nextLayoutTrack = ltv.getConnectC();
563            } else if (slipState == LayoutTurnout.STATE_AD) {
564                points.add(pA);
565                points.add(pACT);
566                points.add(pDBT);
567                points.add(pD);
568                d.nextLayoutTrack = ltv.getConnectD();
569            } else { // OOPS! we're lost!
570                result = false;
571                slip_lost = true;
572            }
573        } else if (ltv.getConnectB().equals(d.getLastTrack())) {
574            if (slipState == LayoutTurnout.STATE_BD) {
575                points.add(pB);
576                points.add(pD);
577                d.nextLayoutTrack = ltv.getConnectD();
578            } else if (slipState == LayoutTurnout.STATE_BC) {
579                points.add(pB);
580                points.add(pBDT);
581                points.add(pCAT);
582                points.add(pC);
583                d.nextLayoutTrack = ltv.getConnectC();
584            } else { // OOPS! we're lost!
585                result = false;
586                slip_lost = true;
587            }
588        } else if (ltv.getConnectC().equals(d.getLastTrack())) {
589            if (slipState == LayoutTurnout.STATE_AC) {
590                points.add(pC);
591                points.add(pA);
592                d.nextLayoutTrack = ltv.getConnectA();
593            } else if (slipState == LayoutTurnout.STATE_BC) {
594                points.add(pC);
595                points.add(pCAT);
596                points.add(pBDT);
597                points.add(pB);
598                d.nextLayoutTrack = ltv.getConnectB();
599            } else { // OOPS! we're lost!
600                result = false;
601                slip_lost = true;
602            }
603        } else if (ltv.getConnectD().equals(d.getLastTrack())) {
604            if (slipState == LayoutTurnout.STATE_BD) {
605                points.add(pD);
606                points.add(pB);
607                d.nextLayoutTrack = ltv.getConnectB();
608            } else if (slipState == LayoutTurnout.STATE_AD) {
609                points.add(pD);
610                points.add(pDBT);
611                points.add(pACT);
612                points.add(pA);
613                d.nextLayoutTrack = ltv.getConnectA();
614            } else { // OOPS! we're lost!
615                result = false;
616                slip_lost = true;
617            }
618        } else { // OOPS! we're lost!
619            result = false;
620        }
621        if (d.nextLayoutTrack != null) {
622            d.setReturnLastTrack(d.nextLayoutTrack);
623            d.setReturnTrack(d.getLayoutTrack());
624        }
625        if (slip_lost) {
626            log.info(" Turnout state not good");
627            d.setDistance(0);
628            d.setReturnDistance(0);
629        }
630
631        if (result) {
632            result = navigate(points, d.nextLayoutTrack);
633        }
634        return result;
635    }
636
637    boolean navigateLevelXing() {
638        boolean result = false;
639        jmri.Block block2 = null;
640        LevelXing lx = (LevelXing) d.getLayoutTrack();
641        if (lx.getConnectA().equals(d.getLastTrack()) || lx.getConnectC().equals(d.getLastTrack())) {
642            block2 = lx.getLayoutBlockAC().getBlock();
643        } else if (lx.getConnectB().equals(d.getLastTrack()) || lx.getConnectD().equals(d.getLastTrack())) {
644            block2 = lx.getLayoutBlockBD().getBlock();
645        }
646        if (use_blocks && block2 != VSDecoderManager.instance().currentBlock.get(d)) {
647            // not in the block (blocks do not match)
648            d.setDistance(0);
649            return result;
650        }
651
652        double distanceOnTrack = d.getDistance() + d.distanceOnTrack;
653
654        LevelXingView lxv = d.getModels().getLevelXingView((LevelXing) d.getLayoutTrack());
655        Point2D pA = lxv.getCoordsA();
656        Point2D pB = lxv.getCoordsB();
657        Point2D pC = lxv.getCoordsC();
658        Point2D pD = lxv.getCoordsD();
659        Point2D p1 = null;
660        Point2D p2 = null;
661
662        d.nextLayoutTrack = null;
663
664        if (lxv.getConnectA().equals(d.getLastTrack())) {
665            p1 = pA;
666            p2 = pC;
667            d.nextLayoutTrack = lxv.getConnectC();
668        } else if (lxv.getConnectB().equals(d.getLastTrack())) {
669            p1 = pB;
670            p2 = pD;
671            d.nextLayoutTrack = lxv.getConnectD();
672        } else if (lxv.getConnectC().equals(d.getLastTrack())) {
673            p1 = pC;
674            p2 = pA;
675            d.nextLayoutTrack = lxv.getConnectA();
676        } else if (lxv.getConnectD().equals(d.getLastTrack())) {
677            p1 = pD;
678            p2 = pB;
679            d.nextLayoutTrack = lxv.getConnectB();
680            result = false;
681        }
682        if (d.nextLayoutTrack != null) {
683            d.setReturnLastTrack(d.nextLayoutTrack);
684            d.setReturnTrack(d.getLayoutTrack());
685        }
686
687        if (p1 != null) {
688            double distance = MathUtil.distance(p1, p2);
689            d.setReturnDistance(distance);
690            if (distanceOnTrack < distance) {
691                // it's on this track
692                double ratio = distanceOnTrack / distance;
693                d.setLocation(MathUtil.lerp(p1, p2, ratio));
694                d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(p2, p1));
695                d.setDistance(0);
696            } else { // it's not on this track
697                d.setDistance(distanceOnTrack - distance);
698                distanceOnTrack = 0;
699                result = true;
700            }
701            d.distanceOnTrack = distanceOnTrack;
702        }
703
704        if (result) { // not on this track
705            // go to next track
706            LayoutTrack last = d.getLayoutTrack();
707            if (d.nextLayoutTrack != null) {
708                d.setLayoutTrack(d.nextLayoutTrack);
709            } else { // OOPS! we're lost!
710                result = false;
711            }
712            if (result) {
713                d.setLastTrack(last);
714                d.setReturnTrack(d.getLayoutTrack());
715                d.setReturnLastTrack(d.getLayoutTrack());
716            }
717        }
718        return result;
719    }
720
721    boolean navigateLayoutTurntable() {
722        boolean result = false;
723        if (use_blocks && !((LayoutTurntable) d.getLayoutTrack()).getBlockName().equals(VSDecoderManager.instance().currentBlock.get(d).getUserName())) {
724            // we are not in the block
725            d.setDistance(0);
726            return false;
727        }
728
729        double distanceOnTrack = d.getDistance() + d.distanceOnTrack;
730        d.nextLayoutTrack = null;
731
732        LayoutTurntable turntable = (LayoutTurntable) d.getLayoutTrack();
733        LayoutTurntableView ttv = d.getModels().getLayoutTurntableView(turntable);
734        int num_rays = turntable.getNumberRays();
735        log.debug("turntable name: {}, number rays: {}", ttv.getName(), num_rays);
736
737        Point2D pStart = null;
738        Point2D pEnd   = null;
739
740        // some checks ...
741        if (num_rays < 1) {
742            log.warn("A turntable must have at least one ray (better two)");
743        } else if (turntable.getPosition() < 0) {
744            log.warn("Turntable position not set"); // setting the correct position allows to continue
745        } else {
746            List<Point2D> points = new ArrayList<>();
747            for (int i = 0; i < num_rays; i++) {
748                points.add(ttv.getRayCoordsOrdered(i));
749            }
750
751            for (LayoutTurntable.RayTrack rt : turntable.getRayTrackList()) {
752                if (rt.getConnect().equals(d.getLastTrack())) {
753                    // is there a counter-ray? If so, get this index
754                    double counterAngle = MathUtil.wrap360(rt.getAngle() + 180.0);
755                    boolean found = false;
756                    int indexT = -1; // init
757                    for (LayoutTurntable.RayTrack rta : turntable.getRayTrackList()) {
758                        if (counterAngle == rta.getAngle()) {
759                            found = true; // yes, counter-ray exists
760                            indexT = rta.getConnectionIndex();
761                            break;
762                        }
763                    }
764                    if (!found) {
765                        // ray without counter-ray - not supported (there is no HitPoint for the bridge end)
766                        if (turntable.getPosition() == rt.getConnectionIndex()) {
767                            log.warn("non-existent opposite ray track; please return"); // going reverse works
768                        } else {
769                            log.warn("Wrong turntable position - please correct or return");
770                        }
771                    } else {
772                        boolean is_turned = false;
773                        int indexH = rt.getConnectionIndex();
774                        if (lastTurntablePosition >= 0 && turntable.getPosition() != lastTurntablePosition) {
775                            // new bridge position detected
776                            is_turned = true;
777                            double newAngle = turntable.getRayTrackList().get(turntable.getPosition()).getAngle();
778                            double lastAngle = MathUtil.wrap360(newAngle + 180.0);
779                            boolean found2 = false;
780                            for (LayoutTurntable.RayTrack rtb : turntable.getRayTrackList()) {
781                                if (lastAngle == rtb.getAngle()) {
782                                    found2 = true; // yes, counter-ray exists
783                                    indexH = rtb.getConnectionIndex();
784                                    break;
785                                }
786                            }
787                            if (found2) {
788                                d.setLastTrack(turntable.getRayConnectIndexed(indexH));
789                                d.nextLayoutTrack = turntable.getRayConnectIndexed(turntable.getPosition());
790                                indexT = turntable.getPosition(); // update index
791                            } else {
792                                log.info("non-existent opposite ray track)");
793                            }
794                        }
795
796                        if (turntable.getPosition() == indexT || turntable.getPosition() == indexH) {
797                            // turntable position is correct
798                            pStart = points.get(indexH);
799                            if (is_turned) {
800                                pEnd = points.get(turntable.getPosition());
801                            } else {
802                                pEnd = points.get(indexT);
803                            }
804                            d.nextLayoutTrack = turntable.getRayConnectIndexed(indexT);
805                            log.debug("Next layout track set to: {}", d.nextLayoutTrack);
806                            lastTurntablePosition = turntable.getPosition();
807                        } else {
808                            log.warn("Wrong turntable position - please correct position");
809                        }
810                    }
811                    break;
812                }
813            }
814        }
815
816        if (d.nextLayoutTrack != null) {
817            d.setReturnLastTrack(d.nextLayoutTrack);
818            d.setReturnTrack(d.getLayoutTrack()); // just in case of a direction change
819            d.setDistance(0);
820        }
821        if (d.nextLayoutTrack == null) {
822            log.debug("Next layout track not set");
823            result = false;
824        }
825
826        if (pStart != null && pEnd != null) {
827            double distance = MathUtil.distance(pStart, pEnd);
828            d.setReturnDistance(distance);
829            if (distanceOnTrack < distance) {
830                // it's on this track
831                double ratio = distanceOnTrack / distance;
832                d.setLocation(MathUtil.lerp(pStart, pEnd, ratio));
833                d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(pEnd, pStart));
834                d.setDistance(0);
835            } else { // it's not on this track
836                d.setDistance(distanceOnTrack - distance);
837                distanceOnTrack = 0;
838                result = true;
839            }
840        } else { // OOPS! we're lost!
841            log.info("Turntable caused a stop"); // correct position or change direction
842            result = false;
843            distanceOnTrack = 0;
844            d.setDistance(0);
845            d.setReturnDistance(0);
846            d.setReturnTrack(d.getLastTrack());
847            log.debug("new d.distanceOnTrack: {}, distanceOnTrack: {}, last: {}", d.distanceOnTrack, distanceOnTrack, d.getLastTrack());
848        }
849        d.distanceOnTrack = distanceOnTrack;
850
851        if (result) { // not on this track
852            // go to next track
853            log.debug("go to next layout track: {}", d.nextLayoutTrack);
854            LayoutTrack last = d.getLayoutTrack();
855            if (d.nextLayoutTrack != null) {
856                d.setLayoutTrack(d.nextLayoutTrack);
857                lastTurntablePosition = -1;
858            } else { // OOPS! we're lost!
859                log.info(" TURNTABLE RESULT lost");
860                result = false;
861            }
862            if (result) {
863                d.setLastTrack(last);
864                d.setReturnTrack(d.getLayoutTrack());
865                d.setReturnLastTrack(d.getLayoutTrack());
866            }
867        }
868        return result;
869    }
870
871    private boolean navigate(List<Point2D> points, @CheckForNull LayoutTrack nextLayoutTrack) {
872        boolean result = false;
873        double distanceOnTrack = d.getDistance() + d.distanceOnTrack;
874        boolean nextLegFlag = true;
875        Point2D lastPoint = null;
876        double trackDistance = 0;
877        for (Point2D p : points) {
878            if (lastPoint != null) {
879                double distance = MathUtil.distance(lastPoint, p);
880                trackDistance += distance;
881                if (distanceOnTrack < trackDistance) { // it's on this leg
882                    d.setLocation(MathUtil.lerp(p, lastPoint, (trackDistance - distanceOnTrack) / distance));
883                    d.setDirectionRAD((Math.PI / 2) - MathUtil.computeAngleRAD(p, lastPoint));
884                    nextLegFlag = false;
885                    break;
886                }
887            }
888            lastPoint = p;
889        }
890        if (nextLegFlag) { // it's not on this track
891            d.setDistance(distanceOnTrack - trackDistance);
892            distanceOnTrack = 0;
893            result = true;
894        } else { // it's on this track
895            d.setDistance(0);
896        }
897        d.distanceOnTrack = distanceOnTrack;
898        if (result) { // not on this track
899            // go to next track
900            LayoutTrack last = d.getLayoutTrack();
901            if (nextLayoutTrack != null) {
902                d.setLayoutTrack(nextLayoutTrack);
903            } else { // OOPS! we're lost!
904                result = false;
905            }
906            if (result) {
907                d.setLastTrack(last);
908                d.setReturnTrack(d.getLayoutTrack());
909                d.setReturnLastTrack(d.getLayoutTrack());
910            }
911        }
912        return result;
913    }
914
915    private static final Logger log = LoggerFactory.getLogger(VSDNavigation.class);
916
917}