diff --git a/src/main/java/seng302/controllers/CanvasController.java b/src/main/java/seng302/controllers/CanvasController.java index fda4eed2..5799d4b5 100644 --- a/src/main/java/seng302/controllers/CanvasController.java +++ b/src/main/java/seng302/controllers/CanvasController.java @@ -18,10 +18,13 @@ 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.text.DecimalFormat; import java.util.*; +import java.util.concurrent.PriorityBlockingQueue; /** * Created by ptg19 on 15/03/17. @@ -105,12 +108,7 @@ public class CanvasController { @Override public void handle(long now) { - boolean raceFinished = true; - boolean descending; - boolean leftToRight; - int boatIndex = 0; - Mark nextMark; long oldFrameTime = frameTimes[frameTimeIndex] ; frameTimes[frameTimeIndex] = now ; @@ -126,20 +124,52 @@ public class CanvasController { } for (RaceObject raceObject : raceObjects) { raceObject.updatePosition(1000 / 60); - for (int id : raceObject.getRaceIds()) { - if (StreamParser.boatPositions.containsKey((long) id)) { - Point3D p = StreamParser.boatPositions.get((long) id); - Point2D p2d = latLonToXY(p.getX(), p.getY()); - double heading = 360.0 / 0xffff * p.getZ(); - raceObject.setDestination(p2d.getX(), p2d.getY(), heading, id); + for (long id : raceObject.getRaceIds()) { + if (StreamParser.boatPositions.containsKey(id)) { + + PriorityBlockingQueue movementQueue = StreamParser.boatPositions.get(id); + if (movementQueue.size() > 0){ + BoatPositionPacket positionPacket = movementQueue.peek(); + + int delayTime = 5000; + int loopTime = delayTime + 1000; + //System.out.println("id = " + id); + long timeDiff = (System.currentTimeMillis()%loopTime - positionPacket.getTimeValid()%loopTime); + if (timeDiff < 0){ + timeDiff = loopTime + timeDiff; + } + if (id == 106) { + //System.out.println("timeDiff = " + timeDiff); + //System.out.println("movementQueue.size() = " + movementQueue.size()); + } + //System.out.println("timeDiff = " + timeDiff); + //System.out.println("movementQueue.size() = " + movementQueue.size()); + if (timeDiff > delayTime) { + if (id == 106){ + //System.out.println("processing"); + } + 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, (int) id); + + } catch (InterruptedException e){ + e.printStackTrace(); + } + } + + + } } - StreamParser.boatPositions.remove((long) id); + //StreamParser.boatPositions.remove((long) id); } } } }; for (Mark m : raceViewController.getRace().getCourse()) { - System.out.println(m.getName()); +// System.out.println(m.getName()); } //timer.start(); } diff --git a/src/main/java/seng302/models/parsers/StreamParser.java b/src/main/java/seng302/models/parsers/StreamParser.java index 3c7890c8..73961179 100644 --- a/src/main/java/seng302/models/parsers/StreamParser.java +++ b/src/main/java/seng302/models/parsers/StreamParser.java @@ -58,13 +58,13 @@ public class StreamParser extends Thread{ long sleepTime = 0; long transitTime = (System.currentTimeMillis()%loopTime - packet.getTimeStamp()%loopTime); if (transitTime < 0){ - transitTime = loopTime + delayTime; + transitTime = loopTime + transitTime; } if (transitTime < delayTime) { sleepTime = delayTime - (transitTime); Thread.sleep(sleepTime); } - System.out.println(sleepTime); +// System.out.println(sleepTime); packet = StreamReceiver.packetBuffer.take(); parsePacket(packet); @@ -169,7 +169,7 @@ public class StreamParser extends Thread{ System.out.println("RACE HAS STARTED"); } if (timeTillStart % 10 == 0){ - System.out.println("Time since start: " + -1 * timeTillStart + " Seconds"); + //System.out.println("Time since start: " + -1 * timeTillStart + " Seconds"); } } long windDir = bytesToLong(Arrays.copyOfRange(payload,18,20)); @@ -332,6 +332,9 @@ public class StreamParser extends Thread{ if ((int)deviceType == 1){ BoatPositionPacket boatPacket = new BoatPositionPacket(boatId, timeValid, lat, lon, heading, groundSpeed); + if (boatId == 106){ + System.out.println("timeValid = " + timeValid); + } if (!boatPositions.containsKey(boatId)){ boatPositions.put(boatId, new PriorityBlockingQueue(256, new Comparator() { diff --git a/src/main/java/seng302/models/parsers/StreamReceiver.java b/src/main/java/seng302/models/parsers/StreamReceiver.java index 7636b6ae..0d50315a 100644 --- a/src/main/java/seng302/models/parsers/StreamReceiver.java +++ b/src/main/java/seng302/models/parsers/StreamReceiver.java @@ -78,9 +78,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"); @@ -126,7 +123,7 @@ public class StreamReceiver extends Thread { * takes an array of up to 7 bytes 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 index b2f07c5c..b0b958df 100644 --- a/src/main/java/seng302/models/parsers/packets/BoatPositionPacket.java +++ b/src/main/java/seng302/models/parsers/packets/BoatPositionPacket.java @@ -20,4 +20,16 @@ public class BoatPositionPacket { public long getTimeValid() { return timeValid; } + + public double getLat() { + return lat; + } + + public double getLon() { + return lon; + } + + public double getHeading() { + return heading; + } } diff --git a/src/main/java/seng302/models/parsers/packets/StreamPacket.java b/src/main/java/seng302/models/parsers/packets/StreamPacket.java index 3b5c4faa..4f10008c 100644 --- a/src/main/java/seng302/models/parsers/packets/StreamPacket.java +++ b/src/main/java/seng302/models/parsers/packets/StreamPacket.java @@ -12,18 +12,11 @@ public class StreamPacket { private long timeStamp; private byte[] payload; - StreamPacket(int type, long messageLength, long timeStamp, 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; -// System.out.println("type = " + this.type.toString()); - //switch the packet type to deal with what ever specific packet you want to deal with -// if (this.type == PacketType.XML_MESSAGE){ -// //System.out.println("--------"); -// System.out.println(new String(payload)); -// StreamParser.parsePacket(this); -// } } public PacketType getType() {