package au.gov.amsa.navigation;
import static com.google.common.base.Optional.of;
import static java.lang.Math.toRadians;
import java.util.concurrent.atomic.AtomicLong;
import com.github.davidmoten.grumpy.core.Position;
import com.google.common.base.Optional;
import com.google.common.base.Preconditions;
import au.gov.amsa.risky.format.AisClass;
import au.gov.amsa.risky.format.Fix;
import au.gov.amsa.risky.format.HasFix;
public class VesselPosition implements HasFix {
public static boolean validate = true;
public enum NavigationalStatus {
// order of these should reflect numerical order in nav status int
// returned from ITU standard ais position report A
UNDER_WAY_USING_ENGINE, AT_ANCHOR, NOT_UNDER_COMMAND, RESTRICTED_MANOEUVRABILITY, CONSTRAINED_BY_HER_DRAUGHT, MOORED, AGROUND, ENGAGED_IN_FISHING, UNDER_WAY, RESERVED_1, RESERVED_2, FUTURE_1, FUTURE_2, FUTURE_3, AIS_SART, NOT_DEFINED;
}
private static final double EARTH_RADIUS_KM = 6378.1;
private static final int maxDimensionMetresWhenUnknown = 30;
private final double lat;
private final double lon;
private final Optional<Integer> lengthMetres;
private final Optional<Integer> widthMetres;
private final Optional<Double> cogDegrees;
private final Optional<Double> headingDegrees;
private final Optional<Double> speedMetresPerSecond;
private final Optional<String> positionAisNmea;
private final Optional<String> shipStaticAisNmea;
private final NavigationalStatus navigationalStatus;
private final long time;
private final Identifier id;
private final VesselClass cls;
private final Optional<Integer> shipType;
private static AtomicLong counter = new AtomicLong();
private final long messageId;
private final Optional<?> data;
private VesselPosition(long messageId, Identifier id, double lat, double lon,
Optional<Integer> lengthMetres, Optional<Integer> widthMetres, Optional<Double> cog,
Optional<Double> heading, Optional<Double> speedMetresPerSecond, VesselClass cls,
NavigationalStatus navigationalStatus, long time, Optional<Integer> shipType,
Optional<String> positionAisNmea, Optional<String> shipStaticAisNmea,
Optional<?> data) {
if (validate) {
Preconditions.checkArgument(lat >= -90 && lat <= 90);
Preconditions.checkArgument(lon >= -180 && lon <= 180);
Preconditions.checkNotNull(id);
Preconditions.checkNotNull(lengthMetres);
Preconditions.checkNotNull(widthMetres);
Preconditions.checkNotNull(shipType);
Preconditions.checkNotNull(positionAisNmea);
Preconditions.checkNotNull(shipStaticAisNmea);
Preconditions.checkNotNull(navigationalStatus);
}
this.messageId = messageId;
this.cls = cls;
this.id = id;
this.lat = lat;
this.lon = lon;
this.lengthMetres = lengthMetres;
this.widthMetres = widthMetres;
this.cogDegrees = cog;
this.headingDegrees = heading;
this.speedMetresPerSecond = speedMetresPerSecond;
this.time = time;
this.navigationalStatus = navigationalStatus;
this.shipType = shipType;
this.positionAisNmea = positionAisNmea;
this.shipStaticAisNmea = shipStaticAisNmea;
this.data = data;
}
public long messageId() {
return messageId;
}
public Identifier id() {
return id;
}
public double lat() {
return lat;
}
public double lon() {
return lon;
}
public Optional<?> data() {
return data;
}
public Optional<Integer> lengthMetres() {
return lengthMetres;
}
public Optional<Integer> widthMetres() {
return widthMetres;
}
public Optional<Integer> maxDimensionMetres() {
if (lengthMetres.isPresent() && widthMetres.isPresent())
return Optional.of(Math.max(lengthMetres.get(), widthMetres.get()));
else
return Optional.absent();
}
public Optional<Double> cogDegrees() {
return cogDegrees;
}
public Optional<Double> headingDegrees() {
return headingDegrees;
}
public Optional<Double> speedMetresPerSecond() {
return speedMetresPerSecond;
}
public Optional<Double> speedKnots() {
return speedMetresPerSecond.transform(x -> x / 0.5144444);
}
public VesselClass cls() {
return cls;
}
public long time() {
return time;
}
public NavigationalStatus navigationalStatus() {
return navigationalStatus;
}
public Optional<Integer> shipType() {
return shipType;
}
public Optional<String> positionAisNmea() {
return positionAisNmea;
}
public Optional<String> shipStaticAisNmea() {
return shipStaticAisNmea;
}
public static Builder builder() {
return new Builder();
}
public static class Builder {
private Identifier id;
private double lat;
private double lon;
private Optional<Integer> lengthMetres = Optional.absent();
private Optional<Integer> widthMetres = Optional.absent();
// leave these null so if not set get an error in VesselPosition
// constructor
private Optional<Double> cogDegrees;
private Optional<Double> headingDegrees;
private Optional<Double> speedMetresPerSecond;
private Optional<String> positionAisNmea;
private Optional<String> shipStaticAisNmea;
private VesselClass cls;
private long time;
private Optional<Integer> shipType = Optional.absent();
private NavigationalStatus navigationalStatus;
private Optional<?> data;
private Builder() {
}
public Builder id(Identifier id) {
this.id = id;
return this;
}
public Builder lat(double lat) {
this.lat = lat;
return this;
}
public Builder lon(double lon) {
this.lon = lon;
return this;
}
public Builder lengthMetres(Optional<Integer> lengthMetres) {
this.lengthMetres = lengthMetres;
return this;
}
public Builder widthMetres(Optional<Integer> widthMetres) {
this.widthMetres = widthMetres;
return this;
}
public Builder cogDegrees(Optional<Double> cog) {
this.cogDegrees = cog;
return this;
}
public Builder headingDegrees(Optional<Double> heading) {
this.headingDegrees = heading;
return this;
}
public Builder speedMetresPerSecond(Optional<Double> speedMetresPerSecond) {
this.speedMetresPerSecond = speedMetresPerSecond;
return this;
}
public Builder time(long time) {
this.time = time;
return this;
}
public Builder cls(VesselClass cls) {
this.cls = cls;
return this;
}
public Builder shipType(Optional<Integer> shipType) {
this.shipType = shipType;
return this;
}
public Builder positionAisNmea(Optional<String> nmea) {
this.positionAisNmea = nmea;
return this;
}
public Builder shipStaticAisNmea(Optional<String> nmea) {
this.shipStaticAisNmea = nmea;
return this;
}
public Builder navigationalStatus(NavigationalStatus status) {
this.navigationalStatus = status;
return this;
}
public Builder data(Optional<?> data) {
this.data = data;
return this;
}
public VesselPosition build() {
return new VesselPosition(counter.incrementAndGet(), id, lat, lon, lengthMetres,
widthMetres, cogDegrees, headingDegrees, speedMetresPerSecond, cls,
navigationalStatus, time, shipType, positionAisNmea, shipStaticAisNmea, data);
}
}
private double metresPerDegreeLongitude() {
return Math.PI / 180 * EARTH_RADIUS_KM * Math.cos(toRadians(lat));
}
private double metresPerDegreeLatitude() {
return 111321.543;
}
// private Area createArea(VesselPosition relativeTo) {
// Vector v = position(relativeTo);
// Rectangle2D.Double r = baseRectangle();
// Area a = new Area(r);
// AffineTransform af = new AffineTransform();
// af.rotate(toRadians(headingDegrees), v.x(), v.y());
// return a.createTransformedArea(af);
// }
public Vector position(VesselPosition relativeTo) {
// TODO longitude wrapping check
double xMetres = (lon - relativeTo.lon()) * relativeTo.metresPerDegreeLongitude();
double yMetres = (lat - relativeTo.lat()) * relativeTo.metresPerDegreeLatitude();
return new Vector(xMetres, yMetres);
}
// public boolean intersects(VesselPosition p) {
// Area area = createArea(this);
// area.intersect(p.createArea(this));
// return !area.isEmpty();
// }
public Optional<VesselPosition> predict(long t) {
if (!speedMetresPerSecond.isPresent() || !cogDegrees.isPresent()
|| navigationalStatus == NavigationalStatus.AT_ANCHOR
|| navigationalStatus == NavigationalStatus.MOORED)
return Optional.absent();
else {
double lat = this.lat - speedMetresPerSecond.get() / metresPerDegreeLatitude()
* (t - time) / 1000.0 * Math.cos(Math.toRadians(cogDegrees.get()));
if (lat > 90)
lat = 90;
else if (lat < -90)
lat = -90;
double lon = Position
.to180(this.lon + speedMetresPerSecond.get() / metresPerDegreeLongitude()
* (t - time) / 1000.0 * Math.sin(Math.toRadians(cogDegrees.get())));
return Optional.of(new VesselPosition(messageId, id, lat, lon, lengthMetres,
widthMetres, cogDegrees, headingDegrees, speedMetresPerSecond, cls,
navigationalStatus, time, shipType, positionAisNmea, shipStaticAisNmea, data));
}
}
private Optional<Vector> velocity() {
if (speedMetresPerSecond.isPresent() && cogDegrees.isPresent())
return Optional.of(new Vector(
speedMetresPerSecond.get() * Math.sin(Math.toRadians(cogDegrees.get())),
speedMetresPerSecond.get() * Math.cos(Math.toRadians(cogDegrees.get()))));
else
return Optional.absent();
}
/**
* Returns absent if no intersection occurs else return the one or two times
* of intersection of circles around the vessel relative to this.time().
*
* @param vp
* @return
*/
public Optional<Times> intersectionTimes(VesselPosition vp) {
// TODO handle vp doesn't have speed or cog but is within collision
// distance given any cog and max speed
Optional<VesselPosition> p = vp.predict(time);
if (!p.isPresent()) {
return Optional.absent();
}
Vector deltaV = velocity().get().minus(p.get().velocity().get());
Vector deltaP = position(this).minus(p.get().position(this));
// imagine a ring around the vessel centroid with maxDimensionMetres/2
// radius. This is the ring we are going to test for collision.
double r = p.get().maxDimensionMetres().or(maxDimensionMetresWhenUnknown) / 2
+ maxDimensionMetres().or(maxDimensionMetresWhenUnknown) / 2;
if (deltaP.dot(deltaP) <= r)
return of(new Times(p.get().time()));
double a = deltaV.dot(deltaV);
double b = 2 * deltaV.dot(deltaP);
double c = deltaP.dot(deltaP) - r * r;
// Now solve the quadratic equation with coefficients a,b,c
double discriminant = b * b - 4 * a * c;
if (a == 0)
return Optional.absent();
else if (discriminant < 0)
return Optional.absent();
else {
if (discriminant == 0) {
return of(new Times(Math.round(-b / 2 / a)));
} else {
long alpha1 = Math.round((-b + Math.sqrt(discriminant)) / 2 / a);
long alpha2 = Math.round((-b - Math.sqrt(discriminant)) / 2 / a);
return of(new Times(alpha1, alpha2));
}
}
}
@Override
public String toString() {
StringBuilder b = new StringBuilder();
b.append("VesselPosition [lat=");
b.append(lat);
b.append(", lon=");
b.append(lon);
b.append(", lengthMetres=");
b.append(lengthMetres);
b.append(", widthMetres=");
b.append(widthMetres);
b.append(", cogDegrees=");
b.append(cogDegrees);
b.append(", headingDegrees=");
b.append(headingDegrees);
b.append(", speedMetresPerSecond=");
b.append(speedMetresPerSecond);
b.append(", positionAisNmea=");
b.append(positionAisNmea);
b.append(", shipStaticAisNmea=");
b.append(shipStaticAisNmea);
b.append(", navStatus=");
b.append(navigationalStatus);
b.append(", time=");
b.append(time);
b.append(", id=");
b.append(id);
b.append(", cls=");
b.append(cls);
b.append(", shipType=");
b.append(shipType);
b.append(", messageId=");
b.append(messageId);
b.append(", data=");
b.append(data);
b.append("]");
return b.toString();
}
@Override
public int hashCode() {
final int prime = 31;
int result = 1;
result = prime * result + ((id == null) ? 0 : id.hashCode());
result = prime * result + (int) (messageId ^ (messageId >>> 32));
result = prime * result + (int) (time ^ (time >>> 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;
VesselPosition other = (VesselPosition) obj;
if (id == null) {
if (other.id != null)
return false;
} else if (!id.equals(other.id))
return false;
if (messageId != other.messageId)
return false;
if (time != other.time)
return false;
return true;
}
@Override
public Fix fix() {
return new Fix() {
@Override
public Fix fix() {
return this;
}
@Override
public int mmsi() {
return (int) ((Mmsi) id).uniqueId();
}
@Override
public long time() {
return time;
}
@Override
public float lat() {
return (float) lat;
}
@Override
public float lon() {
return (float) lon;
}
@Override
public Optional<au.gov.amsa.risky.format.NavigationalStatus> navigationalStatus() {
return Optional.of(au.gov.amsa.risky.format.NavigationalStatus
.values()[navigationalStatus.ordinal()]);
}
@Override
public Optional<Float> speedOverGroundKnots() {
if (speedMetresPerSecond.isPresent())
return Optional.of((float) metresPerSecondToKnots(speedMetresPerSecond.get()));
else
return Optional.absent();
}
@Override
public Optional<Float> courseOverGroundDegrees() {
return toFloat(cogDegrees);
}
@Override
public Optional<Float> headingDegrees() {
return toFloat(headingDegrees);
}
@Override
public AisClass aisClass() {
if (cls == VesselClass.A)
return AisClass.A;
else if (cls == VesselClass.B)
return AisClass.B;
else
throw new RuntimeException("unexpected");
}
@Override
public Optional<Integer> latencySeconds() {
return Optional.absent();
}
@Override
public Optional<Short> source() {
return Optional.absent();
}
@Override
public Optional<Byte> rateOfTurn() {
return Optional.absent();
}
};
}
static double metresPerSecondToKnots(double x) {
return x * 3600.0 / 1852.0;
}
private static Optional<Float> toFloat(Optional<Double> value) {
if (value.isPresent())
return Optional.of(value.get().floatValue());
else
return Optional.absent();
}
}