Index: /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorAction.java
===================================================================
--- /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorAction.java	(revision 3748)
+++ /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorAction.java	(revision 3749)
@@ -22,14 +22,10 @@
  *
  */
-public class NavigatorAction extends AbstractAction implements SelectionChangedListener {
+public class NavigatorAction extends AbstractAction {
   private NavigatorPlugin navigatorPlugin;
-  private List<Node> selectedNodes;
-  private int selectionChangedCalls;
 
   public NavigatorAction(NavigatorPlugin navigatorPlugin) {
     super(tr("Navigate"));
     this.navigatorPlugin = navigatorPlugin;
-    selectedNodes = new ArrayList<Node>();
-    DataSet.listeners.add(this);
     
   }
@@ -39,36 +35,5 @@
    */
   public void actionPerformed(ActionEvent e) {
-    navigatorPlugin.navigate(selectedNodes);
-
-  }
-
-  /* (non-Javadoc)
-   * @see org.openstreetmap.josm.data.SelectionChangedListener#selectionChanged(java.util.Collection)
-   */
-  public void selectionChanged(Collection<? extends OsmPrimitive> newSelection) {
-    ++selectionChangedCalls;
-//    System.out.println("new selection: " + newSelection);
-//    System.out.println("selection till now: " + selectedNodes);
-    if(selectionChangedCalls > 1 && (newSelection == null || newSelection.size() == 0)) {
-      System.out.println("clearing selection for navigation");
-      selectedNodes.clear();
-      selectionChangedCalls = 0;
-      return;
-    }
-    if(selectionChangedCalls > 1) {
-      selectionChangedCalls = 0;
-    }
-    Node node;
-    // find a newly selected node and add it to the selection
-    for(OsmPrimitive selectedElement : newSelection) {
-      if(selectedElement instanceof Node) {
-        node = (Node)selectedElement;
-        if(!selectedNodes.contains(node)) {
-          selectedNodes.add(node);
-//          System.out.println("adding node " + node.id);
-          System.out.println("navigation nodes: " + selectedNodes);
-        }
-      }
-    }    
+    navigatorPlugin.navigate();
   }
 
Index: /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorLayer.java
===================================================================
--- /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorLayer.java	(revision 3749)
+++ /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorLayer.java	(revision 3749)
@@ -0,0 +1,182 @@
+/**
+ * 
+ */
+package at.dallermassl.josm.plugin.navigator;
+
+import java.awt.Component;
+import java.awt.Graphics;
+import java.awt.Point;
+import java.util.List;
+
+import javax.swing.Icon;
+
+import org.openstreetmap.josm.Main;
+import org.openstreetmap.josm.data.osm.Node;
+import org.openstreetmap.josm.data.osm.visitor.BoundingXYVisitor;
+import org.openstreetmap.josm.gui.MapView;
+import org.openstreetmap.josm.gui.layer.Layer;
+import org.openstreetmap.josm.tools.ImageProvider;
+
+import org.openstreetmap.josm.data.osm.Segment;
+
+/**
+ * @author cdaller
+ * 
+ */
+public class NavigatorLayer extends Layer {
+    private NavigatorModel navigatorNodeModel;
+    private Icon startIcon;
+    private Icon middleIcon;
+    private Icon endIcon;
+
+    /**
+     * Constructor
+     * 
+     * @param name
+     *            the name of the layer.
+     */
+    public NavigatorLayer(String name) {
+        super(name);
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#getIcon()
+     */
+    @Override
+    public Icon getIcon() {
+        Icon icon = ImageProvider.get("layer", "navigation_small");
+        return icon;
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#getInfoComponent()
+     */
+    @Override
+    public Object getInfoComponent() {
+        return "Navigation layer"; // FIXXME return info in html about navigation
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#getMenuEntries()
+     */
+    @Override
+    public Component[] getMenuEntries() {
+        // TODO Auto-generated method stub
+        return null;
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#getToolTipText()
+     */
+    @Override
+    public String getToolTipText() {
+        return "Tool Tip for Navigation"; // FIXXME
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#isMergable(org.openstreetmap.josm.gui.layer.Layer)
+     */
+    @Override
+    public boolean isMergable(Layer other) {
+        return false;
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#mergeFrom(org.openstreetmap.josm.gui.layer.Layer)
+     */
+    @Override
+    public void mergeFrom(Layer from) {
+        // do nothing
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#paint(java.awt.Graphics,
+     *      org.openstreetmap.josm.gui.MapView)
+     */
+    @Override
+    public void paint(Graphics g, MapView mv) {
+        List<Node> nodes = navigatorNodeModel.getSelectedNodes();
+        if(nodes == null || nodes.size() == 0) {
+            return;
+        }
+        
+        if(startIcon == null) {
+            startIcon = ImageProvider.get("navigation", "startflag");
+            middleIcon = ImageProvider.get("navigation", "middleflag");
+            endIcon = ImageProvider.get("navigation", "endflag");
+        }
+        // start icon:
+        Node node = nodes.get(0);
+        Point screen = mv.getPoint(node.eastNorth);
+        startIcon.paintIcon(mv, g, screen.x, screen.y - startIcon.getIconHeight());
+        
+        // middle icons:
+        for(int index = 1; index < nodes.size() - 1; ++index) {
+            node = nodes.get(index);
+            screen = mv.getPoint(node.eastNorth);
+            middleIcon.paintIcon(mv, g, screen.x, screen.y - middleIcon.getIconHeight());
+        }
+        // end icon:
+        if(nodes.size() > 1) {
+            node = nodes.get(nodes.size() - 1);
+            screen = mv.getPoint(node.eastNorth);
+            endIcon.paintIcon(mv, g, screen.x, screen.y - endIcon.getIconHeight());
+        }
+        
+        List<Segment> path = navigatorNodeModel.getSegmentPath();
+        if(path != null) {
+            synchronized(path) {
+                // TODO paint path
+            }
+        }
+    }
+
+    /*
+     * (non-Javadoc)
+     * 
+     * @see org.openstreetmap.josm.gui.layer.Layer#visitBoundingBox(org.openstreetmap.josm.data.osm.visitor.BoundingXYVisitor)
+     */
+    @Override
+    public void visitBoundingBox(BoundingXYVisitor v) {
+        for (Node node : navigatorNodeModel.getSelectedNodes()) {
+            v.visit(node);
+        }
+
+    }
+
+    /**
+     * @return the navigatorNodeModel
+     */
+    public NavigatorModel getNavigatorNodeModel() {
+        return this.navigatorNodeModel;
+    }
+
+    /**
+     * 
+     */
+    public void navigate() {
+        navigatorNodeModel.calculateShortesPath();
+        Main.map.repaint();        
+    }
+
+    /**
+     * @param navigatorNodeModel the navigatorNodeModel to set
+     */
+    public void setNavigatorNodeModel(NavigatorModel navigatorNodeModel) {
+        this.navigatorNodeModel = navigatorNodeModel;
+    }
+}
Index: /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorModeAction.java
===================================================================
--- /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorModeAction.java	(revision 3749)
+++ /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorModeAction.java	(revision 3749)
@@ -0,0 +1,65 @@
+/**
+ * 
+ */
+package at.dallermassl.josm.plugin.navigator;
+
+import static org.openstreetmap.josm.tools.I18n.tr;
+
+import java.awt.event.KeyEvent;
+import java.awt.event.MouseEvent;
+
+import org.openstreetmap.josm.Main;
+import org.openstreetmap.josm.actions.mapmode.MapMode;
+import org.openstreetmap.josm.data.osm.Node;
+import org.openstreetmap.josm.tools.ImageProvider;
+
+import org.openstreetmap.josm.gui.MapFrame;
+
+/**
+ * @author cdaller
+ *
+ */
+public class NavigatorModeAction extends MapMode {
+    private NavigatorModel navigatorModel;
+    private NavigatorLayer navigatorLayer;
+    private boolean layerAdded;
+    
+    public NavigatorModeAction(MapFrame mapFrame, NavigatorModel navigatorModel, NavigatorLayer navigationLayer) {
+        super(tr("Navigator"), "navigation", tr("Set start/end for autorouting"), KeyEvent.VK_F, mapFrame, ImageProvider.getCursor("crosshair", "selection"));
+        this.navigatorModel = navigatorModel;
+        this.navigatorLayer = navigationLayer;
+    }
+    
+    @Override public void enterMode() {
+        super.enterMode();
+        Main.map.mapView.addMouseListener(this);
+        if(!layerAdded) {
+            System.out.println("Adding navigatorlayer " + navigatorLayer);
+            Main.main.addLayer(navigatorLayer);
+            layerAdded = true;
+        }
+    }
+
+    @Override public void exitMode() {
+        super.exitMode();
+        Main.map.mapView.removeMouseListener(this);
+        //Main.main.removeLayer(navigatorLayer);
+    }
+
+    @Override public void mouseClicked(MouseEvent e) {
+        if (e.getButton() == MouseEvent.BUTTON2) {
+            navigatorModel.clearNodes();
+        } else if (e.getButton() == MouseEvent.BUTTON1) {        
+            Node node = Main.map.mapView.getNearestNode(e.getPoint());
+            System.out.println("selected node " + node);
+            if(node == null) {
+                return;
+            }
+            navigatorModel.addNode(node);
+        }
+        Main.map.repaint();
+    }
+
+
+
+}
Index: /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorModel.java
===================================================================
--- /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorModel.java	(revision 3749)
+++ /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorModel.java	(revision 3749)
@@ -0,0 +1,126 @@
+/**
+ * 
+ */
+package at.dallermassl.josm.plugin.navigator;
+
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.LinkedList;
+import java.util.List;
+
+import org.jgrapht.Graph;
+import org.jgrapht.alg.DijkstraShortestPath;
+import org.openstreetmap.josm.Main;
+import org.openstreetmap.josm.data.SelectionChangedListener;
+import org.openstreetmap.josm.data.osm.DataSet;
+import org.openstreetmap.josm.data.osm.Node;
+import org.openstreetmap.josm.data.osm.OsmPrimitive;
+import org.openstreetmap.josm.data.osm.Segment;
+
+/**
+ * @author cdaller
+ *
+ */
+public class NavigatorModel {
+    private Graph graph;
+    private List<Node> nodes;
+    private int selectionChangedCalls;
+    List<Segment> segmentPath;
+
+    public NavigatorModel() {
+        nodes = new ArrayList<Node>();
+    }
+    
+    /**
+     * @return the selectedNodes
+     */
+    public List<Node> getSelectedNodes() {
+        return nodes;
+    }
+
+
+    public Graph<Node, SegmentEdge> getGraph() {
+        if (graph == null) {
+            OsmGraphCreator graphCreator = new OsmGraphCreator();
+            // graph = graphCreator.createGraph();
+            graph = graphCreator.createSegmentGraph();
+        }
+        return graph;
+    }
+
+    
+    /**
+     * 
+     */
+    public void calculateShortesPath() {
+        System.out.print("navigate nodes ");
+        for (Node node : nodes) {
+            System.out.print(node.id + ",");
+        }
+        System.out.println();
+
+        double fullWeight = 0;
+        DijkstraShortestPath<Node, SegmentEdge> routing;
+        List<SegmentEdge> fullPath = new ArrayList<SegmentEdge>();
+        List<SegmentEdge> path;
+        for (int index = 1; index < nodes.size(); ++index) {
+            routing = new DijkstraShortestPath<Node, SegmentEdge>(getGraph(), nodes.get(index - 1), nodes.get(index));
+            path = routing.getPathEdgeList();
+            if (path == null) {
+                System.out.println("no path found!");
+                return;
+            }
+            fullPath.addAll(path);
+            fullWeight += routing.getPathLength();
+        }
+        segmentPath = new LinkedList<Segment>();
+        synchronized(segmentPath) {
+            for (SegmentEdge edge : fullPath) {
+                segmentPath.add(edge.getSegment());
+            }
+        }
+        Main.ds.setSelected(segmentPath);
+        Main.map.mapView.repaint();
+        System.out.println("shortest path found: " + fullPath + " weight: " + fullWeight);
+        
+        double weight2 = 0;
+        for(Segment seg : segmentPath) {
+            weight2 += Math.sqrt(seg.from.coor.distance(seg.to.coor)) * 111000;
+        }
+        System.out.println("all added: " + weight2);
+    }
+
+
+    /**
+     * Resets the calculated graph.
+     */
+    public void resetGraph() {
+        graph = null;
+    }
+
+
+    /**
+     * @return the segmentPath
+     */
+    public List<Segment> getSegmentPath() {
+        return this.segmentPath;
+    }
+
+
+    /**
+     * Adds a node to navigate.
+     * @param node the node to add.
+     */
+    public void addNode(Node node) {
+        nodes.add(node);
+    }
+    
+    /**
+     * Clear all nodes to navigate. 
+     */
+    public void clearNodes() {
+        nodes.clear();        
+    }
+
+
+}
Index: /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorPlugin.java
===================================================================
--- /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorPlugin.java	(revision 3748)
+++ /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/NavigatorPlugin.java	(revision 3749)
@@ -20,4 +20,8 @@
 import org.openstreetmap.josm.data.osm.Node;
 import org.openstreetmap.josm.data.osm.Segment;
+import org.openstreetmap.josm.gui.IconToggleButton;
+import org.openstreetmap.josm.gui.MapFrame;
+import org.openstreetmap.josm.gui.dialogs.ToggleDialog;
+import org.openstreetmap.josm.plugins.Plugin;
 
 /**
@@ -25,69 +29,49 @@
  * 
  * @author cdaller
- *
+ * 
  */
-public class NavigatorPlugin {
-  private Graph graph;
+public class NavigatorPlugin extends Plugin {
+    private NavigatorLayer navigatorLayer;
+    private NavigatorModel navigatorModel;
 
-  
-  /**
-   * 
-   */
-  public NavigatorPlugin() {
-    super();
-    JMenuBar menu = Main.main.menu;
-    JMenu navigatorMenu = new JMenu(tr("Navigation"));
-    JMenuItem navigatorMenuItem = new JMenuItem(new NavigatorAction(this));
-    navigatorMenu.add(navigatorMenuItem);
-    JMenuItem resetMenuItem = new JMenuItem(tr("Reset Graph"));
-    resetMenuItem.addActionListener(new ActionListener() {
-      public void actionPerformed(ActionEvent e) {
-        graph = null;        
-      }      
-    });
-    navigatorMenu.add(resetMenuItem);
-    menu.add(navigatorMenu);
+    /**
+     * 
+     */
+    public NavigatorPlugin() {
+        super();
+        
+        navigatorModel = new NavigatorModel();
+        navigatorLayer = new NavigatorLayer(tr("Navigation"));
+        navigatorLayer.setNavigatorNodeModel(navigatorModel);
+        
+        JMenuBar menu = Main.main.menu;
+        JMenu navigatorMenu = new JMenu(tr("Navigation"));
+        JMenuItem navigatorMenuItem = new JMenuItem(new NavigatorAction(this));
+        navigatorMenu.add(navigatorMenuItem);
+        JMenuItem resetMenuItem = new JMenuItem(tr("Reset Graph"));
+        resetMenuItem.addActionListener(new ActionListener() {
+            public void actionPerformed(ActionEvent e) {
+                navigatorModel.resetGraph();
+            }
+        });
+        navigatorMenu.add(resetMenuItem);
+        menu.add(navigatorMenu);
+    }
     
-  }
-  
-  public Graph<Node, SegmentEdge> getGraph() {
-    if(graph == null) {
-      OsmGraphCreator graphCreator = new OsmGraphCreator();
-      //graph = graphCreator.createGraph();
-      graph = graphCreator.createSegmentGraph();
+    /* (non-Javadoc)
+     * @see org.openstreetmap.josm.plugins.Plugin#mapFrameInitialized(org.openstreetmap.josm.gui.MapFrame, org.openstreetmap.josm.gui.MapFrame)
+     */
+    @Override
+    public void mapFrameInitialized(MapFrame oldFrame, MapFrame newFrame) {    
+        newFrame.toolBarActions.add(
+            new IconToggleButton(new NavigatorModeAction(newFrame, navigatorModel, navigatorLayer)));
     }
-    return graph;
-  }
 
-  /**
-   * @param startNode
-   * @param endNode
-   */
-  public void navigate(List<Node> nodes) {
-    System.out.print("navigate nodes ");
-    for(Node node : nodes) {
-      System.out.print(node.id + ",");
+    /**
+     * @param startNode
+     * @param endNode
+     */
+    public void navigate() {
+        navigatorLayer.navigate();
     }
-    System.out.println();
-    
-    DijkstraShortestPath<Node, SegmentEdge> routing;
-    List<SegmentEdge> fullPath = new ArrayList<SegmentEdge>();
-    List<SegmentEdge> path;
-    for(int index = 1; index < nodes.size(); ++index) {
-      routing = new DijkstraShortestPath<Node, SegmentEdge>(getGraph(), nodes.get(index - 1), nodes.get(index));
-      path = routing.getPathEdgeList();
-      if(path == null) {
-        System.out.println("no path found!");
-        return;
-      }
-      fullPath.addAll(path);
-    }
-    List<Segment> segmentPath = new ArrayList<Segment>();
-    for(SegmentEdge edge : fullPath) {
-      segmentPath.add(edge.getSegment());
-    }
-    Main.ds.setSelected(segmentPath);
-    Main.map.mapView.repaint();
-    System.out.println("shortest path found: " + fullPath);
-  }
 }
Index: /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/OsmGraphCreator.java
===================================================================
--- /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/OsmGraphCreator.java	(revision 3748)
+++ /applications/editors/josm/plugins/navigator/src/at/dallermassl/josm/plugin/navigator/OsmGraphCreator.java	(revision 3749)
@@ -42,4 +42,5 @@
     highwayWeight.put("unclassified", 50.0);
     highwayWeight.put("residential", 40.0);
+    highwayWeight.put("footway", 1.0);
   }
   
@@ -94,6 +95,6 @@
       weight = weightValue.doubleValue();
     }
-    double distance = segment.from.coor.distance(segment.to.coor) * 111000; // deg to m (at equator :-)
-    return distance / weight;
+    double distance = Math.sqrt(segment.from.coor.distance(segment.to.coor)) * 111000; // deg to m (at equator :-)
+    return distance; // weight;
   }