Vehicle Tracking on Track

.gradle/buildOutputCleanup/cache.properties

@@ -1,2 +1,2 @@
-#Thu Sep 16 21:34:52 EDT 2021
+#Fri Oct 08 10:23:59 EDT 2021
 gradle.version=6.1

src/main/java/edu/oswego/cs/CPSLab/AutomotiveCPS/controller/ConnectorDAO.java

@@ -110,7 +110,7 @@ public void updateVehicles(){
                 String key = ""+v.getAdvertisement().getIdentifier();
                 System.out.print("Get car: "+v.getAdvertisement().getModel());
                 if (v.getAdvertisement().isCharging())
-                    continue;               
+                    continue;
                 addVehicle(v);   
             }
         }     

src/main/java/edu/oswego/cs/CPSLab/AutomotiveCPS/controller/MapDAO.java

@@ -26,6 +26,7 @@ public Customized2DArray getArray() {
         return array;
     }
     public String[][] getBoard() { return board; }
+    public Integer[] startingLocation;
 
     public void setTracks(List<Block> tracks) {
         this.tracks = tracks;
@@ -55,10 +56,10 @@ private void setArray() {
         boolean positive = true;
 
         this.array = new Customized2DArray();
-        
+
         for (Block track : this.tracks){
             int pieceId = track.getPieceId();
-            
+
             /**
              * START piece.
              */
@@ -218,5 +219,7 @@ public Block getBlock(int i, int j){
     public boolean containsBlock(Block block){
         return this.tracks.contains(block);
     }
+
+    public void addStartingLocation(Integer[] location) {this.startingLocation = location; }
 
 }

src/main/java/edu/oswego/cs/CPSLab/AutomotiveCPS/gui/MapGUI.java

@@ -119,7 +119,7 @@ public void start(Stage stage) throws Exception {
         for (Integer[] integers : trackPieceImageCenter.get(0)) {
             System.out.println(Arrays.toString(integers));
         }
-        stage.show();
+        //stage.show();
     }
 
     public Scene getScene() { return scene; }
@@ -158,6 +158,9 @@ public void drawTrack(){
                     if (board[i][j] == null) {
                         board[i][j] = "null";
                     }
+                    if (board[i][j].equals("SF")) {
+                        map.addStartingLocation(new Integer[]{i, j});
+                    }
                     // draws track with pictures
                     ImageView iv_road_piece = new ImageView(new Image(Parameter.PATH_MEDIA + "Track/" + board[i][j] + ".png"));
                     //ImageView iv_road_piece = new ImageView(new Image("edu/oswego/cs/CPSLab/AutomotiveCPS/gui/img/Track/arrow-up.png"));
@@ -174,7 +177,6 @@ public void drawTrack(){
                 map_GUI.getChildren().add(one_row_map);
             }
             vbox_map.getChildren().add(map_GUI);
-            System.out.println("Starting Coordinate" + size_piece);
             int currentXPos = startingCoordinate[0] + (size_piece / 2);
             int currentYPos = startingCoordinate[1] + (size_piece / 2);
 
@@ -191,7 +193,6 @@ public void drawTrack(){
                     currentXPos += size_piece;
                     trackPieceImageCenter.get(trackPieceImageCenter.size() - 1)[arrayIndex][0] = currentXPos;
                     trackPieceImageCenter.get(trackPieceImageCenter.size() - 1)[arrayIndex][1] = currentYPos;
-                    System.out.println(currentXPos + ", " + currentYPos);
                     arrayIndex += 1;
                 }
                 currentYPos += size_piece;
@@ -328,4 +329,5 @@ public void resetTempIntersection(boolean success){
         temp_roadmap_manager = null;
         temp_iv_intersection = null;
     }
+
 }

src/main/java/edu/oswego/cs/CPSLab/AutomotiveCPS/gui/PositionTrackerOverlay.java

@@ -4,6 +4,7 @@
 
 import de.adesso.anki.MessageListener;
 import de.adesso.anki.messages.LocalizationTransitionUpdateMessage;
+import de.adesso.anki.roadmap.Position;
 import de.adesso.anki.roadmap.roadpieces.StartRoadpiece;
 import edu.oswego.cs.CPSLab.AutomotiveCPS.CPSCar;
 import edu.oswego.cs.CPSLab.AutomotiveCPS.behavior.Speedometer;
@@ -22,10 +23,9 @@
 import javafx.scene.paint.Paint;
 import javafx.scene.shape.Circle;
 import javafx.stage.Stage;
-import java.util.ArrayList;
-import java.util.Arrays;
+
+import java.util.*;
 import java.util.List;
-import java.util.Random;
 import java.util.stream.Collectors;
 
 
@@ -49,16 +49,14 @@ public class PositionTrackerOverlay extends Application {
     private List<Integer[][]> listOfRoadPieceCenters;
     private List<Block> blocks;
     private static List<Integer> trackID;
-    private Integer[][][] formatedArray;
-    private List<Integer[][]> listOfFormatedRoadPieceCenters;
+    private Integer[][][] formattedArray;
+    private List<String> trackOrder ;
 
     // Javafx components
     private GridPane mapGridPane;
     private StackPane entireMapPane = new StackPane();
     private Pane circleTrackingPane = new Pane();
 
-    private Integer[][] roadPieceCenters;
-
     private boolean killThreads = false;
     private final Dimension SIZE = Toolkit.getDefaultToolkit().getScreenSize();
 
@@ -68,7 +66,6 @@ public PositionTrackerOverlay(List<VehicleDAO> vehicles, List<MapDAO> mapDAOs, G
         this.vehicles               = vehicles;
         this.mapDAOs                = mapDAOs;
         this.listOfRoadPieceCenters = roadPieceCenters;
-        this.roadPieceCenters       = roadPieceCenters.get(0);
 
         /*
          * Note to developer : List<MapDAO> mapDAO should only contain 1 mapDAO. If there are other instances nearby and
@@ -85,47 +82,16 @@ public PositionTrackerOverlay(List<VehicleDAO> vehicles, List<MapDAO> mapDAOs, G
                 .map(Block::getPieceId)
                 .collect(Collectors.toList());
 
-        debug();
+        trackOrder       = mapSelected.getArray().getKeys();
+        rearrangeArray();
 
+        debug();
 
-        //rearrangeArray();
     }
 
     private void debug() {
         System.out.println(" RUNNING DEBUG MODE ");
-
-        System.out.println(Arrays.deepToString(mapSelected.getBoard()));
-        System.out.println(Arrays.toString(getRotatedMap()));
-
-    }
-
-    private String[] getRotatedMap() {
-        String[][] board = mapSelected.getBoard();
-        int rows         = board.length;
-        int cols         = board[0].length;
-
-        List<String> modifiedBoard = new ArrayList<>();
-        String[] rotatedBoard = new String[rows * cols];
-
-        for (int i = 0; i < rows; i++) {
-            for (int j = 0; j < cols; j++) {
-                modifiedBoard.add(board[i][j]);
-            }
-        }
-
-        int SFindex = modifiedBoard.indexOf("SF");
-        int spacing = modifiedBoard.size() - SFindex;
-
-        for (int i = 0; i < modifiedBoard.size(); i++) {
-            if (i < SFindex) {
-                int index = i + spacing;
-                rotatedBoard[index] = modifiedBoard.get(i);
-            } else {
-                int index = i - SFindex;
-                rotatedBoard[index] = modifiedBoard.get(i);
-            }
-        }
-        return rotatedBoard;
+        trackOrder.forEach(System.out::println);
     }
 
     public void setBlocks(List<Block> blocks) { this.blocks = blocks; }
@@ -154,6 +120,8 @@ public void killTrackers() {
         killThreads = true;
     }
 
+
+
     /**
      * turns one-dimesional array into two-dimensitional
      */
@@ -163,17 +131,17 @@ private void rearrangeArray() {
         int counter = 0;
         int rows = board.length;
         int cols = board[0].length;
-        formatedArray = new Integer[rows][cols][2];
+        formattedArray = new Integer[rows][cols][2];
         for (int i = 0; i < rows; i++) {
             for (int j = 0; j < cols; j++) {
-                formatedArray[i][j][0] = array[counter][0];
-                formatedArray[i][j][1] = array[counter][1];
+                formattedArray[i][j][0] = array[counter][0];
+                formattedArray[i][j][1] = array[counter][1];
                 counter ++ ;
             }
         }
 
         System.out.println("Array Rotation Method");
-        for (Integer[][] row : formatedArray) {
+        for (Integer[][] row : formattedArray) {
             for (Integer[] col : row) {
                 System.out.print(col[0] + ", " + col[1] + " |||");
             }
@@ -248,16 +216,22 @@ public class MultithreadedTracker implements Runnable {
 
         private CPSCar cpsCar;
         private Speedometer speedometer;
-        private double enteringVelocity;
-        private int trackIndex = 0;
+
+        private int trackIndex = 1;
 
         Circle tracker = new Circle();
 
         public MultithreadedTracker(VehicleDAO vehicle) {
             this.cpsCar      = vehicle.getCpsCar();
             this.speedometer = cpsCar.getSpeedometer();
             tracker.setRadius(10);
-            tracker.setFill(Paint.valueOf("#000FFF"));;
+
+            Random obj = new Random();
+            int rand_num = obj.nextInt(0xffffff + 1);
+
+            String colorCode = String.format("#%06x", rand_num);
+
+            tracker.setFill(Paint.valueOf(colorCode));
             this.cpsCar.setVehicleDAO(vehicle);
         }
 
@@ -270,24 +244,32 @@ public void transition() {
 
             // If going backwards
                 // Then trackIndex *= -1
-
-            trackIndex += 1;
+            if (! Parameter.FINISH_PIECE.contains(cpsCar.getPieceId())) {
+                trackIndex += 1;
+            }
             if (trackIndex == mapSelected.getTracks().size() - 1) {
                 trackIndex = 0;
             }
 
-            System.out.println("CURRENT TRACK " + mapSelected.getTracks().get(trackIndex).getPiece().getType());
-            int randomTrack = new Random().nextInt(roadPieceCenters.length);
-            updatePosition(roadPieceCenters[randomTrack][0], roadPieceCenters[randomTrack][1] );
+            String location = trackOrder.get(trackIndex);
+            int arrX = Integer.parseInt(location.substring(0, location.indexOf('/')));
+            int arrY = Integer.parseInt(location.substring(location.indexOf('/') + 2));
 
+            int newX = PositionTrackerOverlay.this.formattedArray[arrX][arrY][0];
+            int newY = PositionTrackerOverlay.this.formattedArray[arrX][arrY][1];
+
+
+            System.out.println("CURRENT TRACK " + mapSelected.getTracks().get(trackIndex).getPiece().getType());
+            Random rm = new Random();
+            updatePosition(newX, newY );
+            //updatePosition(rm.nextInt(500), rm.nextInt(500) );
             this.debug();
         }
 
         @Override
         public void run() {
 
             while (! PositionTrackerOverlay.this.killThreads) {
-                this.enteringVelocity = speedometer.getVelocity();
                 this.speedometer.setTracker(this);
             }
         }

src/main/java/edu/oswego/cs/CPSLab/AutomotiveCPS/utilities/Customized2DArray.java

@@ -55,7 +55,7 @@ public void normalizeIndex(){
             int ny = this.col.get(k)+j_gap;
             this.row.set(k, nx);
             this.col.set(k, ny);
-            this.keys.add(k,nx+"//"+ny);           
+            this.keys.add(k,nx+"//"+ny);
         }      
         this.normalize = true;
 
@@ -111,4 +111,7 @@ public int minColumnIndex(){
         }
         return j_min;
     }
+
+    public List<String> getKeys() { return keys; }
+
 }