package com.tsi.navi.util;

public class KalmanFilter {
    private final float MIN_ACCURACY = 1;

    private float Q_metres_per_second;
    private long TimeStamp_milliseconds;
    private double lat;
    private double lng;
    private float variance; // P matrix. Negative means object uninitialised.
                            // NB: units irrelevant, as long as same units used
                            // throughout
    public int consecutiveRejectCount;

    public KalmanFilter(float Q_metres_per_second) {
        this.Q_metres_per_second = Q_metres_per_second;
        this.variance = -1;
        this.consecutiveRejectCount = 0;
    }

    public long getTimeStamp() {
        return this.TimeStamp_milliseconds;
    }

    public double getLat() {
        return this.lat;
    }

    public double getLng() {
        return this.lng;
    }

    public float getAccuracy() {
        return (float) Math.sqrt(variance);
    }

    public void setState(double lat, double lng, float accuracy, long TimeStamp_milliseconds) {
        this.lat = lat;
        this.lng = lng;
        this.variance = accuracy * accuracy;
        this.TimeStamp_milliseconds = TimeStamp_milliseconds;
    }

    // / <summary>
    // / Kalman filter processing for lattitude and longitude
    // / </summary>
    // / <param name="lat_measurement_degrees">new measurement of
    // lattidude</param>
    // / <param name="lng_measurement">new measurement of longitude</param>
    // / <param name="accuracy">measurement of 1 standard deviation error in
    // metres</param>
    // / <param name="TimeStamp_milliseconds">time of measurement</param>
    // / <returns>new state</returns>
    public void process(double lat_measurement, double lng_measurement,
                        float accuracy, long TimeStamp_milliseconds, float Q_metres_per_second) {
        this.Q_metres_per_second = Q_metres_per_second;

        if (accuracy < MIN_ACCURACY)
            accuracy = MIN_ACCURACY;
        if (this.variance < 0) {
            // if variance < 0, object is unitialised, so initialise with
            // current values
            this.TimeStamp_milliseconds = TimeStamp_milliseconds;
            this.lat = lat_measurement;
            this.lng = lng_measurement;
            this.variance = accuracy * accuracy;
        }
        else {
            // else apply Kalman filter methodology

            long TimeInc_milliseconds = TimeStamp_milliseconds - this.TimeStamp_milliseconds;
            if (TimeInc_milliseconds > 0) {
                // time has moved on, so the uncertainty in the current position increases
                this.variance += TimeInc_milliseconds * Q_metres_per_second * Q_metres_per_second / 1000;
                this.TimeStamp_milliseconds = TimeStamp_milliseconds;
                // TO DO: USE VELOCITY INFORMATION HERE TO GET A BETTER ESTIMATE
                // OF CURRENT POSITION
            }

            // Kalman gain matrix K = Covariance * Inverse(Covariance + MeasurementVariance)
            // NB: because K is dimensionless, it doesn't matter that variance
            // has different units to lat and lng
            float K = this.variance / (this.variance + accuracy * accuracy);
            // apply K
            this.lat += K * (lat_measurement - this.lat);
            this.lng += K * (lng_measurement - this.lng);
            // new Covariance matrix is (IdentityMatrix - K) * Covariance
            this.variance = (1 - K) * this.variance;
        }
    }

    public int getConsecutiveRejectCount() {
        return consecutiveRejectCount;
    }

    public void setConsecutiveRejectCount(int consecutiveRejectCount) {
        this.consecutiveRejectCount = consecutiveRejectCount;
    }
}
