package net.gcdc.geonetworking;
import java.nio.ByteBuffer;
import org.threeten.bp.Instant;
import org.threeten.bp.LocalDateTime;
import org.threeten.bp.Month;
import org.threeten.bp.OffsetDateTime;
import org.threeten.bp.ZoneOffset;
/**
* Long Position Vector containing detailed station information.
*
* Usually used to describe sender position.
*
* Long Position Vector contains Geonetworking Address, Timestamp, Position, PAI, Speed and Heading.
*
* Timestamp.
* time in milliseconds at which the latitude and longitude of the ITS-S were
* acquired
* by the GeoAdhoc router. The time is encoded as:
* TST = TST(TAI) mod 2^32
* where TST(TAI) is the number of elapsed TAI milliseconds since 2004-01-01
* 00:00:00.000 UTC.
* http://leapsecond.com/java/gpsclock.htm
* TAI just adds 35 leap seconds to UTC.
* 2^32 milliseconds is about 49 days.
*
* PAI.
* Position accuracy indicator of the GeoAdhoc router reference position.
* Set to 1 if the semiMajorConfidence of the PosConfidenceEllipse as specified
* in ETSI TS 102 894-2 [i.7] is smaller than the GN protocol constant
* itsGnPaiInterval / 2.
* Set to 0 otherwise.
*
* Speed.
* Later it has to be encoded as a 15 bit signed integer, where each unit is 0.01 m/s,
* which gives maximum speed of (2^15 - 1) * 0.01 = 327.67 m/s = 1179.61 km/h.
*
* Heading.
* Later it has to be encoded as an unsigned units of 0.1 degree from North.
*/
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
);
}
}