Subversion Repositories mkgmap

/*
 * Copyright (C) 2008 Steve Ratcliffe
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 2 as
 *  published by the Free Software Foundation.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *
 * Author: Steve Ratcliffe
 * Create date: 13-Jul-2008
 */
package uk.me.parabola.imgfmt.app.net;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.BitSet;
import java.util.Iterator;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;
import java.util.Map.Entry;
import java.util.TreeMap;

import uk.me.parabola.imgfmt.app.Coord;
import uk.me.parabola.imgfmt.app.CoordNode;
import uk.me.parabola.log.Logger;
import uk.me.parabola.util.EnhancedProperties;

/**
 * This holds the road network.  That is all the roads and the nodes
 * that connect them together.
 *
 * @see <a href="http://www.movable-type.co.uk/scripts/latlong.html">Distance / bearing calculations</a>
 * @author Steve Ratcliffe
 */
public class RoadNetwork {
        private static final Logger log = Logger.getLogger(RoadNetwork.class);

        private final static int MAX_RESTRICTIONS_ARCS = 7;
        private final Map<Integer, RouteNode> nodes = new LinkedHashMap<>();

        // boundary nodes
        // a node should be in here if the nodes boundary flag is set
        private final List<RouteNode> boundary = new ArrayList<>();
        private final List<RoadDef> roadDefs = new ArrayList<>();
        private List<RouteCenter> centers = new ArrayList<>();
        private AngleChecker angleChecker = new AngleChecker();

        private boolean checkRoundabouts;
        private boolean checkRoundaboutFlares;
        private int maxFlareLengthRatio ;
        private boolean reportSimilarArcs;

        public void config(EnhancedProperties props) {
                checkRoundabouts = props.getProperty("check-roundabouts", false);
                checkRoundaboutFlares = props.getProperty("check-roundabout-flares", false);
                maxFlareLengthRatio = props.getProperty("max-flare-length-ratio", 0);
                reportSimilarArcs = props.getProperty("report-similar-arcs", false);
                angleChecker.config(props);
        }

        public void addRoad(RoadDef roadDef, List<Coord> coordList) {
                roadDefs.add(roadDef);

                CoordNode lastCoord = null;
                int lastIndex = 0;
                double roadLength = 0;
                double arcLength = 0;
                int pointsHash = 0;

                int npoints = coordList.size();
                int numCoordNodes = 0;
                boolean hasInternalNodes = false;
                int numNumberNodes = 0;
                BitSet nodeFlags = new BitSet();
                for (int index = 0; index < npoints; index++) {
                        Coord co = coordList.get(index);
                        int id = co.getId();

                        if (id != 0){
                                nodeFlags.set(numNumberNodes);
                                ++numCoordNodes;
                                if(index > 0 && index < npoints - 1)
                                        hasInternalNodes = true;
                        }
                        if (co.isNumberNode())
                                ++numNumberNodes;
                        if (index == 0){
                                if (id == 0)
                                        roadDef.setStartsWithNode(false);
                               
                        } else {
                                double d = co.distance(coordList.get(index-1));
                                arcLength += d;
                                roadLength += d;
                        }
                        if (roadDef.skipAddToNOD())
                                continue;
                       
                        pointsHash += co.hashCode();

                        if (id == 0)
                                // not a routing node
                                continue;

                        // The next coord determines the heading
                        // If this is the not the first node, then create an arc from
                        // the previous node to this one (and back again).
                        if (lastCoord != null) {
                                int lastId = lastCoord.getId();
                                if(log.isDebugEnabled()) {
                                        log.debug("lastId = " + lastId + " curId = " + id);
                                        log.debug("from " + lastCoord.toDegreeString()
                                                          + " to " + co.toDegreeString());
                                        log.debug("arclength=" + arcLength + " roadlength=" + roadLength);
                                }

                                RouteNode node1 = getOrAddNode(lastId, lastCoord);
                                RouteNode node2 = getOrAddNode(id, co);
                                if(node1 == node2)
                                        log.error("Road " + roadDef + " contains consecutive identical nodes at " + co.toOSMURL() + " - routing will be broken");
                                else if(arcLength == 0)
                                        log.warn("Road " + roadDef + " contains zero length arc at " + co.toOSMURL());

                                Coord forwardBearingPoint = coordList.get(lastIndex + 1);
                                if(lastCoord.equals(forwardBearingPoint) || forwardBearingPoint.isAddedNumberNode()) {
                                        // bearing point is too close to last node to be
                                        // useful - try some more points
                                        for(int bi = lastIndex + 2; bi <= index; ++bi) {
                                                Coord coTest = coordList.get(bi);
                                                if (coTest.isAddedNumberNode() || lastCoord.equals(coTest))
                                                        continue;
                                                forwardBearingPoint = coTest;
                                                break;
                                        }
                                }
                                Coord reverseBearingPoint = coordList.get(index - 1);
                                if(co.equals(reverseBearingPoint) || reverseBearingPoint.isAddedNumberNode()) {
                                        // bearing point is too close to this node to be
                                        // useful - try some more points
                                        for(int bi = index - 2; bi >= lastIndex; --bi) {
                                                Coord coTest = coordList.get(bi);
                                                if (coTest.isAddedNumberNode() || co.equals(coTest))
                                                        continue;
                                                reverseBearingPoint = coTest;
                                                break;
                                        }
                                }
                               
                                double forwardInitialBearing = lastCoord.bearingTo(forwardBearingPoint);
                                double forwardDirectBearing = (co == forwardBearingPoint) ? forwardInitialBearing: lastCoord.bearingTo(co);

                                double reverseInitialBearing = co.bearingTo(reverseBearingPoint);
                                double directLength = (lastIndex + 1 == index) ? arcLength : lastCoord.distance(co);
                                double reverseDirectBearing = 0;
                                if (directLength > 0){
                                        // bearing on rhumb line is a constant, so we can simply revert
                                        reverseDirectBearing = (forwardDirectBearing <= 0) ? 180 + forwardDirectBearing: -(180 - forwardDirectBearing) % 180.0;
                                }
                                // Create forward arc from node1 to node2
                                RouteArc arc = new RouteArc(roadDef,
                                                                                        node1,
                                                                                        node2,
                                                                                        forwardInitialBearing,
                                                                                        forwardDirectBearing,
                                                                                        arcLength,
                                                                                        arcLength,
                                                                                        directLength,
                                                                                        pointsHash);
                                arc.setForward();
                                node1.addArc(arc);
                               
                                // Create the reverse arc
                                RouteArc reverseArc = new RouteArc(roadDef,
                                                                   node2, node1,
                                                                   reverseInitialBearing,
                                                                   reverseDirectBearing,
                                                                   arcLength,
                                                                   arcLength,
                                                                   directLength,
                                                                   pointsHash);
                                node2.addArc(reverseArc);
                                // link the two arcs
                                arc.setReverseArc(reverseArc);
                                reverseArc.setReverseArc(arc);
                        } else {
                                // This is the first node in the road
                                roadDef.setNode(getOrAddNode(id, co));
                        }

                        lastCoord = (CoordNode) co;
                        lastIndex = index;
                        arcLength = 0;
                        pointsHash = co.hashCode();
                }
                if (roadDef.hasHouseNumbers()){
                        // we ignore number nodes when we have no house numbers
                        if (numCoordNodes < numNumberNodes)
                                hasInternalNodes = true;
                        roadDef.setNumNodes(numNumberNodes);
                        roadDef.setNod2BitSet(nodeFlags);
                } else {
                        roadDef.setNumNodes(numCoordNodes);
                }
                if (hasInternalNodes)
                        roadDef.setInternalNodes(true);
                roadDef.setLength(roadLength);
        }

        private RouteNode getOrAddNode(int id, Coord coord) {
                RouteNode node = nodes.get(id);
                if (node == null) {
                        node = new RouteNode(coord);
                        nodes.put(id, node);
                        if (node.isBoundary())
                                boundary.add(node);
                }
                return node;
        }

        public List<RoadDef> getRoadDefs() {
                return roadDefs;
        }

        /**
         * Split the network into RouteCenters.
         *
         * The resulting centers must satisfy several constraints,
         * documented in NOD1Part.
         */
        private void splitCenters() {
                if (nodes.isEmpty())
                        return;
                assert centers.isEmpty() : "already subdivided into centers";

                // sort nodes by NodeGroup
                List<RouteNode> nodeList = new ArrayList<>(nodes.values());
                nodes.clear(); // return to GC
                for (int group = 0; group <= 4; group++){
                        NOD1Part nod1 = new NOD1Part();
                        int n = 0;
                        for (RouteNode node : nodeList) {
                                if (node.getGroup() != group)
                                        continue;
                                if(!node.isBoundary()) {
                                        if(checkRoundabouts)
                                                node.checkRoundabouts();
                                        if(checkRoundaboutFlares)
                                                node.checkRoundaboutFlares(maxFlareLengthRatio);
                                        if(reportSimilarArcs)
                                                node.reportSimilarArcs();
                                }
                               
                                nod1.addNode(node);
                                n++;
                        }
                        if (n > 0)
                                centers.addAll(nod1.subdivide());
                }
        }

        public List<RouteCenter> getCenters() {
                if (centers.isEmpty()){
                        angleChecker.check(nodes);
                        addArcsToMajorRoads();
                        splitCenters();
                }
                return centers;
        }

        /**
         * add indirect arcs for each road class (in descending order)
         */
        private void addArcsToMajorRoads() {
                long t1 = System.currentTimeMillis();
               
                for (RoadDef rd: roadDefs){
                        if (rd.skipAddToNOD())
                                continue;
                        if (rd.getRoadClass() >= 1)
                                rd.getNode().addArcsToMajorRoads(rd);
                }
                log.info(" added major road arcs in " + (System.currentTimeMillis() - t1) + " ms");
        }

        /**
         * Get the list of nodes on the boundary of the network.
         *
         * Currently empty.
         */
        public List<RouteNode> getBoundary() {
                return boundary;
        }

        /**
         * One restriction forbids to travel a specific combination of arcs.
         * We know two kinds: 3 nodes with two arcs and one via node or 4 nodes with 3 arcs
         * and two via nodes. Maybe more nodes are possible, but we don't know for sure how
         * to write them (2014-04-02).
         * Depending on the data in grr we create one or more such restrictions.
         * A restriction with 4 (or more) nodes is added to each via node.    
         *
         * The OSM restriction gives a from way id and a to way id and one or more
         * via nodes. It is possible that the to-way is a loop, so we have to identify
         * the correct arc.
         * @param grr the object that holds the details about the route restriction
         */
        public int addRestriction(GeneralRouteRestriction grr) {
                if (grr.getType() == GeneralRouteRestriction.RestrType.TYPE_NO_TROUGH)
                        return addNoThroughRoute(grr);
                String sourceDesc = grr.getSourceDesc();
               
                List<RouteNode> viaNodes = new ArrayList<>();
                for (CoordNode via : grr.getViaNodes()){
                        RouteNode vn = nodes.get(via.getId());
                        if (vn == null){
                                log.error(sourceDesc, "can't locate 'via' RouteNode with id", via.getId());
                                return 0;
                        }
                        viaNodes.add(vn);
                }
               
                int firstViaId = grr.getViaNodes().get(0).getId();
                int lastViaId = grr.getViaNodes().get(grr.getViaNodes().size()-1).getId();
                RouteNode firstViaNode = nodes.get(firstViaId);
                RouteNode lastViaNode = nodes.get(lastViaId);
                List<List<RouteArc>> viaArcsList = new ArrayList<>();
                if (grr.getViaNodes().size() != grr.getViaWayIds().size() + 1){
                        log.error(sourceDesc, "internal error: number of via nodes and via ways doesn't fit");
                        return 0;
                }
                for (int i = 1; i < grr.getViaNodes().size(); i++){
                        RouteNode vn = viaNodes.get(i-1);
                        Long viaWayId = grr.getViaWayIds().get(i-1);
                        List<RouteArc> viaArcs = vn.getDirectArcsTo(viaNodes.get(i), viaWayId);
                        if (viaArcs.isEmpty()){
                                log.error(sourceDesc, "can't locate arc from 'via' node at",vn.getCoord().toOSMURL(),"to next 'via' node on way",viaWayId);
                                return 0;
                        }
                        viaArcsList.add(viaArcs);
                }
               
                // determine the from node and arc(s)
                int fromId = 0;
                RouteNode fn = null;
                if (grr.getFromNode() != null){
                        fromId = grr.getFromNode().getId();
                        // polish input data provides id
                        fn = nodes.get(fromId);
                        if (fn == null ){
                                log.error(sourceDesc, "can't locate 'from' RouteNode with id", fromId);
                                return 0;
                        }
                } else {
                        List<RouteArc> possibleFromArcs = firstViaNode.getDirectArcsOnWay(grr.getFromWayId());
                        for (RouteArc arc : possibleFromArcs){
                                if (fn == null)
                                        fn = arc.getDest();
                                else if (fn != arc.getDest()){
                                        log.warn(sourceDesc, "found different 'from' arcs for way",grr.getFromWayId(),"restriction is ignored");
                                        return 0;
                                }
                        }
                        if (fn == null){
                                log.warn(sourceDesc, "can't locate 'from' RouteNode for 'from' way", grr.getFromWayId());
                                return 0;
                        }
                        fromId = fn.getCoord().getId();
                }
                List<RouteArc> fromArcs = fn.getDirectArcsTo(firstViaNode, grr.getFromWayId());
                if (fromArcs.isEmpty()){
                        log.error(sourceDesc, "can't locate arc from 'from' node ",fromId,"to 'via' node",firstViaId,"on way",grr.getFromWayId());
                        return 0;
                }
               
                // a bit more complex: determine the to-node and arc(s)
                RouteNode tn = null;
                int toId = 0;
                List<RouteArc> toArcs = new ArrayList<>();
                if (grr.getToNode() != null){
                        // polish input data provides id
                        toId = grr.getToNode().getId();
                        tn = nodes.get(toId);
                        if (tn == null ){
                                log.error(sourceDesc, "can't locate 'to' RouteNode with id", toId);
                                return 0;
                        }
                } else {
                        // we can have multiple arcs between last via node and to node. The
                        // arcs can be on the same OSM way or on different OSM ways.
                        // We can have multiple arcs with different RoadDef objects that refer to the same
                        // OSM way id. The direction indicator tells us what arc is probably meant.
                        List<RouteArc> possibleToArcs = lastViaNode.getDirectArcsOnWay(grr.getToWayId());
                        RouteArc fromArc = fromArcs.get(0);

                        boolean ignoreAngle = false;
                        if (fromArc.getLengthInMeter() <= 0.0001)
                                ignoreAngle = true;
                        if (grr.getDirIndicator() == '?')
                                ignoreAngle = true;
                        log.info(sourceDesc, "found", possibleToArcs.size(), "candidates for to-arc");

                        // group the available arcs by angle
                        Map<Integer, List<RouteArc>> angleMap = new TreeMap<>();
                        for (RouteArc arc : possibleToArcs){
                                if (arc.getLengthInMeter() <= 0.0001)
                                        ignoreAngle = true;
                                Integer angle = Math.round(getAngle(fromArc, arc));
                                List<RouteArc> list = angleMap.get(angle);
                                if (list == null){
                                        list = new ArrayList<>();
                                        angleMap.put(angle, list);
                                }
                                list.add(arc);
                        }

                        // find the group that fits best
                        Iterator<Entry<Integer, List<RouteArc>>> iter = angleMap.entrySet().iterator();
                        Integer bestAngle = null;
                        while (iter.hasNext()){
                                Entry<Integer, List<RouteArc>> entry = iter.next();
                                if (ignoreAngle || matchDirectionInfo(entry.getKey(), grr.getDirIndicator()) ){
                                        if (bestAngle == null)
                                                bestAngle = entry.getKey();
                                        else {
                                                bestAngle = getBetterAngle(bestAngle, entry.getKey(), grr.getDirIndicator());
                                        }
                                }
                        }
                       
                        if (bestAngle == null){
                                log.warn(sourceDesc,"the angle of the from and to way don't match the restriction");
                                return 0;
                        }
                        toArcs = angleMap.get(bestAngle);
                }
                if (toArcs.isEmpty()){
                        log.error(sourceDesc, "can't locate arc from 'via' node ",lastViaId,"to 'to' node",toId,"on way",grr.getToWayId());
                        return 0;
                }
               
                List<RouteArc> badArcs = new ArrayList<>();
               
                if (grr.getType() == GeneralRouteRestriction.RestrType.TYPE_NOT){
                        for (RouteArc toArc: toArcs){
                                badArcs.add(toArc);
                        }
                }
                else if (grr.getType() == GeneralRouteRestriction.RestrType.TYPE_ONLY){
                        // this is the inverse logic, grr gives the allowed path, we have to find the others
                        for (RouteArc badArc : lastViaNode.arcsIteration()){
                                if (!badArc.isDirect() || toArcs.contains(badArc))
                                        continue;
                                badArcs.add(badArc);
                        }
                        if (badArcs.isEmpty()){
                                log.warn(sourceDesc, "restriction ignored because it has no effect");
                                return 0;
                        }
                }
               
                // create all possible paths for which the restriction applies
                List<List<RouteArc>> arcLists = new ArrayList<>();
                arcLists.add(fromArcs);
                arcLists.addAll(viaArcsList);
                arcLists.add(badArcs);
                if (arcLists.size() > MAX_RESTRICTIONS_ARCS){
                        log.warn(sourceDesc, "has more than", MAX_RESTRICTIONS_ARCS, "arcs, this is not supported");
                        return 0;
                }
               
                // remove arcs which cannot be travelled by the vehicles listed in the restriction
                for (int i = 0; i < arcLists.size(); i++){
                        List<RouteArc> arcs =  arcLists.get(i);
                        int countNoEffect = 0;
                        int countOneway= 0;
                        for (int j = arcs.size()-1; j >= 0; --j){
                                RouteArc arc = arcs.get(j);
                                if (isUsable(arc.getRoadDef().getAccess(), grr.getExceptionMask()) == false){
                                        countNoEffect++;
                                        arcs.remove(j);
                                }
                                else if (arc.getRoadDef().isOneway()){
                                        if (!arc.isForward()){
                                                countOneway++;
                                                arcs.remove(j);
                                        }
                                }
                        }
                        String arcType = null;
                        if (arcs.isEmpty()){
                                if (i == 0)
                                        arcType = "from way is";
                                else if (i == arcLists.size()-1){
                                        if (grr.getType() == GeneralRouteRestriction.RestrType.TYPE_ONLY)
                                                arcType = "all possible other ways are";
                                        else
                                                arcType = "to way is";
                                }
                                else
                                        arcType = "via way is";
                                String reason;
                                if (countNoEffect > 0 & countOneway > 0)
                                        reason = "wrong direction in oneway or not accessible for restricted vehicles";
                                else if (countNoEffect > 0)
                                        reason = "not accessible for restricted vehicles";
                                else
                                        reason = "wrong direction in oneway";
                                log.warn(sourceDesc, "restriction ignored because",arcType,reason);
                                return 0;
                        }
                }
                if (viaNodes.contains(fn)){
                        log.warn(sourceDesc, "restriction not written because from node appears also as via node");
                        return 0;
                }
                // determine all possible combinations of arcs. In most cases,
                // this will be 0 or one, but if the style creates multiple roads for one
                // OSM way, this can be a larger number
                int numCombis = 1;
                int [] indexes = new int[arcLists.size()];
                for (int i = 0; i < indexes.length; i++){
                        List<RouteArc> arcs =  arcLists.get(i);
                        numCombis *= arcs.size();
                }
                List<RouteArc> path = new ArrayList<>();
                int added = 0;
                for (int i = 0; i < numCombis; i++){
                        for (RouteNode vn : viaNodes){
                                path.clear();
                                boolean viaNodeFound = false;
                                byte pathNoAccessMask = 0;
                                for (int j = 0; j < indexes.length; j++){
                                        RouteArc arc = arcLists.get(j).get(indexes[j]);
                                        if (arc.getDest() == vn || viaNodeFound == false){
                                                arc = arc.getReverseArc();
                                        }
                                        if (arc.getSource() == vn)
                                                viaNodeFound = true;
                                        if (arc.getDest() == vn){
                                                if (added > 0)
                                                        log.error(sourceDesc, "restriction incompletely written because dest in arc is via node");
                                                else
                                                        log.warn(sourceDesc, "restriction not written because dest in arc is via node");
                                                return added;
                                        }
                                        pathNoAccessMask |= ~arc.getRoadDef().getAccess();
                                        path.add(arc);
                                }
                                byte pathAccessMask = (byte)~pathNoAccessMask;
                                if (isUsable(pathAccessMask, grr.getExceptionMask())){
                                        vn.addRestriction(new RouteRestriction(vn, path, grr.getExceptionMask()));
                                        ++added;
                                }
                        }
                        // get next combination of arcs
                        ++indexes[indexes.length-1];
                        for (int j = indexes.length-1; j > 0; --j){
                                if (indexes[j] >= arcLists.get(j).size()){
                                        indexes[j] = 0;
                                        indexes[j-1]++;
                                }
                        }
                }

                // double check
                if (indexes[0] != arcLists.get(0).size())
                        log.error(sourceDesc, " failed to generate all possible paths");
                log.info(sourceDesc, "added",added,"route restriction(s) to img file");
                return added;
        }

        /**
         * Compare the disallowed vehicles for the path with the exceptions from the restriction
         * @param roadNoAccess
         * @param exceptionMask
         * @return
         */
        private static boolean isUsable(byte roadAccess, byte exceptionMask) {
                if ((roadAccess & (byte) ~exceptionMask) == 0)
                        return false; // no allowed vehicle is concerned by this restriction
                return true;
        }

        private int addNoThroughRoute(GeneralRouteRestriction grr) {
                assert grr.getViaNodes() != null;
                assert grr.getViaNodes().size() == 1;
                int viaId = grr.getViaNodes().get(0).getId();
                RouteNode vn = nodes.get(viaId);
                if (vn == null){
                        log.error(grr.getSourceDesc(), "can't locate 'via' RouteNode with id", viaId);
                        return 0;
                }
                int added = 0;
               
                for (RouteArc out: vn.arcsIteration()){
                        if (!out.isDirect())
                                continue;
                        for (RouteArc in: vn.arcsIteration()){
                                if (!in.isDirect() || in == out || in.getDest() == out.getDest())
                                        continue;
                                byte pathAccessMask = (byte) (out.getRoadDef().getAccess() & in.getRoadDef().getAccess());
                                if (isUsable(pathAccessMask, grr.getExceptionMask())){
                                        vn.addRestriction(new RouteRestriction(vn, Arrays.asList(in,out), grr.getExceptionMask()));
                                        added++;
                                } else {
                                        if (log.isDebugEnabled())
                                                log.debug(grr.getSourceDesc(),"ignored no-through-route",in,"to",out);
                                }
                        }
                }
                return added;
        }
       
        public void addThroughRoute(int junctionNodeId, long roadIdA, long roadIdB) {
                RouteNode node = nodes.get(junctionNodeId);
                assert node != null :  "Can't find node with id " + junctionNodeId;

                node.addThroughRoute(roadIdA, roadIdB);
        }
       
        /**
         * Calculate the "angle" between to arcs. The arcs may not be connected.
         * We do this by "virtually" moving the toArc so that its source
         * node lies on the destination node of the from arc.
         * This makes only sense if move is not over a large distance, we assume that this
         * is the case as via ways should be short.
         * @param fromArc arc with from node as source and first via node as destination
         * @param toArc arc with last via node as source
         * @return angle at in degree [-180;180]
         */
        private static float getAngle(RouteArc fromArc, RouteArc toArc){
                // note that the values do not depend on the isForward() attribute
                float headingFrom = fromArc.getFinalHeading();
                float headingTo = toArc.getInitialHeading();
                float angle = headingTo - headingFrom;
                while(angle > 180)
                        angle -= 360;
                while(angle < -180)
                        angle += 360;
                return angle;
        }
       
        /**
         * Find the angle that comes closer to the direction indicated.
         *
         * @param angle1 1st angle -180:180 degrees
         * @param angle2 2nd angle -180:180 degrees
         * @param dirIndicator l:left, r:right, u:u_turn, s: straight_on
         * @return
         */
        private static Integer getBetterAngle (Integer angle1, Integer angle2, char dirIndicator){
                switch (dirIndicator){
                case 'l':
                        if (Math.abs(-90-angle2) < Math.abs(-90-angle1))
                                return angle2; // closer to -90
                        break;
                case 'r':
                        if (Math.abs(90-angle2) < Math.abs(90-angle1))
                                return angle2; // closer to 90
                        break;
                case 'u':
                        double d1 = (angle1 < 0 ) ? -180-angle1 : 180-angle1;
                        double d2 = (angle2 < 0 ) ? -180-angle2 : 180-angle2;
                        if (Math.abs(d2) < Math.abs(d1))
                                return angle2; // closer to -180
                        break;
                case 's':
                        if (Math.abs(angle2) < Math.abs(angle1))
                                return angle2; // closer to 0
                        break;
                }
               
                return angle1;
        }
       
        /**
         * Check if angle is in the range indicated by the direction
         * @param angle the angle -180:180 degrees
         * @param dirIndicator l:left, r:right, u:u_turn, s: straight_on
         * @return
         */
        private static boolean matchDirectionInfo (float angle, char dirIndicator){
                switch (dirIndicator){
                case 'l':
                        if (angle < -3 && angle > - 177)
                                return true;
                        break;
                case 'r':
                        if (angle > 3 && angle < 177)
                                return true;
                        break;
                case 'u':
                        if (angle < -87 || angle > 93)
                                return true;
                        break;
                case 's':
                        if (angle > -87 && angle < 87)
                                return true;
                        break;
                case '?':
                        return true;
                }
                return false;
        }
       
       
}