package com.wytehat.btlogger;

import java.util.ArrayList;
import java.util.List;

public class SpatialGradientEngine {

    /**
     * Metres of spread the sample track needs across its own axis before a
     * signal gradient means anything. See {@link #gradientBearing}.
     */
    private static final double MIN_TRACK_WIDTH_METERS = 4.0;

    public static class Estimate {
        public double latitude;
        public double longitude;
        public double confidenceMeters;
        public float gradientBearing;
        public int sampleCount;
        public boolean preliminary;
    }

    private static class Sample {
        double latitude, longitude, rssi, distance;
        float accuracy;
        long time;
    }

    private final ArrayList<Sample> samples = new ArrayList<Sample>();
    private final ArrayList<Double> pendingRssi = new ArrayList<Double>();
    private double pendingLatitude, pendingLongitude;
    private int lastConsensusSampleCount = -1;
    private final ArrayList<Estimate> estimateConsensus = new ArrayList<Estimate>();
    private Estimate stableEstimate;

    public synchronized boolean addSample(double latitude, double longitude, float accuracy,
                                          double filteredRssi, int txPower,
                                          double pathLossExponent, long time) {
        if (accuracy <= 0 || accuracy > 60 || pathLossExponent <= 0) return false;
        if (samples.size() > 0) {
            Sample previous = samples.get(samples.size() - 1);
            double required = Math.max(5.0, Math.max(previous.accuracy, accuracy));
            if (haversine(previous.latitude, previous.longitude, latitude, longitude) < required) {
                pendingRssi.clear(); return false;
            }
        }
        if (pendingRssi.isEmpty()) {
            pendingLatitude = latitude; pendingLongitude = longitude;
        } else if (haversine(pendingLatitude, pendingLongitude, latitude, longitude) >
                   Math.max(5.0, accuracy)) {
            pendingRssi.clear(); pendingLatitude = latitude; pendingLongitude = longitude;
        }
        pendingRssi.add(Double.valueOf(filteredRssi));
        if (pendingRssi.size() < 4) return false;
        java.util.Collections.sort(pendingRssi);
        double median = (pendingRssi.get(1).doubleValue() + pendingRssi.get(2).doubleValue()) / 2.0;
        pendingRssi.clear();
        Sample point = new Sample();
        point.latitude = latitude; point.longitude = longitude; point.accuracy = accuracy;
        point.rssi = median;
        point.distance = Math.pow(10.0, (txPower - median) / (10.0 * pathLossExponent));
        point.time = time; samples.add(point);
        while (samples.size() > 60) samples.remove(0);
        prune(time - 15L * 60L * 1000L);
        return true;
    }

    public synchronized Estimate solve() {
        if (samples.size() < 4) return null;
        Sample origin = samples.get(0);
        int count = samples.size();
        double[] xs = new double[count];
        double[] ys = new double[count];
        double cos = Math.cos(Math.toRadians(origin.latitude));
        double metersPerDegree = 111319.49;
        double x = 0, y = 0, total = 0;
        for (int i = 0; i < count; i++) {
            Sample sample = samples.get(i);
            xs[i] = (sample.longitude - origin.longitude) * metersPerDegree * cos;
            ys[i] = (sample.latitude - origin.latitude) * metersPerDegree;
            double weight = 1.0 / Math.max(1.0, sample.distance * sample.distance);
            x += xs[i] * weight;
            y += ys[i] * weight;
            total += weight;
        }
        double maxBaseline = 0;
        for (int i = 0; i < count; i++) for (int j = i + 1; j < count; j++)
            maxBaseline = Math.max(maxBaseline, Math.sqrt(Math.pow(xs[i] - xs[j], 2) + Math.pow(ys[i] - ys[j], 2)));
        if (maxBaseline < 8.0) return null;
        x /= total;
        y /= total;

        double determinant = 0;
        for (int iteration = 0; iteration < 20; iteration++) {
            double a00 = 0, a01 = 0, a11 = 0, b0 = 0, b1 = 0;
            for (int i = 0; i < count; i++) {
                Sample sample = samples.get(i);
                double dx = x - xs[i];
                double dy = y - ys[i];
                double radius = Math.max(0.25, Math.sqrt(dx * dx + dy * dy));
                double residual = radius - sample.distance;
                double jx = dx / radius;
                double jy = dy / radius;
                double uncertainty = sample.accuracy * sample.accuracy +
                    Math.pow(Math.max(1.0, sample.distance * 0.5), 2);
                double weight = 1.0 / uncertainty;
                double scale = Math.sqrt(uncertainty);
                if (Math.abs(residual) > 2.5 * scale)
                    weight *= (2.5 * scale) / Math.abs(residual);
                a00 += weight * jx * jx;
                a01 += weight * jx * jy;
                a11 += weight * jy * jy;
                b0 += weight * jx * residual;
                b1 += weight * jy * residual;
            }
            determinant = a00 * a11 - a01 * a01;
            if (Math.abs(determinant) < 0.000001) return null;
            double stepX = -(a11 * b0 - a01 * b1) / determinant;
            double stepY = -(-a01 * b0 + a00 * b1) / determinant;
            x += stepX;
            y += stepY;
            if (Math.sqrt(stepX * stepX + stepY * stepY) < 0.05) break;
        }

        double error = 0;
        double accuracyTotal = 0;
        for (int i = 0; i < count; i++) {
            double radius = Math.sqrt(Math.pow(x - xs[i], 2) + Math.pow(y - ys[i], 2));
            error += Math.pow(radius - samples.get(i).distance, 2);
            accuracyTotal += samples.get(i).accuracy;
        }
        Estimate result = new Estimate();
        result.latitude = origin.latitude + y / metersPerDegree;
        result.longitude = origin.longitude + x / (metersPerDegree * cos);
        double averageAccuracy = accuracyTotal / count;
        result.confidenceMeters = Math.max(averageAccuracy * 0.75,
            Math.sqrt(error / count) + averageAccuracy / Math.sqrt(Math.max(1.0, count / 3.0)));
        result.gradientBearing = gradientBearing(xs, ys);
        Sample latest = samples.get(count - 1);
        double latestX = xs[count - 1], latestY = ys[count - 1];
        double solvedGap = Math.sqrt(Math.pow(x - latestX, 2) + Math.pow(y - latestY, 2));
        double gradientEast = Math.sin(Math.toRadians(result.gradientBearing));
        double gradientNorth = Math.cos(Math.toRadians(result.gradientBearing));
        double solvedEast = x - latestX;
        double solvedNorth = y - latestY;
        boolean oppositeGradient = solvedEast * gradientEast + solvedNorth * gradientNorth <= 0;
        if (!Float.isNaN(result.gradientBearing) && latest.distance > 3.0 &&
            (oppositeGradient || solvedGap < Math.max(3.0, latest.distance * 0.5))) {
            double angle = Math.toRadians(result.gradientBearing);
            double adjustedX = latestX + Math.sin(angle) * latest.distance;
            double adjustedY = latestY + Math.cos(angle) * latest.distance;
            result.latitude = origin.latitude + adjustedY / metersPerDegree;
            result.longitude = origin.longitude + adjustedX / (metersPerDegree * cos);
        }
        result.sampleCount = count;
        result.preliminary = false;
        return result;
    }

    /**
     * A bearing solved from samples that all sit on one line is not a
     * measurement, it is an echo of the walk.
     *
     * Centre the positions and the correlation below is a sum of
     * (position - mean) * signal. On a straight walk every centred position
     * is t*u for one unit vector u along the track, so the result is exactly
     * parallel to u no matter what the RSSI does - the "estimated" bearing
     * comes back as the direction the user is already walking, the pin gets
     * projected dead ahead, and the compass arrow appears to point at it.
     *
     * Distances alone still place the item off-axis, but which side of the
     * track it lies on is a genuine mirror ambiguity. It takes a leg at an
     * angle to resolve, so report NaN until the track has real width and let
     * the caller ask the user to turn.
     */
    private float gradientBearing(double[] xs, double[] ys) {
        double meanX = 0, meanY = 0, meanRssi = 0, meanAccuracy = 0;
        for (int i = 0; i < samples.size(); i++) {
            meanX += xs[i]; meanY += ys[i]; meanRssi += samples.get(i).rssi;
            meanAccuracy += samples.get(i).accuracy;
        }
        meanX /= samples.size(); meanY /= samples.size(); meanRssi /= samples.size();
        meanAccuracy /= samples.size();
        if (perpendicularSpread(xs, ys, meanX, meanY) <
            Math.max(MIN_TRACK_WIDTH_METERS, meanAccuracy * 0.5)) {
            return Float.NaN;
        }
        double east = 0, north = 0;
        for (int i = 0; i < samples.size(); i++) {
            double signal = samples.get(i).rssi - meanRssi;
            east += (xs[i] - meanX) * signal;
            north += (ys[i] - meanY) * signal;
        }
        if (Math.abs(east) + Math.abs(north) < 0.000001) return Float.NaN;
        double degrees = Math.toDegrees(Math.atan2(east, north));
        if (degrees < 0) degrees += 360;
        return (float) degrees;
    }

    /**
     * Spread of the sample track across its own dominant axis, in metres:
     * the smaller eigenvalue of the position covariance, square-rooted.
     *
     * Straight walk plus GPS jitter lands around 1-3 m. An L of two 20 m
     * legs lands near 6 m.
     */
    private double perpendicularSpread(double[] xs, double[] ys,
                                       double meanX, double meanY) {
        int count = samples.size();
        if (count < 3) return 0;
        double sxx = 0, syy = 0, sxy = 0;
        for (int i = 0; i < count; i++) {
            double dx = xs[i] - meanX, dy = ys[i] - meanY;
            sxx += dx * dx; syy += dy * dy; sxy += dx * dy;
        }
        sxx /= count; syy /= count; sxy /= count;
        double half = (sxx + syy) / 2.0;
        double gap = Math.sqrt(Math.pow((sxx - syy) / 2.0, 2) + sxy * sxy);
        return Math.sqrt(Math.max(0, half - gap));
    }

    public synchronized Estimate preliminaryEstimate() {
        if (samples.size() < 2) return null;
        Sample origin = samples.get(0);
        int count = samples.size();
        double[] xs = new double[count]; double[] ys = new double[count];
        double cos = Math.cos(Math.toRadians(origin.latitude));
        double metersPerDegree = 111319.49;
        for (int i = 0; i < count; i++) {
            xs[i] = (samples.get(i).longitude - origin.longitude) * metersPerDegree * cos;
            ys[i] = (samples.get(i).latitude - origin.latitude) * metersPerDegree;
        }
        float bearing = gradientBearing(xs, ys);
        if (Float.isNaN(bearing)) return null;
        Sample latest = samples.get(count - 1);
        double projected = Math.max(5.0, Math.min(50.0, latest.distance));
        double radians = Math.toRadians(bearing);
        double north = Math.cos(radians) * projected;
        double east = Math.sin(radians) * projected;
        Estimate result = new Estimate();
        result.latitude = latest.latitude + north / metersPerDegree;
        result.longitude = latest.longitude + east / (metersPerDegree * Math.cos(Math.toRadians(latest.latitude)));
        result.confidenceMeters = Math.max(latest.accuracy, projected * 1.5);
        result.gradientBearing = bearing; result.sampleCount = count; result.preliminary = true;
        return result;
    }

    public synchronized Estimate consensusEstimate() {
        if (samples.size() == lastConsensusSampleCount) return stableEstimate;
        lastConsensusSampleCount = samples.size();
        Estimate candidate = solve();
        if (candidate == null) candidate = preliminaryEstimate();
        if (candidate == null) return stableEstimate;
        if (!estimateConsensus.isEmpty()) {
            Estimate previous = estimateConsensus.get(estimateConsensus.size() - 1);
            double separation = haversine(previous.latitude, previous.longitude,
                candidate.latitude, candidate.longitude);
            double allowed = Math.max(20.0, previous.confidenceMeters + candidate.confidenceMeters);
            if (separation > allowed) estimateConsensus.clear();
        }
        estimateConsensus.add(candidate);
        while (estimateConsensus.size() > 5) estimateConsensus.remove(0);
        if (estimateConsensus.size() < 3) return stableEstimate;
        int start = estimateConsensus.size() - 3;
        Estimate result = new Estimate(); double east = 0, north = 0;
        for (int i = start; i < estimateConsensus.size(); i++) {
            Estimate value = estimateConsensus.get(i);
            result.latitude += value.latitude; result.longitude += value.longitude;
            result.confidenceMeters += value.confidenceMeters;
            east += Math.sin(Math.toRadians(value.gradientBearing));
            north += Math.cos(Math.toRadians(value.gradientBearing));
            result.preliminary = result.preliminary || value.preliminary;
        }
        result.latitude /= 3.0; result.longitude /= 3.0;
        result.confidenceMeters = Math.max(2.0, result.confidenceMeters / 3.0);
        double bearing = Math.toDegrees(Math.atan2(east, north));
        if (bearing < 0) bearing += 360; result.gradientBearing = (float)bearing;
        result.sampleCount = samples.size(); stableEstimate = result; return stableEstimate;
    }

    public synchronized double calibratePathLoss(double targetLat, double targetLon, int txPower) {
        if (samples.size() < 3) return Double.NaN;
        double numerator = 0, denominator = 0; int used = 0;
        for (int i = 0; i < samples.size(); i++) {
            Sample sample = samples.get(i);
            double distance = haversine(sample.latitude, sample.longitude, targetLat, targetLon);
            if (distance < 1.0 || distance > 100.0) continue;
            double x = 10.0 * Math.log10(distance);
            numerator += x * (txPower - sample.rssi); denominator += x * x; used++;
        }
        if (used < 3 || denominator <= 0) return Double.NaN;
        double exponent = numerator / denominator;
        return exponent >= 1.2 && exponent <= 5.0 ? exponent : Double.NaN;
    }

    private void prune(long oldest) {
        while (samples.size() > 0 && samples.get(0).time < oldest) samples.remove(0);
    }

    public synchronized int size() { return samples.size(); }
    public synchronized void clear() { samples.clear(); pendingRssi.clear(); estimateConsensus.clear(); stableEstimate = null; lastConsensusSampleCount = -1; }

    private double haversine(double lat1, double lon1, double lat2, double lon2) {
        double radius = 6371000.0;
        double dLat = Math.toRadians(lat2 - lat1);
        double dLon = Math.toRadians(lon2 - lon1);
        double a = Math.sin(dLat / 2) * Math.sin(dLat / 2) +
            Math.cos(Math.toRadians(lat1)) * Math.cos(Math.toRadians(lat2)) *
            Math.sin(dLon / 2) * Math.sin(dLon / 2);
        return radius * 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
    }
}