diff --git a/src/main/java/seng302/controllers/CanvasController.java b/src/main/java/seng302/controllers/CanvasController.java index cfdff2dd..94721e39 100644 --- a/src/main/java/seng302/controllers/CanvasController.java +++ b/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) { - 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); - } - } + updateRaceObjects(); + } }; 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 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() { diff --git a/src/main/java/seng302/models/parsers/StreamParser.java b/src/main/java/seng302/models/parsers/StreamParser.java index e9cce878..dc42963f 100644 --- a/src/main/java/seng302/models/parsers/StreamParser.java +++ b/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 boatPositions = new ConcurrentHashMap<>(); - private static ConcurrentHashMap boatSpeeds = new ConcurrentHashMap<>(); + public static ConcurrentHashMap> boatPositions = 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 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){ -// StreamPacket packet = StreamReceiver.packetBuffer.peek(); -// int delayTime = 1000; -// int loopTime = delayTime + 1000; -// long sleepTime = 0; -// long transitTime = (System.currentTimeMillis()%loopTime - packet.getTimeStamp()%loopTime); -// if (transitTime < 0){ -// transitTime = loopTime + delayTime; -// } -// if (transitTime < delayTime) { -// sleepTime = delayTime - (transitTime); -// Thread.sleep(sleepTime); -// } -// System.out.println(sleepTime); - StreamPacket packet = StreamReceiver.packetBuffer.take(); + while (true){ + StreamPacket packet = StreamReceiver.packetBuffer.peek(); + //this code adds a delay to reading from the packetBuffer so + //out of order packets have time to order themselves in the queue + int delayTime = 1000; + int loopTime = delayTime * 10; + long transitTime = (System.currentTimeMillis()%loopTime - packet.getTimeStamp()%loopTime); + if (transitTime < 0){ + transitTime = loopTime + transitTime; + } + if (transitTime < delayTime) { + long sleepTime = delayTime - (transitTime); + Thread.sleep(sleepTime); + } + 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 lon = bytesToLong(lonBytes); + long rawLat = bytesToLong(latBytes); + 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); -// System.out.println("seq = " + seq); - //needs to be validated - Point3D point = new Point3D(((180d * (double)lat)/Math.pow(2,31)),((180d *(double)lon)/Math.pow(2,31)),(double)heading); - boatPositions.put(boatId, point); - boatSpeeds.put(boatId, groundSpeed); -// boatPositions.replace(boatId, point); -// 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))); + + BoatPositionPacket boatPacket = new BoatPositionPacket(boatId, timeValid, lat, lon, heading, groundSpeed); + + if (!boatPositions.containsKey(boatId)){ + boatPositions.put(boatId, new PriorityBlockingQueue(256, new Comparator() { + @Override + public int compare(BoatPositionPacket p1, BoatPositionPacket p2) { + return (int) (p1.getTimeValid() - p2.getTimeValid()); + } + })); + } + boatPositions.get(boatId).put(boatPacket); } } @@ -447,14 +448,6 @@ public class StreamParser extends Thread{ return boats; } - public static ConcurrentHashMap getBoatPositions() { - return boatPositions; - } - - public static ConcurrentHashMap getBoatSpeeds() { - return boatSpeeds; - } - public static XMLParser getXmlObject() { return xmlObject; } diff --git a/src/main/java/seng302/models/parsers/StreamReceiver.java b/src/main/java/seng302/models/parsers/StreamReceiver.java index 9ceee0b0..4c16991e 100644 --- a/src/main/java/seng302/models/parsers/StreamReceiver.java +++ b/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; diff --git a/src/main/java/seng302/models/parsers/packets/BoatPositionPacket.java b/src/main/java/seng302/models/parsers/packets/BoatPositionPacket.java new file mode 100644 index 00000000..d6f0700d --- /dev/null +++ b/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; + } +} diff --git a/src/main/java/seng302/models/parsers/packets/PacketType.java b/src/main/java/seng302/models/parsers/packets/PacketType.java new file mode 100644 index 00000000..f522dec9 --- /dev/null +++ b/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; + } + + +} diff --git a/src/main/java/seng302/models/parsers/packets/StreamPacket.java b/src/main/java/seng302/models/parsers/packets/StreamPacket.java new file mode 100644 index 00000000..4f10008c --- /dev/null +++ b/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; + } +}