Merge remote-tracking branch 'origin/wake_remake' into wake_remake

src/main/java/seng302/controllers/CanvasController.java

@@ -18,11 +18,14 @@ import seng302.models.BoatGroup;
 import seng302.models.Colors;
 import seng302.models.RaceObject;
 import seng302.models.mark.*;
+import seng302.models.parsers.StreamPacket;
 import seng302.models.parsers.StreamParser;
+import seng302.models.parsers.packets.BoatPositionPacket;
 
 import java.sql.Time;
 import java.text.DecimalFormat;
 import java.util.*;
+import java.util.concurrent.PriorityBlockingQueue;
 
 /**
  * Created by ptg19 on 15/03/17.
@@ -103,13 +106,8 @@ public class CanvasController {
 
             @Override
             public void handle(long now) {
-                boolean raceFinished = true;
-                boolean descending;
-                boolean leftToRight;
-                int boatIndex = 0;
-
-                Mark nextMark;
 
+                //fps stuff
                 long oldFrameTime = frameTimes[frameTimeIndex] ;
                 frameTimes[frameTimeIndex] = now ;
                 frameTimeIndex = (frameTimeIndex + 1) % frameTimes.length ;
@@ -123,21 +121,11 @@ public class CanvasController {
                     Double frameRate = 1_000_000_000.0 / elapsedNanosPerFrame ;
                     drawFps(frameRate.intValue());
                 }
+
                 // TODO: 1/05/17 cir27 - Make the RaceObjects update on the actual delay.
                 elapsedNanos = 1000 / 60;
-                for (RaceObject raceObject : raceObjects) {
+                updateRaceObjects();
-                    raceObject.updatePosition(elapsedNanos);
+
-                    for (int id : raceObject.getRaceIds()) {
-                        if (StreamParser.getBoatPositions().containsKey((long) id)) {
-                            Point3D p = StreamParser.getBoatPositions().get((long) id);
-                            Point2D p2d = latLonToXY(p.getX(), p.getY());
-                            double speed = StreamParser.getBoatSpeeds().get((long) id);
-                            double heading = 360.0 / 0xffff * p.getZ();
-                            raceObject.setDestination(p2d.getX(), p2d.getY(), heading, speed, id);
-                        }
-                        StreamParser.getBoatPositions().remove((long) id);
-                    }
-                }
             }
         };
         for (Mark m : raceViewController.getRace().getCourse()) {
@@ -146,6 +134,46 @@ public class CanvasController {
         //timer.start();
     }
 
+    private void updateRaceObjects(){
+        for (RaceObject raceObject : raceObjects) {
+            raceObject.updatePosition(1000 / 60);
+            // some raceObjects will have multiply ID's (for instance gate marks)
+            for (long id : raceObject.getRaceIds()) {
+                //checking if the current "ID" has any updates associated with it
+                if (StreamParser.boatPositions.containsKey(id)) {
+                    move(id, raceObject);
+                }
+            }
+        }
+    }
+
+    private void move(long id, RaceObject raceObject){
+        PriorityBlockingQueue<BoatPositionPacket> movementQueue = StreamParser.boatPositions.get(id);
+        if (movementQueue.size() > 0){
+            BoatPositionPacket positionPacket = movementQueue.peek();
+
+            //this code adds a delay to reading from the movementQueue
+            //in case things being put into the movement queue are slightly
+            //out of order
+            int delayTime = 1000;
+            int loopTime = delayTime * 10;
+            long timeDiff = (System.currentTimeMillis()%loopTime - positionPacket.getTimeValid()%loopTime);
+            if (timeDiff < 0){
+                timeDiff = loopTime + timeDiff;
+            }
+            if (timeDiff > delayTime) {
+                try {
+                    positionPacket = movementQueue.take();
+                    Point2D p2d = latLonToXY(positionPacket.getLat(), positionPacket.getLon());
+                    double heading = 360.0 / 0xffff * positionPacket.getHeading();
+                    raceObject.setDestination(p2d.getX(), p2d.getY(), heading, positionPacket.getGroundSpeed(), (int) id);
+                } catch (InterruptedException e){
+                    e.printStackTrace();
+                }
+            }
+        }
+    }
+
     class ResizableCanvas extends Canvas {
 
         ResizableCanvas() {

src/main/java/seng302/models/parsers/StreamParser.java

@@ -6,6 +6,8 @@ import org.w3c.dom.Document;
 import org.xml.sax.InputSource;
 import org.xml.sax.SAXException;
 import seng302.models.Boat;
+import seng302.models.parsers.packets.BoatPositionPacket;
+import seng302.models.parsers.packets.StreamPacket;
 
 import javax.xml.parsers.DocumentBuilder;
 import javax.xml.parsers.DocumentBuilderFactory;
@@ -13,12 +15,11 @@ import javax.xml.parsers.ParserConfigurationException;
 import java.io.ByteArrayInputStream;
 import java.io.IOException;
 import java.io.StringReader;
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
 import java.text.DateFormat;
 import java.text.SimpleDateFormat;
 import java.util.*;
 import java.util.concurrent.ConcurrentHashMap;
+import java.util.concurrent.PriorityBlockingQueue;
 
 /**
  * The purpose of this class is to take in the stream of divided packets so they can be read
@@ -28,8 +29,7 @@ import java.util.concurrent.ConcurrentHashMap;
  */
 public class StreamParser extends Thread{
 
-     private static ConcurrentHashMap<Long,Point3D> boatPositions = new ConcurrentHashMap<>();
+     public static ConcurrentHashMap<Long, PriorityBlockingQueue<BoatPositionPacket>> boatPositions = new ConcurrentHashMap<>();
-     private static ConcurrentHashMap<Long,Double> boatSpeeds = new ConcurrentHashMap<>();
      private String threadName;
      private Thread t;
      private static boolean raceStarted = false;
@@ -40,7 +40,7 @@ public class StreamParser extends Thread{
      private static List<Boat> boats = new ArrayList<>();
 
      public StreamParser(String threadName){
-        this.threadName = threadName;
+         this.threadName = threadName;
      }
 
     /**
@@ -55,28 +55,27 @@ public class StreamParser extends Thread{
              while (StreamReceiver.packetBuffer == null || StreamReceiver.packetBuffer.size() < 1) {
                  Thread.sleep(1);
              }
-             while (StreamReceiver.packetBuffer.peek() != null){
+             while (true){
-//                 StreamPacket packet = StreamReceiver.packetBuffer.peek();
+                 StreamPacket packet = StreamReceiver.packetBuffer.peek();
-//                 int delayTime = 1000;
+                 //this code adds a delay to reading from the packetBuffer so
-//                 int loopTime = delayTime + 1000;
+                 //out of order packets have time to order themselves in the queue
-//                 long sleepTime = 0;
+                 int delayTime = 1000;
-//                 long transitTime = (System.currentTimeMillis()%loopTime - packet.getTimeStamp()%loopTime);
+                 int loopTime = delayTime * 10;
-//                 if (transitTime < 0){
+                 long transitTime = (System.currentTimeMillis()%loopTime - packet.getTimeStamp()%loopTime);
-//                     transitTime = loopTime + delayTime;
+                 if (transitTime < 0){
-//                 }
+                     transitTime = loopTime + transitTime;
-//                 if (transitTime < delayTime) {
+                 }
-//                     sleepTime = delayTime - (transitTime);
+                 if (transitTime < delayTime) {
-//                     Thread.sleep(sleepTime);
+                     long sleepTime = delayTime - (transitTime);
-//                 }
+                     Thread.sleep(sleepTime);
-//                 System.out.println(sleepTime);
+                 }
-                 StreamPacket packet = StreamReceiver.packetBuffer.take();
+                 packet = StreamReceiver.packetBuffer.take();
                  parsePacket(packet);
                  Thread.sleep(1);
                  while (StreamReceiver.packetBuffer.peek() == null) {
                      Thread.sleep(1);
                  }
              }
-             System.out.println("END OF STREAM");
          } catch (Exception e){
              e.printStackTrace();
          }
@@ -311,28 +310,30 @@ public class StreamParser extends Thread{
         byte[] headingBytes = Arrays.copyOfRange(payload,28,30);
         byte[] groundSpeedBytes = Arrays.copyOfRange(payload,38,40);
 
-        long timeStamp = bytesToLong(Arrays.copyOfRange(payload,1,7));
+        long timeValid = bytesToLong(Arrays.copyOfRange(payload,1,7));
 //        int boatSeq =  ByteBuffer.wrap(seqBytes).getInt();
         long seq = bytesToLong(seqBytes);
         long boatId = bytesToLong(boatIdBytes);
-        long lat = bytesToLong(latBytes);
+        long rawLat = bytesToLong(latBytes);
-        long lon = bytesToLong(lonBytes);
+        long rawLon = bytesToLong(lonBytes);
+        double lat = ((180d * (double)rawLat)/Math.pow(2,31));
+        double lon = ((180d *(double)rawLon)/Math.pow(2,31));
         long heading = bytesToLong(headingBytes);
-//        long speed = bytesToLong(speedBytes);
         double groundSpeed = bytesToLong(groundSpeedBytes)/1000.0;
         short s = (short) ((groundSpeedBytes[1] & 0xFF) << 8 | (groundSpeedBytes[0] & 0xFF));
         if ((int)deviceType == 1 || (int)deviceType == 3){
-//            System.out.println("boatId = " + boatId);
+
-//            System.out.println("deviceType = " + (long)deviceType);
+            BoatPositionPacket boatPacket = new BoatPositionPacket(boatId, timeValid, lat, lon, heading, groundSpeed);
-//            System.out.println("seq = " + seq);
+
-            //needs to be validated
+            if (!boatPositions.containsKey(boatId)){
-            Point3D point = new Point3D(((180d * (double)lat)/Math.pow(2,31)),((180d *(double)lon)/Math.pow(2,31)),(double)heading);
+                boatPositions.put(boatId, new PriorityBlockingQueue<BoatPositionPacket>(256, new Comparator<BoatPositionPacket>() {
-            boatPositions.put(boatId, point);
+                    @Override
-            boatSpeeds.put(boatId, groundSpeed);
+                    public int compare(BoatPositionPacket p1, BoatPositionPacket p2) {
-//            boatPositions.replace(boatId, point);
+                        return (int) (p1.getTimeValid() - p2.getTimeValid());
-//            boatSpeeds.replace(boatId, groundSpeed);
+                    }
-//            System.out.println("lon = " + ((180d * (double)lon)/Math.pow(2,31)));
+                }));
-//            System.out.println("lat = " +  ((180d *(double)lat)/Math.pow(2,31)));
+            }
+            boatPositions.get(boatId).put(boatPacket);
         }
     }
 
@@ -447,14 +448,6 @@ public class StreamParser extends Thread{
         return boats;
     }
 
-    public static ConcurrentHashMap<Long, Point3D> getBoatPositions() {
-        return boatPositions;
-    }
-
-    public static ConcurrentHashMap<Long, Double> getBoatSpeeds() {
-        return boatSpeeds;
-    }
-
     public static XMLParser getXmlObject() {
         return xmlObject;
     }

src/main/java/seng302/models/parsers/StreamReceiver.java

@@ -1,5 +1,7 @@
 package seng302.models.parsers;
 
+import seng302.models.parsers.packets.StreamPacket;
+
 import java.io.ByteArrayOutputStream;
 import java.io.IOException;
 import java.io.InputStream;
@@ -85,9 +87,6 @@ public class StreamReceiver extends Thread {
                     long computedCrc = checksum.getValue();
                     long packetCrc = bytesToLong(getBytes(4));
                     if (computedCrc == packetCrc) {
-//                        System.out.println("message type: " + type);
-//                        System.out.println("timeStamp = " + timeStamp);
-//                        System.out.println("payload length: " + payloadLength);
                         packetBuffer.add(new StreamPacket(type, payloadLength, timeStamp, payload));
                     } else {
                         System.err.println("Packet has been dropped");
@@ -133,7 +132,7 @@ public class StreamReceiver extends Thread {
      * takes an array of up to 7 bytes in little endian format and
      * returns a positive long constructed from the input bytes
      *
-     * @return a positive long if there is less than 7 bytes -1 otherwise
+     * @return a positive long if there is less than 8 bytes -1 otherwise
      */
     private long bytesToLong(byte[] bytes){
         long partialLong = 0;

src/main/java/seng302/models/parsers/packets/BoatPositionPacket.java

@@ -0,0 +1,39 @@
+package seng302.models.parsers.packets;
+
+public class BoatPositionPacket {
+    private long boatId;
+    private long timeValid;
+    private double lat;
+    private double lon;
+    private double heading;
+    private double groundSpeed;
+
+    public BoatPositionPacket(long boatId, long timeValid, double lat, double lon, double heading, double groundSpeed) {
+        this.boatId = boatId;
+        this.timeValid = timeValid;
+        this.lat = lat;
+        this.lon = lon;
+        this.heading = heading;
+        this.groundSpeed = groundSpeed;
+    }
+
+    public long getTimeValid() {
+        return timeValid;
+    }
+
+    public double getLat() {
+        return lat;
+    }
+
+    public double getLon() {
+        return lon;
+    }
+
+    public double getHeading() {
+        return heading;
+    }
+
+    public double getGroundSpeed() {
+        return groundSpeed;
+    }
+}

src/main/java/seng302/models/parsers/packets/PacketType.java

@@ -0,0 +1,53 @@
+package seng302.models.parsers.packets;
+
+/**
+ * Created by Kusal on 4/24/2017.
+ */
+public enum PacketType {
+    HEARTBEAT,
+    RACE_STATUS,
+    DISPLAY_TEXT_MESSAGE,
+    XML_MESSAGE,
+    RACE_START_STATUS,
+    YACHT_EVENT_CODE,
+    YACHT_ACTION_CODE,
+    CHATTER_TEXT,
+    BOAT_LOCATION,
+    MARK_ROUNDING,
+    COURSE_WIND,
+    AVG_WIND,
+    OTHER;
+
+    public static PacketType assignPacketType(int packetType){
+        switch(packetType){
+            case 1:
+                return HEARTBEAT;
+            case 12:
+                return RACE_STATUS;
+            case 20:
+                return DISPLAY_TEXT_MESSAGE;
+            case 26:
+                return XML_MESSAGE;
+            case 27:
+                return RACE_START_STATUS;
+            case 29:
+                return YACHT_EVENT_CODE;
+            case 31:
+                return YACHT_ACTION_CODE;
+            case 36:
+                return CHATTER_TEXT;
+            case 37:
+                return BOAT_LOCATION;
+            case 38:
+                return MARK_ROUNDING;
+            case 44:
+                return COURSE_WIND;
+            case 47:
+                return AVG_WIND;
+            default:
+        }
+        return OTHER;
+    }
+
+
+}

src/main/java/seng302/models/parsers/packets/StreamPacket.java

@@ -0,0 +1,37 @@
+package seng302.models.parsers.packets;
+
+/**
+ * Created by kre39 on 23/04/17.
+ */
+public class StreamPacket {
+
+    //Change int to an ENUM for the type
+    private PacketType type;
+
+    private long messageLength;
+    private long timeStamp;
+    private byte[] payload;
+
+    public StreamPacket(int type, long messageLength, long timeStamp, byte[] payload) {
+        this.type = PacketType.assignPacketType(type);
+        this.messageLength = messageLength;
+        this.timeStamp = timeStamp;
+        this.payload = payload;
+    }
+
+    public PacketType getType() {
+        return type;
+    }
+
+    public long getMessageLength() {
+        return messageLength;
+    }
+
+    public byte[] getPayload() {
+        return payload;
+    }
+
+    public long getTimeStamp() {
+        return timeStamp;
+    }
+}