public final class Address {


    public static final int LENGTH = 8;

    private final long value;

    public Address(long value) {
        this.value = value;
    }

    public Address(boolean isManual, StationType stationType, int countryCode, long lowLevelAddress) {
        this(
                ((isManual ? 1L : 0L)                            << 63) |  // bit  63
                ((stationType.value() & 0b0001_1111L)            << 58) |  // bits 62-58
                ((countryCode & 0b0011_1111_1111L)               << 48) |  // bits 57-48
                ((lowLevelAddress & 0x00_00_ff_ff_ff_ff_ff_ffL))          // bits 47-0
        );
        if (countryCode > 0b0011_1111_1111) {
            throw new IllegalArgumentException("Country code is outside of the range 0-" +
                    0b0011_1111_1111);
        }
    }

    /** For MANAGED address configuration, returns true if address is manually configured by
     * geonet Management, and false if the address from geonet Management.
     * For AUTO address configuration (station takes a random number) - undefined in standard? */
    public boolean isManual() {
        return (value >>> 63) != 0;
    }
    public StationType stationType() {
        return StationType.fromValue((int)((value >>> 58) & 0b0001_1111));
    }
    /** Returns country code.
     *
     * Country codes are according to Annex to ITU Operational Bulletin No. 741 - 1.VI.2001:
     * "Complement to Recommendation ITU-T E.212 (11/98)".
     * See http://www.itu.int/dms_pub/itu-t/opb/sp/T-SP-OB.741-2001-PDF-E.pdf
     */
    public int countryCode() {
        return (int)((value >>> 48) & 0b0011_1111_1111);
    }
    public long lowLevelAddress() {
        return value & 0x00_00_ff_ff_ff_ff_ff_ffL;
    }

    public long value() {
        return value;
    }

    @Override
    public String toString() {
        // TODO: make sure that the length is correct!
        return new StringBuilder(Long.toHexString(value).toUpperCase()).toString();
//            .insert(12, ':')
//            .insert( 8, ':')
//            .insert( 4, ':')
//            .toString();
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + (int) (value ^ (value >>> 32));
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getClass() != obj.getClass())
            return false;
        Address other = (Address) obj;
        if (value != other.value)
            return false;
        return true;
    }

    public ByteBuffer putTo(ByteBuffer buffer) {
        return buffer.putLong(value);
    }

    public static Address getFrom(ByteBuffer buffer) {
        return new Address(buffer.getLong());
    }

    /**
     *
     * @param isManual Address is manually configured, e.g. after DAD, and is not an initial random.
     * @param stationType
     * @param countryCode
     * @return
     */
    public static Address random(boolean isManual, StationType stationType, int countryCode) {
        return new Address(isManual, stationType, countryCode,
                new Random().nextLong() & 0x00_00_ff_ff_ff_ff_ff_ffL);
    }
} public class BtpPacket {

    private final Optional<Short>              sourcePort;
    private final short                        destinationPort;
    private final Optional<Short>              destinationPortInfo;
    private final byte[]                       payload;  // Final reference, but mutable content!
    private final Destination                  destination;
    private final Optional<TrafficClass>       trafficClass;
    private final Optional<LongPositionVector> senderPosition;

    private byte[] gnPayload = null;

    public static final int HEADER_LENGTH = 4;

    private BtpPacket(
            Optional<Short>              sourcePort,
            short                        destinationPort,
            Optional<Short>              destinationPortInfo,
            byte[]                       payload,
            Destination                  destination,
            Optional<TrafficClass>       trafficClass,
            Optional<LongPositionVector> senderPosition
        ) {
        this.sourcePort          = sourcePort;
        this.destinationPort     = destinationPort;
        this.destinationPortInfo = destinationPortInfo;
        this.payload             = payload.clone();  // Defensive. Just in case.
        this.destination         = destination;
        this.trafficClass        = trafficClass;
        this.senderPosition      = senderPosition;
    }

    public static BtpPacket singleHop(byte[] payload, short destinationPort) {
        Optional<Short>              emptySourcePort          = Optional.empty();
        Optional<Short>              emptyDestinationPortInfo = Optional.empty();
        Optional<TrafficClass>       emptyTrafficClass        = Optional.empty();
        Optional<LongPositionVector> emptySenderPosition      = Optional.empty();

        return new BtpPacket(
                emptySourcePort,
                destinationPort,
                emptyDestinationPortInfo,
                payload,
                Destination.singleHop(),
                emptyTrafficClass,
                emptySenderPosition
                );
    }

    public static BtpPacket singleHop(byte[] payload, short destinationPort, double maxLifetimeSec) {
        Optional<Short>              emptySourcePort          = Optional.empty();
        Optional<Short>              emptyDestinationPortInfo = Optional.empty();
        Optional<TrafficClass>       emptyTrafficClass        = Optional.empty();
        Optional<LongPositionVector> emptySenderPosition      = Optional.empty();

        return new BtpPacket(
                emptySourcePort,
                destinationPort,
                emptyDestinationPortInfo,
                payload,
                Destination.singleHop().withMaxLifetimeSeconds(maxLifetimeSec),
                emptyTrafficClass,
                emptySenderPosition
                );
    }
    /** Dummy BTP-A packet for Plugtest. */
    public static BtpPacket singleHopEmptyA(short destinationPort, short sourcePort) {
        Optional<Short>              emptyDestinationPortInfo = Optional.empty();
        Optional<TrafficClass>       emptyTrafficClass        = Optional.empty();
        Optional<LongPositionVector> emptySenderPosition      = Optional.empty();

        return new BtpPacket(
                Optional.of(sourcePort),
                destinationPort,
                emptyDestinationPortInfo,
                new byte[] {},
                Destination.singleHop(),
                emptyTrafficClass,
                emptySenderPosition
                );
    }

    /** Dummy BTP-B packet for Plugtest. */
    public static BtpPacket singleHopEmptyB(short destinationPort, short DestinationPortInfo) {
        Optional<Short>              emptySourcePort          = Optional.empty();
        Optional<TrafficClass>       emptyTrafficClass        = Optional.empty();
        Optional<LongPositionVector> emptySenderPosition      = Optional.empty();

        return new BtpPacket(
                emptySourcePort,
                destinationPort,
                Optional.of(DestinationPortInfo),
                new byte[] {},
                Destination.singleHop(),
                emptyTrafficClass,
                emptySenderPosition
                );
    }

    public static BtpPacket customDestination(byte[] payload, short destinationPort,
            Destination destination) {
        Optional<Short>              emptySourcePort          = Optional.empty();
        Optional<Short>              emptyDestinationPortInfo = Optional.empty();
        Optional<TrafficClass>       emptyTrafficClass        = Optional.empty();
        Optional<LongPositionVector> emptySenderPosition      = Optional.empty();

        return new BtpPacket(
                emptySourcePort,
                destinationPort,
                emptyDestinationPortInfo,
                payload,
                destination,
                emptyTrafficClass,
                emptySenderPosition
                );
    }

    public Optional<Short>              sourcePort()           { return sourcePort;          }
    public short                        destinationPort()      { return destinationPort;     }
    public Optional<Short>              destinationPortInfo () { return destinationPortInfo; }
    public byte[]                       payload ()             { return payload.clone();     }  // Extra defensive here. Just in case.
    public Destination                  destination ()         { return destination;         }
    public Optional<TrafficClass>       trafficClass ()        { return trafficClass;        }
    public Optional<LongPositionVector> senderPosition ()      { return senderPosition;      }

    public byte[] asBytes() {
        if (gnPayload == null) {
            ByteBuffer buffer = ByteBuffer.allocate(HEADER_LENGTH + payload.length);
            gnPayload = putHeaderTo(buffer).put(payload).array();
        }
        return gnPayload;
    }

    private ByteBuffer putHeaderTo(ByteBuffer buffer) {
        return buffer.putShort(destinationPort)
                     .putShort(sourcePort.orElse(destinationPortInfo.orElse((short)0x00)));
    }

    public static BtpPacket fromGeonetData(GeonetData data) {
        ByteBuffer buffer = ByteBuffer.wrap(data.payload);
        Optional<Short>         sourcePort;
        short                   destinationPort;
        Optional<Short>         destinationPortInfo;
        switch (data.protocol) {
            case BTP_A:
            case BTP_B: {  // Carries the source and the destination port.
                boolean isA = data.protocol == UpperProtocolType.BTP_A;
                Optional<Short> emptyPort = Optional.empty();
                destinationPort = buffer.getShort();
                short port2 = buffer.getShort();
                sourcePort          = isA ? Optional.of(port2)  :         emptyPort;
                destinationPortInfo = isA ?           emptyPort : Optional.of(port2);
                byte[] btpPayload = Arrays.copyOfRange(data.payload, HEADER_LENGTH,
                        data.payload.length);
                return new BtpPacket(
                        sourcePort,
                        destinationPort,
                        destinationPortInfo,
                        btpPayload,
                        data.destination,
                        data.trafficClass,
                        data.sender
                        );
            }
            case ANY:
            case IPv6:
            default:
                throw new IllegalArgumentException("Unsupported BTP protocol: " + data.protocol);
        }
    }
} public class BtpSocket implements AutoCloseable {

    private GeonetStation station;

    private BtpSocket(GeonetStation station) {
        this.station = station;
    }

    public static BtpSocket on(StationConfig config, LinkLayer linkLayer, PositionProvider positionProvider) {
        GeonetStation station = new GeonetStation(config, linkLayer, positionProvider);
        new Thread(station).start();
        station.startBecon();
        return BtpSocket.on(station);
    }

    public static BtpSocket on(GeonetStation runningStation) {
        return new BtpSocket(runningStation);
    }

    public void send(BtpPacket packet) throws IOException {
        station.send(new GeonetData(
                packet.sourcePort().isPresent() ?
                        UpperProtocolType.BTP_A :
                        UpperProtocolType.BTP_B,
                packet.destination(),
                packet.trafficClass(),
                packet.senderPosition(),
                packet.asBytes()
                ));
    }

    public BtpPacket receive() throws InterruptedException {
        GeonetData data = station.receive();
        while(data.protocol != UpperProtocolType.BTP_A &&
                data.protocol != UpperProtocolType.BTP_B) {
            data = station.receive();
        }
        return BtpPacket.fromGeonetData(data);
    }

    @Override
    public void close() {
        station.close();
    }
} public abstract class Destination {

    public abstract DestinationType  typeAndSubtype();
    public abstract Optional<Double> maxLifetimeSeconds();
    public abstract Optional<Byte>   maxHopLimit();
    public abstract Optional<Byte>   remainingHopLimit();
    
    public static final class SingleHop extends Destination {
        private final Optional<Double> maxLifetimeSeconds;

        private SingleHop(Optional<Double> maxLifetimeSeconds) {
            this.maxLifetimeSeconds = maxLifetimeSeconds;
        }

        @Override public DestinationType  typeAndSubtype()     { return DestinationType.SINGLE_HOP; }
        @Override public Optional<Double> maxLifetimeSeconds() { return maxLifetimeSeconds;    }
        @Override public Optional<Byte>   maxHopLimit()        { return Optional.of((byte) 1); }
        @Override public Optional<Byte>   remainingHopLimit()  { return Optional.of((byte) 1); }

        public SingleHop withMaxLifetimeSeconds(double lifetimeSeconds) {
            return new SingleHop(Optional.of(lifetimeSeconds));
        }

        @Override public int hashCode() {
            return 31 + ((maxLifetimeSeconds == null) ? 0 : maxLifetimeSeconds.hashCode());
        }

        @Override public boolean equals(Object obj) {
            if (this == obj) return true;
            if (obj == null) return false;
            if (getClass() != obj.getClass()) return false;
            SingleHop other = (SingleHop) obj;
            if (maxLifetimeSeconds == null) {
                if (other.maxLifetimeSeconds != null) return false;
            } else if (!maxLifetimeSeconds.equals(other.maxLifetimeSeconds))
                return false;
            return true;
        }

    }

    public static final class Geobroadcast extends Destination {
        
    	final private Area              area;
        final private Optional<Double>  maxLifetimeSeconds;
        final private Optional<Byte>    maxHopLimit;
        final private Optional<Byte>    remainingHopLimit;
        final private boolean           isAnycast;

        private Geobroadcast (
                Area              area,
                Optional<Double>  maxLifetimeSeconds,
                Optional<Byte>    maxHopLimit,
                Optional<Byte>    remainingHopLimit,
                boolean           isAnycast
                ) {
            this.area               = area;
            this.maxLifetimeSeconds = maxLifetimeSeconds;
            this.maxHopLimit        = maxHopLimit;
            this.remainingHopLimit  = remainingHopLimit;
            this.isAnycast          = isAnycast;
        }

    	@Override
        public int hashCode() {
            final int prime = 31;
            int result = 1;
            result = prime * result + ((area == null) ? 0 : area.hashCode());
            result = prime * result + (isAnycast ? 1231 : 1237);
            result = prime * result + ((maxHopLimit == null) ? 0 : maxHopLimit.hashCode());
            result = prime * result
                    + ((maxLifetimeSeconds == null) ? 0 : maxLifetimeSeconds.hashCode());
            result = prime * result
                    + ((remainingHopLimit == null) ? 0 : remainingHopLimit.hashCode());
            return result;
        }

        @Override
        public boolean equals(Object obj) {
            if (this == obj)
                return true;
            if (obj == null)
                return false;
            if (getClass() != obj.getClass())
                return false;
            Geobroadcast other = (Geobroadcast) obj;
            if (area == null) {
                if (other.area != null)
                    return false;
            } else if (!area.equals(other.area))
                return false;
            if (isAnycast != other.isAnycast)
                return false;
            if (maxHopLimit == null) {
                if (other.maxHopLimit != null)
                    return false;
            } else if (!maxHopLimit.equals(other.maxHopLimit))
                return false;
            if (maxLifetimeSeconds == null) {
                if (other.maxLifetimeSeconds != null)
                    return false;
            } else if (!maxLifetimeSeconds.equals(other.maxLifetimeSeconds))
                return false;
            if (remainingHopLimit == null) {
                if (other.remainingHopLimit != null)
                    return false;
            } else if (!remainingHopLimit.equals(other.remainingHopLimit))
                return false;
            return true;
        }

        public Area area() { return area; }

        @Override
        public DestinationType typeAndSubtype() {
            if (isAnycast) {
                switch (area.type()) {
                    case CIRCLE:    return DestinationType.GEOANYCAST_CIRCLE;
                    case RECTANGLE: return DestinationType.GEOANYCAST_RECTANGLE;
                    case ELLIPSE:   return DestinationType.GEOANYCAST_ELLIPSE;
                    default:        return DestinationType.ANY;
                }
            } else {
                switch (area.type()) {
                    case CIRCLE:    return DestinationType.GEOBROADCAST_CIRCLE;
                    case RECTANGLE: return DestinationType.GEOBROADCAST_RECTANGLE;
                    case ELLIPSE:   return DestinationType.GEOBROADCAST_ELLIPSE;
                    default:        return DestinationType.ANY;
                }
            }
        }

        @Override public Optional<Double> maxLifetimeSeconds() { return maxLifetimeSeconds; }
        @Override public Optional<Byte>   maxHopLimit()        { return maxHopLimit;        }
        @Override public Optional<Byte>   remainingHopLimit()  { return remainingHopLimit;  }
        public boolean isAnycast() { return isAnycast; }

        public Geobroadcast withMaxLifetimeSeconds(double lifetimeSeconds) {
            return new Geobroadcast(
                this.area,
                Optional.of(lifetimeSeconds),
                this.maxHopLimit,
                this.remainingHopLimit,
                this.isAnycast
            );
        }
        public Geobroadcast withMaxHopLimit(byte maxHopLimit) {
            return new Geobroadcast(
                this.area,
                this.maxLifetimeSeconds,
                Optional.of(maxHopLimit),
                this.remainingHopLimit,
                this.isAnycast
            );
        }
        public Geobroadcast withRemainingHopLimit(byte remainingHopLimit) {
            return new Geobroadcast(
                this.area,
                this.maxLifetimeSeconds,
                this.maxHopLimit,
                Optional.of(remainingHopLimit),
                this.isAnycast
            );
        }
    }

    public static final class Beacon extends Destination {
        private Beacon() {}
        @Override public DestinationType  typeAndSubtype()     { return DestinationType.BEACON; }
        @Override public Optional<Double> maxLifetimeSeconds() { return Optional.empty();       }
        @Override public Optional<Byte>   maxHopLimit()        { return Optional.of((byte) 1);  }
        @Override public Optional<Byte>   remainingHopLimit()  { return Optional.of((byte) 1);  }
    }

    public static final class TopoScopedBroadcast extends Destination {
        private final Optional<Double> maxLifetimeSeconds;
        private final Optional<Byte> maxHopLimit;
        private final Optional<Byte> remainingHopLimit;

        private TopoScopedBroadcast(Optional<Double> maxLifetimeSeconds, Optional<Byte> maxHopLimit, Optional<Byte> remainingHopLimit) {
            this.maxLifetimeSeconds = maxLifetimeSeconds;
            this.maxHopLimit = maxHopLimit;
            this.remainingHopLimit = remainingHopLimit;
        }

        @Override public DestinationType  typeAndSubtype()     { return DestinationType.MULTI_HOP; }
        @Override public Optional<Double> maxLifetimeSeconds() { return maxLifetimeSeconds; }
        @Override public Optional<Byte>   maxHopLimit()        { return maxHopLimit; }
        @Override public Optional<Byte>   remainingHopLimit()  { return remainingHopLimit; }

        public TopoScopedBroadcast withMaxLifetimeSeconds(double lifetimeSeconds) {
            return new TopoScopedBroadcast(
                Optional.of(lifetimeSeconds),
                this.maxHopLimit,
                this.remainingHopLimit
            );
        }
        public TopoScopedBroadcast withMaxHopLimit(byte maxHopLimit) {
            return new TopoScopedBroadcast(
                this.maxLifetimeSeconds,
                Optional.of(maxHopLimit),
                this.remainingHopLimit
            );
        }
        public TopoScopedBroadcast withRemainingHopLimit(byte remainingHopLimit) {
            return new TopoScopedBroadcast(
                this.maxLifetimeSeconds,
                this.maxHopLimit,
                Optional.of(remainingHopLimit)
            );
        }
    }

    public static final class GeoUnicast extends Destination {
        private final Address address;
        private final Optional<Double> maxLifetimeSeconds;
        private final Optional<Byte> maxHopLimit;
        private final Optional<Byte> remainingHopLimit;

        private GeoUnicast(Address address, Optional<Double> maxLifetimeSeconds, Optional<Byte> maxHopLimit, Optional<Byte> remainingHopLimit) {
            this.address = address;
            this.maxLifetimeSeconds = maxLifetimeSeconds;
            this.maxHopLimit = maxHopLimit;
            this.remainingHopLimit = remainingHopLimit;
        }

        @Override public DestinationType  typeAndSubtype()     { return DestinationType.GEOUNICAST; }
        @Override public Optional<Double> maxLifetimeSeconds() { return maxLifetimeSeconds; }
        @Override public Optional<Byte>   maxHopLimit()        { return maxHopLimit; }
        @Override public Optional<Byte>   remainingHopLimit()  { return remainingHopLimit; }

        public GeoUnicast withMaxLifetimeSeconds(double lifetimeSeconds) {
            return new GeoUnicast(
                this.address,
                Optional.of(lifetimeSeconds),
                this.maxHopLimit,
                this.remainingHopLimit
            );
        }
        public GeoUnicast withMaxHopLimit(byte maxHopLimit) {
            return new GeoUnicast(
                this.address,
                this.maxLifetimeSeconds,
                Optional.of(maxHopLimit),
                this.remainingHopLimit
            );
        }
        public GeoUnicast withRemainingHopLimit(byte remainingHopLimit) {
            return new GeoUnicast(
                this.address,
                this.maxLifetimeSeconds,
                this.maxHopLimit,
                Optional.of(remainingHopLimit)
            );
        }
    }


    public static SingleHop singleHop() {
        Optional<Double> lifetime = Optional.empty();
        return new SingleHop(lifetime);
    }

    public static Geobroadcast geobroadcast(Area area) {
        Optional<Double> emptyLifetime = Optional.empty();
        Optional<Byte>   emptyHops     = Optional.empty();
        boolean isAnycast = false;
        return new Geobroadcast(area, emptyLifetime, emptyHops, emptyHops, isAnycast);
    }

    public static Geobroadcast geoanycast(Area area) {
        Optional<Double> emptyLifetime = Optional.empty();
        Optional<Byte>   emptyHops     = Optional.empty();
        boolean isAnycast = true;
        return new Geobroadcast(area, emptyLifetime, emptyHops, emptyHops, isAnycast);
    }

    public static Beacon beacon() { return new Beacon(); }

    public static TopoScopedBroadcast toposcopedbroadcast() {
        Optional<Double> emptyLifetime = Optional.empty();
        Optional<Byte>   emptyHops     = Optional.empty();
        return new TopoScopedBroadcast(emptyLifetime, emptyHops, emptyHops);
    }

    public static GeoUnicast geounicast(Address address) {
        Optional<Double> emptyLifetime = Optional.empty();
        Optional<Byte>   emptyHops     = Optional.empty();
        return new GeoUnicast(address, emptyLifetime, emptyHops, emptyHops);
    }

} public final class GeonetData {

    public final UpperProtocolType            protocol;
    public final Destination                  destination;
    public final Optional<TrafficClass>       trafficClass;
    public final Optional<LongPositionVector> sender;
    public final byte[]                       payload;  // Final, but mutable content!

    public GeonetData(
            UpperProtocolType            protocol,
            Destination                  destination,
            Optional<TrafficClass>       trafficClass,
            Optional<LongPositionVector> sender,
            byte[]                       payload
            ) {
        this.protocol     = protocol;
        this.destination  = destination;
        this.trafficClass = trafficClass;
        this.sender       = sender;
        this.payload      = payload;
    }

    public GeonetData withSender(final Optional<LongPositionVector> sender) {
        return new GeonetData(this.protocol, this.destination, this.trafficClass, sender,
                this.payload.clone());
    }

    public GeonetData withDestination(final Destination destination) {
        return new GeonetData(this.protocol, destination, this.trafficClass, this.sender,
                this.payload.clone());
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + ((destination == null) ? 0 : destination.hashCode());
        result = prime * result + Arrays.hashCode(payload);
        result = prime * result + ((protocol == null) ? 0 : protocol.hashCode());
        result = prime * result + ((sender == null) ? 0 : sender.hashCode());
        result = prime * result + ((trafficClass == null) ? 0 : trafficClass.hashCode());
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getClass() != obj.getClass())
            return false;
        GeonetData other = (GeonetData) obj;
        if (destination == null) {
            if (other.destination != null)
                return false;
        } else if (!destination.equals(other.destination))
            return false;
        if (!Arrays.equals(payload, other.payload))
            return false;
        if (protocol != other.protocol)
            return false;
        if (sender == null) {
            if (other.sender != null)
                return false;
        } else if (!sender.equals(other.sender))
            return false;
        if (trafficClass == null) {
            if (other.trafficClass != null)
                return false;
        } else if (!trafficClass.equals(other.trafficClass))
            return false;
        return true;
    }

} public class GeonetStation implements Runnable, AutoCloseable {

    private StationConfig                         config;
    private LinkLayer                             linkLayer;
    private PositionProvider                      positionProvider;
    private final LinkedBlockingQueue<GeonetData> queueUpward = new LinkedBlockingQueue<>();
    private final Collection<GeonetDataListener>  listeners = new ArrayList<>();
    private int                                   nextFreeSequenceNumber = 0;
    private final MacAddress senderMac;

    private LocationTable locationTable;
    private final static Logger logger = LoggerFactory.getLogger(GeonetStation.class);

    public final static short GN_ETHER_TYPE = (short) 0x8947;
    private final static MacAddress BROADCAST_MAC = new MacAddress(0xff_ff_ff_ff_ff_ffL);
    private final static MacAddress EMPTY_MAC = new MacAddress(0);
    private final static int ETHER_HEADER_LENGTH = 14;

    // Common scheduler for beacon, Duplicate packet detection and Contention-based forwarding.
    // Change to custom-clock scheduler for non-real-time time. (Dependency injection?)
    private final ScheduledExecutorService scheduler =
            Executors.newSingleThreadScheduledExecutor();

    private boolean isPromiscuous = true;
 // Do we need concurrent?
    private final Map<PacketId, ScheduledFuture<?>> contentionSet = new ConcurrentHashMap<>();
    private final Set<PacketId> seenPackets = new HashSet<>();

    
    public GeonetStation(StationConfig config, LinkLayer linkLayer, PositionProvider positionProvider) {
        this(config, linkLayer, positionProvider, EMPTY_MAC);
    }

    public GeonetStation(StationConfig config, LinkLayer linkLayer, PositionProvider positionProvider, MacAddress senderMac) {
        this.config = config;
        if (senderMac.value() != 0) {
            config.setItsGnLocalGnAddr(new Address(false, StationType.Passenger_Car, 752, senderMac.value()).value());
        }
        this.linkLayer = linkLayer;
        this.positionProvider = positionProvider;
        this.senderMac = senderMac;
        this.locationTable = new LocationTable(new ConfigProvider() {
            @Override public StationConfig config() { return GeonetStation.this.config;}
        });
        logger.info("Initialized station with GN address {} and MAC address {}", config.getItsGnLoacalGnAddr(), this.senderMac);
    }

    // Common function to get the current time.
    private Instant timeInstantNow() {
        return Instant.now();  // Add clock here for non-real-time.
    }

    private short sequenceNumber() {
        short result = (short) nextFreeSequenceNumber;
        nextFreeSequenceNumber++;
        if (nextFreeSequenceNumber > (1 << 16) - 1) { nextFreeSequenceNumber = 0; }
        return result;
    }

    private BasicHeader basicHeader(GeonetData data) {
        return new BasicHeader(
                (byte) config.getItsGnProtocolVersion(),
                BasicHeader.NextHeader.COMMON_HEADER,
                BasicHeader.Lifetime.fromSeconds(data.destination.maxLifetimeSeconds().orElse(
                        (double) config.getItsGnDefaultPacketLifetime())),
                data.destination.remainingHopLimit().orElse((byte) config.getItsGnDefaultHopLimit()));
    }

    private CommonHeader commonHeader(GeonetData data) {
        return new CommonHeader(
                data.protocol,
                data.destination.typeAndSubtype(),
                data.trafficClass.orElse(TrafficClass.fromByte(config.getItsGnDefaultTrafficClass())),
                config.getItsGnIsMobile() == 1,
                (short) data.payload.length,
                data.destination.maxHopLimit().orElse((byte) config.getItsGnDefaultHopLimit()));
    }

    /** Adds a listener for GeonetData indications (received messages from link layer).
     *
     * Listeners do not disable queue-based solution, so someone have to empty that queue using
     * {@link #receive()} to avoid out-of-memory problems.
     **/
    public void addGeonetDataListener(GeonetDataListener listener) {
        listeners.add(listener);
    }

    public void removeGeonetDataListener(GeonetDataListener listener) {
        listeners.remove(listener);
    }

    public void send(GeonetData data) throws IOException {

        logger.debug("Sending message, hasEtherHeader: {}, sender mac {}", linkLayer.hasEthernetHeader(), senderMac);

        BasicHeader basicHeader = basicHeader(data);
        CommonHeader commonHeader = commonHeader(data);
        LongPositionVector senderPosition = data.sender.orElse(positionProvider.getLatestPosition());
        LongPositionVector positionVector = senderPosition.address().isPresent() ?
                senderPosition : senderPosition.withAddress(new Address(config.getItsGnLoacalGnAddr()));

        GeonetData completeData = data.withSender(Optional.of(positionVector));

        switch (data.destination.typeAndSubtype()) {
            case SINGLE_HOP: {
                int mediaInfo = 0x0000; // Reserved 32-bit field for media-dependent operations.
                                        // Set to 0 if not used. Can be used for DCC-related
                                        // information in ITS-G5 (ETSI TS 102 636-4-2).

                ByteBuffer llPayload = ByteBuffer.allocate(
                        (linkLayer.hasEthernetHeader() ? ETHER_HEADER_LENGTH : 0) +
                        40 + data.payload.length);
                if (linkLayer.hasEthernetHeader()) {
                    llPayload.put(BROADCAST_MAC.asBytes());
                    llPayload.put(senderMac.asBytes());
                    llPayload.putShort(GN_ETHER_TYPE);
                }
                basicHeader.putTo(llPayload);    // octets 0-3
                commonHeader.putTo(llPayload);   // octets 4-11
                positionVector.putTo(llPayload); // octets 12-35
                llPayload.putInt(mediaInfo);     // octets 36-39
                llPayload.put(data.payload);

                sendToLowerLayer(llPayload);
                beaconService.skipNextBeacon();  // Beacon is redundant with Single Hop Broadcast.
                break;
            }
            case GEOBROADCAST_CIRCLE:
            case GEOBROADCAST_ELLIPSE:
            case GEOBROADCAST_RECTANGLE:
            case GEOANYCAST_CIRCLE:
            case GEOANYCAST_ELLIPSE:
            case GEOANYCAST_RECTANGLE: {
                MacAddress dstMac = BROADCAST_MAC;
                if (linkLayer.hasEthernetHeader()) {
                    Area area = ((Destination.Geobroadcast)data.destination).area();
                    if (!area.contains(position())) {
                        logger.debug("Area {} don't contains me {}", area, position());
                        Optional<MacAddress> betterDstMac = locationTable.closerThanMeTo(
                            area.center(), position(), new HashSet<MacAddress>());
                        if (betterDstMac.isPresent()) { dstMac = betterDstMac.get(); }
                    }
                }
                ByteBuffer llPayload = packGeobroadcast(dstMac, senderMac, completeData, sequenceNumber());

                sendToLowerLayer(llPayload);
                break;
            }
            case BEACON:
                logger.debug("Send, BEACON, hasEtherHeader: {}", linkLayer.hasEthernetHeader());
                ByteBuffer llPayload = ByteBuffer.allocate(
                        (linkLayer.hasEthernetHeader() ? ETHER_HEADER_LENGTH : 0) +
                        36);
                if (linkLayer.hasEthernetHeader()) {
                    llPayload.put(BROADCAST_MAC.asBytes());
                    llPayload.put(senderMac.asBytes());
                    llPayload.putShort(GN_ETHER_TYPE);
                }
                basicHeader.putTo(llPayload);    // octets 0-3
                commonHeader.putTo(llPayload);   // octets 4-11
                positionVector.putTo(llPayload); // octets 12-35

                sendToLowerLayer(llPayload);
                break;
            case GEOUNICAST:
            case LOCATION_SERVICE_REPLY:
            case LOCATION_SERVICE_REQUEST:
            case MULTI_HOP:  // Topologically Scoped Broadcast (TSB)
            case ANY:
            default:
                // Ignore for now.
                logger.info("Ignored sending {}", data.destination.typeAndSubtype().toString());
                break;
        }
    }

    /** Interface to lower layer (Ethernet/ITS-G5/802.11p, Link Layer) */
    private void onReceiveFromLowerLayer(byte[] payload) throws InterruptedException {
        logger.debug("GN Received payload of size {}", payload.length);
        // Do we want to clone payload before we start reading from it?
        ByteBuffer buffer = ByteBuffer.wrap(payload).asReadOnlyBuffer();  // I promise not to write.
        try {
            byte[] llDstAddress = new byte[6];  // Initialized to 0.
            byte[] llSrcAddress = new byte[6];
            if (linkLayer.hasEthernetHeader()) {
                buffer.get(llDstAddress);
                // Should be either me or broadcast.
                MacAddress dstMac = MacAddress.fromBytes(llDstAddress);
                if (!isPromiscuous  && !dstMac.equals(senderMac) &&
                        !dstMac.equals(BROADCAST_MAC)) {
                    return;
                }
                buffer.get(llSrcAddress);
                short ethertype = buffer.getShort();
                if (ethertype != GN_ETHER_TYPE) {
                    logger.warn("Ethertype is not Geonetworking (no filtering in LinkLayer?)");
                    return;
                }
            }

            BasicHeader  basicHeader  = BasicHeader.getFrom(buffer);
            if (basicHeader.version() != config.getItsGnProtocolVersion()) {
                logger.warn("Unrecognized protocol version: " + basicHeader.version());
                return;
            }
            CommonHeader commonHeader = CommonHeader.getFrom(buffer);

            switch (commonHeader.typeAndSubtype()) {
                case SINGLE_HOP: {
                    LongPositionVector senderLpv = LongPositionVector.getFrom(buffer);
                    @SuppressWarnings("unused")
                    int reserved = buffer.getInt();  // 32 bit media-dependent info.
                    byte[] upperPayload = new byte[commonHeader.payloadLength()];
                    buffer.slice().get(upperPayload, 0, commonHeader.payloadLength());
                    GeonetData indication = new GeonetData(
                            commonHeader.nextHeader(),
                            Destination.singleHop().withMaxLifetimeSeconds(
                                    basicHeader.lifetime().asSeconds()),
                            Optional.of(commonHeader.trafficClass()),
                            Optional.of(senderLpv),
                            upperPayload
                            );
                    sendToUpperLayer(indication);
                    // If there is no Ethernet header, llSrcAddress is all 0.
                    locationTable.updateFromDirectMessage(senderLpv.address().get(),
                            MacAddress.fromBytes(llSrcAddress), senderLpv);
                    break;
                }
                case MULTI_HOP: {  // TODO: test case.
                    short sequenceNumber = buffer.getShort();
                    buffer.getShort();  // Reserved 16-bit.
                    LongPositionVector senderLpv = LongPositionVector.getFrom(buffer);
                    byte[] upperPayload = new byte[commonHeader.payloadLength()];
                    buffer.slice().get(upperPayload, 0, commonHeader.payloadLength());

                    Destination.TopoScopedBroadcast destination = Destination.toposcopedbroadcast()
                            .withMaxLifetimeSeconds(basicHeader.lifetime().asSeconds())
                            .withMaxHopLimit(commonHeader.maximumHopLimit())
                            .withRemainingHopLimit(basicHeader.remainingHopLimit());
                    GeonetData indication = new GeonetData(
                            commonHeader.nextHeader(),
                            destination,
                            Optional.of(commonHeader.trafficClass()),
                            Optional.of(senderLpv),
                            upperPayload
                            );
                    if (!isDuplicate(indication, sequenceNumber)) {
                        sendToUpperLayer(indication);
                    }
                    locationTable.updateFromForwardedMessage(senderLpv.address().get(), senderLpv);
                    forwardIfNecessary(indication, sequenceNumber, MacAddress.fromBytes(llSrcAddress));
                    markAsSeen(indication, sequenceNumber);  // Duplicate packet detection.

                    break;
                }
                case GEOBROADCAST_CIRCLE:
                case GEOBROADCAST_ELLIPSE:
                case GEOBROADCAST_RECTANGLE:
                case GEOANYCAST_CIRCLE:
                case GEOANYCAST_ELLIPSE:
                case GEOANYCAST_RECTANGLE:
                {
                    short sequenceNumber = buffer.getShort();
                    buffer.getShort();  // Reserved 16-bit.
                    LongPositionVector senderLpv = LongPositionVector.getFrom(buffer);
                    Area area = Area.getFrom(buffer, Area.Type.fromCode(commonHeader.typeAndSubtype().subtype()));
                    buffer.getShort();  // Reserved 16-bit.
                    byte[] upperPayload = new byte[commonHeader.payloadLength()];
                    buffer.slice().get(upperPayload, 0, commonHeader.payloadLength());

                    Destination.Geobroadcast destination = Destination.geobroadcast(area)
                            .withMaxLifetimeSeconds(basicHeader.lifetime().asSeconds())
                            .withRemainingHopLimit(basicHeader.remainingHopLimit())
                            .withMaxHopLimit(commonHeader.maximumHopLimit());
                    GeonetData indication = new GeonetData(
                            commonHeader.nextHeader(),
                            destination,
                            Optional.of(commonHeader.trafficClass()),
                            Optional.of(senderLpv),
                            upperPayload
                            );
                    if (area.contains(position()) && !isDuplicate(indication, sequenceNumber)) {
                        sendToUpperLayer(indication);
                    }
                                        
                    locationTable.updateFromForwardedMessage(senderLpv.address().get(), senderLpv);
                    forwardIfNecessary(indication, sequenceNumber, MacAddress.fromBytes(llSrcAddress));
                    markAsSeen(indication, sequenceNumber);  // Duplicate packet detection.
                    break;
                }
                case BEACON: {
                    LongPositionVector senderLpv = LongPositionVector.getFrom(buffer);
                    locationTable.updateFromDirectMessage(senderLpv.address().get(),
                            MacAddress.fromBytes(llSrcAddress), senderLpv);
                    break;
                }
                case LOCATION_SERVICE_REQUEST:
                case LOCATION_SERVICE_REPLY:
                    // Do nothing.
                    // At the moment we don't maintain Location Table.
                    logger.info("Ignoring Location Service {}",
                            commonHeader.typeAndSubtype().toString());
                    break;
                case GEOUNICAST: {
                    short sequenceNumber = buffer.getShort();
                    buffer.getShort();  // Reserved 16-bit.
                    LongPositionVector senderLpv = LongPositionVector.getFrom(buffer);
                    ShortPositionVector destSpv = ShortPositionVector.getFrom(buffer);
                    byte[] upperPayload = new byte[commonHeader.payloadLength()];
                    buffer.slice().get(upperPayload, 0, commonHeader.payloadLength());

                    Destination.GeoUnicast destination = Destination.geounicast(destSpv.address())
                            .withMaxLifetimeSeconds(basicHeader.lifetime().asSeconds())
                            .withRemainingHopLimit(basicHeader.remainingHopLimit())
                            .withMaxHopLimit(commonHeader.maximumHopLimit());
                    GeonetData indication = new GeonetData(
                            commonHeader.nextHeader(),
                            destination,
                            Optional.of(commonHeader.trafficClass()),
                            Optional.of(senderLpv),
                            upperPayload
                            );
                    final long myMac = (new Address(config.getItsGnLoacalGnAddr())).lowLevelAddress();
                    if (destSpv.address().lowLevelAddress() == myMac
                            && !isDuplicate(indication, sequenceNumber)) {
                        sendToUpperLayer(indication);
                    }
                    locationTable.updateFromForwardedMessage(senderLpv.address().get(), senderLpv);
                    //forwardIfNecessary(indication, sequenceNumber, MacAddress.fromBytes(llSrcAddress));
                    markAsSeen(indication, sequenceNumber);  // Duplicate packet detection.
                    break;
                }
                case ANY:
                default:
                    // Ignore for now.
                    logger.info("Ignoring {}", commonHeader.typeAndSubtype().toString());
                    break;
            }
        } catch (BufferUnderflowException | IllegalArgumentException ex) {
            logger.warn("Can't parse the packet, ignoring.", ex);
        }
    }

    

    private void sendForwardedPacket(GeonetData data, int sequenceNumber, Instant timeAdded, MacAddress dstMac) {
        if (!linkLayer.hasEthernetHeader()) { return; }  // Forwarding does not work without MAC.
        double queuingTimeInSeconds = Duration.between(timeAdded, timeInstantNow()).toMillis() * 0.001;
        double newLifetime = data.destination.maxLifetimeSeconds().get() - queuingTimeInSeconds;
        byte newHops = (byte)(data.destination.remainingHopLimit().get() - 1);
        if (newLifetime <= 0 || newHops < 2) { return; }
        Geobroadcast destination = ((Geobroadcast) data.destination)
                .withMaxHopLimit(newHops).withMaxLifetimeSeconds(newLifetime);
        GeonetData newData = data.withDestination(destination);
        ByteBuffer llPayload = packGeobroadcast(dstMac, senderMac, newData, sequenceNumber);

        try {
            sendToLowerLayer(llPayload);
        } catch (IOException e) {
            logger.error("Exception in sending forwarded packet", e);
        }

    }

    private ByteBuffer packGeobroadcast(MacAddress dstMac, MacAddress srcMac, GeonetData data, int sequenceNumber) {
        ByteBuffer llPayload = ByteBuffer.allocate(
            (linkLayer.hasEthernetHeader() ? ETHER_HEADER_LENGTH : 0) + 56 + data.payload.length);

        if (linkLayer.hasEthernetHeader()) {
            llPayload.put(dstMac.asBytes());
            llPayload.put(senderMac.asBytes());
            llPayload.putShort(GN_ETHER_TYPE);
        }
        basicHeader(data).putTo(llPayload);               // Octets  0- 3
        commonHeader(data).putTo(llPayload);              // Octets  4-11
        llPayload.putShort((short)sequenceNumber);        // Octets 12-13
        llPayload.putShort((short)0);        // Reserved. // Octets 14-15
        data.sender.get().putTo(llPayload);               // Octets 16-39
        ((Geobroadcast)data.destination).area().putTo(llPayload);  // Octets 40-53
        llPayload.putShort((short)0);        // Reserved. // Octets 54-55
        llPayload.put(data.payload);
        return llPayload;
    }


    private void contentionBasedForwarding(final GeonetData data, final short sequenceNumber,
            final MacAddress lastForwarderMac) {
        // Packet can arrive from another forwarder, but will have the same sequence number,
        // the same source position vector (even with timestamp), but different remaining hop limit
        // different lifetime, different source MAC.
        final PacketId packetId = new PacketId(data.sender.get().timestamp(), sequenceNumber,
                data.sender.get().address().get());
        if (contentionSet.containsKey(packetId)) {
            contentionSet.get(packetId).cancel(false);
            contentionSet.remove(packetId);
            return;
        }

        if (isDuplicate(data, sequenceNumber)) {
            logger.debug("Packet has been seen, but is not in the contention set. Discarding.");
            return;
        }

        if(data.destination.remainingHopLimit().orElse((byte) 1) < 2) { return; }

        if (((Geobroadcast)data.destination).area().contains(position())) {
            long   maxTimeout  = config.getItsGnGeoBroadcastCbfMaxTime();        // In milliseconds.
            long   minTimeout  = config.getItsGnGeoBroadcastCbfMinTime();        // In milliseconds.
            double maxDistance = config.getItsGnDefaultMaxCommunicationRange();  // In meters.


            Position lastForwarderPosition = locationTable.getPosition(lastForwarderMac);

            // If the message was forwarded by someone who never sent a beacon or SHB, assume that
            // it is out of range and the distance is maxDistance.
            double lastDistance = lastForwarderPosition == null ? maxDistance :
                    position().distanceInMetersTo(lastForwarderPosition);

            // If distance is 0, then timeout is maxTimeout.
            // If distance is maxDistance or more, then timeout is minTimeout.
            // If distance is between 0 and maxDistance, then timeout is linear interpolation.
            long timeoutMillis = lastDistance >= maxDistance ? minTimeout :
                    (long) (maxTimeout - (maxTimeout - minTimeout) * (lastDistance / maxDistance));

            final Instant schedulingInstant = timeInstantNow();

            ScheduledFuture<?> sendingFuture = scheduler.schedule(
                    new Runnable() { @Override public void run() {
                        sendForwardedPacket(data, sequenceNumber, schedulingInstant, BROADCAST_MAC); }; },
                    timeoutMillis, TimeUnit.MILLISECONDS);

            contentionSet.put(packetId, sendingFuture);

        } else {
            greedyForwarding(data, sequenceNumber, lastForwarderMac);
        }
    }

    private void simpleForwarding(GeonetData indication, int sequenceNumber, MacAddress lastForwarderMac) {
        Area destinationArea = ((Geobroadcast)indication.destination).area();
        if (destinationArea.contains(position())) {  // We're inside - broadcast to all!
            sendForwardedPacket(indication, sequenceNumber, timeInstantNow(), BROADCAST_MAC);
        } else {
            Position lastForwarderPosition = locationTable.getPosition(lastForwarderMac);
            if (lastForwarderPosition != null && destinationArea.contains(lastForwarderPosition)) {
                // Last forwarder is already inside, we drop the packet.
            } else {  // Last forwarder is outside (as we are) or never sent SHB or beacon.
                greedyForwarding(indication, sequenceNumber, lastForwarderMac);
            }
        }
    }

    private void greedyForwarding(GeonetData indication, int sequenceNumber, MacAddress lastForwarderMac) {
        Optional<MacAddress> neighborMac = locationTable.closerThanMeTo(
                ((Geobroadcast)indication.destination).area().center(),
                position(), new HashSet<MacAddress>(Arrays.asList(lastForwarderMac)));
        // Fall back to broadcast if no neighbor is fond. Alternative is to buffer and wait.
        MacAddress dstMac = neighborMac.orElse(BROADCAST_MAC);
        sendForwardedPacket(indication, sequenceNumber, timeInstantNow(), dstMac);
    }

    private void forwardIfNecessary(GeonetData indication, short sequenceNumber, MacAddress lastForwarderMac) {
        // We can't forward if we don't know who was the last forwarder.
        // Packets have only the original sender, so for the last forwarder we need MAC address from LL.
        if (!linkLayer.hasEthernetHeader()) { return; }

        // Do not forward if remaining hop limit (RHL) is too low.
        // TODO: is it 1 or 0 which is too low? Beacons are sent with RHL=1, but they are never
        // forwarded. Does this imply that anything with RHL=1 should not be forwarded?
        if (indication.destination.remainingHopLimit().orElse((byte) config.getItsGnDefaultHopLimit())
                <= 1) {
            return;
        }

        // Do not forward GeoAnycast if we are one of the recipients.
        Destination.Geobroadcast destination = (Geobroadcast)indication.destination;
        if (destination.isAnycast() && destination.area().contains(position())) {
            return;
        }

        switch (config.getItsGnGeoBroadcastForwardingAlgorithm()) {
            case 0:
                logger.debug("Fwd alg set to 0 (unspecified), forwarding disabled");
                break;
            case 1:
                if (!isDuplicate(indication, sequenceNumber)) {
                    simpleForwarding(indication, sequenceNumber, lastForwarderMac);
                }
                break;
            case 2:
                contentionBasedForwarding(indication, sequenceNumber, lastForwarderMac);
                break;
            default:
                logger.error("Unsupported forwarding algorithm: {}. Forwarding disabled.",
                        config.getItsGnGeoBroadcastForwardingAlgorithm());
                break;
        }
    }

    /** Returns the position of this station, from {@link #positionProvider}. */
    public Position position() {
        return positionProvider.getLatestPosition().position();
    }

    /** Interface to lower layer (Ethernet/ITS-G5/802.11p, Link Layer) */
    private void sendToLowerLayer(ByteBuffer payload) throws IOException {
        linkLayer.send(payload.array());
    }

    /** Private interface to send to upper layer (BTP, Transport Layer).
     *
     * This function serves as a 'single point of contact' for GeonetStation functions that need to
     * send data to upper layer.
     *
     * Provides an ability to refactor queue solution to a reactive-style solution later.
     * */
    private void sendToUpperLayer(GeonetData indication) throws InterruptedException {
        queueUpward.put(indication);
        for (GeonetDataListener l : listeners) {
            l.onGeonetDataReceived(indication);
        }
    }

    /** Returns the next Geonetworking Packet addressed to this station.
     * This operation blocks until the next packet is received.
     * Service packets (Beacon, Location Service, forwarded packets) are omitted here.
     *
     * This is an interface to upper layer (BTP, Transport Layer). */
    public GeonetData receive() throws InterruptedException {
        return queueUpward.take();
    }

    @Override
    public void run() {
        while (true) {
            byte[] payload = new byte[] {};
            try {
                payload = linkLayer.receive();
            } catch (IOException e) {
                logger.error("Geonetworking station got an IO exception, shutting down", e);
                break;
            } catch (InterruptedException e) {
                logger.error("Geonetworking station got an InterruptedException in LinkLayer receive, shutting down", e);
                break;
            }
            try {
                onReceiveFromLowerLayer(payload);
            } catch (InterruptedException e) {
                logger.error("Geonetworking station got an InterruptedException in handling the received message, shutting down", e);
                break;
            }
        }
    }

    @Override
    public void close() {
        beaconService.stop();
        try {
            linkLayer.close();
        } catch (Exception e) {
            logger.error("Exception in LinkLayer close()", e);
        }
        scheduler.shutdownNow();
    }

    private class PacketId {
        private final Instant timestamp;
        private final Short sequenceNumber;
        private final Address sender;

        public PacketId(Instant timestamp, short sequenceNumber, Address sender) {
            this.timestamp = timestamp;
            this.sequenceNumber = sequenceNumber;
            this.sender = sender;
        }

        private GeonetStation getOuterType() {
            return GeonetStation.this;
        }

        @Override public int hashCode() {
            final int prime = 31;
            int result = 1;
            result = prime * result + getOuterType().hashCode();
            result = prime * result + ((sender == null) ? 0 : sender.hashCode());
            result = prime * result + ((sequenceNumber == null) ? 0 : sequenceNumber.hashCode());
            result = prime * result + ((timestamp == null) ? 0 : timestamp.hashCode());
            return result;
        }

        @Override public boolean equals(Object obj) {
            if (this == obj) return true;
            if (obj == null) return false;
            if (getClass() != obj.getClass()) return false;
            PacketId other = (PacketId) obj;
            if (!getOuterType().equals(other.getOuterType())) return false;
            if (sender == null) {
                if (other.sender != null) return false;
            } else if (!sender.equals(other.sender)) return false;
            if (sequenceNumber == null) {
                if (other.sequenceNumber != null) return false;
            } else if (!sequenceNumber.equals(other.sequenceNumber)) return false;
            if (timestamp == null) {
                if (other.timestamp != null) return false;
            } else if (!timestamp.equals(other.timestamp)) return false;
            return true;
        }
    }

    /**
     * Indicates if a packet is a duplicate.
     *
     *  Version 1.2.1 of standard describes actually if a packet is duplicate or a predecessor of
     *  another packet.
     *
     * Standard first considers timestamps. If the arrived timestamp is before the last seen
     * timestamp from the same address, the packet is a duplicate.
     *
     * Since timestamps has a resolution of one millisecond, there might be several packets sent
     * within one millisecond all with the same timestamp. In that case, sequence numbers are
     * compared.
     *
     * Standard's method drops packets if there are two parallel broadcasts from the same sender.
     * E.g. RoadWorks and SlipperyRoad, sent every second. RW is sent, then SR is sent, but SR was
     * forwarded better and is delivered before RW. Then when RW finally arrives later, it is
     * discarded.
     *
     * We don't want to remove predecessors, so:
     *  - use as a key in the last-seen-set (GN address + seq number + timestamp)
     *  - keep entries in last-seen-set for 105 minutes (time equal to BasicHeader.Lifetime.MaximumLifetime)
     *
     * Without timestamp, seq number wraps at 65535. Max lifetime is 105 minutes.
     * The rate needed to send 65535 packets in 105 minutes is about 1 packet in 95 milliseconds.
     * If there will be broadcasts or unicasts faster than that, seq number alone will not be enough,
     * so we add timestamp too. It's just a moderate computation and memory overhead, we'll keep
     * a lot of GN addresses and sequence numbers anyway.
     *
     */
    private boolean isDuplicate(GeonetData data, short sequenceNumber) {
        final PacketId packetId = new PacketId(data.sender.get().timestamp(), sequenceNumber,
                data.sender.get().address().get());
        return seenPackets.contains(packetId);
    }

    private void markAsSeen(GeonetData data, short sequenceNumber) {
        final PacketId packetId = new PacketId(data.sender.get().timestamp(), sequenceNumber,
                data.sender.get().address().get());
        boolean isDuplicate = seenPackets.contains(packetId);
        if (!isDuplicate) {
            seenPackets.add(packetId);
            long lifetimeMillis = (long) (1000 * data.destination.maxLifetimeSeconds().orElse(
                    BasicHeader.Lifetime.MAX_VALUE.asSeconds()));
            scheduler.schedule(
                new Runnable() { @Override public void run() { seenPackets.remove(packetId); } },
                lifetimeMillis, TimeUnit.MILLISECONDS
            );
        } else {
            logger.debug("Duplicate! From {}, sn {}", data.sender.get().address().get(), sequenceNumber);
        }
    }

    private interface BeaconService {
        void start();
        void stop();
        void skipNextBeacon();
    }

    private final BeaconService beaconService = new BeaconService () {

        private Instant nextBeaconTime = timeInstantNow();
        private boolean isActive = false;  // TODO: Handle synchronization properly.

        @Override public void start() { isActive = true; scheduleNextBeacon(); }

        @Override public void stop()  { isActive = false; }

        @Override public void skipNextBeacon() {
            nextBeaconTime = timeInstantNow().plusMillis(randomDelayMs());
        }

        private GeonetData beaconData() {
            Optional<TrafficClass> emptyTrafficClass = Optional.empty();    // #send will use default.
            Optional<LongPositionVector> emptyPosition = Optional.empty();  // #send fills in current.
            return new GeonetData(UpperProtocolType.ANY, Destination.beacon(),
                    emptyTrafficClass, emptyPosition, new byte[] {});
        }

        private void maybeSendBeacon() {
            if (!timeInstantNow().isBefore(nextBeaconTime)) {
                try {
                    logger.info("Sending beacon");
                    send(beaconData());
                } catch (IOException e) {
                    logger.error("Exception in sending beacon", e);
                }
            }
        }

        private void scheduleNextBeacon() {
            scheduler.schedule(
                new Runnable() { @Override public void run() {
                    if (isActive) { maybeSendBeacon(); scheduleNextBeacon(); } }; },
                randomDelayMs(),
                TimeUnit.MILLISECONDS);
        }

        private long randomDelayMs() {
            return config.getItsGnBeaconServiceRetransmitTimer() +
                    new Random().nextInt(config.getItsGnBeaconServiceMaxJitter());
        }
    };

    public void startBecon() {
        logger.info("Starting BEACON service");
        beaconService.start();
    }

} public interface LinkLayer extends AutoCloseable {
    public byte[] receive() throws IOException, InterruptedException;
    public void send(byte[] payload) throws IOException;
    public boolean hasEthernetHeader();
} public class LinkLayerUdpToEthernet implements LinkLayer, AutoCloseable {
    private final static Logger logger = LoggerFactory.getLogger(LinkLayerUdpToEthernet.class);

    final SocketAddress remoteAddress;
    final boolean hasEthernetHeader;

    final DatagramSocket socket;
    final int BUFFER_LENGTH = 65535;
    final byte[] buffer = new byte[BUFFER_LENGTH];
    final DatagramPacket receptionPacket = new DatagramPacket(buffer, BUFFER_LENGTH);

    private final static int ETHERTYPE_START_BIT = 12;

    public LinkLayerUdpToEthernet(int localPort, SocketAddress remoteAddress, boolean hasEthernetHeader)
            throws SocketException {
        this.socket = new DatagramSocket(localPort);
        this.remoteAddress = remoteAddress;
        this.hasEthernetHeader = hasEthernetHeader;
    }

    @Override
    public byte[] receive() throws IOException, SocketTimeoutException {
        socket.receive(receptionPacket);
        if (hasEthernetHeader) {
            short ethertype = ByteBuffer.wrap(buffer).getShort(ETHERTYPE_START_BIT);
            while (ethertype != GeonetStation.GN_ETHER_TYPE) {
                logger.debug("Ignoring packet with ethertype {}", ethertype);
                socket.receive(receptionPacket);
                ethertype = ByteBuffer.wrap(buffer).getShort(12);
            }
        }
        return Arrays.copyOfRange(receptionPacket.getData(), 0, receptionPacket.getLength());
    }

    @Override
    public void send(byte[] payload) throws IOException {
        socket.send(new DatagramPacket(payload, payload.length, remoteAddress));
    }

    @Override
    public void close() {
        socket.close();
    }

    @Override
    public boolean hasEthernetHeader() {
        return hasEthernetHeader;
    }

} public final class LongPositionVector {
	
	private final Optional<Address>  address;
    private final Instant            timestamp;
    private final Position           position;
    private final boolean            isPositionConfident;
    private final double             speedMetersPerSecond;
    private final double             headingDegreesFromNorth;

    /** Long Position Vector length in bytes. */
    public static final int LENGTH = 24;
    private final static double SPEED_STORE_SCALE   = 0.01;  // 0.01 meters per second.
    private final static double HEADING_STORE_SCALE = 0.1;   // 0.1 degrees from north.
    private static final long LEAP_SECONDS_SINCE_2004 = 4;  // Let's assume we're always in 2015.

  //private long taiMillisSince2004Mod32;
    public LongPositionVector(
            Address  address,
            Instant  timestamp,
            Position position,
            boolean  isPositionConfident,
            double   speedMetersPerSecond,
            double   headingDegreesFromNorth)
    {
        this(Optional.of(address),
            timestamp,
            position,
            isPositionConfident,
            speedMetersPerSecond,
            headingDegreesFromNorth);
    }

    public LongPositionVector(
            Optional<Address>  address,
            Instant  timestamp,
            Position position,
            boolean  isPositionConfident,
            double   speedMetersPerSecond,
            double   headingDegreesFromNorth)
    {
        this.address                 = address;
        this.timestamp               = timestamp;
        this.position                = position;
        this.isPositionConfident     = isPositionConfident;
        this.speedMetersPerSecond    = speedMetersPerSecond;
        this.headingDegreesFromNorth = headingDegreesFromNorth;
    }

    @Override
    public String toString() {
        return "LPV[" + (address.isPresent() ? address.get() : "") + " " + timestamp
                + " " + position + " PAI=" + isPositionConfident
                + ", " + speedMetersPerSecond + " m/s, bearing "
                + headingDegreesFromNorth + " degrees]";
    }

    

    public Optional<Address> address()                 { return address; }
    public Instant           timestamp()               { return timestamp; }
    public Position          position()                { return position; }
    public boolean           isPositionConfident()     { return isPositionConfident; }
    public double            speedMetersPerSecond()    { return speedMetersPerSecond; }
    public double            headingDegreesFromNorth() { return headingDegreesFromNorth; }
   
    private int speedAsStoreUnit(double speedMetersPerSecond) {
        return (int) Math.round(speedMetersPerSecond / SPEED_STORE_SCALE);
    }

    private short headingAsStoreUnit(double headingDegreesFromNorth) {
        return (short) Math.round(headingDegreesFromNorth / HEADING_STORE_SCALE);
    }

    public ByteBuffer putTo(ByteBuffer buffer) {
        if (address.isPresent()) {
            address.get().putTo(buffer);
        } else {
            throw new IllegalStateException("Address not initialized in Long Position Vector.");
        }
        buffer.putInt((int)instantToTaiMillisSince2004Mod32(timestamp));
        position.putTo(buffer);
        // Bit 15 is position accuracy indication isPositionAccurate
        // Bits 0-14 are speed. Negative numbers have all 1-s in the BIG-END, so if speed is
        // negative, we need to remove the 1 from the bit 15.
        short speedMask = 0b0111_1111_1111_1111;
        int speedRoundedCentimetersPerSecond = speedAsStoreUnit(speedMetersPerSecond);
        if (Math.abs(speedRoundedCentimetersPerSecond) >= Math.pow(2, 15)) {
            throw new IllegalStateException("Speed is too high and requires longer that 14 bits (" +
                    speedMetersPerSecond  + " m/s, max is " + ((Math.pow(2, 15) - 1) * 0.01) + ")");
        }
        short confidenceAndSpeed = (short) (( (isPositionConfident ? 1 : 0) << 15 ) |
                (speedMask & (short) speedRoundedCentimetersPerSecond));
        buffer.putShort(confidenceAndSpeed);
        buffer.putShort(headingAsStoreUnit(headingDegreesFromNorth));
        return buffer;
    }

    public static LongPositionVector getFrom(ByteBuffer buffer) {
        Address  address         = Address.getFrom(buffer);
        Instant  timestamp       = millisMod32ToInstant(buffer.getInt());
        Position position        = Position.getFrom(buffer);
        short confidenceAndSpeed = buffer.getShort();
        // Bit 15 is a position accuracy indicator.
        boolean  isPositionConfident = ((confidenceAndSpeed & 0xFFFF) >> 15) == 1;  // & 0xFFFF to deal with signed short to int without sign.
        // Bits 0-14 are signed units of speed, in 0.01 meters per second.
        short speedMask     = 0b0111_1111_1111_1111;
        short speed15bit    = (short) (confidenceAndSpeed & speedMask);
        // Since speed is encoded in two-complement, the last bit (14) can be used as a sign bit.
        short speedSignMask = 0b0100_0000_0000_0000;
        boolean isNegativeSpeed = (speed15bit & speedSignMask) != 0;
        // Positive: shortened 15-bit and normal 16-bit integers are the same.
        // Negative: shortened 15-bit and normal 16-bit integers are different only in the last bit.
        short speed = (short) (speed15bit | (isNegativeSpeed ? 1<<15 : 0));
        // Speed was encoded as 0.01 meters per second.
        double   speedMetersPerSecond = speed * SPEED_STORE_SCALE;
        // Heading was encoded as an unsigned units of 0.1 degree from North.
        double   headingDegreesFromNorth = buffer.getShort() * HEADING_STORE_SCALE;
        return new LongPositionVector(
            address,
            timestamp,
            position,
            isPositionConfident,
            speedMetersPerSecond,
            headingDegreesFromNorth
            );
    }

    
    /** Returns TAI milliseconds mod 2^32 for the given date.
     *
     * Since java int is signed 32 bit integer, return long instead.
     * It is the same on byte level, but just to avoid confusing people with negative values here.
     *
     *
     * From http://stjarnhimlen.se/comp/time.html:
     *
     * TAI (Temps Atomique International or International Atomic Time) is
     * defined as the weighted average of the time kept by about 200
     * atomic clocks in over 50 national laboratories worldwide.
     * TAI-UT1 was approximately 0 on 1958 Jan 1.
     * (TAI is ahead of UTC by 35 seconds as of 2014.)
     *
     * GPS time = TAI - 19 seconds.  GPS time matched UTC from 1980-01-01
     * to 1981-07-01.  No leap seconds are inserted into GPS time, thus
     * GPS time is 13 seconds ahead of UTC on 2000-01-01.  The GPS epoch
     * is 00:00 (midnight) UTC on 1980-01-06.
     * The difference between GPS Time and UTC changes in increments of
     * seconds each time a leap second is added to UTC time scale.
     */
    public static long instantToTaiMillisSince2004Mod32(Instant instantX) {
        OffsetDateTime gnEpochStart =
                OffsetDateTime.of(LocalDateTime.of(2004, Month.JANUARY, 1, 0, 0), ZoneOffset.UTC);
        long millis2004 = gnEpochStart.toInstant().toEpochMilli();
        long millisAtX = instantX.toEpochMilli();
        long taiMillis = (millisAtX + LEAP_SECONDS_SINCE_2004*1000) - millis2004;
        return taiMillis % (1L << 32);
    }

    /** Returns the nearest to now instant that will have given amount of TAI millis since 2004. */
    public static Instant millisMod32ToInstant(int intMillisX) {
        long millisX = Long.parseLong(Integer.toBinaryString(intMillisX), 2);  // unsigned int...
        Instant now = Instant.now();
        long millisNow = instantToTaiMillisSince2004Mod32(now);
        long delta = millisNow - millisX;
        // Small positive delta is what we expect.
        // Small negative delta is fine too, it would mean that the packet came from the future,
        // which can be explained by our clock being a little behind.
        // Huge negative delta is from previous mod32, and should be changed to small positive.
        // Huge positive delta might come from a packet a little from the future and next mod32,
        // we want instead a small negative delta.
        if (delta < -(1L << 31)) { delta += (1L << 32); }
        if (delta >  (1L << 31)) { delta -= (1L << 32); }
        Instant instantX = now.minusMillis(delta);
        return instantX;
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + ((address == null) ? 0 : address.hashCode());
        result = prime * result + headingAsStoreUnit(headingDegreesFromNorth);
        result = prime * result + (isPositionConfident ? 1231 : 1237);
        result = prime * result + ((position == null) ? 0 : position.hashCode());
        result = prime * result + speedAsStoreUnit(speedMetersPerSecond);
        result = prime * result + ((timestamp == null) ? 0 : timestamp.hashCode());
        return result;
    }
    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getClass() != obj.getClass())
            return false;
        LongPositionVector other = (LongPositionVector) obj;
        if (address == null) {
            if (other.address != null)
                return false;
        } else if (!address.equals(other.address))
            return false;
        if (headingAsStoreUnit(headingDegreesFromNorth) != headingAsStoreUnit(other.headingDegreesFromNorth))
            return false;
        if (isPositionConfident != other.isPositionConfident)
            return false;
        if (position == null) {
            if (other.position != null)
                return false;
        } else if (!position.equals(other.position))
            return false;
        if (speedAsStoreUnit(speedMetersPerSecond) != speedAsStoreUnit(other.speedMetersPerSecond))
            return false;
        if (timestamp == null) {
            if (other.timestamp != null)
                return false;
        } else if (!timestamp.equals(other.timestamp))
            return false;
        return true;
    }

    public LongPositionVector withAddress(Address address) {
        return new LongPositionVector(
                address,
                timestamp,
                position,
                isPositionConfident,
                speedMetersPerSecond,
                headingDegreesFromNorth
                );
    }
} public final class MacAddress {
    
	private final long address;

    public MacAddress(long address) {
        if ((address & 0xffff_0000_0000_0000L) != 0) {
            throw new IllegalArgumentException("address is longer than 6 bytes: " + address);
        }
        this.address = address;
    }

    public MacAddress(String str) {
        this(parseFromString(str));
    }

	@Override public String toString() {
        return "MacAddress[" + address + "]";
    }

    
    public long value() { return address; }

    /** Returns 6-bytes array representing this address. */
    public byte[] asBytes() {
        return Arrays.copyOfRange(ByteBuffer.allocate(8).putLong(address).array(), 2, 8);
    }

    public static MacAddress fromBytes(byte[] bytes) {
        if (bytes.length > 6) {
            throw new IllegalArgumentException("Address is too long: " + bytes.length + "bytes");
        }
        return new MacAddress(new BigInteger(+1, bytes).longValue());
    }

    @Override public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + (int) (address ^ (address >>> 32));
        return result;
    }

    public static long parseFromString(String s) {
        String[] bytes = s.split(":");
        if (bytes.length != 6) {
            throw new IllegalArgumentException("Malformed MAC, expected xx:xx:xx:xx:xx:xx, got " + s);
        }
        return new BigInteger(s.replace(":", ""), 16).longValue();
    }

    @Override public boolean equals(Object obj) {
        if (this == obj) return true;
        if (obj == null) return false;
        if (getClass() != obj.getClass()) return false;
        MacAddress other = (MacAddress) obj;
        if (address != other.address) return false;
        return true;
    }
} public class Optional<T> {

    private final T       element;
    private final boolean isPresent;

    private Optional(T element, boolean isPresent) {
        this.element = element;
        this.isPresent = isPresent;
    }

    /** @return true if the Option contains a value */
    public boolean isPresent() {
        return isPresent;
    }

    /** @return the element if the option is not empty
     * @throws java.util.NoSuchElementException if the option is empty */
    public T get() {
        if (isPresent) {
            return element;
        } else {
            throw new NoSuchElementException("None.get");
        }
    }

    /** @return the value, if present, otherwise return {@code other}
     * @param other the value to be returned if there is no value present */
    public T orElse(T other) {
        return isPresent() ? get() : other;
    }

    /**
     * Indicates whether some other object is "equal to" this Optional. The
     * other object is considered equal if:
     * <ul>
     * <li>it is also an {@code Optional} and;
     * <li>both instances have no value present or;
     * <li>the present values are "equal to" each other via {@code equals()}.
     * </ul>
     *
     * @param obj an object to be tested for equality
     * @return {code true} if the other object is "equal to" this object
     * otherwise {@code false}
     */
    @Override
    public boolean equals(Object obj) {
        if (this == obj) {
            return true;
        }

        if (!(obj instanceof Optional)) {
            return false;
        }

        Optional<?> other = (Optional<?>) obj;
        return Objects.equals(element, other.element);
    }

    /**
     * Returns the hash code value of the present value, if any, or 0 (zero) if
     * no value is present.
     *
     * @return hash code value of the present value or 0 if no value is present
     */
    @Override
    public int hashCode() {
        return Objects.hashCode(element);
    }

    /** Returns an Option containing the value.
     *
     * @param <A> the type of the value
     * @param element contained value
     * @return a new Option that contains the value */
    public static <A> Optional<A> of(final A element) {
        return new Optional<A>(element, true);
    }

    /** Returns an empty option.
     *
     * @param <A>
     * @return an empty Option */
    public static <A> Optional<A> empty() {
        return new Optional<A>(null, false);
    }

    @Override public String toString() {
        return isPresent ? "Optional(" + element + ")" : "Optional.empty";
    }

} public final class Position {
	
	final static double MICRODEGREE = 1E-6;
	final static double STORE_UNIT = 0.1 * MICRODEGREE;
	final static double earthRadius = 6371000; // In meters. In miles: 3958.75;
	public static final int LENGTH = 2*4;  // Two 32-bit integers.

	private final double lattitudeDegrees;
	private final double longitudeDegrees;

	public Position(double lattitudeDegrees, double longitudeDegrees) {
	    this.lattitudeDegrees = lattitudeDegrees;
	    this.longitudeDegrees = longitudeDegrees;
	}
	
	@Override
    public String toString() {
        return "Pos(" + lattitudeDegrees + ", " + longitudeDegrees + ")";
    }
    
    public double lattitudeDegrees() { return lattitudeDegrees; }
    public double longitudeDegrees() { return longitudeDegrees; }

    private static double fromStoreUnit(int latOrLong) {
        return latOrLong * STORE_UNIT;
    }

    private static int asStoreUnit(double latOrLong) {
        return (int)Math.round(latOrLong / STORE_UNIT);
    }

    public ByteBuffer putTo(ByteBuffer buffer) {
        buffer.putInt(asStoreUnit(lattitudeDegrees));
        buffer.putInt(asStoreUnit(longitudeDegrees));
        return buffer;
    }

    public static Position getFrom(ByteBuffer buffer) {
        return new Position(fromStoreUnit(buffer.getInt()), fromStoreUnit(buffer.getInt()));
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + asStoreUnit(lattitudeDegrees);
        result = prime * result + asStoreUnit(longitudeDegrees);
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getClass() != obj.getClass())
            return false;
        Position other = (Position) obj;
        if (asStoreUnit(lattitudeDegrees) != asStoreUnit(other.lattitudeDegrees))
            return false;
        if (asStoreUnit(longitudeDegrees) != asStoreUnit(other.longitudeDegrees))
            return false;
        return true;
    }

    public double distanceInMetersTo(Position other) {
        return distanceMeters(this, other);
    }

    public double bearingInDegreesTowards(Position other) {
        return bearingDegrees(this, other);
    }

    /** Returns the destination point from 'this' point having traveled the given distance on the given initial bearing (bearing normally varies around path followed).
     *
     * <p>
     * Taken from <a href="http://www.movable-type.co.uk/scripts/latlong.html">http://www.movable-type.co.uk/scripts/latlong.html</a>
     *
     * @param distance Distance traveled, in meters
     * @param bearing  Initial bearing in degrees from north
     * @return Destination point
     */
    public Position moved(double distance, double bearing) {
        double angDistRad = distance / earthRadius;  // Angular distance in radians.
        double bearingRad = Math.toRadians(bearing);
        double latRad1 = Math.toRadians(this.lattitudeDegrees);
        double lonRad1 = Math.toRadians(this.longitudeDegrees);
        double latRad2 = asin(
                sin(latRad1) * cos(angDistRad) +
                cos(latRad1) * sin(angDistRad) * cos(bearingRad));
        double lonRad2 = lonRad1 + Math.atan2(sin(bearingRad) * sin(angDistRad) * cos(latRad1),
                        cos(angDistRad) - sin(latRad1) * sin(latRad2));
        lonRad2 = (lonRad2 + 3 * Math.PI) % (2 * Math.PI) - Math.PI; // normalise to -180..+180°

        return new Position(Math.toDegrees(latRad2), Math.toDegrees(lonRad2));
    }

    public static double distanceMeters(Position pos1, Position pos2) {
        return distanceMeters(
                pos1.lattitudeDegrees(), pos1.longitudeDegrees(),
                pos2.lattitudeDegrees(), pos2.longitudeDegrees());
    }

    public static double distanceMeters(double lat1, double lng1, double lat2, double lng2) {
        double dLat = Math.toRadians(lat2-lat1);
        double dLng = Math.toRadians(lng2-lng1);
        double a = Math.pow(Math.sin(dLat / 2), 2) +
                Math.pow(Math.sin(dLng / 2), 2) *
                Math.cos(Math.toRadians(lat1)) * Math.cos(Math.toRadians(lat2));
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
        double dist = earthRadius * c;

        return dist;
    }

    public static double bearingDegrees(Position pos1, Position pos2) {
        return bearingDegrees(
                pos1.lattitudeDegrees(), pos1.longitudeDegrees(),
                pos2.lattitudeDegrees(), pos2.longitudeDegrees());
    }

    public static double bearingDegrees(double lat1, double lng1, double lat2, double lng2){
        double latRad1 = Math.toRadians(lat1);
        double latRad2 = Math.toRadians(lat2);
        double lngDiffRad = Math.toRadians(lng2-lng1);
        double y = Math.sin(lngDiffRad)*Math.cos(latRad2);
        double x = Math.cos(latRad1) * Math.sin(latRad2) -
                Math.sin(latRad1) * Math.cos(latRad2) * Math.cos(lngDiffRad);

        return (Math.toDegrees(Math.atan2(y, x)) + 360) % 360;
    }
} public interface PositionProvider {
    LongPositionVector getLatestPosition();
} public class StationConfig {

    private static  final byte ZERO_BYTE = 0x00;

    private   long itsGnLoacalGnAddr;

    /** Version of the GeoNetworking protocol set in the GeoNetworking protocol headers. */
    private   int itsGnProtocolVersion;  // Version for EN 302 636-4-1 (V1.2.1).

    /** Default packet lifetime in seconds. */
    private  int itsGnDefaultPacketLifetime;

    /** Default hop limit indicating the maximum number of hops a packet travels. */
    private  int itsGnDefaultHopLimit;

    /** Traffic class that represents Facility-layer requirements on packet transport.
     *  Here - Forwarding: Default traffic class */
    private  byte itsGnDefaultTrafficClass;

    /** Indicates whether ITS-S is stationary or mobile. Stationary = 0, Mobile = 1. */
    private  int itsGnIsMobile = 1;

    /** Beacon service: Duration of Beacon retransmit timer in ms. */
    private  int itsGnBeaconServiceRetransmitTimer;

    /** Beacon service: Maximum Beacon Jitter in ms. */
    private  int itsGnBeaconServiceMaxJitter;

    /** Default theoretical maximum communication range in meters. */
    private  int itsGnDefaultMaxCommunicationRange;

    /** Minimum duration a GeoBroadcast packet shall be buffered in the CBF packet buffer in ms. */
    private  int itsGnGeoBroadcastCbfMinTime;

    /** Maximum duration a GeoBroadcast packet shall be buffered in the CBF packet buffer in ms. */
    private  int itsGnGeoBroadcastCbfMaxTime;

    /** Lifetime of location table entry in seconds */
    private  int itsGnLifetimeLocTE;

    /** Default GeoBroadcast forwarding algorithm: 0 unspecified, 1 simple, 2 cbf, 3 advanced. */
    private  int itsGnGeoBroadcastForwardingAlgorithm;

    public long getItsGnLoacalGnAddr() {
        return itsGnLoacalGnAddr;
    }

    public StationConfig() {
        setItsGnLocalGnAddr(Address.random(false, StationType.Passenger_Car, 752).value());
        setItsGnProtocolVersion(0);
        setItsGnDefaultPacketLifetime(60);
        setItsGnDefaultHopLimit(10);
        setItsGnDefaultTrafficClass(ZERO_BYTE);
        setItsGnIsMobile(1);
        setItsGnBeaconServiceRetransmitTimer(3000);
        setItsGnBeaconServiceMaxJitter(getItsGnBeaconServiceRetransmitTimer()/4);
        setItsGnDefaultMaxCommunicationRange(1000);
        setItsGnGeoBroadcastCbfMinTime(1);
        setItsGnGeoBroadcastCbfMaxTime(100);
        setItsGnLifetimeLocTE(20);
        setItsGnGeoBroadcastForwardingAlgorithm(0);
    }



    public void setItsGnLocalGnAddr(long itsGnLoacalGnAddr) {
        this.itsGnLoacalGnAddr = itsGnLoacalGnAddr;
    }

    public int getItsGnProtocolVersion() {
        return itsGnProtocolVersion;
    }

    public void setItsGnProtocolVersion(int itsGnProtocolVersion) {
        this.itsGnProtocolVersion = itsGnProtocolVersion;
    }

    public int getItsGnDefaultPacketLifetime() {
        return itsGnDefaultPacketLifetime;
    }

    public void setItsGnDefaultPacketLifetime(int itsGnDefaultPacketLifetime) {
        this.itsGnDefaultPacketLifetime = itsGnDefaultPacketLifetime;
    }

    public int getItsGnDefaultHopLimit() {
        return itsGnDefaultHopLimit;
    }

    public void setItsGnDefaultHopLimit(int itsGnDefaultHopLimit) {
        this.itsGnDefaultHopLimit = itsGnDefaultHopLimit;
    }

    public byte getItsGnDefaultTrafficClass() {
        return itsGnDefaultTrafficClass;
    }

    public void setItsGnDefaultTrafficClass(byte itsGnDefaultTrafficClass) {
        this.itsGnDefaultTrafficClass = itsGnDefaultTrafficClass;
    }

    public int getItsGnIsMobile() {
        return itsGnIsMobile;
    }

    public void setItsGnIsMobile(int itsGnIsMobile) {
        this.itsGnIsMobile = itsGnIsMobile;
    }

    public int getItsGnBeaconServiceRetransmitTimer() {
        return itsGnBeaconServiceRetransmitTimer;
    }

    public void setItsGnBeaconServiceRetransmitTimer(int itsGnBeaconServiceRetransmitTimer) {
        this.itsGnBeaconServiceRetransmitTimer = itsGnBeaconServiceRetransmitTimer;
    }

    public int getItsGnBeaconServiceMaxJitter() {
        return itsGnBeaconServiceMaxJitter;
    }

    public void setItsGnBeaconServiceMaxJitter(int itsGnBeaconServiceMaxJitter) {
        this.itsGnBeaconServiceMaxJitter = itsGnBeaconServiceMaxJitter;
    }

    public int getItsGnDefaultMaxCommunicationRange() {
        return itsGnDefaultMaxCommunicationRange;
    }

    public void setItsGnDefaultMaxCommunicationRange(int itsGnDefaultMaxCommunicationRange) {
        this.itsGnDefaultMaxCommunicationRange = itsGnDefaultMaxCommunicationRange;
    }

    public int getItsGnGeoBroadcastCbfMinTime() {
        return itsGnGeoBroadcastCbfMinTime;
    }

    public void setItsGnGeoBroadcastCbfMinTime(int itsGnGeoBroadcastCbfMinTime) {
        this.itsGnGeoBroadcastCbfMinTime = itsGnGeoBroadcastCbfMinTime;
    }

    public int getItsGnGeoBroadcastCbfMaxTime() {
        return itsGnGeoBroadcastCbfMaxTime;
    }

    public void setItsGnGeoBroadcastCbfMaxTime(int itsGnGeoBroadcastCbfMaxTime) {
        this.itsGnGeoBroadcastCbfMaxTime = itsGnGeoBroadcastCbfMaxTime;
    }

    public int getItsGnLifetimeLocTE() {
        return itsGnLifetimeLocTE;
    }

    public void setItsGnLifetimeLocTE(int itsGnLifetimeLocTE) {
        this.itsGnLifetimeLocTE = itsGnLifetimeLocTE;
    }

    public int getItsGnGeoBroadcastForwardingAlgorithm() {
        return itsGnGeoBroadcastForwardingAlgorithm;
    }

    public void setItsGnGeoBroadcastForwardingAlgorithm(int itsGnGeoBroadcastForwardingAlgorithm) {
        this.itsGnGeoBroadcastForwardingAlgorithm = itsGnGeoBroadcastForwardingAlgorithm;
    }
} public final class TrafficClass {

    private final byte code;
    
    public TrafficClass(byte code) {
        this.code = code;
    }
    
    @Override
    public String toString() {
        return "TrafficClass [0x" + Integer.toHexString(code) + "]";
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + code;
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getClass() != obj.getClass())
            return false;
        TrafficClass other = (TrafficClass) obj;
        if (code != other.code)
            return false;
        return true;
    }

    

    public byte asByte() {
        return code;
    }

    public static TrafficClass fromByte(byte code) {
        return new TrafficClass(code);
    }

} public enum UpperProtocolType {
    ANY     (0),
    BTP_A   (1),  // Carries the source and the destination port ("Don't use it" - ETSI Plugtest).
    BTP_B   (2),  // Carries the destination port, but no source port.
    IPv6    (3);
    private final int value;
    private UpperProtocolType(int value) { this.value = value; }
    public  int value()           { return value;       }

    public static UpperProtocolType fromCode(int code) {
        for (UpperProtocolType h: UpperProtocolType.values()) { if (h.value() == code) { return h; } }
        throw new IllegalArgumentException("Can't recognize upper protocol: " + code);
    }
} public class GpsdClient implements PositionProvider, AutoCloseable,TelnetNotificationHandler,Runnable {
    private final static Logger logger = LoggerFactory.getLogger(GpsdClient.class);

    private TPV                   lastSeenTPV = null;
    private final Gson            gson        = new GsonBuilder().create();
    private final ExecutorService executor    = Executors.newSingleThreadExecutor();
    private final InetSocketAddress gpsdAddress;
    private Future<?>       runner;

    private static TelnetClient tc = null;
    
    public GpsdClient(InetSocketAddress address) throws IOException {
        logger.info("Starting GPSd client");
        gpsdAddress = address;
    }

    @Override
    public void run(){

        tc = new TelnetClient();

        TerminalTypeOptionHandler ttopt = new TerminalTypeOptionHandler("VT100", false, false, true, false);
        EchoOptionHandler echoopt = new EchoOptionHandler(true, false, true, false);
        SuppressGAOptionHandler gaopt = new SuppressGAOptionHandler(true, true, true, true);
        try{
            tc.addOptionHandler(ttopt);
            tc.addOptionHandler(echoopt);
            tc.addOptionHandler(gaopt);
        }
        catch (InvalidTelnetOptionException e){
            logger.error("GpsdClient:Error registering option handlers: " + e.getMessage());
        } catch (IOException e) {
            logger.error("IO Error : " + e.getMessage(), e);
		}

        while (true){
            try{
                tc.connect(gpsdAddress.getAddress(), gpsdAddress.getPort());
                tc.registerNotifHandler(this);

                createReader();

                OutputStream outstr = tc.getOutputStream();

                Thread.sleep(1000);//Wait till ready

                String line = "?WATCH={\"enable\":true,\"json\":true}\r\n";
                byte[] buff = line.getBytes();
                outstr.write(buff, 0 , buff.length);
                outstr.flush();

                //Wait till finished or error occured
                runner.get();

            }catch (IOException | InterruptedException | ExecutionException e){
            	//System.err.println("Exception while connecting:" + e.getMessage());

            	//PANIC?
            	logger.error("GpsdClient:Exception while connecting:" + e.getMessage());

            	//return;//set failsafe
            	//System.exit(1);
            }
        }
    }

    public void onMsgReceived(String line){
    	//System.err.println(line);
    	try{
	    	if (line.equals(""))
	            return;
	        if (line.startsWith("{\"class\":\"TPV\"")) {
	            TPV msg = gson.fromJson(line, TPV.class);
	            logger.debug("tpv update:"+msg.time()+": ("+msg.lat()+","+msg.lon()+")");
	            lastSeenTPV = msg;
	        } else if (line.startsWith("{\"class\":\"SKY\"")) {
	            //@SuppressWarnings("unused")

	            //SKY msg = gson.fromJson(line, SKY.class);  // Ignore.
	        } else if(line.startsWith("{\"class\":\"DEVICES\"")){
	        	//DEVICES
	    	} else if(line.startsWith("{\"class\":\"DEVICE\"")){
	    		//DEVICE (activated)
	    	}else if(line.startsWith("{\"class\":\"VERSION\"")){
	    		//VERSION INFO
	    	}else{
	        	logger.debug("??:'"+line+"'");
	        }
    	}catch(com.google.gson.JsonSyntaxException j){
    		//Parsing error
    		logger.debug("Json parsing error :"+j.getMessage()+" :: "+line);//probably partial message
    	}catch(NumberFormatException n){
    		//ignore for printing
    		logger.debug("Json parsing error (NumberFormatException) :"+n.getMessage()+" :: "+line);
    	}catch(Exception e){
    		logger.info("Error " + e.getMessage(), e);
    		return;
    	}
    }

    private Future<?> createReader(){
    	 runner = executor.submit(new Runnable() {
             @Override public void run() {
            	final int buffsize = 4096;
 	            InputStream instr = tc.getInputStream();
 	            BufferedInputStream bf = new BufferedInputStream(instr);
 	            BufferedReader reader = new BufferedReader(
 	        	        new InputStreamReader(bf, StandardCharsets.UTF_8));

 	            try{
 	                byte[] buff = new byte[buffsize];
 	                int ret_read = 0;

 	                do{
 	                   String line = reader.readLine();
 	                   if(!line.isEmpty()){
 	                	  onMsgReceived(line);
 	                   }
 	                } while (ret_read >= 0);
 	            }catch (IOException e){
 	            	logger.error("Exception while reading socket:" + e.getMessage());
 	            }
 	            //STOP
 	            try{
 	                tc.disconnect();
 	                //STOPPING
 	            }
 	            catch (IOException e){
 	            	logger.error("Exception while closing telnet:" + e.getMessage());
 	            }
             }
         });
    	 return runner;
    }

    
    @Override
    public LongPositionVector getLatestPosition() {
        Optional<Address> emptyAddress = Optional.empty();
        if (lastSeenTPV == null) {
            Instant timestamp = Instant.now();
            Position position = new Position(Double.NaN, Double.NaN);  // NaN or 0?
            boolean isPositionConfident = false;
            double speedMetersPerSecond = 0;
            double headingDegreesFromNorth = 0;
            return new LongPositionVector(emptyAddress, timestamp, position, isPositionConfident,
                    speedMetersPerSecond, headingDegreesFromNorth);
        } else {
            final TPV tpv = lastSeenTPV;  // Is this enough to ensure that tpv will remain the same
                                          // through the rest of the method?
            Instant timestamp = OffsetDateTime.parse(tpv.time()).toInstant();
            Position position = new Position(tpv.lat(), tpv.lon());
            boolean isPositionConfident = false;  // TODO: double-check conditions for PAI=true.
            double speedMetersPerSecond = tpv.speed();
            double headingDegreesFromNorth = tpv.track();
            return new LongPositionVector(emptyAddress, timestamp, position, isPositionConfident,
                    speedMetersPerSecond, headingDegreesFromNorth);
        }
    }

    @Override
    public void close() throws IOException {
    	//socket.close
    	try{
            tc.disconnect();
        }
        catch (IOException e){
        	logger.error("Exception while closing telnet:" + e.getMessage());
        }

        runner.cancel(true);
        executor.shutdownNow();
    }

    public GpsdClient startClient(){
    	(new Thread(this)).start();
    	return this;
    }

    // Just a test main method.
    // java -cp ... net.gcdc.geonetworking.gpsdclient.GpsdClient 212.234.160.4:1945
    public static void main(String[] args) {
        final String usage = "Usage: java ... net.gcdc.geonetworking.gpsdclient.GpsdClient host:port";
        if (args.length != 1) {
            System.err.println(usage);
            System.exit(1);
        }

        String[] hostAndPort = args[0].split(":");
        if (hostAndPort.length != 2) {
            System.err.println(usage);
            System.exit(1);
        }
        System.out.println("GPS: "+hostAndPort[0]+":"+hostAndPort[1]);
        InetSocketAddress remoteAddress =
                new InetSocketAddress(hostAndPort[0], Integer.parseInt(hostAndPort[1]));

        try (GpsdClient client = new GpsdClient(remoteAddress)) {
        	client.startClient();
        	while(true) {
                Thread.sleep(1000);
            }
        } catch (IOException | InterruptedException e) {
            // TODO Auto-generated catch block
           logger.info("Exception :"+e.getMessage(), e);
        }
        System.err.println("Stopping GpsdClient..");
    }

	@Override
	public void receivedNegotiation(int arg0, int arg1) {
		// TODO Auto-generated method stub

	}
}
import java.io.IOException;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetSocketAddress;
import java.net.SocketAddress;
import java.net.SocketException;
import java.util.Arrays;
import java.util.logging.Level;
import net.gcdc.geonetworking.Address;
import net.gcdc.geonetworking.BtpPacket;
import net.gcdc.geonetworking.BtpSocket;
import net.gcdc.geonetworking.Destination;
import net.gcdc.geonetworking.GeonetData;
import net.gcdc.geonetworking.GeonetStation;
import net.gcdc.geonetworking.LinkLayer;
import net.gcdc.geonetworking.LinkLayerUdpToEthernet;
import net.gcdc.geonetworking.LongPositionVector;
import net.gcdc.geonetworking.MacAddress;
import net.gcdc.geonetworking.Optional;
import net.gcdc.geonetworking.Position;
import net.gcdc.geonetworking.PositionProvider;
import net.gcdc.geonetworking.StationConfig;
import net.gcdc.geonetworking.TrafficClass;
import net.gcdc.geonetworking.UpperProtocolType;
import net.gcdc.geonetworking.gpsdclient.GpsdClient;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import org.threeten.bp.Instant;
package net.gcdc;




public final  class BtpUdpClient {

    private final static Logger logger = LoggerFactory.getLogger(BtpUdpClient.class);

    private final static String usage =
            "Usage: java -cp gn.jar BtpClient --local-udp2eth-port <local-port> --remote-udp2eth-address <udp-to-ethernet-remote-host-and-port> --local-data-port <port> --remote-data-address <host:port> --gpsd-server <host:port> --mac-address <xx:xx:xx:xx:xx:xx>" + "\n" +
    "BTP ports: 2001 (CAM), 2002 (DENM), 2003 (MAP), 2004 (SPAT).";
    
    private BtpUdpClient(){}

    public static void main(String[] args) throws IOException {
        if (args.length < 7) {
            System.err.println(usage);
            System.exit(1);
        }

        int localUdp2EthPort = 0;
        InetSocketAddress remoteUdp2EthAddress = null;
        int localDataPort = 0;
        InetSocketAddress remoteDataAddress = null;
        int localDataCamPort = 0;
        InetSocketAddress remoteDataCamAddress = null;
        int localDataIclcmPort = 0;
        InetSocketAddress remoteDataIclcmAddress = null;
        boolean hasEthernetHeader = false;
        PositionProvider positionProvider = null;
        short btpDestinationPort = (short) 2001;  // CAM
        hasEthernetHeader = true;
        MacAddress macAddress = new MacAddress(0);