diff --git a/.gitignore b/.gitignore index d4c3a57e..5ae98cf8 100644 --- a/.gitignore +++ b/.gitignore @@ -13,4 +13,5 @@ .externalNativeBuild .cxx local.properties +secrets.properties /.idea/ diff --git a/app/src/main/AndroidManifest.xml b/app/src/main/AndroidManifest.xml index 6398bce9..98eed0c5 100644 --- a/app/src/main/AndroidManifest.xml +++ b/app/src/main/AndroidManifest.xml @@ -85,7 +85,7 @@ android:label="@string/app_name" android:roundIcon="@mipmap/ic_launcher_icon_map_round" android:supportsRtl="true" - android:theme="@style/Theme.Material3.DayNight.NoActionBar" + android:theme="@style/Theme.App" android:requestLegacyExternalStorage="true" > diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java index 2d2b1cbf..9b4afbf5 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/local/TrajParser.java @@ -104,6 +104,12 @@ private static class GnssRecord { public double latitude, longitude; // GNSS coordinates } + /** Represents a fused/corrected position record from protobuf corrected_positions. */ + private static class CorrectedRecord { + public long relativeTimestamp; + public double latitude, longitude; + } + /** * Parses trajectory data from a JSON file and reconstructs a list of replay points. * @@ -150,43 +156,71 @@ public static List parseTrajectoryData(String filePath, Context con List imuList = parseImuData(root.getAsJsonArray("imuData")); List pdrList = parsePdrData(root.getAsJsonArray("pdrData")); List gnssList = parseGnssData(root.getAsJsonArray("gnssData")); + JsonArray correctedArray = root.has("correctedPositions") + ? root.getAsJsonArray("correctedPositions") + : root.getAsJsonArray("corrected_positions"); + List correctedList = parseCorrectedData(correctedArray); Log.i(TAG, "Parsed data - IMU: " + imuList.size() + " records, PDR: " - + pdrList.size() + " records, GNSS: " + gnssList.size() + " records"); + + pdrList.size() + " records, GNSS: " + gnssList.size() + " records" + + ", Corrected: " + correctedList.size() + " records"); + + if (!correctedList.isEmpty()) { + for (int i = 0; i < correctedList.size(); i++) { + CorrectedRecord corrected = correctedList.get(i); + + ImuRecord closestImu = findClosestImuRecord(imuList, corrected.relativeTimestamp); + float orientationDeg = closestImu != null ? computeOrientationFromRotationVector( + closestImu.rotationVectorX, + closestImu.rotationVectorY, + closestImu.rotationVectorZ, + closestImu.rotationVectorW, + context + ) : 0f; + + LatLng correctedLocation = new LatLng(corrected.latitude, corrected.longitude); - for (int i = 0; i < pdrList.size(); i++) { - PdrRecord pdr = pdrList.get(i); + GnssRecord closestGnss = findClosestGnssRecord(gnssList, corrected.relativeTimestamp); + LatLng gnssLocation = closestGnss != null ? + new LatLng(closestGnss.latitude, closestGnss.longitude) : null; + + result.add(new ReplayPoint(correctedLocation, gnssLocation, orientationDeg, + 0f, corrected.relativeTimestamp)); + } + } else { + for (int i = 0; i < pdrList.size(); i++) { + PdrRecord pdr = pdrList.get(i); - ImuRecord closestImu = findClosestImuRecord(imuList, pdr.relativeTimestamp); - float orientationDeg = closestImu != null ? computeOrientationFromRotationVector( + ImuRecord closestImu = findClosestImuRecord(imuList, pdr.relativeTimestamp); + float orientationDeg = closestImu != null ? computeOrientationFromRotationVector( closestImu.rotationVectorX, closestImu.rotationVectorY, closestImu.rotationVectorZ, closestImu.rotationVectorW, context - ) : 0f; + ) : 0f; - float speed = 0f; - if (i > 0) { + float speed = 0f; + if (i > 0) { PdrRecord prev = pdrList.get(i - 1); double dt = (pdr.relativeTimestamp - prev.relativeTimestamp) / 1000.0; double dx = pdr.x - prev.x; double dy = pdr.y - prev.y; double distance = Math.sqrt(dx * dx + dy * dy); if (dt > 0) speed = (float) (distance / dt); - } - + } - double lat = originLat + pdr.y * 1E-5; - double lng = originLng + pdr.x * 1E-5; - LatLng pdrLocation = new LatLng(lat, lng); + double lat = originLat + pdr.y * 1E-5; + double lng = originLng + pdr.x * 1E-5; + LatLng pdrLocation = new LatLng(lat, lng); - GnssRecord closestGnss = findClosestGnssRecord(gnssList, pdr.relativeTimestamp); - LatLng gnssLocation = closestGnss != null ? + GnssRecord closestGnss = findClosestGnssRecord(gnssList, pdr.relativeTimestamp); + LatLng gnssLocation = closestGnss != null ? new LatLng(closestGnss.latitude, closestGnss.longitude) : null; - result.add(new ReplayPoint(pdrLocation, gnssLocation, orientationDeg, - 0f, pdr.relativeTimestamp)); + result.add(new ReplayPoint(pdrLocation, gnssLocation, orientationDeg, + speed, pdr.relativeTimestamp)); + } } Collections.sort(result, Comparator.comparingLong(rp -> rp.timestamp)); @@ -199,35 +233,172 @@ public static List parseTrajectoryData(String filePath, Context con return result; } -/** Parses IMU data from JSON. */ +/** Parses IMU data from JSON, handling multiple field naming conventions. */ private static List parseImuData(JsonArray imuArray) { List imuList = new ArrayList<>(); if (imuArray == null) return imuList; - Gson gson = new Gson(); + for (int i = 0; i < imuArray.size(); i++) { - ImuRecord record = gson.fromJson(imuArray.get(i), ImuRecord.class); - imuList.add(record); + try { + JsonObject imuObj = imuArray.get(i).getAsJsonObject(); + ImuRecord record = new ImuRecord(); + + // Handle both naming conventions + if (imuObj.has("relativeTimestamp")) { + record.relativeTimestamp = imuObj.get("relativeTimestamp").getAsLong(); + } else if (imuObj.has("relative_timestamp")) { + record.relativeTimestamp = imuObj.get("relative_timestamp").getAsLong(); + } + + // Standard field names + if (imuObj.has("accX")) { + record.accX = imuObj.get("accX").getAsFloat(); + } + if (imuObj.has("accY")) { + record.accY = imuObj.get("accY").getAsFloat(); + } + if (imuObj.has("accZ")) { + record.accZ = imuObj.get("accZ").getAsFloat(); + } + if (imuObj.has("gyrX")) { + record.gyrX = imuObj.get("gyrX").getAsFloat(); + } + if (imuObj.has("gyrY")) { + record.gyrY = imuObj.get("gyrY").getAsFloat(); + } + if (imuObj.has("gyrZ")) { + record.gyrZ = imuObj.get("gyrZ").getAsFloat(); + } + if (imuObj.has("rotationVectorX")) { + record.rotationVectorX = imuObj.get("rotationVectorX").getAsFloat(); + } + if (imuObj.has("rotationVectorY")) { + record.rotationVectorY = imuObj.get("rotationVectorY").getAsFloat(); + } + if (imuObj.has("rotationVectorZ")) { + record.rotationVectorZ = imuObj.get("rotationVectorZ").getAsFloat(); + } + if (imuObj.has("rotationVectorW")) { + record.rotationVectorW = imuObj.get("rotationVectorW").getAsFloat(); + } + + imuList.add(record); + } catch (Exception e) { + Log.w(TAG, "Failed to parse IMU record " + i, e); + } } + return imuList; -}/** Parses PDR data from JSON. */ +}/** Parses PDR data from JSON, handling multiple field naming conventions. */ private static List parsePdrData(JsonArray pdrArray) { List pdrList = new ArrayList<>(); if (pdrArray == null) return pdrList; - Gson gson = new Gson(); + for (int i = 0; i < pdrArray.size(); i++) { - PdrRecord record = gson.fromJson(pdrArray.get(i), PdrRecord.class); - pdrList.add(record); + try { + JsonObject pdrObj = pdrArray.get(i).getAsJsonObject(); + PdrRecord record = new PdrRecord(); + + // Handle both naming conventions + if (pdrObj.has("relativeTimestamp")) { + record.relativeTimestamp = pdrObj.get("relativeTimestamp").getAsLong(); + } else if (pdrObj.has("relative_timestamp")) { + record.relativeTimestamp = pdrObj.get("relative_timestamp").getAsLong(); + } + + if (pdrObj.has("x")) { + record.x = pdrObj.get("x").getAsFloat(); + } + if (pdrObj.has("y")) { + record.y = pdrObj.get("y").getAsFloat(); + } + + pdrList.add(record); + } catch (Exception e) { + Log.w(TAG, "Failed to parse PDR record " + i, e); + } } + return pdrList; -}/** Parses GNSS data from JSON. */ +}/** Parses corrected (fused) data from JSON, handling multiple field naming conventions. */ +private static List parseCorrectedData(JsonArray correctedArray) { + List correctedList = new ArrayList<>(); + if (correctedArray == null) return correctedList; + + for (int i = 0; i < correctedArray.size(); i++) { + try { + JsonObject corrObj = correctedArray.get(i).getAsJsonObject(); + CorrectedRecord record = new CorrectedRecord(); + + // Handle both naming conventions + if (corrObj.has("relativeTimestamp")) { + record.relativeTimestamp = corrObj.get("relativeTimestamp").getAsLong(); + } else if (corrObj.has("relative_timestamp")) { + record.relativeTimestamp = corrObj.get("relative_timestamp").getAsLong(); + } + + if (corrObj.has("latitude")) { + record.latitude = corrObj.get("latitude").getAsDouble(); + } + if (corrObj.has("longitude")) { + record.longitude = corrObj.get("longitude").getAsDouble(); + } + + correctedList.add(record); + } catch (Exception e) { + Log.w(TAG, "Failed to parse corrected record " + i, e); + } + } + + return correctedList; +} + +/** Parses GNSS data from JSON, handling protobuf nested structure. */ private static List parseGnssData(JsonArray gnssArray) { List gnssList = new ArrayList<>(); - if (gnssArray == null) return gnssList; - Gson gson = new Gson(); + if (gnssArray == null || gnssArray.size() == 0) { + return gnssList; + } + for (int i = 0; i < gnssArray.size(); i++) { - GnssRecord record = gson.fromJson(gnssArray.get(i), GnssRecord.class); - gnssList.add(record); + try { + JsonObject gnssObj = gnssArray.get(i).getAsJsonObject(); + GnssRecord record = new GnssRecord(); + + // In protobuf JSON, position data is nested under "position" object + JsonObject positionObj = null; + if (gnssObj.has("position") && gnssObj.get("position").isJsonObject()) { + positionObj = gnssObj.getAsJsonObject("position"); + } else { + // Fallback: try top-level fields if no nested structure + positionObj = gnssObj; + } + + // Handle both "relativeTimestamp" and "relative_timestamp" + if (positionObj.has("relativeTimestamp")) { + String ts = positionObj.get("relativeTimestamp").getAsString(); + record.relativeTimestamp = Long.parseLong(ts); + } else if (positionObj.has("relative_timestamp")) { + String ts = positionObj.get("relative_timestamp").getAsString(); + record.relativeTimestamp = Long.parseLong(ts); + } + + // Extract latitude + if (positionObj.has("latitude")) { + record.latitude = positionObj.get("latitude").getAsDouble(); + } + + // Extract longitude + if (positionObj.has("longitude")) { + record.longitude = positionObj.get("longitude").getAsDouble(); + } + + gnssList.add(record); + } catch (Exception e) { + Log.w(TAG, "Failed to parse GNSS record " + i, e); + } } + return gnssList; }/** Finds the closest IMU record to the given timestamp. */ private static ImuRecord findClosestImuRecord(List imuList, long targetTimestamp) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/data/remote/ServerCommunications.java b/app/src/main/java/com/openpositioning/PositionMe/data/remote/ServerCommunications.java index 80e86283..1e495636 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/data/remote/ServerCommunications.java +++ b/app/src/main/java/com/openpositioning/PositionMe/data/remote/ServerCommunications.java @@ -91,9 +91,6 @@ public class ServerCommunications implements Observable { private static final String userKey = BuildConfig.OPENPOSITIONING_API_KEY; private static final String masterKey = BuildConfig.OPENPOSITIONING_MASTER_KEY; private static final String DEFAULT_CAMPAIGN = "nucleus_building"; - private static final String downloadURL = - "https://openpositioning.org/api/live/trajectory/download/" + userKey - + "?skip=0&limit=30&key=" + masterKey; private static final String infoRequestURL = "https://openpositioning.org/api/live/users/trajectories/" + userKey + "?key=" + masterKey; @@ -155,6 +152,19 @@ private static String buildUploadURL(String campaign) { + campaign + "/" + userKey + "/?key=" + masterKey; } + /** + * Builds the download URL for a specific trajectory window. + */ + private static String buildDownloadURL(int skip, int limit) { + return "https://openpositioning.org/api/live/trajectory/download/" + userKey + + "?skip=" + Math.max(0, skip) + + "&limit=" + Math.max(1, limit) + + "&key=" + masterKey; + } + + private static final int ID_RETRY_WINDOW_RADIUS = 75; + private static final int ID_RETRY_WINDOW_LIMIT = ID_RETRY_WINDOW_RADIUS * 2 + 1; + /** * Public default constructor of {@link ServerCommunications}. The constructor saves context, * initialises a {@link ConnectivityManager}, {@link Observer} and gets the user preferences. @@ -576,9 +586,13 @@ public void downloadTrajectory(int position, String id, String dateSubmitted) { // Initialise OkHttp client OkHttpClient client = new OkHttpClient(); + // Fetch only the selected row to avoid out-of-window mismatches (e.g., position > 29). + String requestedDownloadURL = buildDownloadURL(position, 1); + Log.i("DOWNLOAD", "Requesting trajectory window skip=" + position + " limit=1 id=" + id); + // Create GET request with required header okhttp3.Request request = new okhttp3.Request.Builder() - .url(downloadURL) + .url(requestedDownloadURL) .addHeader("accept", PROTOCOL_ACCEPT_TYPE) .get() .build(); @@ -595,34 +609,139 @@ public void onResponse(Call call, Response response) throws IOException { try (ResponseBody responseBody = response.body()) { if (!response.isSuccessful()) throw new IOException("Unexpected code " + response); - // Extract the nth entry from the zip + // Extract entries and pick a meaningful trajectory robustly. + byte[] selectedBytes = null; + String selectedEntryName = null; + String selectionReason = null; + final boolean hasIdHint = id != null && !id.isEmpty(); + byte[] idMatchedBytes = null; + String idMatchedEntryName = null; + byte[] indexMatchedBytes = null; + String indexMatchedEntryName = null; + byte[] firstMeaningfulBytes = null; + String firstMeaningfulEntryName = null; + int entryIndex = 0; InputStream inputStream = responseBody.byteStream(); ZipInputStream zipInputStream = new ZipInputStream(inputStream); - - java.util.zip.ZipEntry zipEntry; - int zipCount = 0; - while ((zipEntry = zipInputStream.getNextEntry()) != null) { - if (zipCount == position) { - // break if zip entry position matches the desired position - break; + try { + java.util.zip.ZipEntry zipEntry; + while ((zipEntry = zipInputStream.getNextEntry()) != null) { + ByteArrayOutputStream byteArrayOutputStream = new ByteArrayOutputStream(); + byte[] buffer = new byte[1024]; + int bytesRead; + while ((bytesRead = zipInputStream.read(buffer)) != -1) { + byteArrayOutputStream.write(buffer, 0, bytesRead); + } + + byte[] entryBytes = byteArrayOutputStream.toByteArray(); + String entryName = zipEntry.getName(); + Traj.Trajectory parsedEntry; + try { + parsedEntry = Traj.Trajectory.parseFrom(entryBytes); + } catch (Exception parseEx) { + Log.w("DOWNLOAD", "Skipping non-protobuf zip entry index=" + entryIndex + + " name=" + entryName, parseEx); + zipInputStream.closeEntry(); + entryIndex++; + continue; + } + + boolean meaningful = isTrajectoryMeaningful(parsedEntry); + boolean indexMatch = entryIndex == position; + boolean idMatch = false; + if (hasIdHint) { + String trajectoryId = parsedEntry.getTrajectoryId(); + idMatch = (trajectoryId != null && id.equals(trajectoryId)) + || (entryName != null && entryName.contains(id)); + } + + if (idMatch && !meaningful) { + Log.w("DOWNLOAD", "Ignoring id-matched but empty trajectory entry index=" + + entryIndex + " name=" + entryName + " id=" + id); + } + + if (idMatch && meaningful && idMatchedBytes == null) { + idMatchedBytes = entryBytes; + idMatchedEntryName = entryName; + } + + if (indexMatch && meaningful && indexMatchedBytes == null) { + indexMatchedBytes = entryBytes; + indexMatchedEntryName = entryName; + } + + // Fallback only when no ID hint is provided. + if (firstMeaningfulBytes == null && meaningful) { + firstMeaningfulBytes = entryBytes; + firstMeaningfulEntryName = entryName; + } + + zipInputStream.closeEntry(); + entryIndex++; } - zipCount++; + } finally { + zipInputStream.close(); + inputStream.close(); } - // Initialise a byte array output stream - ByteArrayOutputStream byteArrayOutputStream = new ByteArrayOutputStream(); - - // Read the zipped data and write it to the byte array output stream - byte[] buffer = new byte[1024]; - int bytesRead; - while ((bytesRead = zipInputStream.read(buffer)) != -1) { - byteArrayOutputStream.write(buffer, 0, bytesRead); + if (hasIdHint) { + if (idMatchedBytes != null) { + selectedBytes = idMatchedBytes; + selectedEntryName = idMatchedEntryName; + selectionReason = "id"; + } else { + int retrySkip = Math.max(0, position - ID_RETRY_WINDOW_RADIUS); + String retryUrl = buildDownloadURL(retrySkip, ID_RETRY_WINDOW_LIMIT); + Log.w("DOWNLOAD", "No id match in primary window; retrying broader window skip=" + + retrySkip + " limit=" + ID_RETRY_WINDOW_LIMIT + " id=" + id); + + okhttp3.Request retryRequest = new okhttp3.Request.Builder() + .url(retryUrl) + .addHeader("accept", PROTOCOL_ACCEPT_TYPE) + .get() + .build(); + + try (Response retryResponse = client.newCall(retryRequest).execute()) { + if (!retryResponse.isSuccessful() || retryResponse.body() == null) { + Log.e("DOWNLOAD", "Retry request failed for id=" + id + + " code=" + retryResponse.code()); + return; + } + TrajectoryIdMatch retryMatch = findMeaningfulIdMatch( + retryResponse.body(), id); + if (retryMatch != null) { + selectedBytes = retryMatch.bytes; + selectedEntryName = retryMatch.entryName; + selectionReason = "id-retry-window"; + } else { + Log.e("DOWNLOAD", "No meaningful id-matched trajectory found in zip for id=" + + id + " position=" + position + + "; refusing to fall back to a different trajectory."); + return; + } + } + } + } else if (indexMatchedBytes != null) { + selectedBytes = indexMatchedBytes; + selectedEntryName = indexMatchedEntryName; + selectionReason = "index"; + } else if (firstMeaningfulBytes != null) { + selectedBytes = firstMeaningfulBytes; + selectedEntryName = firstMeaningfulEntryName; + selectionReason = "first-meaningful"; } + if (selectedBytes == null) { + Log.e("DOWNLOAD", "No valid trajectory found in zip for position=" + position + + " id=" + id); + return; + } - // Convert the byte array to protobuf - byte[] byteArray = byteArrayOutputStream.toByteArray(); - Traj.Trajectory receivedTrajectory = Traj.Trajectory.parseFrom(byteArray); + Traj.Trajectory receivedTrajectory = Traj.Trajectory.parseFrom(selectedBytes); + Log.i("DOWNLOAD", "Selected zip entry for replay name=" + selectedEntryName + + " position=" + position + " id=" + id + + " meaningful=" + isTrajectoryMeaningful(receivedTrajectory) + + " reason=" + selectionReason); // Inspect the size of the received trajectory logDataSize(receivedTrajectory); @@ -645,23 +764,93 @@ public void onResponse(Call call, Response response) throws IOException { System.err.println("Received trajectory stored in: " + file.getAbsolutePath()); } catch (IOException ee) { System.err.println("Trajectory download failed"); - } finally { - // Close all streams and entries to release resources - zipInputStream.closeEntry(); - byteArrayOutputStream.close(); - zipInputStream.close(); - inputStream.close(); } // Save the download record saveDownloadRecord(startTimestamp, fileName, id, dateSubmitted); loadDownloadRecords(); + + // Notify UI to transition to Replay + notifyObservers(1); } } }); } + private boolean isTrajectoryMeaningful(Traj.Trajectory trajectory) { + if (trajectory == null) { + return false; + } + return trajectory.getImuDataCount() > 0 + || trajectory.getPdrDataCount() > 0 + || trajectory.getGnssDataCount() > 0 + || trajectory.getCorrectedPositionsCount() > 0; + } + + private static class TrajectoryIdMatch { + final byte[] bytes; + final String entryName; + + TrajectoryIdMatch(byte[] bytes, String entryName) { + this.bytes = bytes; + this.entryName = entryName; + } + } + + private TrajectoryIdMatch findMeaningfulIdMatch(ResponseBody responseBody, String id) + throws IOException { + InputStream inputStream = responseBody.byteStream(); + ZipInputStream zipInputStream = new ZipInputStream(inputStream); + int entryIndex = 0; + try { + java.util.zip.ZipEntry zipEntry; + while ((zipEntry = zipInputStream.getNextEntry()) != null) { + ByteArrayOutputStream byteArrayOutputStream = new ByteArrayOutputStream(); + byte[] buffer = new byte[1024]; + int bytesRead; + while ((bytesRead = zipInputStream.read(buffer)) != -1) { + byteArrayOutputStream.write(buffer, 0, bytesRead); + } + + byte[] entryBytes = byteArrayOutputStream.toByteArray(); + String entryName = zipEntry.getName(); + + Traj.Trajectory parsedEntry; + try { + parsedEntry = Traj.Trajectory.parseFrom(entryBytes); + } catch (Exception parseEx) { + Log.w("DOWNLOAD", "Retry scan skipping non-protobuf zip entry index=" + + entryIndex + " name=" + entryName, parseEx); + zipInputStream.closeEntry(); + entryIndex++; + continue; + } + + String trajectoryId = parsedEntry.getTrajectoryId(); + boolean idMatch = (trajectoryId != null && id.equals(trajectoryId)) + || (entryName != null && entryName.contains(id)); + boolean meaningful = isTrajectoryMeaningful(parsedEntry); + + if (idMatch && meaningful) { + return new TrajectoryIdMatch(entryBytes, entryName); + } + + if (idMatch) { + Log.w("DOWNLOAD", "Retry scan found id match but trajectory is empty index=" + + entryIndex + " name=" + entryName + " id=" + id); + } + + zipInputStream.closeEntry(); + entryIndex++; + } + return null; + } finally { + zipInputStream.close(); + inputStream.close(); + } + } + /** * API request for information about submitted trajectories. If the response is successful, * the {@link ServerCommunications#infoResponse} field is updated and observes notified. @@ -729,12 +918,22 @@ private void logDataSize(Traj.Trajectory trajectory) { Log.i(tag, "Proximity Data size: " + trajectory.getProximityDataCount()); Log.i(tag, "PDR Data size: " + trajectory.getPdrDataCount()); Log.i(tag, "GNSS Data size: " + trajectory.getGnssDataCount()); + Log.i(tag, "Corrected positions size: " + trajectory.getCorrectedPositionsCount()); Log.i(tag, "WiFi fingerprints size: " + trajectory.getWifiFingerprintsCount()); Log.i(tag, "APS Data size: " + trajectory.getApsDataCount()); Log.i(tag, "WiFi RTT Data size: " + trajectory.getWifiRttDataCount()); Log.i(tag, "BLE fingerprints size: " + trajectory.getBleFingerprintsCount()); Log.i(tag, "BLE Data size: " + trajectory.getBleDataCount()); Log.i(tag, "Test points size: " + trajectory.getTestPointsCount()); + + if (trajectory.hasInitialPosition()) { + Traj.GNSSPosition initial = trajectory.getInitialPosition(); + Log.i(tag, "Initial position present: true lat=" + initial.getLatitude() + + " lon=" + initial.getLongitude() + + " ts=" + initial.getRelativeTimestamp()); + } else { + Log.w(tag, "Initial position present: false"); + } } /** diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java index 3dde48cd..6d31aeb6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/MainActivity.java @@ -15,7 +15,6 @@ import androidx.annotation.NonNull; import androidx.appcompat.app.AlertDialog; import androidx.appcompat.app.AppCompatActivity; -import androidx.appcompat.app.AppCompatDelegate; import androidx.appcompat.widget.Toolbar; import androidx.core.content.ContextCompat; @@ -34,6 +33,7 @@ import com.openpositioning.PositionMe.sensors.SensorFusion; import com.openpositioning.PositionMe.service.SensorCollectionService; import com.openpositioning.PositionMe.utils.PermissionManager; +import com.openpositioning.PositionMe.utils.ThemePreferences; import java.util.Objects; @@ -90,7 +90,7 @@ public class MainActivity extends AppCompatActivity implements Observer { @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); - AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_NO); + ThemePreferences.applyThemeFromPreferences(this); setContentView(R.layout.activity_main); // Set up navigation and fragments @@ -102,8 +102,6 @@ protected void onCreate(Bundle savedInstanceState) { Toolbar toolbar = findViewById(R.id.main_toolbar); setSupportActionBar(toolbar); toolbar.showOverflowMenu(); - toolbar.setBackgroundColor(ContextCompat.getColor(getApplicationContext(), R.color.md_theme_light_surface)); - toolbar.setTitleTextColor(ContextCompat.getColor(getApplicationContext(), R.color.black)); toolbar.setNavigationIcon(R.drawable.ic_baseline_back_arrow); // Set up back action with NavigationUI diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java index 9497848e..c739998a 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/RecordingActivity.java @@ -16,6 +16,7 @@ import com.openpositioning.PositionMe.presentation.fragment.StartLocationFragment; import com.openpositioning.PositionMe.presentation.fragment.RecordingFragment; import com.openpositioning.PositionMe.presentation.fragment.CorrectionFragment; +import com.openpositioning.PositionMe.utils.ThemePreferences; /** @@ -48,6 +49,7 @@ public class RecordingActivity extends AppCompatActivity { @Override protected void onCreate(@Nullable Bundle savedInstanceState) { super.onCreate(savedInstanceState); + ThemePreferences.applyThemeFromPreferences(this); setContentView(R.layout.activity_recording); if (savedInstanceState == null) { diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java index c6a30472..438984e6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/activity/ReplayActivity.java @@ -10,6 +10,7 @@ import com.openpositioning.PositionMe.R; import com.openpositioning.PositionMe.presentation.fragment.ReplayFragment; import com.openpositioning.PositionMe.presentation.fragment.StartLocationFragment; +import com.openpositioning.PositionMe.utils.ThemePreferences; /** @@ -51,6 +52,7 @@ public class ReplayActivity extends AppCompatActivity { @Override protected void onCreate(@Nullable Bundle savedInstanceState) { super.onCreate(savedInstanceState); + ThemePreferences.applyThemeFromPreferences(this); setContentView(R.layout.activity_replay); // Get the trajectory file path from the Intent filePath = getIntent().getStringExtra(EXTRA_TRAJECTORY_FILE_PATH); diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java index 654c4bfd..4d576657 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/HomeFragment.java @@ -12,7 +12,6 @@ import android.widget.Button; import android.widget.TextView; import android.util.Log; -import android.widget.Toast; import androidx.annotation.NonNull; import androidx.annotation.Nullable; @@ -54,7 +53,6 @@ public class HomeFragment extends Fragment implements OnMapReadyCallback { private Button start; private Button measurements; private Button files; - private Button indoorButton; private TextView gnssStatusTextView; // For the map @@ -121,12 +119,6 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat Navigation.findNavController(v).navigate(action); }); - // Indoor Positioning button - indoorButton = view.findViewById(R.id.indoorButton); - indoorButton.setOnClickListener(v -> { - Toast.makeText(requireContext(), R.string.indoor_mode_hint, Toast.LENGTH_SHORT).show(); - }); - // TextView to display GNSS disabled message gnssStatusTextView = view.findViewById(R.id.gnssStatusTextView); diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java index 340843ca..be53566d 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/RecordingFragment.java @@ -83,6 +83,7 @@ public class RecordingFragment extends Fragment { private float distance = 0f; private float previousPosX = 0f; private float previousPosY = 0f; + private LatLng lastWifiObservation; // References to the child map fragment private TrajectoryMapFragment trajectoryMapFragment; @@ -264,44 +265,67 @@ private void updateUIandPosition() { float elevationVal = sensorFusion.getElevation(); elevation.setText(getString(R.string.elevation, String.format("%.1f", elevationVal))); - // Current location - // Convert PDR coordinates to actual LatLng if you have a known starting lat/lon - // Or simply pass relative data for the TrajectoryMapFragment to handle - // For example: - float[] latLngArray = sensorFusion.getGNSSLatitude(true); - if (latLngArray != null) { - LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); // or store locally - LatLng newLocation = UtilFunctions.calculateNewPos( - oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, - new float[]{ pdrValues[0] - previousPosX, pdrValues[1] - previousPosY } - ); - - // Pass the location + orientation to the map + // Current location: use fused estimate when available, otherwise keep PDR dead-reckoning fallback. + LatLng fusedLocation = sensorFusion.getFusedLatLng(); + if (fusedLocation != null) { if (trajectoryMapFragment != null) { - trajectoryMapFragment.updateUserLocation(newLocation, + trajectoryMapFragment.updateUserLocation( + fusedLocation, (float) Math.toDegrees(sensorFusion.passOrientation())); } + } else { + float[] latLngArray = sensorFusion.getGNSSLatitude(true); + if (latLngArray != null) { + LatLng oldLocation = trajectoryMapFragment.getCurrentLocation(); + LatLng newLocation = UtilFunctions.calculateNewPos( + oldLocation == null ? new LatLng(latLngArray[0], latLngArray[1]) : oldLocation, + new float[]{pdrValues[0] - previousPosX, pdrValues[1] - previousPosY} + ); + + if (trajectoryMapFragment != null) { + trajectoryMapFragment.updateUserLocation(newLocation, + (float) Math.toDegrees(sensorFusion.passOrientation())); + } + } } // GNSS logic if you want to show GNSS error, etc. float[] gnss = sensorFusion.getSensorValueMap().get(SensorTypes.GNSSLATLONG); if (gnss != null && trajectoryMapFragment != null) { - // If user toggles showing GNSS in the map, call e.g. + LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); + // Always forward GNSS observations so GNSS circles are shown regardless of toggle. + trajectoryMapFragment.updateGNSS(gnssLocation); + if (trajectoryMapFragment.isGnssEnabled()) { - LatLng gnssLocation = new LatLng(gnss[0], gnss[1]); LatLng currentLoc = trajectoryMapFragment.getCurrentLocation(); if (currentLoc != null) { double errorDist = UtilFunctions.distanceBetweenPoints(currentLoc, gnssLocation); gnssError.setVisibility(View.VISIBLE); gnssError.setText(String.format(getString(R.string.gnss_error) + "%.2fm", errorDist)); } - trajectoryMapFragment.updateGNSS(gnssLocation); } else { gnssError.setVisibility(View.GONE); trajectoryMapFragment.clearGNSS(); } } + if (trajectoryMapFragment != null) { + LatLng wifiLocation = sensorFusion.getLatLngWifiPositioning(); + if (wifiLocation != null && (lastWifiObservation == null + || !lastWifiObservation.equals(wifiLocation))) { + trajectoryMapFragment.updateWiFiObservation(wifiLocation); + lastWifiObservation = wifiLocation; + } + + float[] startLatLng = sensorFusion.getGNSSLatitude(true); + if (startLatLng != null && !(startLatLng[0] == 0f && startLatLng[1] == 0f)) { + LatLng pdrAbsolute = UtilFunctions.calculateNewPos( + new LatLng(startLatLng[0], startLatLng[1]), + new float[]{pdrValues[0], pdrValues[1]}); + trajectoryMapFragment.updatePdrObservation(pdrAbsolute); + } + } + // Update previous previousPosX = pdrValues[0]; previousPosY = pdrValues[1]; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java index d15a4a83..68069820 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/ReplayFragment.java @@ -131,19 +131,32 @@ public void onViewCreated(@NonNull View view, .commit(); } + // In replay mode every recorded position must be drawn; bypass the live-recording + // distance gate so even densely-sampled trajectories render fully. + trajectoryMapFragment.setReplayMode(true); - // 1) Check if the file contains any GNSS data + + // 1) Determine best initial camera position: + // Priority: user-chosen start → first replay point's actual position + LatLng bestInitialPosition = null; + if (initialLat != 0f || initialLon != 0f) { + bestInitialPosition = new LatLng(initialLat, initialLon); + } else if (!replayData.isEmpty() && replayData.get(0).pdrLocation != null) { + // GPS wasn't fixed at replay time — fall back to the trajectory's own start + bestInitialPosition = replayData.get(0).pdrLocation; + Log.i(TAG, "No GPS fix for camera; using first trajectory point: " + bestInitialPosition); + } + + // 2) Check if the file contains any GNSS data boolean gnssExists = hasAnyGnssData(replayData); if (gnssExists) { - showGnssChoiceDialog(); + showGnssChoiceDialog(bestInitialPosition); } else { - // No GNSS data -> automatically use param lat/lon - if (initialLat != 0f || initialLon != 0f) { - LatLng startPoint = new LatLng(initialLat, initialLon); - Log.i(TAG, "Setting initial map position: " + startPoint.toString()); - trajectoryMapFragment.setInitialCameraPosition(startPoint); + if (bestInitialPosition != null) { + Log.i(TAG, "Setting initial map position: " + bestInitialPosition); + trajectoryMapFragment.setInitialCameraPosition(bestInitialPosition); } } @@ -235,6 +248,9 @@ public void onProgressChanged(SeekBar seekBar, int progress, boolean fromUser) { * Checks if any ReplayPoint contains a non-null gnssLocation. */ private boolean hasAnyGnssData(List data) { + if (data == null || data.isEmpty()) { + return false; + } for (TrajParser.ReplayPoint point : data) { if (point.gnssLocation != null) { return true; @@ -247,32 +263,47 @@ private boolean hasAnyGnssData(List data) { /** * Show a simple dialog asking user to pick: * 1) GNSS from file - * 2) Lat/Lon from ReplayActivity arguments + * 2) Lat/Lon from ReplayActivity arguments (or trajectory start as fallback) + * + * @param fallbackPosition the best available camera position when the user + * chooses "Manual Set" (may be null if nothing is known) */ - private void showGnssChoiceDialog() { + private void showGnssChoiceDialog(LatLng fallbackPosition) { new AlertDialog.Builder(requireContext()) .setTitle("Choose Starting Location") .setMessage("GNSS data is found in the file. Would you like to use the file's GNSS as the start, or the one you manually picked?") .setPositiveButton("Use File's GNSS", (dialog, which) -> { LatLng firstGnss = getFirstGnssLocation(replayData); if (firstGnss != null) { - setupInitialMapPosition((float) firstGnss.latitude, (float) firstGnss.longitude); + reanchorReplayToGnss(firstGnss); + applyReplayStartPosition(firstGnss); } else { - // Fallback if no valid GNSS found - setupInitialMapPosition(initialLat, initialLon); + if (fallbackPosition != null) { + applyReplayStartPosition(fallbackPosition); + } } dialog.dismiss(); }) .setNegativeButton("Use Manual Set", (dialog, which) -> { - setupInitialMapPosition(initialLat, initialLon); + if (fallbackPosition != null) { + applyReplayStartPosition(fallbackPosition); + } dialog.dismiss(); }) .setCancelable(false) .show(); } + private void applyReplayStartPosition(@NonNull LatLng start) { + Log.i(TAG, "Setting replay start position: " + start); + trajectoryMapFragment.setInitialCameraPosition(start); + // Seed indoor map selection from replay start, independent of tester live location. + trajectoryMapFragment.updateUserLocation(start, 0f); + trajectoryMapFragment.refreshIndoorMapForLocation(start); + } + private void setupInitialMapPosition(float latitude, float longitude) { - LatLng startPoint = new LatLng(initialLat, initialLon); + LatLng startPoint = new LatLng(latitude, longitude); Log.i(TAG, "Setting initial map position: " + startPoint.toString()); trajectoryMapFragment.setInitialCameraPosition(startPoint); } @@ -283,12 +314,47 @@ private void setupInitialMapPosition(float latitude, float longitude) { private LatLng getFirstGnssLocation(List data) { for (TrajParser.ReplayPoint point : data) { if (point.gnssLocation != null) { - return new LatLng(replayData.get(0).gnssLocation.latitude, replayData.get(0).gnssLocation.longitude); + return point.gnssLocation; } } return null; // None found } + /** + * Re-anchor replayed PDR points so playback starts at the chosen GNSS origin. + * This avoids stale initial-position metadata forcing trajectories to a wrong building. + */ + private void reanchorReplayToGnss(@NonNull LatLng targetStartGnss) { + if (replayData == null || replayData.isEmpty()) { + return; + } + + TrajParser.ReplayPoint firstPoint = replayData.get(0); + if (firstPoint.pdrLocation == null) { + return; + } + + double deltaLat = targetStartGnss.latitude - firstPoint.pdrLocation.latitude; + double deltaLng = targetStartGnss.longitude - firstPoint.pdrLocation.longitude; + + // Skip tiny floating-point differences. + if (Math.abs(deltaLat) < 1e-9 && Math.abs(deltaLng) < 1e-9) { + return; + } + + for (TrajParser.ReplayPoint point : replayData) { + if (point.pdrLocation == null) { + continue; + } + point.pdrLocation = new LatLng( + point.pdrLocation.latitude + deltaLat, + point.pdrLocation.longitude + deltaLng); + } + + Log.i(TAG, "Re-anchored replay to file GNSS start: dLat=" + deltaLat + + " dLng=" + deltaLng + " points=" + replayData.size()); + } + /** * Runnable for playback of trajectory data. @@ -329,7 +395,9 @@ private void updateMapForIndex(int newIndex) { boolean isSequentialForward = (newIndex == lastIndex + 1); if (!isSequentialForward) { - // Clear everything and redraw up to newIndex + // Clear everything and redraw history up to newIndex. + // Camera moves are suppressed during the bulk loop (replay mode) to avoid + // dozens of rapid camera jumps; we recentre on the final point afterwards. trajectoryMapFragment.clearMapAndReset(); for (int i = 0; i <= newIndex; i++) { TrajParser.ReplayPoint p = replayData.get(i); @@ -338,6 +406,11 @@ private void updateMapForIndex(int newIndex) { trajectoryMapFragment.updateGNSS(p.gnssLocation); } } + // Recentre camera on the current position after the seek + TrajParser.ReplayPoint current = replayData.get(newIndex); + if (current.pdrLocation != null) { + trajectoryMapFragment.setInitialCameraPosition(current.pdrLocation); + } } else { // Normal sequential forward step: add just the new point TrajParser.ReplayPoint p = replayData.get(newIndex); @@ -345,6 +418,10 @@ private void updateMapForIndex(int newIndex) { if (p.gnssLocation != null) { trajectoryMapFragment.updateGNSS(p.gnssLocation); } + // Keep camera centred on the moving point during playback + if (p.pdrLocation != null) { + trajectoryMapFragment.setInitialCameraPosition(p.pdrLocation); + } } lastIndex = newIndex; diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java index c1f6501c..f2d983d3 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/SettingsFragment.java @@ -4,9 +4,11 @@ import android.text.InputType; import androidx.preference.EditTextPreference; +import androidx.preference.ListPreference; import androidx.preference.PreferenceFragmentCompat; import com.openpositioning.PositionMe.R; +import com.openpositioning.PositionMe.utils.ThemePreferences; /** * SettingsFragment that inflates and displays the preferences (settings). @@ -25,6 +27,7 @@ public class SettingsFragment extends PreferenceFragmentCompat { private EditTextPreference epsilon; private EditTextPreference accelFilter; private EditTextPreference wifiInterval; + private ListPreference themeMode; /** * {@inheritDoc} @@ -53,5 +56,13 @@ public void onCreatePreferences(Bundle savedInstanceState, String rootKey) { wifiInterval.setOnBindEditTextListener(editText -> editText.setInputType( InputType.TYPE_CLASS_NUMBER)); + themeMode = findPreference(ThemePreferences.KEY_THEME_MODE); + if (themeMode != null) { + themeMode.setOnPreferenceChangeListener((preference, newValue) -> { + ThemePreferences.applyThemeMode(String.valueOf(newValue)); + return true; + }); + } + } } \ No newline at end of file diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java index 0951e85a..c602ddf1 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/StartLocationFragment.java @@ -1,7 +1,9 @@ package com.openpositioning.PositionMe.presentation.fragment; import android.graphics.Color; +import android.os.Handler; import android.os.Bundle; +import android.os.Looper; import android.util.Log; import android.view.LayoutInflater; import android.view.View; @@ -51,9 +53,11 @@ public class StartLocationFragment extends Fragment { private static final String TAG = "StartLocationFragment"; + private static final long INITIAL_ESTIMATE_POLL_MS = 500L; // UI elements private Button button; + private Button loadingButton; private TextView instructionText; private View buildingInfoCard; private TextView buildingNameText; @@ -73,6 +77,22 @@ public class StartLocationFragment extends Fragment { private final List buildingPolygons = new ArrayList<>(); private final Map floorplanBuildingMap = new HashMap<>(); private Polygon selectedPolygon; + private boolean userAdjustedStartPosition; + private boolean initialEstimateApplied; + private boolean floorplanRequestInFlight; + private boolean floorplanDataReady; + + private final Handler initialEstimateHandler = new Handler(Looper.getMainLooper()); + private final Runnable initialEstimatePoller = new Runnable() { + @Override + public void run() { + if (!isAdded()) { + return; + } + updateSetButtonAvailability(); + initialEstimateHandler.postDelayed(this, INITIAL_ESTIMATE_POLL_MS); + } + }; // Vector shapes drawn as floor plan preview (cleared when switching buildings) private final List previewPolygons = new ArrayList<>(); @@ -161,6 +181,7 @@ public void onMarkerDragStart(Marker marker) {} public void onMarkerDragEnd(Marker marker) { startPosition[0] = (float) marker.getPosition().latitude; startPosition[1] = (float) marker.getPosition().longitude; + userAdjustedStartPosition = true; } @Override @@ -183,6 +204,9 @@ public void onMarkerDrag(Marker marker) {} */ private void requestBuildingData() { FloorplanApiClient apiClient = new FloorplanApiClient(); + floorplanRequestInFlight = true; + floorplanDataReady = false; + updateSetButtonAvailability(); // Collect observed WiFi AP MAC addresses from latest scan List observedMacs = new ArrayList<>(); @@ -201,14 +225,20 @@ private void requestBuildingData() { new FloorplanApiClient.FloorplanCallback() { @Override public void onSuccess(List buildings) { - if (!isAdded() || mMap == null) return; - + floorplanRequestInFlight = false; + floorplanDataReady = true; sensorFusion.setFloorplanBuildings(buildings); floorplanBuildingMap.clear(); for (FloorplanApiClient.BuildingInfo building : buildings) { floorplanBuildingMap.put(building.getName(), building); } + if (!isAdded() || mMap == null) { + return; + } + + updateSetButtonAvailability(); + if (buildings.isEmpty()) { Log.d(TAG, "No buildings returned by API"); if (instructionText != null) { @@ -222,9 +252,12 @@ public void onSuccess(List buildings) { @Override public void onFailure(String error) { - if (!isAdded()) return; + floorplanRequestInFlight = false; + floorplanDataReady = true; sensorFusion.setFloorplanBuildings(new ArrayList<>()); floorplanBuildingMap.clear(); + if (!isAdded()) return; + updateSetButtonAvailability(); Log.e(TAG, "Floorplan API failed: " + error); } }); @@ -306,6 +339,7 @@ private void onBuildingSelected(String buildingName, Polygon polygon) { } startPosition[0] = (float) center.latitude; startPosition[1] = (float) center.longitude; + userAdjustedStartPosition = true; // Zoom to the building mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(center, 20f)); @@ -315,6 +349,7 @@ private void onBuildingSelected(String buildingName, Polygon polygon) { // Update UI with building name updateBuildingInfoDisplay(buildingName); + updateSetButtonAvailability(); Log.d(TAG, "Building selected: " + buildingName); } @@ -454,11 +489,23 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat super.onViewCreated(view, savedInstanceState); this.button = view.findViewById(R.id.startLocationDone); + this.loadingButton = view.findViewById(R.id.startLocationLoading); this.instructionText = view.findViewById(R.id.correctionInfoView); this.buildingInfoCard = view.findViewById(R.id.buildingInfoCard); this.buildingNameText = view.findViewById(R.id.buildingNameText); + updateSetButtonAvailability(); + if (requireActivity() instanceof RecordingActivity) { + initialEstimateHandler.post(initialEstimatePoller); + } + this.button.setOnClickListener(v -> { + if (requireActivity() instanceof RecordingActivity + && !(hasInitialEstimate() && hasRequiredFloorplanDataForRecording())) { + updateSetButtonAvailability(); + return; + } + float chosenLat = startPosition[0]; float chosenLon = startPosition[1]; @@ -483,4 +530,78 @@ public void onViewCreated(@NonNull View view, @Nullable Bundle savedInstanceStat } }); } + + @Override + public void onDestroyView() { + initialEstimateHandler.removeCallbacks(initialEstimatePoller); + super.onDestroyView(); + } + + private boolean hasInitialEstimate() { + return sensorFusion.getFusedLatLng() != null; + } + + private void updateSetButtonAvailability() { + if (button == null || !isAdded()) { + return; + } + + boolean isReplayFlow = requireActivity() instanceof ReplayActivity; + boolean hasEstimate = hasInitialEstimate(); + boolean hasFloorplan = hasRequiredFloorplanDataForRecording(); + boolean ready = isReplayFlow || (hasEstimate && hasFloorplan); + + button.setEnabled(ready); + button.setAlpha(ready ? 1.0f : 0.5f); + if (loadingButton != null) { + loadingButton.setVisibility(ready ? View.GONE : View.VISIBLE); + } + button.setVisibility(ready ? View.VISIBLE : View.GONE); + + if (!isReplayFlow && instructionText != null) { + if (!hasEstimate) { + instructionText.setText(R.string.initialEstimateWaiting); + } else if (!hasFloorplan) { + instructionText.setText(R.string.floorplanLoadingWaiting); + } else { + instructionText.setText(R.string.buildingSelectionInstructions); + } + } + + applyInitialEstimateToMarkerIfNeeded(ready, isReplayFlow); + } + + private boolean hasRequiredFloorplanDataForRecording() { + if (requireActivity() instanceof ReplayActivity) { + return true; + } + + if (selectedBuildingId != null && !selectedBuildingId.isEmpty()) { + FloorplanApiClient.BuildingInfo selectedBuilding = + sensorFusion.getFloorplanBuilding(selectedBuildingId); + return selectedBuilding != null + && selectedBuilding.getFloorShapesList() != null + && !selectedBuilding.getFloorShapesList().isEmpty(); + } + + return floorplanDataReady && !floorplanRequestInFlight; + } + + private void applyInitialEstimateToMarkerIfNeeded(boolean ready, boolean isReplayFlow) { + if (!ready || isReplayFlow || initialEstimateApplied || userAdjustedStartPosition + || mMap == null || startMarker == null) { + return; + } + + LatLng fusedPosition = sensorFusion.getFusedLatLng(); + if (fusedPosition == null) { + return; + } + + startPosition[0] = (float) fusedPosition.latitude; + startPosition[1] = (float) fusedPosition.longitude; + startMarker.setPosition(fusedPosition); + mMap.animateCamera(CameraUpdateFactory.newLatLngZoom(fusedPosition, zoom)); + initialEstimateApplied = true; + } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java index 479ea51b..232f8838 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java +++ b/app/src/main/java/com/openpositioning/PositionMe/presentation/fragment/TrajectoryMapFragment.java @@ -57,20 +57,40 @@ public class TrajectoryMapFragment extends Fragment { + private static final int MAX_OBSERVATION_MARKERS = 20; + private static final long TRAJECTORY_APPEND_MIN_INTERVAL_MS = 500; + private static final double TRAJECTORY_APPEND_MIN_METERS = 0.70; + + // When true the distance gate is bypassed so every recorded point is drawn + private boolean replayMode = false; + private static final double OBSERVATION_CIRCLE_RADIUS_M = 1.4; + private static final long GNSS_OBSERVATION_TTL_MS = 15000; + private static final long WIFI_OBSERVATION_TTL_MS = 20000; + private GoogleMap gMap; // Google Maps instance private LatLng currentLocation; // Stores the user's current location private Marker orientationMarker; // Marker representing user's heading private Marker gnssMarker; // GNSS position marker // Keep test point markers so they can be cleared when recording ends private final List testPointMarkers = new ArrayList<>(); + private final List gnssObservationCircles = new ArrayList<>(); + private final List wifiObservationCircles = new ArrayList<>(); + private final List pdrObservationCircles = new ArrayList<>(); + private final List gnssObservationTimesMs = new ArrayList<>(); + private final List wifiObservationTimesMs = new ArrayList<>(); + private final List pdrObservationTimesMs = new ArrayList<>(); private Polyline polyline; // Polyline representing user's movement path private boolean isRed = true; // Tracks whether the polyline color is red private boolean isGnssOn = false; // Tracks if GNSS tracking is enabled + private long lastTrajectoryAppendMs = 0L; + private LatLng lastTrajectoryPoint; private Polyline gnssPolyline; // Polyline for GNSS path private LatLng lastGnssLocation = null; // Stores the last GNSS location + private LatLng lastPdrLocationForViz = null; // Previous PDR position for delta calculation + private LatLng lastFusionLocationForViz = null; // Previous fusion position for delta calculation private LatLng pendingCameraPosition = null; // Stores pending camera movement private boolean hasPendingCameraMove = false; // Tracks if camera needs to move @@ -91,6 +111,14 @@ public class TrajectoryMapFragment extends Fragment { private SwitchMaterial gnssSwitch; private SwitchMaterial autoFloorSwitch; + private SwitchMaterial pdrPointsSwitch; + private SwitchMaterial gnssPointsSwitch; + private SwitchMaterial wifiPointsSwitch; + + // Visibility flags for each observation type + private boolean showPdrPoints = true; + private boolean showGnssPoints = true; + private boolean showWifiPoints = true; private com.google.android.material.floatingactionbutton.FloatingActionButton floorUpButton, floorDownButton; private TextView floorLabel; @@ -123,7 +151,34 @@ public void onViewCreated(@NonNull View view, floorUpButton = view.findViewById(R.id.floorUpButton); floorDownButton = view.findViewById(R.id.floorDownButton); floorLabel = view.findViewById(R.id.floorLabel); - switchColorButton = view.findViewById(R.id.lineColorButton); + switchColorButton = null; // color button removed from layout + pdrPointsSwitch = view.findViewById(R.id.pdrPointsSwitch); + gnssPointsSwitch = view.findViewById(R.id.gnssPointsSwitch); + wifiPointsSwitch = view.findViewById(R.id.wifiPointsSwitch); + + // Collapsible left panel + View leftPanelContent = view.findViewById(R.id.leftPanelContent); + TextView leftToggle = view.findViewById(R.id.leftPanelToggle); + View leftPanelHeader = view.findViewById(R.id.leftPanelHeader); + View.OnClickListener leftPanelToggleClick = v -> { + boolean visible = leftPanelContent.getVisibility() == View.VISIBLE; + leftPanelContent.setVisibility(visible ? View.GONE : View.VISIBLE); + leftToggle.setText(visible ? "▼" : "▲"); + }; + leftToggle.setOnClickListener(leftPanelToggleClick); + leftPanelHeader.setOnClickListener(leftPanelToggleClick); + + // Collapsible right panel + View rightPanelContent = view.findViewById(R.id.rightPanelContent); + TextView rightToggle = view.findViewById(R.id.rightPanelToggle); + View rightPanelHeader = view.findViewById(R.id.rightPanelHeader); + View.OnClickListener rightPanelToggleClick = v -> { + boolean visible = rightPanelContent.getVisibility() == View.VISIBLE; + rightPanelContent.setVisibility(visible ? View.GONE : View.VISIBLE); + rightToggle.setText(visible ? "▼" : "▲"); + }; + rightToggle.setOnClickListener(rightPanelToggleClick); + rightPanelHeader.setOnClickListener(rightPanelToggleClick); // Setup floor up/down UI hidden initially until we know there's an indoor map setFloorControlsVisibility(View.GONE); @@ -149,6 +204,10 @@ public void onMapReady(@NonNull GoogleMap googleMap) { drawBuildingPolygon(); + if (autoFloorSwitch != null && autoFloorSwitch.isChecked()) { + startAutoFloor(); + } + Log.d("TrajectoryMapFragment", "onMapReady: Map is ready!"); @@ -168,20 +227,27 @@ public void onMapReady(@NonNull GoogleMap googleMap) { } }); - // Color switch - switchColorButton.setOnClickListener(v -> { - if (polyline != null) { - if (isRed) { - switchColorButton.setBackgroundColor(Color.BLACK); - polyline.setColor(Color.BLACK); - isRed = false; - } else { - switchColorButton.setBackgroundColor(Color.RED); - polyline.setColor(Color.RED); - isRed = true; - } - } - }); + // Data-point visibility toggles + if (pdrPointsSwitch != null) { + pdrPointsSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + showPdrPoints = isChecked; + setCircleBucketVisible(pdrObservationCircles, isChecked); + }); + } + if (gnssPointsSwitch != null) { + gnssPointsSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + showGnssPoints = isChecked; + setCircleBucketVisible(gnssObservationCircles, isChecked); + }); + } + if (wifiPointsSwitch != null) { + wifiPointsSwitch.setOnCheckedChangeListener((buttonView, isChecked) -> { + showWifiPoints = isChecked; + setCircleBucketVisible(wifiObservationCircles, isChecked); + }); + } + + // color button removed // Auto-floor toggle: start/stop periodic floor evaluation sensorFusion = SensorFusion.getInstance(); @@ -192,6 +258,9 @@ public void onMapReady(@NonNull GoogleMap googleMap) { stopAutoFloor(); } }); + if (autoFloorSwitch.isChecked()) { + startAutoFloor(); + } floorUpButton.setOnClickListener(v -> { // If user manually changes floor, turn off auto floor @@ -237,6 +306,7 @@ private void initMapSettings(GoogleMap map) { polyline = map.addPolyline(new PolylineOptions() .color(Color.RED) .width(5f) + .zIndex(10f) .add() // start empty ); @@ -244,6 +314,7 @@ private void initMapSettings(GoogleMap map) { gnssPolyline = map.addPolyline(new PolylineOptions() .color(Color.BLUE) .width(5f) + .zIndex(10f) .add() // start empty ); } @@ -273,9 +344,10 @@ private void initMapTypeSpinner() { }; ArrayAdapter adapter = new ArrayAdapter<>( requireContext(), - android.R.layout.simple_spinner_dropdown_item, + R.layout.item_map_type_spinner, maps ); + adapter.setDropDownViewResource(R.layout.item_map_type_spinner_dropdown); switchMapSpinner.setAdapter(adapter); switchMapSpinner.setOnItemSelectedListener(new AdapterView.OnItemSelectedListener() { @@ -301,7 +373,20 @@ public void onNothingSelected(AdapterView parent) {} } /** - * Update the user's current location on the map, create or move orientation marker, + * Enables or disables replay mode. + * In replay mode the distance gate in {@link #maybeAppendTrajectoryPoint} is bypassed + * so every recorded position is drawn, regardless of how small the step is. + * Live recording should keep this false so that sensor noise is not committed to + * the polyline when the user is standing still. + * + * @param replay true when this fragment is displaying a recorded-trajectory replay + */ + public void setReplayMode(boolean replay) { + this.replayMode = replay; + } + + /** + * Update the user’s current location on the map, create or move orientation marker, * and append to polyline if the user actually moved. * * @param newLocation The new location to plot. @@ -310,27 +395,37 @@ public void onNothingSelected(AdapterView parent) {} public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { if (gMap == null) return; + // Expire stale GNSS/WiFi observation points even when no new fixes arrive. + pruneExpiredObservations(); + + LatLng displayLocation = newLocation; + // Keep track of current location LatLng oldLocation = this.currentLocation; - this.currentLocation = newLocation; + this.currentLocation = displayLocation; // If no marker, create it if (orientationMarker == null) { orientationMarker = gMap.addMarker(new MarkerOptions() - .position(newLocation) + .position(displayLocation) .flat(true) .title("Current Position") .icon(BitmapDescriptorFactory.fromBitmap( UtilFunctions.getBitmapFromVector(requireContext(), R.drawable.ic_baseline_navigation_24))) ); - gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(newLocation, 19f)); + if (!replayMode) { + gMap.moveCamera(CameraUpdateFactory.newLatLngZoom(displayLocation, 19f)); + } } else { // Update marker position + orientation - orientationMarker.setPosition(newLocation); + orientationMarker.setPosition(displayLocation); orientationMarker.setRotation(orientation); - // Move camera a bit - gMap.moveCamera(CameraUpdateFactory.newLatLng(newLocation)); + // In replay mode skip constant camera follow so seeking doesn't cause + // dozens of rapid camera moves; the camera was already set by setInitialCameraPosition. + if (!replayMode) { + gMap.moveCamera(CameraUpdateFactory.newLatLng(displayLocation)); + } } // Extend polyline if movement occurred @@ -340,28 +435,28 @@ public void updateUserLocation(@NonNull LatLng newLocation, float orientation) { polyline.setPoints(points); }*/ // Extend polyline - if (polyline != null) { - List points = new ArrayList<>(polyline.getPoints()); - - // First position fix: add the first polyline point - if (oldLocation == null) { - points.add(newLocation); - polyline.setPoints(points); - } else if (!oldLocation.equals(newLocation)) { - // Subsequent movement: append a new polyline point - points.add(newLocation); - polyline.setPoints(points); - } - } + maybeAppendTrajectoryPoint(oldLocation, displayLocation); // Update indoor map overlay if (indoorMapManager != null) { - indoorMapManager.setCurrentLocation(newLocation); + indoorMapManager.setCurrentLocation(displayLocation); setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); } } + /** + * Forces indoor-map building detection from a replay/location seed without + * affecting playback state. + */ + public void refreshIndoorMapForLocation(@NonNull LatLng location) { + if (indoorMapManager == null) { + return; + } + indoorMapManager.setCurrentLocation(location); + setFloorControlsVisibility(indoorMapManager.getIsIndoorMapSet() ? View.VISIBLE : View.GONE); + } + /** @@ -418,6 +513,16 @@ public void addTestPointMarker(int index, long timestampMs, @NonNull LatLng posi */ public void updateGNSS(@NonNull LatLng gnssLocation) { if (gMap == null) return; + + addObservationMarker( + gnssObservationCircles, + gnssObservationTimesMs, + gnssLocation, + Color.argb(220, 33, 150, 243), + Color.argb(80, 33, 150, 243), + false, + GNSS_OBSERVATION_TTL_MS); + if (!isGnssOn) return; if (gnssMarker == null) { @@ -442,6 +547,68 @@ public void updateGNSS(@NonNull LatLng gnssLocation) { } } + /** + * Adds a WiFi observation marker (last N points). + */ + public void updateWiFiObservation(@NonNull LatLng wifiLocation) { + addObservationMarker( + wifiObservationCircles, + wifiObservationTimesMs, + wifiLocation, + Color.argb(220, 67, 160, 71), + Color.argb(80, 67, 160, 71), + false, + WIFI_OBSERVATION_TTL_MS); + } + + /** + * Adds a PDR observation marker (last N points). + * Visualized as: currentLocation + (delta_pdr - delta_fusion) to show only the + * current discrepancy between PDR and fusion, without cumulative drift. + */ + public void updatePdrObservation(@NonNull LatLng pdrLocation) { + if (currentLocation == null) return; + + // Calculate deltas from last positions + double pdrDeltaLat = 0; + double pdrDeltaLng = 0; + double fusionDeltaLat = 0; + double fusionDeltaLng = 0; + + if (lastPdrLocationForViz != null) { + pdrDeltaLat = pdrLocation.latitude - lastPdrLocationForViz.latitude; + pdrDeltaLng = pdrLocation.longitude - lastPdrLocationForViz.longitude; + } + + if (lastFusionLocationForViz != null) { + fusionDeltaLat = currentLocation.latitude - lastFusionLocationForViz.latitude; + fusionDeltaLng = currentLocation.longitude - lastFusionLocationForViz.longitude; + } + + // PDR discrepancy: how much PDR moved vs fusion in this step + double discrepancyLat = pdrDeltaLat - fusionDeltaLat; + double discrepancyLng = pdrDeltaLng - fusionDeltaLng; + + // Visualize at: currentLocation + discrepancy + LatLng visualPdrLocation = new LatLng( + currentLocation.latitude + discrepancyLat, + currentLocation.longitude + discrepancyLng + ); + + addObservationMarker( + pdrObservationCircles, + pdrObservationTimesMs, + visualPdrLocation, + Color.argb(220, 251, 140, 0), + Color.argb(80, 251, 140, 0), + false, + Long.MAX_VALUE); + + // Update tracking positions for next call + lastPdrLocationForViz = pdrLocation; + lastFusionLocationForViz = currentLocation; + } + /** * Remove GNSS marker if user toggles it off @@ -482,8 +649,11 @@ private void updateFloorLabel() { public void clearMapAndReset() { stopAutoFloor(); if (autoFloorSwitch != null) { - autoFloorSwitch.setChecked(false); + autoFloorSwitch.setChecked(true); } + startAutoFloor(); + lastPdrLocationForViz = null; + lastFusionLocationForViz = null; if (polyline != null) { polyline.remove(); polyline = null; @@ -502,12 +672,15 @@ public void clearMapAndReset() { } lastGnssLocation = null; currentLocation = null; + lastTrajectoryPoint = null; + lastTrajectoryAppendMs = 0L; // Clear test point markers for (Marker m : testPointMarkers) { m.remove(); } testPointMarkers.clear(); + clearObservationMarkers(); // Re-create empty polylines with your chosen colors @@ -515,14 +688,154 @@ public void clearMapAndReset() { polyline = gMap.addPolyline(new PolylineOptions() .color(Color.RED) .width(5f) + .zIndex(10f) .add()); gnssPolyline = gMap.addPolyline(new PolylineOptions() .color(Color.BLUE) .width(5f) + .zIndex(10f) .add()); } } + private void maybeAppendTrajectoryPoint(@Nullable LatLng oldLocation, + @NonNull LatLng newLocation) { + if (polyline == null) { + return; + } + + List points = new ArrayList<>(polyline.getPoints()); + if (oldLocation == null || points.isEmpty()) { + points.add(newLocation); + polyline.setPoints(points); + lastTrajectoryPoint = newLocation; + lastTrajectoryAppendMs = SystemClock.elapsedRealtime(); + return; + } + + long now = SystemClock.elapsedRealtime(); + double moved = lastTrajectoryPoint == null + ? UtilFunctions.distanceBetweenPoints(oldLocation, newLocation) + : UtilFunctions.distanceBetweenPoints(lastTrajectoryPoint, newLocation); + + // Distance-only gate: never add a point just because time passed. + // WiFi/GNSS noise causes small constant drift in the estimate — a time-based + // condition would commit that drift to the polyline even when standing still. + // In replay mode every recorded position is drawn regardless of distance so + // densely-sampled trajectories are not filtered to a single dot. + if (replayMode || moved >= TRAJECTORY_APPEND_MIN_METERS) { + points.add(newLocation); + polyline.setPoints(points); + lastTrajectoryPoint = newLocation; + lastTrajectoryAppendMs = now; + } + } + + private void addObservationMarker(@NonNull List bucket, + @NonNull List timesBucket, + @NonNull LatLng location, + int strokeColor, + int fillColor, + boolean respectGnssSwitch, + long ttlMs) { + if (gMap == null) { + return; + } + if (respectGnssSwitch && !isGnssOn) { + return; + } + + pruneExpiredObservationCircles(bucket, timesBucket, ttlMs); + + // Determine whether this new circle should start visible based on its bucket's toggle + boolean visibleNow; + if (bucket == gnssObservationCircles) { + visibleNow = showGnssPoints; + } else if (bucket == wifiObservationCircles) { + visibleNow = showWifiPoints; + } else { + visibleNow = showPdrPoints; + } + + Circle circle = gMap.addCircle(new CircleOptions() + .center(location) + .radius(OBSERVATION_CIRCLE_RADIUS_M) + .strokeWidth(2f) + .strokeColor(strokeColor) + .fillColor(fillColor) + .zIndex(3f) + .visible(visibleNow)); + + if (circle == null) { + return; + } + + bucket.add(circle); + timesBucket.add(SystemClock.elapsedRealtime()); + while (bucket.size() > MAX_OBSERVATION_MARKERS) { + Circle stale = bucket.remove(0); + stale.remove(); + if (!timesBucket.isEmpty()) { + timesBucket.remove(0); + } + } + } + + private void pruneExpiredObservations() { + long now = SystemClock.elapsedRealtime(); + pruneExpiredObservationCircles(gnssObservationCircles, gnssObservationTimesMs, + GNSS_OBSERVATION_TTL_MS, now); + pruneExpiredObservationCircles(wifiObservationCircles, wifiObservationTimesMs, + WIFI_OBSERVATION_TTL_MS, now); + } + + private void pruneExpiredObservationCircles(@NonNull List bucket, + @NonNull List timesBucket, + long ttlMs) { + pruneExpiredObservationCircles(bucket, timesBucket, ttlMs, SystemClock.elapsedRealtime()); + } + + private void pruneExpiredObservationCircles(@NonNull List bucket, + @NonNull List timesBucket, + long ttlMs, + long nowMs) { + if (ttlMs == Long.MAX_VALUE) { + return; + } + + while (!bucket.isEmpty() && !timesBucket.isEmpty()) { + long ageMs = nowMs - timesBucket.get(0); + if (ageMs <= ttlMs) { + break; + } + Circle stale = bucket.remove(0); + stale.remove(); + timesBucket.remove(0); + } + } + + private void clearObservationMarkers() { + clearObservationCircles(gnssObservationCircles, gnssObservationTimesMs); + clearObservationCircles(wifiObservationCircles, wifiObservationTimesMs); + clearObservationCircles(pdrObservationCircles, pdrObservationTimesMs); + } + + /** Shows or hides every circle in a bucket without removing them from the map. */ + private void setCircleBucketVisible(@NonNull List bucket, boolean visible) { + for (Circle c : bucket) { + c.setVisible(visible); + } + } + + private void clearObservationCircles(@NonNull List bucket, + @NonNull List timesBucket) { + for (Circle c : bucket) { + c.remove(); + } + bucket.clear(); + timesBucket.clear(); + } + /** * Draw the building polygon on the map *

@@ -623,6 +936,9 @@ private void drawBuildingPolygon() { * of consistent readings). */ private void startAutoFloor() { + if (autoFloorHandler != null && autoFloorTask != null) { + return; + } if (autoFloorHandler == null) { autoFloorHandler = new Handler(Looper.getMainLooper()); } @@ -676,6 +992,7 @@ private void stopAutoFloor() { if (autoFloorHandler != null && autoFloorTask != null) { autoFloorHandler.removeCallbacks(autoFloorTask); } + autoFloorTask = null; lastCandidateFloor = Integer.MIN_VALUE; lastCandidateTime = 0; Log.d(TAG, "Auto-floor stopped"); diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java new file mode 100644 index 00000000..3751e23b --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEngine.java @@ -0,0 +1,1703 @@ +package com.openpositioning.PositionMe.sensors; + +import android.util.Log; + +import com.google.android.gms.maps.model.LatLng; +import com.openpositioning.PositionMe.data.remote.FloorplanApiClient; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.HashMap; +import java.util.List; +import java.util.Locale; +import java.util.regex.Matcher; +import java.util.regex.Pattern; +import java.util.Map; +import java.util.Random; + +/** + * SIR particle filter fusion engine in local East/North coordinates. + * + *

The filter predicts with step-based PDR displacement and updates particle + * weights with GNSS/WiFi absolute fixes. Resampling is triggered when + * the effective particle count falls below a threshold.

+ */ +public class PositionFusionEngine { + + private static final String TAG = "PositionFusionPF"; + private static final boolean DEBUG_LOGS = true; + + private static final double EARTH_RADIUS_M = 6378137.0; + + private static final int PARTICLE_COUNT = 200; + private static final double RESAMPLE_RATIO = 0.5; + private static final double PDR_NOISE_STD_M = 0.55; + private static final double INIT_STD_M = 2.0; + private static final double ROUGHEN_STD_M = 0.15; + private static final double WIFI_SIGMA_M = 3; + private static final double WIFI_HARD_SNAP_DISTANCE_M = 7.0; + private static final double OUTLIER_GATE_SIGMA_MULT_GNSS = 2.8; + private static final double OUTLIER_GATE_SIGMA_MULT_WIFI = 10.0; + private static final double OUTLIER_GATE_MIN_M = 6.0; + private static final double MAX_OUTLIER_SIGMA_SCALE = 4.0; + private static final double GNSS_INDOOR_SIGMA_MULTIPLIER = 6.0; + private static final double GNSS_INDOOR_MIN_SIGMA_M = 18.0; + private static final double OUTPUT_SMOOTHING_ALPHA = 0.45; + private static final double EPS = 1e-300; + private static final double CONNECTOR_RADIUS_M = 3.0; + private static final double LIFT_HORIZONTAL_MAX_M = 0.50; + private static final double ORIENTATION_BIAS_LEARN_RATE = 0.18; + private static final double ORIENTATION_BIAS_MAX_STEP_RAD = Math.toRadians(5.0); + private static final double ORIENTATION_BIAS_MAX_ABS_RAD = Math.toRadians(170.0); + private static final double ORIENTATION_BIAS_MIN_STEP_M = 0.35; + private static final double ORIENTATION_BIAS_MIN_INNOVATION_M = 0.30; + private static final double WIFI_PATTERN_HEADING_MIN_MOVE_M = 1.2; + private static final int WIFI_SNAP_HISTORY_POINTS = 5; + private static final int WIFI_SNAP_MIN_HISTORY_POINTS = 1; + private static final double WIFI_SNAP_MIN_VECTOR_M = 0.80; + private static final double WIFI_SNAP_DIRECTION_MIN_MOVE_M = 0.50; + private static final double ORIENTATION_BIAS_WIFI_PATTERN_LEARN_RATE = 0.8; + private static final double ORIENTATION_BIAS_WIFI_PATTERN_MAX_STEP_RAD = Math.toRadians(10.0); + private static final double ORIENTATION_BIAS_WIFI_SNAP_MAX_STEP_RAD = Math.toRadians(60.0); + private static final boolean ENABLE_WALL_SLIDE = true; + private static final double WALL_STOP_MARGIN_RATIO = 0.02; + private static final double MAX_WALL_SLIDE_M = 0.60; + private static final double WALL_PENALTY_HIT_INCREMENT = .5; + private static final double WALL_PENALTY_DECAY_ON_FREE_MOVE = 0.65; + private static final double WALL_PENALTY_STRENGTH = 0.2; + private static final double WALL_PENALTY_SCORE_MAX = 8.0; + private static final double FIX_WALL_CROSS_PROB_GNSS = 0.35; + private static final double FIX_WALL_CROSS_PROB_WIFI = 0.60; + private static final Pattern FLOOR_NUMBER_PATTERN = Pattern.compile("-?\\d+"); + + private final float floorHeightMeters; + private final Random random = new Random(); + + // Local tangent frame anchor + private double anchorLatDeg; + private double anchorLonDeg; + private boolean hasAnchor; + + private final List particles = new ArrayList<>(PARTICLE_COUNT); + private int fallbackFloor; + private long updateCounter; + private String activeBuildingName; + private final Map floorConstraints = new HashMap<>(); + private double recentStepMotionMeters; + private double recentStepEastMeters; + private double recentStepNorthMeters; + private double headingBiasRad; + private double smoothedEastMeters; + private double smoothedNorthMeters; + private boolean hasSmoothedEstimate; + private double lastWifiFixEastMeters; + private double lastWifiFixNorthMeters; + private int lastWifiFixFloor; + private boolean hasLastWifiFix; + private final List wifiFixHistory = new ArrayList<>(); + private boolean hasSnapOrientationOverride; + private double snapOrientationOverrideRad; + private double latestRawHeadingRad; + private boolean hasLatestRawHeading; + + private static final class Particle { + double xEast; + double yNorth; + int floor; + double weight; + double wallPenaltyScore; + } + + private static final class Point2D { + final double x; + final double y; + + Point2D(double x, double y) { + this.x = x; + this.y = y; + } + } + + private static final class Segment { + final Point2D a; + final Point2D b; + + Segment(Point2D a, Point2D b) { + this.a = a; + this.b = b; + } + } + + private static final class WallIntersection { + final Segment wall; + final double t; + + WallIntersection(Segment wall, double t) { + this.wall = wall; + this.t = t; + } + } + + private static final class FloorConstraint { + final List walls = new ArrayList<>(); + final List stairs = new ArrayList<>(); + final List lifts = new ArrayList<>(); + } + + private static final class WifiFixSample { + final double east; + final double north; + final int floor; + + WifiFixSample(double east, double north, int floor) { + this.east = east; + this.north = north; + this.floor = floor; + } + } + + public PositionFusionEngine(float floorHeightMeters) { + this.floorHeightMeters = floorHeightMeters > 0f ? floorHeightMeters : 4f; + } + + /** + * Re-anchors the local tangent frame and reinitializes all particles. + */ + public synchronized void reset(double latDeg, double lonDeg, int initialFloor) { + anchorLatDeg = latDeg; + anchorLonDeg = lonDeg; + hasAnchor = true; + + fallbackFloor = initialFloor; + headingBiasRad = 0.0; + recentStepEastMeters = 0.0; + recentStepNorthMeters = 0.0; + recentStepMotionMeters = 0.0; + hasSmoothedEstimate = false; + hasLastWifiFix = false; + wifiFixHistory.clear(); + hasSnapOrientationOverride = false; + hasLatestRawHeading = false; + initParticlesAtOrigin(initialFloor); + if (DEBUG_LOGS) { + Log.i(TAG, String.format(Locale.US, + "Reset anchor=(%.7f, %.7f) floor=%d particles=%d headingBiasDeg=%.2f", + latDeg, lonDeg, initialFloor, PARTICLE_COUNT, + Math.toDegrees(headingBiasRad))); + } + } + + /** + * Prediction step: propagate particles using step displacement + process noise. + * + *

When a predicted segment intersects an indoor wall, motion is blocked for + * that particle to keep trajectories inside mapped traversable space.

+ */ + public synchronized void updatePdrDisplacement(float dxEastMeters, float dyNorthMeters) { + if (!hasAnchor || particles.isEmpty()) { + return; + } + + recentStepEastMeters = dxEastMeters; + recentStepNorthMeters = dyNorthMeters; + recentStepMotionMeters = Math.hypot(dxEastMeters, dyNorthMeters); + double correctedDx = dxEastMeters; + double correctedDy = dyNorthMeters; + int blockedByWall = 0; + int slidAlongWall = 0; + int stoppedAtWall = 0; + + for (Particle p : particles) { + double oldX = p.xEast; + double oldY = p.yNorth; + double candidateX = oldX + correctedDx + random.nextGaussian() * PDR_NOISE_STD_M; + double candidateY = oldY + correctedDy + random.nextGaussian() * PDR_NOISE_STD_M; + + WallIntersection hit = firstWallIntersection(p.floor, oldX, oldY, candidateX, candidateY); + if (hit != null) { + blockedByWall++; + + p.wallPenaltyScore = Math.min( + WALL_PENALTY_SCORE_MAX, + p.wallPenaltyScore + WALL_PENALTY_HIT_INCREMENT); + + if (ENABLE_WALL_SLIDE) { + Point2D wallDir = normalize(hit.wall.b.x - hit.wall.a.x, hit.wall.b.y - hit.wall.a.y); + double travelRatio = clamp(hit.t - WALL_STOP_MARGIN_RATIO, 0.0, 1.0); + double baseX = oldX + (candidateX - oldX) * travelRatio; + double baseY = oldY + (candidateY - oldY) * travelRatio; + + double remDx = candidateX - baseX; + double remDy = candidateY - baseY; + double slideMag = remDx * wallDir.x + remDy * wallDir.y; + slideMag = clamp(slideMag, -MAX_WALL_SLIDE_M, MAX_WALL_SLIDE_M); + + double slideX = baseX + wallDir.x * slideMag; + double slideY = baseY + wallDir.y * slideMag; + + if (!crossesWall(p.floor, oldX, oldY, baseX, baseY) + && !crossesWall(p.floor, baseX, baseY, slideX, slideY)) { + p.xEast = slideX; + p.yNorth = slideY; + slidAlongWall++; + continue; + } + + if (!crossesWall(p.floor, oldX, oldY, baseX, baseY)) { + p.xEast = baseX; + p.yNorth = baseY; + stoppedAtWall++; + continue; + } + } + continue; + } + + p.xEast = candidateX; + p.yNorth = candidateY; + p.wallPenaltyScore *= WALL_PENALTY_DECAY_ON_FREE_MOVE; + } + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "Predict dPDRraw=(%.2fE, %.2fN) dPDRcorr=(%.2fE, %.2fN) headingBiasDeg=%.2f noiseStd=%.2f blockedByWall=%d slid=%d stopAtWall=%d", + dxEastMeters, dyNorthMeters, + correctedDx, correctedDy, + Math.toDegrees(headingBiasRad), + PDR_NOISE_STD_M, + blockedByWall, + slidAlongWall, + stoppedAtWall)); + } + } + + /** + * GNSS measurement update. Accuracy is converted into measurement sigma. + */ + public synchronized void updateGnss(double latDeg, double lonDeg, float accuracyMeters) { + // Match WiFi sigma floor so both sources contribute equally indoors. + // When GNSS reports better accuracy outdoors it naturally gets a lower sigma. + double sigma = Math.max(accuracyMeters, 6.0f); + if (isIndoors()) { + sigma = Math.max(sigma * GNSS_INDOOR_SIGMA_MULTIPLIER, GNSS_INDOOR_MIN_SIGMA_M); + } + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "GNSS update lat=%.7f lon=%.7f acc=%.2f sigma=%.2f indoors=%s", + latDeg, lonDeg, accuracyMeters, sigma, String.valueOf(isIndoors()))); + } + applyAbsoluteFix(latDeg, lonDeg, sigma, null); + } + + /** + * WiFi absolute-fix update with fixed sigma and floor hint support. + */ + public synchronized void updateWifi(double latDeg, double lonDeg, int wifiFloor) { + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "WiFi update lat=%.7f lon=%.7f floor=%d sigma=%.2f", + latDeg, lonDeg, wifiFloor, WIFI_SIGMA_M)); + } + applyAbsoluteFix(latDeg, lonDeg, WIFI_SIGMA_M, wifiFloor); + } + + /** + * Floor-transition update from barometer/elevator cues. + * + *

Transitions are allowed only near mapped stairs/lifts when those + * connectors are available for the floor.

+ */ + public synchronized void updateElevation(float elevationMeters, boolean elevatorLikely) { + int floorFromBarometer = Math.round(elevationMeters / floorHeightMeters); + fallbackFloor = floorFromBarometer; + int blockedTransitions = 0; + int allowedTransitions = 0; + if (!particles.isEmpty()) { + for (Particle p : particles) { + if (p.floor == floorFromBarometer) { + continue; + } + + int step = floorFromBarometer > p.floor ? 1 : -1; + int nextFloor = p.floor + step; + if (canUseConnector(p.floor, p.xEast, p.yNorth, elevatorLikely)) { + p.floor = nextFloor; + allowedTransitions++; + } else { + blockedTransitions++; + } + } + } + + if (DEBUG_LOGS && (allowedTransitions > 0 || blockedTransitions > 0)) { + Log.d(TAG, String.format(Locale.US, + "Elevation floor target=%d elevator=%s transitions allowed=%d blocked=%d", + floorFromBarometer, + String.valueOf(elevatorLikely), + allowedTransitions, + blockedTransitions)); + } + } + + /** + * Updates indoor map-matching constraints for the currently containing building. + */ + public synchronized void updateMapMatchingContext( + double currentLatDeg, + double currentLonDeg, + List buildings) { + if (!hasAnchor || buildings == null || buildings.isEmpty()) { + floorConstraints.clear(); + activeBuildingName = null; + return; + } + + FloorplanApiClient.BuildingInfo containing = null; + LatLng current = new LatLng(currentLatDeg, currentLonDeg); + // First try the provided position (which may be the user's chosen start point). + for (FloorplanApiClient.BuildingInfo b : buildings) { + List outline = b.getOutlinePolygon(); + if (outline != null && outline.size() >= 3 && pointInPolygon(current, outline)) { + containing = b; + break; + } + } + + // Fallback: GNSS is often unreliable indoors, placing the fix outside the building. + // If no match by polygon, try the local-frame anchor point (the user's start position) + // which is far more reliable than a live GNSS reading inside a building. + if (containing == null) { + LatLng anchor = new LatLng(anchorLatDeg, anchorLonDeg); + for (FloorplanApiClient.BuildingInfo b : buildings) { + List outline = b.getOutlinePolygon(); + if (outline != null && outline.size() >= 3 && pointInPolygon(anchor, outline)) { + containing = b; + break; + } + } + } + + if (containing == null) { + // Neither GNSS nor anchor is inside any known building outline. + // Keep existing constraints rather than wiping them on a bad reading. + return; + } + + if (containing.getName().equals(activeBuildingName) && !floorConstraints.isEmpty()) { + return; + } + + Map parsed = new HashMap<>(); + List floorShapes = + normalizeFloorOrder(containing.getFloorShapesList()); + for (int i = 0; i < floorShapes.size(); i++) { + FloorplanApiClient.FloorShapes floor = floorShapes.get(i); + Integer logicalFloor = parseLogicalFloor(floor, i); + if (logicalFloor == null) { + continue; + } + FloorConstraint constraint = parsed.get(logicalFloor); + if (constraint == null) { + constraint = new FloorConstraint(); + parsed.put(logicalFloor, constraint); + } + + for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { + String type = feature.getIndoorType(); + List> parts = feature.getParts(); + if (parts == null || parts.isEmpty()) { + continue; + } + + if ("wall".equals(type)) { + for (List part : parts) { + addWallSegments(part, constraint.walls); + } + } else if ("stairs".equals(type) || "lift".equals(type)) { + for (List part : parts) { + Point2D center = toLocalCentroid(part); + if (center == null) { + continue; + } + if ("stairs".equals(type)) { + constraint.stairs.add(center); + } else { + constraint.lifts.add(center); + } + } + } + } + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "Map floor parsed building=%s idx=%d display=%s logical=%d walls=%d stairs=%d lifts=%d", + containing.getName(), + i, + floor.getDisplayName(), + logicalFloor, + constraint.walls.size(), + constraint.stairs.size(), + constraint.lifts.size())); + } + } + + floorConstraints.clear(); + floorConstraints.putAll(parsed); + activeBuildingName = containing.getName(); + if (DEBUG_LOGS) { + Log.i(TAG, String.format(Locale.US, + "Map matching enabled for building=%s floors=%d", + activeBuildingName, + floorConstraints.size())); + } + } + + /** + * Returns the current fused estimate as the weighted particle mean. + */ + public synchronized PositionFusionEstimate getEstimate() { + if (!hasAnchor || particles.isEmpty()) { + return new PositionFusionEstimate(null, fallbackFloor, false); + } + + double meanX = 0.0; + double meanY = 0.0; + Map floorWeights = new HashMap<>(); + + for (Particle p : particles) { + meanX += p.weight * p.xEast; + meanY += p.weight * p.yNorth; + floorWeights.put(p.floor, floorWeights.getOrDefault(p.floor, 0.0) + p.weight); + } + + int bestFloor = fallbackFloor; + double bestFloorWeight = -1.0; + for (Map.Entry entry : floorWeights.entrySet()) { + if (entry.getValue() > bestFloorWeight) { + bestFloor = entry.getKey(); + bestFloorWeight = entry.getValue(); + } + } + + if (!hasSmoothedEstimate) { + smoothedEastMeters = meanX; + smoothedNorthMeters = meanY; + hasSmoothedEstimate = true; + } else { + smoothedEastMeters += OUTPUT_SMOOTHING_ALPHA * (meanX - smoothedEastMeters); + smoothedNorthMeters += OUTPUT_SMOOTHING_ALPHA * (meanY - smoothedNorthMeters); + } + + LatLng latLng = toLatLng(smoothedEastMeters, smoothedNorthMeters); + return new PositionFusionEstimate(latLng, bestFloor, true); + } + + /** + * Absolute-fix measurement update (GNSS/WiFi): reweight, normalize and resample. + */ + private void applyAbsoluteFix(double latDeg, double lonDeg, double sigmaMeters, Integer floorHint) { + if (!hasAnchor) { + reset(latDeg, lonDeg, 0); + return; + } + + if (particles.isEmpty()) { + initParticlesAtOrigin(fallbackFloor); + } + + double[] z = toLocal(latDeg, lonDeg); + double priorMeanEast = 0.0; + double priorMeanNorth = 0.0; + for (Particle p : particles) { + priorMeanEast += p.weight * p.xEast; + priorMeanNorth += p.weight * p.yNorth; + } + + // Innovation is measured against the prior weighted mean in local EN coordinates. + double innovationEast = z[0] - priorMeanEast; + double innovationNorth = z[1] - priorMeanNorth; + double innovationDistance = Math.hypot(innovationEast, innovationNorth); + + // If WiFi is clearly far from the displayed mean, hard-snap particles to the WiFi fix. + if (floorHint != null && innovationDistance >= WIFI_HARD_SNAP_DISTANCE_M) { + recalculateOrientationBiasOnWifiSnap( + z[0], + z[1], + floorHint, + innovationEast, + innovationNorth); + recordWifiFix(z[0], z[1], floorHint); + Log.d(TAG, String.format(Locale.US, + "WiFi hard-snap innovation=%.2fm drift detected, resetting to fix", + innovationDistance)); + for (Particle p : particles) { + p.xEast = z[0] + random.nextGaussian() * (ROUGHEN_STD_M * 0.5); + p.yNorth = z[1] + random.nextGaussian() * (ROUGHEN_STD_M * 0.5); + p.floor = floorHint; + p.weight = 1.0 / particles.size(); + p.wallPenaltyScore = 0.0; + } + smoothedEastMeters = z[0]; + smoothedNorthMeters = z[1]; + hasSmoothedEstimate = true; + updateCounter++; + return; + } + + if (floorHint != null) { + updateOrientationBiasFromWifiPattern(z[0], z[1], floorHint); + recordWifiFix(z[0], z[1], floorHint); + } + + double gateSigmaMultiplier = floorHint == null + ? OUTLIER_GATE_SIGMA_MULT_GNSS + : OUTLIER_GATE_SIGMA_MULT_WIFI; + double gateMeters = Math.max(gateSigmaMultiplier * sigmaMeters, OUTLIER_GATE_MIN_M); + double effectiveSigma = sigmaMeters; + // Outlier damping: inflate sigma instead of discarding large residual fixes. + if (innovationDistance > gateMeters) { + double sigmaScale = Math.min(innovationDistance / gateMeters, MAX_OUTLIER_SIGMA_SCALE); + effectiveSigma = sigmaMeters * sigmaScale; + if (DEBUG_LOGS) { + Log.w(TAG, String.format(Locale.US, + "Outlier damping src=%s innovation=%.2fm gate=%.2fm sigma %.2f->%.2f", + floorHint == null ? "GNSS" : "WiFi", + innovationDistance, + gateMeters, + sigmaMeters, + effectiveSigma)); + } + } + + double effectiveBefore = computeEffectiveSampleSize(); + + double sigma2 = effectiveSigma * effectiveSigma; + double maxLogWeight = Double.NEGATIVE_INFINITY; + double[] logWeights = new double[particles.size()]; + int fixWallBlockedCount = 0; + + for (int i = 0; i < particles.size(); i++) { + Particle p = particles.get(i); + double dx = p.xEast - z[0]; + double dy = p.yNorth - z[1]; + double distance2 = dx * dx + dy * dy; + double logLikelihood = -0.5 * (distance2 / sigma2); + + // Softly down-weight particles with repeated recent wall collisions. + double wallPenaltyFactor = Math.exp(-WALL_PENALTY_STRENGTH * p.wallPenaltyScore); + logLikelihood += Math.log(Math.max(wallPenaltyFactor, EPS)); + + // Map-aware fix gating: avoid rewarding through-wall attraction. + if (crossesWall(p.floor, p.xEast, p.yNorth, z[0], z[1])) { + fixWallBlockedCount++; + double blockedFixProb = floorHint == null + ? FIX_WALL_CROSS_PROB_GNSS + : FIX_WALL_CROSS_PROB_WIFI; + logLikelihood += Math.log(Math.max(blockedFixProb, EPS)); + } + + if (floorHint != null) { + // Soft floor gating: keep mismatch possible, but less probable. + logLikelihood += (p.floor == floorHint) ? Math.log(0.90) : Math.log(0.10); + } + + double logWeight = Math.log(Math.max(p.weight, EPS)) + logLikelihood; + logWeights[i] = logWeight; + if (logWeight > maxLogWeight) { + maxLogWeight = logWeight; + } + } + + double sumW = 0.0; + for (int i = 0; i < particles.size(); i++) { + double normalized = Math.exp(logWeights[i] - maxLogWeight); + particles.get(i).weight = Math.max(normalized, EPS); + sumW += particles.get(i).weight; + } + + if (sumW <= 0.0) { + reinitializeAroundMeasurement(z[0], z[1], floorHint != null ? floorHint : fallbackFloor); + return; + } + + for (Particle p : particles) { + p.weight /= sumW; + } + + if (floorHint != null) { + fallbackFloor = floorHint; + } + + double effectiveN = computeEffectiveSampleSize(); + boolean resampled = false; + if (effectiveN < PARTICLE_COUNT * RESAMPLE_RATIO) { + resampleSystematic(); + roughenParticles(); + resampled = true; + } + + updateOrientationBiasFromInnovation(innovationEast, innovationNorth, floorHint == null ? "GNSS" : "WiFi"); + + updateCounter++; + logUpdateSummary(z[0], z[1], effectiveSigma, floorHint, effectiveBefore, effectiveN, resampled); + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "Fix wall-aware src=%s blockedLOS=%d/%d", + floorHint == null ? "GNSS" : "WiFi", + fixWallBlockedCount, + particles.size())); + } + } + + /** + * Updates the heading-bias calibration from the innovation residual. + * + *

This method learns gyroscope bias by observing the direction difference between + * the predicted step displacement and the absolute-fix correction. It uses a cross-product + * test to determine if the absolute fix is left or right of the step, then applies + * a bounded adaptive update to {@code headingBiasRad}.

+ * + *

The function only updates when:

+ *
    + *
  • Recent step displacement exceeds {@link #ORIENTATION_BIAS_MIN_STEP_M}
    • + *
  • Innovation residual magnitude exceeds {@link #ORIENTATION_BIAS_MIN_INNOVATION_M}
    • + *
+ * + *

The bias delta is clamped per-step to {@link #ORIENTATION_BIAS_MAX_STEP_RAD} and + * the absolute bias is constrained to ±{@link #ORIENTATION_BIAS_MAX_ABS_RAD}.

+ * + * @param innovationEast residual in East (meters), fix_east - predicted_mean_east + * @param innovationNorth residual in North (meters), fix_north - predicted_mean_north + * @param source measurement source ("GNSS" or "WiFi") for logging + */ + private void updateOrientationBiasFromInnovation(double innovationEast, + double innovationNorth, + String source) { + if (recentStepMotionMeters < ORIENTATION_BIAS_MIN_STEP_M) { + return; + } + + double innovationNorm = Math.hypot(innovationEast, innovationNorth); + if (innovationNorm < ORIENTATION_BIAS_MIN_INNOVATION_M) { + return; + } + + double stepNorm2 = recentStepEastMeters * recentStepEastMeters + + recentStepNorthMeters * recentStepNorthMeters; + if (stepNorm2 < 1e-6) { + return; + } + + // cross(step, innovation) tells whether absolute fixes lie left/right of step heading. + double cross = recentStepEastMeters * innovationNorth + - recentStepNorthMeters * innovationEast; + double rawBiasDelta = ORIENTATION_BIAS_LEARN_RATE * (cross / stepNorm2); + double boundedBiasDelta = clamp(rawBiasDelta, + -ORIENTATION_BIAS_MAX_STEP_RAD, + ORIENTATION_BIAS_MAX_STEP_RAD); + + headingBiasRad = clamp(headingBiasRad + boundedBiasDelta, + -ORIENTATION_BIAS_MAX_ABS_RAD, + ORIENTATION_BIAS_MAX_ABS_RAD); + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "HeadingBias update src=%s innovation=(%.2fE,%.2fN)|%.2fm step=(%.2fE,%.2fN)|%.2fm deltaDeg=%.2f biasDeg=%.2f", + source, + innovationEast, + innovationNorth, + innovationNorm, + recentStepEastMeters, + recentStepNorthMeters, + recentStepMotionMeters, + Math.toDegrees(boundedBiasDelta), + Math.toDegrees(headingBiasRad))); + } + } + + /** + * Learns heading bias from consecutive WiFi fixes, even when no hard snap is triggered. + */ + private void updateOrientationBiasFromWifiPattern(double wifiEast, + double wifiNorth, + int wifiFloor) { + if (!hasLastWifiFix || wifiFloor != lastWifiFixFloor) { + lastWifiFixEastMeters = wifiEast; + lastWifiFixNorthMeters = wifiNorth; + lastWifiFixFloor = wifiFloor; + hasLastWifiFix = true; + return; + } + + double wifiDeltaEast = wifiEast - lastWifiFixEastMeters; + double wifiDeltaNorth = wifiNorth - lastWifiFixNorthMeters; + double wifiMoveMeters = Math.hypot(wifiDeltaEast, wifiDeltaNorth); + + lastWifiFixEastMeters = wifiEast; + lastWifiFixNorthMeters = wifiNorth; + + if (wifiMoveMeters < WIFI_PATTERN_HEADING_MIN_MOVE_M + || recentStepMotionMeters < ORIENTATION_BIAS_MIN_STEP_M) { + return; + } + + double stepNorm2 = recentStepEastMeters * recentStepEastMeters + + recentStepNorthMeters * recentStepNorthMeters; + if (stepNorm2 < 1e-6) { + return; + } + + double cross = recentStepEastMeters * wifiDeltaNorth + - recentStepNorthMeters * wifiDeltaEast; + double rawBiasDelta = ORIENTATION_BIAS_WIFI_PATTERN_LEARN_RATE * (cross / stepNorm2); + double boundedBiasDelta = clamp(rawBiasDelta, + -ORIENTATION_BIAS_WIFI_PATTERN_MAX_STEP_RAD, + ORIENTATION_BIAS_WIFI_PATTERN_MAX_STEP_RAD); + + headingBiasRad = clamp(headingBiasRad + boundedBiasDelta, + -ORIENTATION_BIAS_MAX_ABS_RAD, + ORIENTATION_BIAS_MAX_ABS_RAD); + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "WiFi-pattern heading move=(%.2fE,%.2fN)|%.2fm step=(%.2fE,%.2fN)|%.2fm deltaDeg=%.2f biasDeg=%.2f", + wifiDeltaEast, + wifiDeltaNorth, + wifiMoveMeters, + recentStepEastMeters, + recentStepNorthMeters, + recentStepMotionMeters, + Math.toDegrees(boundedBiasDelta), + Math.toDegrees(headingBiasRad))); + } + } + + /** + * Recalculates heading bias at WiFi hard-snap time using previous WiFi fix direction. + */ + private void recalculateOrientationBiasOnWifiSnap(double wifiEast, + double wifiNorth, + int wifiFloor, + double innovationEast, + double innovationNorth) { + if (!hasLastWifiFix || wifiFloor != lastWifiFixFloor) { + lastWifiFixEastMeters = wifiEast; + lastWifiFixNorthMeters = wifiNorth; + lastWifiFixFloor = wifiFloor; + hasLastWifiFix = true; + return; + } + + HeadingMetric wifiHeadingMetric = buildWifiSnapHeadingMetric(wifiEast, wifiNorth, wifiFloor); + + lastWifiFixEastMeters = wifiEast; + lastWifiFixNorthMeters = wifiNorth; + + if (wifiHeadingMetric == null) { + return; + } + + // Publish hard heading override for UI orientation at snap time. + snapOrientationOverrideRad = Math.atan2(wifiHeadingMetric.east, wifiHeadingMetric.north); + hasSnapOrientationOverride = true; + + double wifiHeadingRad = Math.atan2(wifiHeadingMetric.east, wifiHeadingMetric.north); + double previousBiasRad = headingBiasRad; + + if (hasLatestRawHeading) { + // Direct snap alignment: corrected heading = raw heading + bias -> WiFi heading. + double desiredBiasRad = normalizeAngleRad(wifiHeadingRad - latestRawHeadingRad); + headingBiasRad = clamp(desiredBiasRad, + -ORIENTATION_BIAS_MAX_ABS_RAD, + ORIENTATION_BIAS_MAX_ABS_RAD); + } else { + // Fallback when no recent raw heading is available. + double refEast; + double refNorth; + if (recentStepMotionMeters >= ORIENTATION_BIAS_MIN_STEP_M) { + refEast = recentStepEastMeters; + refNorth = recentStepNorthMeters; + } else { + refEast = innovationEast; + refNorth = innovationNorth; + } + + double refNorm2 = refEast * refEast + refNorth * refNorth; + if (refNorm2 < 1e-6) { + return; + } + double refHeadingRad = Math.atan2(refEast, refNorth); + double desiredBiasRad = normalizeAngleRad(wifiHeadingRad - refHeadingRad); + headingBiasRad = clamp(desiredBiasRad, + -ORIENTATION_BIAS_MAX_ABS_RAD, + ORIENTATION_BIAS_MAX_ABS_RAD); + } + double appliedBiasDeltaRad = normalizeAngleRad(headingBiasRad - previousBiasRad); + + if (DEBUG_LOGS) { + Log.d(TAG, String.format(Locale.US, + "WiFi snap heading recalc metric=(%.2fE,%.2fN)|%.2fm hist=%d rel=%.2f rawHeadingDeg=%.2f deltaDeg=%.2f biasDeg=%.2f", + wifiHeadingMetric.east, + wifiHeadingMetric.north, + wifiHeadingMetric.magnitude, + wifiHeadingMetric.samples, + wifiHeadingMetric.reliability, + Math.toDegrees(hasLatestRawHeading ? latestRawHeadingRad : Double.NaN), + Math.toDegrees(appliedBiasDeltaRad), + Math.toDegrees(headingBiasRad))); + } + } + + private void recordWifiFix(double wifiEast, double wifiNorth, int wifiFloor) { + wifiFixHistory.add(new WifiFixSample(wifiEast, wifiNorth, wifiFloor)); + while (wifiFixHistory.size() > WIFI_SNAP_HISTORY_POINTS) { + wifiFixHistory.remove(0); + } + } + + private static final class HeadingMetric { + final double east; + final double north; + final double magnitude; + final int samples; + final double reliability; + + HeadingMetric(double east, double north, double magnitude, int samples, double reliability) { + this.east = east; + this.north = north; + this.magnitude = magnitude; + this.samples = samples; + this.reliability = reliability; + } + } + + private HeadingMetric buildWifiSnapHeadingMetric(double snappedEast, + double snappedNorth, + int wifiFloor) { + double sumUnitEast = 0.0; + double sumUnitNorth = 0.0; + double sumRawEast = 0.0; + double sumRawNorth = 0.0; + int used = 0; + + for (int i = wifiFixHistory.size() - 1; + i >= 0 && used < WIFI_SNAP_HISTORY_POINTS; + i--) { + WifiFixSample sample = wifiFixHistory.get(i); + if (sample.floor != wifiFloor) { + continue; + } + + double vEast = snappedEast - sample.east; + double vNorth = snappedNorth - sample.north; + double vMag = Math.hypot(vEast, vNorth); + if (vMag < WIFI_SNAP_DIRECTION_MIN_MOVE_M) { + continue; + } + + sumUnitEast += vEast / vMag; + sumUnitNorth += vNorth / vMag; + sumRawEast += vEast; + sumRawNorth += vNorth; + used++; + } + + if (used < WIFI_SNAP_MIN_HISTORY_POINTS) { + return null; + } + + double meanEast = sumRawEast / used; + double meanNorth = sumRawNorth / used; + double meanMag = Math.hypot(meanEast, meanNorth); + if (meanMag < WIFI_SNAP_MIN_VECTOR_M) { + return null; + } + + double concentration = Math.hypot(sumUnitEast, sumUnitNorth) / used; + return new HeadingMetric(meanEast, meanNorth, meanMag, used, concentration); + } + + /** + * Returns and clears a pending snap-orientation override, if any. + */ + public synchronized Double consumeSnapOrientationOverrideRad() { + if (!hasSnapOrientationOverride) { + return null; + } + hasSnapOrientationOverride = false; + return snapOrientationOverrideRad; + } + + /** Returns the current PDR heading-bias correction (radians). */ + public synchronized double getHeadingBiasRad() { + return headingBiasRad; + } + + /** Updates the latest raw sensor heading (radians, Android azimuth frame). */ + public synchronized void updateRawHeadingRad(float rawHeadingRad) { + latestRawHeadingRad = rawHeadingRad; + hasLatestRawHeading = true; + } + + private void initParticlesAtOrigin(int initialFloor) { + particles.clear(); + double w = 1.0 / PARTICLE_COUNT; + for (int i = 0; i < PARTICLE_COUNT; i++) { + Particle p = new Particle(); + p.xEast = random.nextGaussian() * INIT_STD_M; + p.yNorth = random.nextGaussian() * INIT_STD_M; + p.floor = initialFloor; + p.weight = w; + p.wallPenaltyScore = 0.0; + particles.add(p); + } + } + + /** Re-seeds particles around the latest absolute measurement when weights collapse. */ + private void reinitializeAroundMeasurement(double x, double y, int floor) { + particles.clear(); + double w = 1.0 / PARTICLE_COUNT; + for (int i = 0; i < PARTICLE_COUNT; i++) { + Particle p = new Particle(); + // Try a few candidate positions to avoid spawning inside a wall + double candidateX = x; + double candidateY = y; + for (int attempt = 0; attempt < 6; attempt++) { + double cx = x + random.nextGaussian() * INIT_STD_M; + double cy = y + random.nextGaussian() * INIT_STD_M; + if (!crossesWall(floor, x, y, cx, cy)) { + candidateX = cx; + candidateY = cy; + break; + } + // Last attempt: fall back to exact measurement point + } + p.xEast = candidateX; + p.yNorth = candidateY; + p.floor = floor; + p.weight = w; + p.wallPenaltyScore = 0.0; + particles.add(p); + } + } + + private double computeEffectiveSampleSize() { + double sumSquared = 0.0; + for (Particle p : particles) { + sumSquared += p.weight * p.weight; + } + if (sumSquared <= 0.0) { + return 0.0; + } + return 1.0 / sumSquared; + } + + /** + * Performs systematic resampling to recover particle diversity when effective count drops. + * + *

This method implements the systematic resampling step of the Sequential Importance + * Resampling (SIR) particle filter. When particle weights become skewed (many particles + * with negligible weight), resampling duplicates high-weight particles and discards + * low-weight ones, restoring the effective particle count and preventing weight collapse.

+ * + *

Algorithm:

+ *
    + *
  1. Compute cumulative distribution function (CDF) of particle weights
    2. + *
  2. Generate evenly-spaced quantile positions u + m/N (deterministic: reduces variance)
    3. + *
  3. For each quantile, find the corresponding particle via CDF lookup
    4. + *
  4. Copy selected particles with reset uniform weight (1/N)
    5. + *
  5. Wall penalty scores are preserved from source particles
    6. + *
+ * + *

After resampling, all particles have equal weight 1/N. The filter then calls + * {@link #roughenParticles()} to add process noise and prevent duplicate collapse.

+ */ + private void resampleSystematic() { + List resampled = new ArrayList<>(PARTICLE_COUNT); + double step = 1.0 / PARTICLE_COUNT; + double u = random.nextDouble() * step; + double cdf = particles.get(0).weight; + int idx = 0; + + for (int m = 0; m < PARTICLE_COUNT; m++) { + double threshold = u + m * step; + while (threshold > cdf && idx < particles.size() - 1) { + idx++; + cdf += particles.get(idx).weight; + } + + Particle src = particles.get(idx); + Particle copy = new Particle(); + copy.xEast = src.xEast; + copy.yNorth = src.yNorth; + copy.floor = src.floor; + copy.weight = step; + copy.wallPenaltyScore = src.wallPenaltyScore; + resampled.add(copy); + } + + particles.clear(); + particles.addAll(resampled); + } + + /** + * Adds process noise to particles after resampling to prevent collapse. + * + *

When systematic resampling duplicates high-weight particles, identical copies + * can cause divergence (filter collapse). This function perturbs each particle's + * position by Gaussian noise (std {@link #ROUGHEN_STD_M}) to restore diversity.

+ * + *

Noise is applied only if the perturbed position does not cross a mapped wall. + * If roughening would violate wall constraints, the particle remains at its + * resampled position.

+ */ + private void roughenParticles() { + for (Particle p : particles) { + double oldX = p.xEast; + double oldY = p.yNorth; + double newX = oldX + random.nextGaussian() * ROUGHEN_STD_M; + double newY = oldY + random.nextGaussian() * ROUGHEN_STD_M; + if (!crossesWall(p.floor, oldX, oldY, newX, newY)) { + p.xEast = newX; + p.yNorth = newY; + } + // If roughening would cross a wall, leave particle in place + } + } + + /** + * Applies heading-bias correction by rotating a step vector. + * + *

Applies a 2D rotation matrix by angle {@code angleRad}. Used to correct + * PDR step displacements when the gyroscope has a known systematic bias relative + * to magnetic north (as learned from absolute-fix innovations).

+ * + *

Formula: [rotated_east; rotated_north] = R(angle) · [east; north] + * where R is the standard 2D CCW rotation matrix.

+ * + * @param east East component of step (meters) + * @param north North component of step (meters) + * @param angleRad rotation angle in radians (positive = CCW) + * @return array [rotated_east, rotated_north] + */ + private static double[] rotateVector(double east, double north, double angleRad) { + double cos = Math.cos(angleRad); + double sin = Math.sin(angleRad); + double rotatedEast = east * cos - north * sin; + double rotatedNorth = east * sin + north * cos; + return new double[]{rotatedEast, rotatedNorth}; + } + + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } + + private static double normalizeAngleRad(double angleRad) { + double result = angleRad; + while (result > Math.PI) result -= 2.0 * Math.PI; + while (result < -Math.PI) result += 2.0 * Math.PI; + return result; + } + + /** + * Projects WGS84 lat/lon coordinates to local East/North meters. + * + *

Establishes a local tangent plane at {@code (anchorLatDeg, anchorLonDeg)} + * and converts global coordinates to meters in that frame. This linearization + * is accurate for small areas (< 1 km2) typical of indoor positioning scenarios.

+ * + *

Projection:

+ *
    + *
  • East (meters) = Deltalon · cos(lat0) · R_earth
    • + *
  • North (meters) = Deltalat · R_earth
    • + *
+ * + * @param latDeg latitude in degrees + * @param lonDeg longitude in degrees + * @return [east_meters, north_meters] in local frame + */ + private double[] toLocal(double latDeg, double lonDeg) { + double lat0Rad = Math.toRadians(anchorLatDeg); + double dLat = Math.toRadians(latDeg - anchorLatDeg); + double dLon = Math.toRadians(lonDeg - anchorLonDeg); + + double east = dLon * EARTH_RADIUS_M * Math.cos(lat0Rad); + double north = dLat * EARTH_RADIUS_M; + return new double[]{east, north}; + } + + /** + * Inverse projection: converts local East/North meters back to WGS84 lat/lon. + * + *

Reverses the local tangent plane transformation performed by {@link #toLocal}. + * Inverts the linearized projection to recover global coordinates.

+ * + *

Inverse projection:

+ *
    + *
  • Δlat (degrees) = north_meters / R_earth
    • + *
  • Δlon (degrees) = east_meters / (R_earth · cos(lat0))
    • + *
+ * + * @param eastMeters position in East direction (meters in local frame) + * @param northMeters position in North direction (meters in local frame) + * @return WGS84 LatLng coordinate + */ + private LatLng toLatLng(double eastMeters, double northMeters) { + double lat0Rad = Math.toRadians(anchorLatDeg); + + double dLat = northMeters / EARTH_RADIUS_M; + double cosLat = Math.cos(lat0Rad); + if (Math.abs(cosLat) < 1e-9) { + cosLat = 1e-9; + } + double dLon = eastMeters / (EARTH_RADIUS_M * cosLat); + + double lat = anchorLatDeg + Math.toDegrees(dLat); + double lon = anchorLonDeg + Math.toDegrees(dLon); + + return new LatLng(lat, lon); + } + + /** + * Attempts to slide a particle along the wall it would cross instead of stopping it dead. + * Projects the intended displacement onto the wall's direction vector and applies the + * parallel component only, so particles continue moving along corridors rather than + * piling up against walls. + * + * @return new [x, y] after sliding, or null if sliding is not possible + */ + private double[] trySlideAlongWall(int floor, double x0, double y0, double cx, double cy) { + FloorConstraint fc = floorConstraints.get(floor); + if (fc == null || fc.walls.isEmpty()) return null; + + Point2D start = new Point2D(x0, y0); + Point2D end = new Point2D(cx, cy); + + for (Segment wall : fc.walls) { + if (!segmentsIntersect(start, end, wall.a, wall.b)) continue; + + double wallDx = wall.b.x - wall.a.x; + double wallDy = wall.b.y - wall.a.y; + double wallLen2 = wallDx * wallDx + wallDy * wallDy; + if (wallLen2 < 1e-9) continue; + + // Project movement onto wall direction + double moveDx = cx - x0; + double moveDy = cy - y0; + double dot = moveDx * wallDx + moveDy * wallDy; + double scale = dot / wallLen2; + + // Apply 70% of the parallel component to leave a small gap from the wall + double slideX = x0 + scale * wallDx * 0.70; + double slideY = y0 + scale * wallDy * 0.70; + + // Discard negligible slides and slides that cross another wall + if (Math.hypot(slideX - x0, slideY - y0) < 0.05) return null; + if (!crossesWall(floor, x0, y0, slideX, slideY)) { + return new double[]{slideX, slideY}; + } + + break; // Sliding is also blocked — fall through to frozen + } + return null; + } + + /** + * Checks if a motion segment crosses any mapped wall on a floor. + * + *

Convenience wrapper that returns true if {@link #firstWallIntersection} finds + * any wall hit, false otherwise. Used for constraint validation during particle + * prediction and during position roughening after resampling.

+ * + * @param floor logical floor ID + * @param x0 starting East position (meters) + * @param y0 starting North position (meters) + * @param x1 ending East position (meters) + * @param y1 ending North position (meters) + * @return true if segment crosses a wall, false if free path + */ + private boolean crossesWall(int floor, double x0, double y0, double x1, double y1) { + return firstWallIntersection(floor, x0, y0, x1, y1) != null; + } + + /** + * Finds the first wall intersection along a particle's motion segment. + * + *

This method searches all mapped wall segments on a floor for the earliest + * intersection along the motion vector from (x0, y0) to (x1, y1). It returns + * the wall segment and the progress parameter t ∈ [0,1] where intersection occurs.

+ * + *

Used for:

+ *
    + *
  • Wall collision detection during PDR prediction
    • + *
  • Blocking or sliding particles that would cross walls
    • + *
  • Height-map aware trajectory constraint validation
    • + *
+ * + * @param floor logical floor ID to query constraints + * @param x0 starting East position (meters in local frame) + * @param y0 starting North position (meters in local frame) + * @param x1 candidate East position (meters in local frame) + * @param y1 candidate North position (meters in local frame) + * @return {@link WallIntersection} with wall segment and smallest t value, or null if no hit + */ + private WallIntersection firstWallIntersection(int floor, double x0, double y0, double x1, double y1) { + FloorConstraint fc = floorConstraints.get(floor); + if (fc == null || fc.walls.isEmpty()) { + return null; + } + + Point2D a = new Point2D(x0, y0); + Point2D b = new Point2D(x1, y1); + WallIntersection best = null; + for (Segment wall : fc.walls) { + if (segmentsIntersect(a, b, wall.a, wall.b)) { + double t = intersectionProgress(a, b, wall.a, wall.b); + if (Double.isNaN(t)) { + t = 1.0; + } + if (best == null || t < best.t) { + best = new WallIntersection(wall, t); + } + } + } + return best; + } + + /** + * Validates whether a floor transition is plausible at the particle position. + * + *

Floor transitions (via stairs or elevators) are only allowed at mapped connector + * locations. This method checks whether the particle's current position is near + * a valid connector (stairs or lift) on the current floor.

+ * + *

Logic:

+ *
    + *
  • If elevator is detected and horizontal motion is minimal (≤ {@link #LIFT_HORIZONTAL_MAX_M}), + * require proximity to a mapped lift
    • + *
  • Otherwise, require proximity to stairs (within {@link #CONNECTOR_RADIUS_M})
    • + *
  • If no connectors are mapped for the floor, allow transition (fail-open)
    • + *
+ * + * @param floor current logical floor ID + * @param x current East position (meters in local frame) + * @param y current North position (meters in local frame) + * @param elevatorLikely true if barometer and motion cues suggest elevator (vertical-only) + * @return true if transition is allowed, false if blocked by connector constraint + */ + private boolean canUseConnector(int floor, double x, double y, boolean elevatorLikely) { + if (floorConstraints.isEmpty()) { + return true; + } + + FloorConstraint fc = floorConstraints.get(floor); + if (fc == null) { + return true; + } + + if (elevatorLikely && recentStepMotionMeters <= LIFT_HORIZONTAL_MAX_M) { + if (!fc.lifts.isEmpty()) { + return isNearAny(fc.lifts, x, y, CONNECTOR_RADIUS_M); + } + return false; + } + + if (!fc.stairs.isEmpty()) { + return isNearAny(fc.stairs, x, y, CONNECTOR_RADIUS_M); + } + + // If stairs are not mapped for this floor, do not hard-block transitions. + return true; + } + + private boolean isNearAny(List points, double x, double y, double radius) { + double r2 = radius * radius; + for (Point2D p : points) { + double dx = p.x - x; + double dy = p.y - y; + if (dx * dx + dy * dy <= r2) { + return true; + } + } + return false; + } + + /** + * Converts a polyline from FloorPlan API into local-frame wall segments. + * + *

Takes a list of WGS84 LatLng points (forming a wall boundary) and converts + * them to sequential segments in the local East/North coordinate system. + * Each pair of consecutive points becomes a {@link Segment} for wall intersection tests.

+ * + * @param points list of LatLng coordinates (≥2 required for a valid wall) + * @param out output list to accumulate converted segments + */ + private void addWallSegments(List points, List out) { + if (points == null || points.size() < 2) { + return; + } + for (int i = 0; i < points.size() - 1; i++) { + Point2D a = toLocalPoint(points.get(i)); + Point2D b = toLocalPoint(points.get(i + 1)); + if (a != null && b != null) { + out.add(new Segment(a, b)); + } + } + } + + /** + * Converts a single WGS84 point to local East/North coordinates. + * + * @param latLng WGS84 coordinate (null safe) + * @return local Point2D, or null if input is null + */ + private Point2D toLocalPoint(LatLng latLng) { + if (latLng == null) { + return null; + } + double[] local = toLocal(latLng.latitude, latLng.longitude); + return new Point2D(local[0], local[1]); + } + + /** + * Computes the centroid of a connector feature (stairs/lift) in local coordinates. + * + *

Takes a polyline (list of LatLng points) representing a stair or lift area, + * converts each point to the local frame, and returns their arithmetic mean. + * Centroid is used as the contact point for floor-transition validation.

+ * + * @param points LatLng coordinates of the connector boundary + * @return centroid as local Point2D, or null if pointlist is empty or all out-of-bounds + */ + private Point2D toLocalCentroid(List points) { + if (points == null || points.isEmpty()) { + return null; + } + + double sx = 0.0; + double sy = 0.0; + int count = 0; + for (LatLng latLng : points) { + Point2D p = toLocalPoint(latLng); + if (p == null) { + continue; + } + sx += p.x; + sy += p.y; + count++; + } + + if (count == 0) { + return null; + } + return new Point2D(sx / count, sy / count); + } + + /** + * Returns floor list sorted by logical floor when labels are parseable. + * Keeping this ordering aligned with indoor map rendering prevents constraints + * from being attached to the wrong logical floor. + */ + private List normalizeFloorOrder( + List input) { + if (input == null || input.isEmpty()) { + return input; + } + + List ordered = new ArrayList<>(input); + Collections.sort(ordered, (a, b) -> { + Integer floorA = parseLogicalFloorFromDisplayName(a == null ? null : a.getDisplayName()); + Integer floorB = parseLogicalFloorFromDisplayName(b == null ? null : b.getDisplayName()); + + if (floorA != null && floorB != null) { + return Integer.compare(floorA, floorB); + } + if (floorA != null) { + return -1; + } + if (floorB != null) { + return 1; + } + return 0; + }); + return ordered; + } + + /** Maps floor display labels (e.g. LG, G, 1, F2) to numeric logical floors. */ + private Integer parseLogicalFloor(FloorplanApiClient.FloorShapes floor, int index) { + if (floor == null) { + return null; + } + + Integer parsed = parseLogicalFloorFromDisplayName(floor.getDisplayName()); + return parsed != null ? parsed : index; + } + + private Integer parseLogicalFloorFromDisplayName(String displayName) { + if (displayName == null) { + return null; + } + + String normalized = displayName.trim().toUpperCase(Locale.US).replace(" ", ""); + if (normalized.isEmpty()) { + return null; + } + + if ("LG".equals(normalized) || "L".equals(normalized) + || "LOWERGROUND".equals(normalized)) { + return -1; + } + if ("G".equals(normalized) || "GF".equals(normalized) + || "GROUND".equals(normalized) || "GROUNDFLOOR".equals(normalized)) { + return 0; + } + + if (normalized.startsWith("F") || normalized.startsWith("L")) { + normalized = normalized.substring(1); + } + + Matcher matcher = FLOOR_NUMBER_PATTERN.matcher(normalized); + if (matcher.matches()) { + try { + return Integer.parseInt(normalized); + } catch (NumberFormatException ignored) { + return null; + } + } + + return null; + } + + /** + * Tests whether a WGS84 point lies inside a polygon using the ray-casting algorithm. + * + *

Counts the number of times a ray from the point crosses polygon edges. + * If the count is odd, the point is inside; if even (including 0), outside. + * Used to determine which building (outline) contains the user's current position.

+ * + * @param point WGS84 coordinate to test + * @param polygon ordered list of WGS84 vertices forming a closed polygon + * @return true if point is inside polygon, false otherwise + */ + private boolean pointInPolygon(LatLng point, List polygon) { + boolean inside = false; + for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) { + double xi = polygon.get(i).longitude; + double yi = polygon.get(i).latitude; + double xj = polygon.get(j).longitude; + double yj = polygon.get(j).latitude; + + boolean intersect = ((yi > point.latitude) != (yj > point.latitude)) + && (point.longitude + < (xj - xi) * (point.latitude - yi) / (yj - yi + 1e-12) + xi); + if (intersect) { + inside = !inside; + } + } + return inside; + } + + /** + * Detects whether two line segments intersect, with robust collinearity handling. + * + *

Uses the orientation method to classify point configurations. Two segments + * intersect if the endpoints of one segment are on opposite sides of the other + * segment's line (orientation test), OR if they are collinear and overlapping + * (onSegment bounding box test).

+ * + *

Used for wall intersection detection and floor-transition validation.

+ * + * @param p1 segment 1 start + * @param p2 segment 1 end + * @param q1 segment 2 start + * @param q2 segment 2 end + * @return true if segments intersect (including touching at endpoints) + */ + private boolean segmentsIntersect(Point2D p1, Point2D p2, Point2D q1, Point2D q2) { + double o1 = orientation(p1, p2, q1); + double o2 = orientation(p1, p2, q2); + double o3 = orientation(q1, q2, p1); + double o4 = orientation(q1, q2, p2); + + if ((o1 > 0) != (o2 > 0) && (o3 > 0) != (o4 > 0)) { + return true; + } + + return (Math.abs(o1) < 1e-9 && onSegment(p1, q1, p2)) + || (Math.abs(o2) < 1e-9 && onSegment(p1, q2, p2)) + || (Math.abs(o3) < 1e-9 && onSegment(q1, p1, q2)) + || (Math.abs(o4) < 1e-9 && onSegment(q1, p2, q2)); + } + + /** + * Computes the orientation of an ordered triplet of points. + * + *

Returns the signed cross product (b - a) × (c - a):

+ *
    + *
  • > 0: c is left of the vector (a → b) (counter-clockwise)
    • + *
  • < 0: c is right of the vector (a → b) (clockwise)
    • + *
  • ≈ 0: points are collinear
    • + *
+ * + *

Used by segment intersection tests and point-in-polygon algorithms.

+ * + * @param a first point + * @param b second point (vector start) + * @param c third point + * @return signed cross product magnitude + */ + private double orientation(Point2D a, Point2D b, Point2D c) { + return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x); + } + + /** + * Normalizes a 2D vector to unit length. + * + *

Returns the unit vector in the direction of (x, y). If the norm is + * negligible (< 1e-9), falls back to unit East vector (1, 0) to avoid + * division by zero and provide a reasonable default direction.

+ * + * @param x East component + * @param y North component + * @return unit vector: (x', y') where x'² + y'² ≈ 1 (or (1, 0) for tiny inputs) + */ + private Point2D normalize(double x, double y) { + double norm = Math.hypot(x, y); + if (norm < 1e-9) { + return new Point2D(1.0, 0.0); + } + return new Point2D(x / norm, y / norm); + } + + /** + * Computes the progress parameter t where two lines intersect. + * + *

Finds the parameter t ∈ [0, 1] along segment AB where it intersects + * line CD. Uses the parametric form: intersection = A + t·(B - A). + * If lines are parallel (cross product ≈ 0), returns NaN.

+ * + *

Used to determine depth of wall hit for collision response (e.g., how far + * along the motion vector the particle would hit a wall).

+ * + * @param a segment start (AB) + * @param b segment end (AB) + * @param c line start (CD) + * @param d line end (CD) + * @return progress t ∈ [0, 1] clamped, or NaN if parallel + */ + private double intersectionProgress(Point2D a, Point2D b, Point2D c, Point2D d) { + double rX = b.x - a.x; + double rY = b.y - a.y; + double sX = d.x - c.x; + double sY = d.y - c.y; + + double rxs = rX * sY - rY * sX; + if (Math.abs(rxs) < 1e-12) { + return Double.NaN; + } + + double qmpX = c.x - a.x; + double qmpY = c.y - a.y; + double t = (qmpX * sY - qmpY * sX) / rxs; + return clamp(t, 0.0, 1.0); + } + + /** + * Checks if point B lies on segment AC (collinearity bounding box test). + * + *

Used when points a, b, c are collinear (determined by orientation). + * This test verifies that B is within the bounding box of segment AC. + * Includes small tolerance (1e-9) for numerical stability.

+ * + * @param a segment start + * @param b point to test + * @param c segment end + * @return true if b is on segment ac, false otherwise + */ + private boolean onSegment(Point2D a, Point2D b, Point2D c) { + return b.x >= Math.min(a.x, c.x) - 1e-9 + && b.x <= Math.max(a.x, c.x) + 1e-9 + && b.y >= Math.min(a.y, c.y) - 1e-9 + && b.y <= Math.max(a.y, c.y) + 1e-9; + } + + private void logUpdateSummary(double zEast, double zNorth, + double sigmaMeters, + Integer floorHint, + double effectiveBefore, + double effectiveAfter, + boolean resampled) { + if (!DEBUG_LOGS || particles.isEmpty()) { + return; + } + + double minW = Double.POSITIVE_INFINITY; + double maxW = 0.0; + double entropy = 0.0; + double meanX = 0.0; + double meanY = 0.0; + int bestFloor = fallbackFloor; + Map floorWeights = new HashMap<>(); + + Particle bestParticle = particles.get(0); + for (Particle p : particles) { + minW = Math.min(minW, p.weight); + maxW = Math.max(maxW, p.weight); + if (p.weight > bestParticle.weight) { + bestParticle = p; + } + + if (p.weight > 0.0) { + entropy -= p.weight * Math.log(p.weight); + } + + meanX += p.weight * p.xEast; + meanY += p.weight * p.yNorth; + floorWeights.put(p.floor, floorWeights.getOrDefault(p.floor, 0.0) + p.weight); + } + + double bestFloorWeight = -1.0; + for (Map.Entry entry : floorWeights.entrySet()) { + if (entry.getValue() > bestFloorWeight) { + bestFloorWeight = entry.getValue(); + bestFloor = entry.getKey(); + } + } + + double entropyNorm = entropy / Math.log(PARTICLE_COUNT); + String source = floorHint == null ? "GNSS" : "WiFi"; + Log.i(TAG, String.format(Locale.US, + "u=%d src=%s z=(%.2fE,%.2fN) sigma=%.2f floorHint=%s Neff=%.1f->%.1f resampled=%s w[min=%.5f max=%.5f Hn=%.3f] mean=(%.2fE,%.2fN) bestP=(%.2fE,%.2fN,f=%d,w=%.5f) bestFloor=%d(%.3f)", + updateCounter, + source, + zEast, + zNorth, + sigmaMeters, + floorHint == null ? "-" : String.valueOf(floorHint), + effectiveBefore, + effectiveAfter, + String.valueOf(resampled), + minW, + maxW, + entropyNorm, + meanX, + meanY, + bestParticle.xEast, + bestParticle.yNorth, + bestParticle.floor, + bestParticle.weight, + bestFloor, + bestFloorWeight)); + } + + /** + * Returns true when the filter has an active mapped building and floor constraints. + * + *

This is used as a coarse indoor detector for tuning measurement trust. + * When indoor map constraints are active, GNSS is usually much less reliable than + * WiFi or PDR, so we downweight it aggressively.

+ */ + private boolean isIndoors() { + return activeBuildingName != null && !floorConstraints.isEmpty(); + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java new file mode 100644 index 00000000..9ef97302 --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/PositionFusionEstimate.java @@ -0,0 +1,42 @@ +package com.openpositioning.PositionMe.sensors; + +import com.google.android.gms.maps.model.LatLng; + +/** + * Immutable snapshot of the fused position estimate. + * + *

This object is intentionally tiny and read-only so UI code can safely + * consume it on every refresh without sharing mutable filter state.

+ */ +public class PositionFusionEstimate { + + private final LatLng latLng; + private final int floor; + private final boolean available; + + /** + * @param latLng fused position in global coordinates, null when unavailable + * @param floor inferred floor index + * @param available true when the estimate is valid for consumption + */ + public PositionFusionEstimate(LatLng latLng, int floor, boolean available) { + this.latLng = latLng; + this.floor = floor; + this.available = available; + } + + /** Returns the fused global position, or null when unavailable. */ + public LatLng getLatLng() { + return latLng; + } + + /** Returns the inferred floor value for this estimate. */ + public int getFloor() { + return floor; + } + + /** Returns whether the estimate contains a usable position. */ + public boolean isAvailable() { + return available; + } +} diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java index 639fc5c2..64db44b6 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorEventHandler.java @@ -22,6 +22,18 @@ */ public class SensorEventHandler { + public interface PdrStepListener { + void onPdrStep(float dxEastMeters, float dyNorthMeters, long relativeTimestampMs); + } + + public interface HeadingBiasProvider { + float getHeadingBiasRad(); + } + + public interface RawHeadingListener { + void onRawHeading(float rawHeadingRad); + } + private static final float ALPHA = 0.8f; private static final long LARGE_GAP_THRESHOLD_MS = 500; @@ -29,6 +41,9 @@ public class SensorEventHandler { private final PdrProcessing pdrProcessing; private final PathView pathView; private final TrajectoryRecorder recorder; + private final PdrStepListener pdrStepListener; + private final HeadingBiasProvider headingBiasProvider; + private final RawHeadingListener rawHeadingListener; // Timestamp tracking private final HashMap lastEventTimestamps = new HashMap<>(); @@ -38,6 +53,9 @@ public class SensorEventHandler { // Acceleration magnitude buffer between steps private final List accelMagnitude = new ArrayList<>(); + private float lastPdrX = 0f; + private float lastPdrY = 0f; + private boolean hasPdrReference = false; /** * Creates a new SensorEventHandler. @@ -50,12 +68,18 @@ public class SensorEventHandler { */ public SensorEventHandler(SensorState state, PdrProcessing pdrProcessing, PathView pathView, TrajectoryRecorder recorder, - long bootTime) { + long bootTime, + PdrStepListener pdrStepListener, + HeadingBiasProvider headingBiasProvider, + RawHeadingListener rawHeadingListener) { this.state = state; this.pdrProcessing = pdrProcessing; this.pathView = pathView; this.recorder = recorder; this.bootTime = bootTime; + this.pdrStepListener = pdrStepListener; + this.headingBiasProvider = headingBiasProvider; + this.rawHeadingListener = rawHeadingListener; } /** @@ -143,6 +167,9 @@ public void handleSensorEvent(SensorEvent sensorEvent) { float[] rotationVectorDCM = new float[9]; SensorManager.getRotationMatrixFromVector(rotationVectorDCM, state.rotation); SensorManager.getOrientation(rotationVectorDCM, state.orientation); + if (rawHeadingListener != null) { + rawHeadingListener.onRawHeading(state.orientation[0]); + } break; case Sensor.TYPE_STEP_DETECTOR: @@ -165,12 +192,31 @@ public void handleSensorEvent(SensorEvent sensorEvent) { + accelMagnitude.size()); } - float[] newCords = this.pdrProcessing.updatePdr( + float headingForPdr = state.orientation[0]; + if (headingBiasProvider != null) { + headingForPdr += headingBiasProvider.getHeadingBiasRad(); + } + + float[] newCords = this.pdrProcessing.updatePdr( stepTime, this.accelMagnitude, - state.orientation[0] + headingForPdr ); + float dx = 0f; + float dy = 0f; + if (hasPdrReference) { + dx = newCords[0] - lastPdrX; + dy = newCords[1] - lastPdrY; + } + lastPdrX = newCords[0]; + lastPdrY = newCords[1]; + hasPdrReference = true; + + if (pdrStepListener != null && hasPdrReference) { + pdrStepListener.onPdrStep(dx, dy, stepTime); + } + this.accelMagnitude.clear(); if (recorder.isRecording()) { @@ -203,5 +249,8 @@ public void logSensorFrequencies() { */ void resetBootTime(long newBootTime) { this.bootTime = newBootTime; + this.hasPdrReference = false; + this.lastPdrX = 0f; + this.lastPdrY = 0f; } } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java index aeb6386a..617b953f 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorFusion.java @@ -11,6 +11,7 @@ import android.os.Handler; import android.os.Looper; import android.os.SystemClock; +import android.util.Log; import android.widget.Toast; import androidx.annotation.NonNull; @@ -51,6 +52,7 @@ public class SensorFusion implements SensorEventListener { //region Static variables private static final SensorFusion sensorFusion = new SensorFusion(); + private static final String TAG = "SensorFusion"; //endregion //region Instance variables @@ -63,6 +65,7 @@ public class SensorFusion implements SensorEventListener { private SensorEventHandler eventHandler; private TrajectoryRecorder recorder; private WifiPositionManager wifiPositionManager; + private PositionFusionEngine fusionEngine; // Movement sensor instances (lifecycle managed here) private MovementSensor accelerometerSensor; @@ -94,6 +97,15 @@ public class SensorFusion implements SensorEventListener { // Floorplan API cache (latest result from start-location step) private final Map floorplanBuildingCache = new HashMap<>(); + + // Trajectory-based heading tracking for arrow orientation + private static final double TRAJECTORY_HEADING_MIN_MOVE_M = 0.40; + private static final float TRAJECTORY_HEADING_BLEND_ALPHA = 0.30f; + private double lastTrajectoryHeadingLatDeg; + private double lastTrajectoryHeadingLonDeg; + private boolean hasTrajectoryHeadingAnchor; + private double trajectoryHeadingRad; + private boolean hasTrajectoryHeading; //endregion //region Initialisation @@ -151,6 +163,7 @@ public void setContext(Context context) { this.pdrProcessing = new PdrProcessing(context); this.pathView = new PathView(context, null); WiFiPositioning wiFiPositioning = new WiFiPositioning(context); + this.fusionEngine = new PositionFusionEngine(settings.getInt("floor_height", 4)); // Create internal modules this.recorder = new TrajectoryRecorder(appContext, state, serverCommunications, settings); @@ -162,7 +175,19 @@ public void setContext(Context context) { long bootTime = SystemClock.uptimeMillis(); this.eventHandler = new SensorEventHandler( - state, pdrProcessing, pathView, recorder, bootTime); + state, pdrProcessing, pathView, recorder, bootTime, + (dxEastMeters, dyNorthMeters, relativeTimestampMs) -> { + fusionEngine.updatePdrDisplacement(dxEastMeters, dyNorthMeters); + fusionEngine.updateElevation(state.elevation, state.elevator); + updateFusedState(); + }, + () -> (float) fusionEngine.getHeadingBiasRad(), + rawHeadingRad -> fusionEngine.updateRawHeadingRad(rawHeadingRad)); + + this.wifiPositionManager.setWifiFixListener((wifiLocation, floor) -> { + fusionEngine.updateWifi(wifiLocation.latitude, wifiLocation.longitude, floor); + updateFusedState(); + }); // Register WiFi observer on WifiPositionManager (not on SensorFusion) this.wifiProcessor = new WifiDataProcessor(context); @@ -421,6 +446,23 @@ public void setFloorplanBuildings(List building continue; } floorplanBuildingCache.put(building.getName(), building); + + List floors = building.getFloorShapesList(); + Log.i(TAG, "Floorplan cache building=" + building.getName() + + " floors=" + floors.size()); + for (int i = 0; i < floors.size(); i++) { + FloorplanApiClient.FloorShapes floor = floors.get(i); + Log.d(TAG, "Floorplan floor index=" + i + + " display=" + floor.getDisplayName() + + " features=" + floor.getFeatures().size()); + } + } + + if (fusionEngine != null) { + fusionEngine.updateMapMatchingContext( + state.latitude, + state.longitude, + getFloorplanBuildings()); } } @@ -490,6 +532,17 @@ public float[] getGNSSLatitude(boolean start) { public void setStartGNSSLatitude(float[] startPosition) { state.startLocation[0] = startPosition[0]; state.startLocation[1] = startPosition[1]; + if (recorder != null) { + recorder.ensureInitialPosition(startPosition[0], startPosition[1]); + } + if (fusionEngine != null) { + fusionEngine.reset(startPosition[0], startPosition[1], 0); + // Anchor is now valid — immediately load wall geometry from cached building data + // so map matching is active from the very first step, not just after the first GNSS fix. + fusionEngine.updateMapMatchingContext( + startPosition[0], startPosition[1], getFloorplanBuildings()); + updateFusedState(); + } } /** @@ -512,11 +565,61 @@ public float passAverageStepLength() { /** * Getter function for device orientation. + * Blends raw sensor orientation toward the trajectory heading computed from + * sustained fused position movement. * - * @return orientation of device in radians. + * @return orientation of device in radians, blended toward trajectory direction. */ public float passOrientation() { - return state.orientation[0]; + float rawHeading = state.orientation[0]; + float correctedHeading = normalizeAngleRad(rawHeading + fusionEngine.getHeadingBiasRad()); + return correctedHeading; + } + + /** + * Wraps an angle in radians to the range [-π, π]. + */ + private float normalizeAngleRad(double angleRad) { + float result = (float) angleRad; + while (result > Math.PI) result -= (float) (2 * Math.PI); + while (result < -Math.PI) result += (float) (2 * Math.PI); + return result; + } + + /** + * Tracks the bearing direction from sustained fused position movement. + * On significant position changes, updates the trajectory heading via bearing calculation. + * This provides a secondary heading reference independent of raw sensor orientation. + */ + private void updateTrajectoryHeading(double latDeg, double lonDeg) { + if (!hasTrajectoryHeadingAnchor) { + lastTrajectoryHeadingLatDeg = latDeg; + lastTrajectoryHeadingLonDeg = lonDeg; + hasTrajectoryHeadingAnchor = true; + hasTrajectoryHeading = false; + return; + } + + // Flat-earth distance in metres + double dLat = (latDeg - lastTrajectoryHeadingLatDeg) * 111320.0; + double dLon = (lonDeg - lastTrajectoryHeadingLonDeg) + * 111320.0 * Math.cos(Math.toRadians(lastTrajectoryHeadingLatDeg)); + double distM = Math.sqrt(dLat * dLat + dLon * dLon); + + if (distM >= TRAJECTORY_HEADING_MIN_MOVE_M) { + // Compute bearing: atan2(E, N) gives bearing from north + double newHeadingRad = Math.atan2(dLon, dLat); + if (!hasTrajectoryHeading) { + trajectoryHeadingRad = newHeadingRad; + hasTrajectoryHeading = true; + } else { + // EMA blend to smooth trajectory heading + float delta = normalizeAngleRad(newHeadingRad - trajectoryHeadingRad); + trajectoryHeadingRad = trajectoryHeadingRad + 0.4f * delta; + } + lastTrajectoryHeadingLatDeg = latDeg; + lastTrajectoryHeadingLonDeg = lonDeg; + } } /** @@ -614,6 +717,23 @@ public LatLng getLatLngWifiPositioning() { return wifiPositionManager.getLatLngWifiPositioning(); } + /** + * Returns the best fused location estimate, if available. + */ + public LatLng getFusedLatLng() { + if (!state.fusedAvailable) { + return null; + } + return new LatLng(state.fusedLatitude, state.fusedLongitude); + } + + /** + * Returns the current fused floor estimate. + */ + public int getFusedFloor() { + return state.fusedFloor; + } + /** * Returns the current floor the user is on, obtained using WiFi positioning. * @@ -644,9 +764,56 @@ class MyLocationListener implements LocationListener { public void onLocationChanged(@NonNull Location location) { state.latitude = (float) location.getLatitude(); state.longitude = (float) location.getLongitude(); + if (recorder != null && recorder.isRecording()) { + recorder.ensureInitialPosition(location.getLatitude(), location.getLongitude()); + } + if (fusionEngine != null) { + // Update GNSS first so the local-frame anchor is established, + // then load wall geometry which is converted into that frame. + fusionEngine.updateGnss( + location.getLatitude(), + location.getLongitude(), + location.getAccuracy()); + fusionEngine.updateMapMatchingContext( + location.getLatitude(), + location.getLongitude(), + getFloorplanBuildings()); + updateFusedState(); + } recorder.addGnssData(location); } } + private void updateFusedState() { + if (fusionEngine == null) { + return; + } + PositionFusionEstimate estimate = fusionEngine.getEstimate(); + if (!estimate.isAvailable() || estimate.getLatLng() == null) { + state.fusedAvailable = false; + return; + } + + state.fusedLatitude = (float) estimate.getLatLng().latitude; + state.fusedLongitude = (float) estimate.getLatLng().longitude; + state.fusedFloor = estimate.getFloor(); + state.fusedAvailable = true; + + // Update trajectory-based heading from fused position movement + updateTrajectoryHeading(estimate.getLatLng().latitude, estimate.getLatLng().longitude); + + if (recorder != null && recorder.isRecording()) { + long relativeTimestamp = SystemClock.uptimeMillis() - recorder.getBootTime(); + if (relativeTimestamp < 0) { + relativeTimestamp = 0; + } + recorder.addCorrectedPosition( + relativeTimestamp, + estimate.getLatLng().latitude, + estimate.getLatLng().longitude, + estimate.getFloor()); + } + } + //endregion } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java index 1554f61d..e5dbdadc 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/SensorState.java @@ -37,6 +37,12 @@ public class SensorState { public volatile float longitude; public final float[] startLocation = new float[2]; + // Fused position estimate + public volatile float fusedLatitude; + public volatile float fusedLongitude; + public volatile int fusedFloor; + public volatile boolean fusedAvailable; + // Step counting public volatile int stepCounter; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java index 8c771758..2cb06709 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/TrajectoryRecorder.java @@ -222,18 +222,45 @@ public void writeInitialMetadata() { trajectory.setTrajectoryId(trajectoryId); } - if (state.startLocation != null - && (state.startLocation[0] != 0 || state.startLocation[1] != 0)) { - trajectory.setInitialPosition( - Traj.GNSSPosition.newBuilder() - .setRelativeTimestamp(0) - .setLatitude(state.startLocation[0]) - .setLongitude(state.startLocation[1]) - .setAltitude(0.0) - ); + if (hasValidStartLocation()) { + setInitialPosition(state.startLocation[0], state.startLocation[1]); + } else if (isValidReplayCoordinate(state.latitude, state.longitude)) { + // Fallback when start location was not explicitly set before recording. + state.startLocation[0] = state.latitude; + state.startLocation[1] = state.longitude; + setInitialPosition(state.startLocation[0], state.startLocation[1]); } } + /** + * Ensures initial_position metadata is present for replay anchoring. + */ + public void ensureInitialPosition(double latitude, double longitude) { + if (trajectory == null) return; + if (!isValidReplayCoordinate(latitude, longitude)) return; + + state.startLocation[0] = (float) latitude; + state.startLocation[1] = (float) longitude; + + if (!trajectory.hasInitialPosition()) { + setInitialPosition(latitude, longitude); + } + } + + private void setInitialPosition(double latitude, double longitude) { + trajectory.setInitialPosition( + Traj.GNSSPosition.newBuilder() + .setRelativeTimestamp(0) + .setLatitude(latitude) + .setLongitude(longitude) + .setAltitude(0.0) + ); + } + + private boolean hasValidStartLocation() { + return isValidReplayCoordinate(state.startLocation[0], state.startLocation[1]); + } + //endregion //region Data writing (called from other modules) @@ -250,6 +277,45 @@ public void addPdrData(long relativeTimestamp, float x, float y) { } } + /** + * Adds a fused/corrected position entry to the trajectory. + */ + public void addCorrectedPosition(long relativeTimestamp, + double latitude, + double longitude, + int floor) { + if (trajectory == null || !saveRecording) return; + + // Guard against replay-breaking placeholder coordinates. + if (!isValidReplayCoordinate(latitude, longitude)) { + return; + } + + if (relativeTimestamp < 0) { + relativeTimestamp = 0; + } + + Traj.GNSSPosition.Builder corrected = Traj.GNSSPosition.newBuilder() + .setRelativeTimestamp(relativeTimestamp) + .setLatitude(latitude) + .setLongitude(longitude) + .setAltitude(0.0); + + corrected.setFloor(String.valueOf(floor)); + trajectory.addCorrectedPositions(corrected); + } + + private boolean isValidReplayCoordinate(double latitude, double longitude) { + if (!Double.isFinite(latitude) || !Double.isFinite(longitude)) { + return false; + } + if (Math.abs(latitude) > 90.0 || Math.abs(longitude) > 180.0) { + return false; + } + // (0,0) is typically an uninitialised fallback and breaks replay focus. + return !(Math.abs(latitude) < 1e-7 && Math.abs(longitude) < 1e-7); + } + /** * Adds a GNSS reading to the trajectory. */ diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java index dbf809dd..3b7af45c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WiFiPositioning.java @@ -4,6 +4,7 @@ import com.android.volley.Request; import com.android.volley.RequestQueue; +import com.android.volley.VolleyError; import com.android.volley.toolbox.JsonObjectRequest; import com.android.volley.toolbox.Volley; import com.google.android.gms.maps.model.LatLng; @@ -30,8 +31,11 @@ public class WiFiPositioning { // Queue for storing the POST requests made private RequestQueue requestQueue; - // URL for WiFi positioning API - private static final String url="https://openpositioning.org/api/position/fine"; + // Try fine first, then fallback to coarse when fine cannot localize this fingerprint. + private static final String[] URL_CANDIDATES = new String[] { + "https://openpositioning.org/api/position/fine", + "https://openpositioning.org/api/position/coarse" + }; /** * Getter for the WiFi positioning coordinates obtained using openpositioning API @@ -81,39 +85,7 @@ public WiFiPositioning(Context context){ * @param jsonWifiFeatures WiFi Fingerprint from device */ public void request(JSONObject jsonWifiFeatures) { - // Creating the POST request using WiFi fingerprint (a JSON object) - JsonObjectRequest jsonObjectRequest = new JsonObjectRequest( - Request.Method.POST, url, jsonWifiFeatures, - // Parses the response to obtain the WiFi location and WiFi floor - response -> { - try { - wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); - floor = response.getInt("floor"); - } catch (JSONException e) { - // Error log to keep record of errors (for secure programming and maintainability) - Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); - } - }, - // Handles the errors obtained from the POST request - error -> { - // Validation Error - if (error.networkResponse!=null && error.networkResponse.statusCode==422){ - Log.e("WiFiPositioning", "Validation Error "+ error.getMessage()); - } - // Other Errors - else{ - // When Response code is available - if (error.networkResponse!=null) { - Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - } - else{ - Log.e("WiFiPositioning","Error message: " + error.getMessage()); - } - } - } - ); - // Adds the request to the request queue - requestQueue.add(jsonObjectRequest); + requestWithRetry(jsonWifiFeatures, 0, null); } @@ -132,44 +104,88 @@ public void request(JSONObject jsonWifiFeatures) { * @param callback callback function to allow user to use location when ready */ public void request( JSONObject jsonWifiFeatures, final VolleyCallback callback) { - // Creating the POST request using WiFi fingerprint (a JSON object) + requestWithRetry(jsonWifiFeatures, 0, callback); + } + + private void requestWithRetry(JSONObject jsonWifiFeatures, + int urlIndex, + final VolleyCallback callback) { + if (urlIndex >= URL_CANDIDATES.length) { + String message = "WiFi positioning failed for all URL candidates"; + Log.e("WiFiPositioning", message); + if (callback != null) { + callback.onError(message); + } + return; + } + + final String url = URL_CANDIDATES[urlIndex]; + Log.d("WiFiPositioning", "Request URL=" + url); JsonObjectRequest jsonObjectRequest = new JsonObjectRequest( - Request.Method.POST, url, jsonWifiFeatures, + Request.Method.POST, url, jsonWifiFeatures, response -> { try { - Log.d("jsonObject",response.toString()); - wifiLocation = new LatLng(response.getDouble("lat"),response.getDouble("lon")); + Log.d("WiFiPositioning", "Response URL=" + url + " body=" + response); + wifiLocation = new LatLng(response.getDouble("lat"), response.getDouble("lon")); floor = response.getInt("floor"); - callback.onSuccess(wifiLocation,floor); + if (callback != null) { + callback.onSuccess(wifiLocation, floor); + } } catch (JSONException e) { - Log.e("jsonErrors","Error parsing response: "+e.getMessage()+" "+ response); - callback.onError("Error parsing response: " + e.getMessage()); + String msg = "Error parsing response from " + url + ": " + e.getMessage(); + Log.e("WiFiPositioning", msg + " " + response); + if (callback != null) { + callback.onError(msg); + } } }, error -> { - // Validation Error - if (error.networkResponse!=null && error.networkResponse.statusCode==422){ - Log.e("WiFiPositioning", "Validation Error "+ error.getMessage()); - callback.onError( "Validation Error (422): "+ error.getMessage()); - } - // Other Errors - else{ - // When Response code is available - if (error.networkResponse!=null) { - Log.e("WiFiPositioning","Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); - callback.onError("Response Code: " + error.networkResponse.statusCode + ", " + error.getMessage()); + int code = error.networkResponse != null ? error.networkResponse.statusCode : -1; + String responseBody = extractErrorBody(error); + Log.e("WiFiPositioning", + "Request failed URL=" + url + + " code=" + code + + " message=" + error.getMessage() + + " body=" + responseBody); + + if (code == 404 && responseBody.contains("Position unknown")) { + // Endpoint is reachable, but this fingerprint is not known at current granularity. + // Fallback from fine -> coarse if available. + if (urlIndex + 1 < URL_CANDIDATES.length) { + requestWithRetry(jsonWifiFeatures, urlIndex + 1, callback); + return; } - else{ - Log.e("WiFiPositioning","Error message: " + error.getMessage()); - callback.onError("Error message: " + error.getMessage()); + if (callback != null) { + callback.onError("Position unknown"); } + return; + } + + if (code == 404 && urlIndex + 1 < URL_CANDIDATES.length) { + requestWithRetry(jsonWifiFeatures, urlIndex + 1, callback); + return; + } + + if (callback != null) { + callback.onError("Response Code: " + code + ", " + + error.getMessage() + ", body=" + responseBody); } } ); - // Adds the request to the request queue requestQueue.add(jsonObjectRequest); } + private String extractErrorBody(VolleyError error) { + if (error == null || error.networkResponse == null || error.networkResponse.data == null) { + return ""; + } + try { + return new String(error.networkResponse.data, "UTF-8"); + } catch (Exception ignored) { + return ""; + } + } + /** * Interface defined for the callback to access response obtained after POST request */ diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java index 9143c7a1..9a90d110 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiDataProcessor.java @@ -40,8 +40,8 @@ */ public class WifiDataProcessor implements Observable { - //Time over which a new scan will be initiated - private static final long scanInterval = 5000; + // Time over which a new scan will be initiated (1 second). + private static final long scanInterval = 1000; // Application context for handling permissions and WifiManager instances private final Context context; diff --git a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java index 1edfd68a..b36ac4c9 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/sensors/WifiPositionManager.java @@ -1,6 +1,7 @@ package com.openpositioning.PositionMe.sensors; import android.util.Log; +import android.os.SystemClock; import com.google.android.gms.maps.model.LatLng; @@ -22,11 +23,25 @@ */ public class WifiPositionManager implements Observer { + public interface WifiFixListener { + void onWifiFix(LatLng wifiLocation, int floor); + } + private static final String WIFI_FINGERPRINT = "wf"; + // Exponential moving average smoothing for WiFi positions. + // Prevents sudden large jumps from individual noisy scan results bugging out the trajectory. + private static final double EMA_ALPHA = 0.80; // stronger pull to newest WiFi fix + private static final double JUMP_THRESHOLD_M = 10.0; // dampen very large jumps + private static final long WIFI_TIME_DECAY_HALF_LIFE_MS = 10000L; + private final WiFiPositioning wiFiPositioning; private final TrajectoryRecorder recorder; private List wifiList; + private WifiFixListener wifiFixListener; + private LatLng smoothedWifiPosition = null; + private int lastSmoothedFloor = 0; + private long lastSmoothedWifiFixMs = 0L; /** * Creates a new WifiPositionManager. @@ -61,11 +76,31 @@ private void createWifiPositioningRequest() { try { JSONObject wifiAccessPoints = new JSONObject(); for (Wifi data : this.wifiList) { - wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); + String bssidKey = getBssidKey(data); + if (bssidKey == null) { + continue; + } + wifiAccessPoints.put(bssidKey, data.getLevel()); + } + if (wifiAccessPoints.length() == 0) { + Log.w("WifiPositionManager", "Skipping WiFi positioning request: no valid BSSID keys"); + return; } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); - this.wiFiPositioning.request(wifiFingerPrint); + this.wiFiPositioning.request(wifiFingerPrint, new WiFiPositioning.VolleyCallback() { + @Override + public void onSuccess(LatLng wifiLocation, int floor) { + if (wifiFixListener != null && wifiLocation != null) { + wifiFixListener.onWifiFix(smoothWifiPosition(wifiLocation, floor), floor); + } + } + + @Override + public void onError(String message) { + Log.e("WifiPositionManager", "WiFi positioning request failed: " + message); + } + }); } catch (JSONException e) { Log.e("jsonErrors", "Error creating json object" + e.toString()); } @@ -78,7 +113,15 @@ private void createWifiPositionRequestCallback() { try { JSONObject wifiAccessPoints = new JSONObject(); for (Wifi data : this.wifiList) { - wifiAccessPoints.put(String.valueOf(data.getBssid()), data.getLevel()); + String bssidKey = getBssidKey(data); + if (bssidKey == null) { + continue; + } + wifiAccessPoints.put(bssidKey, data.getLevel()); + } + if (wifiAccessPoints.length() == 0) { + Log.w("WifiPositionManager", "Skipping WiFi callback request: no valid BSSID keys"); + return; } JSONObject wifiFingerPrint = new JSONObject(); wifiFingerPrint.put(WIFI_FINGERPRINT, wifiAccessPoints); @@ -124,4 +167,98 @@ public int getWifiFloor() { public List getWifiList() { return this.wifiList; } + + /** + * Registers a listener receiving WiFi absolute fixes for downstream fusion. + */ + public void setWifiFixListener(WifiFixListener wifiFixListener) { + this.wifiFixListener = wifiFixListener; + } + + /** + * Applies exponential moving average smoothing to consecutive WiFi positions. + * Large jumps (beyond JUMP_THRESHOLD_M) are dampened to avoid abrupt spikes. + * Smaller jumps use EMA blending with time decay. + * The floor resets the smoothed position when the user changes floor so cross-floor + * averaging is avoided. + */ + private LatLng smoothWifiPosition(LatLng raw, int floor) { + long nowMs = SystemClock.elapsedRealtime(); + if (smoothedWifiPosition == null || floor != lastSmoothedFloor) { + smoothedWifiPosition = raw; + lastSmoothedFloor = floor; + lastSmoothedWifiFixMs = nowMs; + return raw; + } + + // Flat-earth distance in metres between smoothed position and new raw fix + double dLat = (raw.latitude - smoothedWifiPosition.latitude) * 111320.0; + double dLon = (raw.longitude - smoothedWifiPosition.longitude) + * 111320.0 * Math.cos(Math.toRadians(smoothedWifiPosition.latitude)); + double distM = Math.sqrt(dLat * dLat + dLon * dLon); + + // Age-based decay: older WiFi fixes fade faster, newer fixes retain more weight. + long ageMs = Math.max(0L, nowMs - lastSmoothedWifiFixMs); + double timeDecay = Math.pow(0.5, ageMs / (double) WIFI_TIME_DECAY_HALF_LIFE_MS); + double alpha = EMA_ALPHA; + if (distM > JUMP_THRESHOLD_M) { + alpha *= JUMP_THRESHOLD_M / distM; + } + alpha *= timeDecay; + + double smoothLat = smoothedWifiPosition.latitude + + alpha * (raw.latitude - smoothedWifiPosition.latitude); + double smoothLon = smoothedWifiPosition.longitude + + alpha * (raw.longitude - smoothedWifiPosition.longitude); + + smoothedWifiPosition = new LatLng(smoothLat, smoothLon); + lastSmoothedWifiFixMs = nowMs; + Log.d("WifiPositionManager", String.format( + "WiFi EMA raw=(%.6f,%.6f) dist=%.1fm age=%dms alpha=%.2f smooth=(%.6f,%.6f)", + raw.latitude, raw.longitude, distM, ageMs, alpha, + smoothLat, smoothLon)); + return smoothedWifiPosition; + } + + private String getBssidKey(Wifi wifi) { + String bssidString = wifi.getBssidString(); + if (bssidString != null && !bssidString.trim().isEmpty()) { + String normalizedHex = normalizeMacToHex(bssidString.trim()); + if (normalizedHex != null) { + long macValue = Long.parseUnsignedLong(normalizedHex, 16); + return Long.toUnsignedString(macValue); + } + } + + // Fallback: convert packed long (lower 48 bits) to unsigned decimal string. + long mac = wifi.getBssid() & 0x0000FFFFFFFFFFFFL; + return Long.toUnsignedString(mac); + } + + private String normalizeMacToHex(String value) { + if (value == null) { + return null; + } + String normalized = value.replace(":", "").replace("-", "").toUpperCase(); + if (!isValidHexMac(normalized)) { + return null; + } + return normalized; + } + + private boolean isValidHexMac(String value) { + if (value.length() != 12) { + return false; + } + for (int i = 0; i < value.length(); i++) { + char c = value.charAt(i); + boolean digit = c >= '0' && c <= '9'; + boolean upperHex = c >= 'A' && c <= 'F'; + if (!digit && !upperHex) { + return false; + } + } + return true; + } + } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java index f8058603..59f954dd 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/IndoorMapManager.java @@ -13,7 +13,11 @@ import com.openpositioning.PositionMe.sensors.SensorFusion; import java.util.ArrayList; +import java.util.Collections; import java.util.List; +import java.util.Locale; +import java.util.regex.Matcher; +import java.util.regex.Pattern; /** * Manages indoor floor map display for all supported buildings @@ -49,16 +53,42 @@ public class IndoorMapManager { // Per-floor vector shape data for the current building private List currentFloorShapes; + // Outline polygon of the current building — used as the orange base fill + private List currentBuildingOutline; + // Average floor heights per building (meters), used for barometric auto-floor public static final float NUCLEUS_FLOOR_HEIGHT = 4.2F; public static final float LIBRARY_FLOOR_HEIGHT = 3.6F; public static final float MURCHISON_FLOOR_HEIGHT = 4.0F; // Colours for different indoor feature types - private static final int WALL_STROKE = Color.argb(200, 80, 80, 80); - private static final int ROOM_STROKE = Color.argb(180, 33, 150, 243); - private static final int ROOM_FILL = Color.argb(40, 33, 150, 243); - private static final int DEFAULT_STROKE = Color.argb(150, 100, 100, 100); + // Walls — black stroke, transparent fill (structural outlines only) + private static final int WALL_STROKE = Color.argb(255, 0, 0, 0); + + // Rooms — beige fill, dark brown stroke + private static final int ROOM_STROKE = Color.argb(200, 140, 110, 70); + private static final int ROOM_FILL = Color.argb(210, 245, 230, 200); + + // Corridors / hallways — slightly darker beige to distinguish from rooms + private static final int CORRIDOR_STROKE = Color.argb(180, 140, 110, 70); + private static final int CORRIDOR_FILL = Color.argb(180, 225, 210, 175); + + // Stairs — amber/orange so they pop as a navigation landmark + private static final int STAIRS_STROKE = Color.argb(255, 180, 90, 0); + private static final int STAIRS_FILL = Color.argb(220, 255, 160, 40); + + // Lifts / elevators — violet, distinct from stairs + private static final int LIFT_STROKE = Color.argb(255, 110, 40, 180); + private static final int LIFT_FILL = Color.argb(220, 200, 150, 240); + + // Unknown — beige same as rooms + private static final int UNKNOWN_STROKE = Color.argb(160, 140, 110, 70); + private static final int UNKNOWN_FILL = Color.argb(180, 235, 220, 190); + + // Fallback for any unrecognised indoor type — beige fill, dark brown stroke + private static final int DEFAULT_STROKE = Color.argb(200, 140, 110, 70); + private static final int DEFAULT_FILL = Color.argb(210, 245, 230, 200); + private static final Pattern FLOOR_NUMBER_PATTERN = Pattern.compile("-?\\d+"); /** * Constructor to set the map instance. @@ -161,14 +191,27 @@ public int getAutoFloorBias() { public void setCurrentFloor(int newFloor, boolean autoFloor) { if (currentFloorShapes == null || currentFloorShapes.isEmpty()) return; + int requestedFloor = newFloor; + int bias = getAutoFloorBias(); + if (autoFloor) { - newFloor += getAutoFloorBias(); + newFloor += bias; + Log.d(TAG, "Auto-floor request logical=" + requestedFloor + + " bias=" + bias + " -> index=" + newFloor); } if (newFloor >= 0 && newFloor < currentFloorShapes.size() && newFloor != this.currentFloor) { this.currentFloor = newFloor; + Log.i(TAG, "Set floor index=" + newFloor + + " display=" + getCurrentFloorDisplayName() + + " totalFloors=" + currentFloorShapes.size()); drawFloorShapes(newFloor); + } else { + Log.d(TAG, "Ignored floor change request requested=" + requestedFloor + + " mappedIndex=" + newFloor + + " current=" + this.currentFloor + + " totalFloors=" + currentFloorShapes.size()); } } @@ -229,12 +272,34 @@ private void setBuildingOverlay() { FloorplanApiClient.BuildingInfo building = SensorFusion.getInstance().getFloorplanBuilding(apiName); if (building != null) { - currentFloorShapes = building.getFloorShapesList(); + currentBuildingOutline = building.getOutlinePolygon(); + currentFloorShapes = normalizeFloorOrder(building.getFloorShapesList()); + Log.i(TAG, "Loaded floorplan building=" + apiName + + " floors=" + (currentFloorShapes == null ? 0 : currentFloorShapes.size())); + if (currentFloorShapes != null) { + for (int i = 0; i < currentFloorShapes.size(); i++) { + FloorplanApiClient.FloorShapes floorShapes = currentFloorShapes.get(i); + Log.d(TAG, "Floor index=" + i + " display=" + floorShapes.getDisplayName() + + " features=" + floorShapes.getFeatures().size()); + } + } + } + + if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { + int groundFloorIndex = findFloorIndexForLogicalFloor(0); + if (groundFloorIndex >= 0) { + currentFloor = groundFloorIndex; + } else if (currentFloor < 0 || currentFloor >= currentFloorShapes.size()) { + currentFloor = 0; + } } if (currentFloorShapes != null && !currentFloorShapes.isEmpty()) { drawFloorShapes(currentFloor); isIndoorMapSet = true; + Log.i(TAG, "Indoor overlay enabled building=" + apiName + + " startFloorIndex=" + currentFloor + + " display=" + getCurrentFloorDisplayName()); } } else if (!inAnyBuilding && isIndoorMapSet) { @@ -243,6 +308,8 @@ private void setBuildingOverlay() { currentBuilding = BUILDING_NONE; currentFloor = 0; currentFloorShapes = null; + currentBuildingOutline = null; + Log.i(TAG, "Indoor overlay disabled (left mapped buildings)"); } } catch (Exception ex) { Log.e(TAG, "Error with overlay: " + ex.toString()); @@ -261,7 +328,21 @@ private void drawFloorShapes(int floorIndex) { if (currentFloorShapes == null || floorIndex < 0 || floorIndex >= currentFloorShapes.size()) return; + // Draw building outline as a beige base fill so the interior is visible + // and data points don't blend in with the background. + if (currentBuildingOutline != null && currentBuildingOutline.size() >= 3) { + Polygon baseFill = gMap.addPolygon(new PolygonOptions() + .addAll(currentBuildingOutline) + .strokeColor(Color.TRANSPARENT) + .strokeWidth(0f) + .fillColor(Color.argb(210, 245, 230, 200)) + .zIndex(0f)); + drawnPolygons.add(baseFill); + } + FloorplanApiClient.FloorShapes floor = currentFloorShapes.get(floorIndex); + Log.d(TAG, "Draw floor index=" + floorIndex + " display=" + floor.getDisplayName() + + " featureCount=" + floor.getFeatures().size()); for (FloorplanApiClient.MapShapeFeature feature : floor.getFeatures()) { String geoType = feature.getGeometryType(); String indoorType = feature.getIndoorType(); @@ -269,11 +350,15 @@ private void drawFloorShapes(int floorIndex) { if ("MultiPolygon".equals(geoType) || "Polygon".equals(geoType)) { for (List ring : feature.getParts()) { if (ring.size() < 3) continue; + // Walls get a thicker stroke; filled areas get a thinner one + // so the fill colour is the dominant visual cue + float sw = "wall".equals(indoorType) ? 5f : 2.5f; Polygon p = gMap.addPolygon(new PolygonOptions() .addAll(ring) .strokeColor(getStrokeColor(indoorType)) - .strokeWidth(5f) - .fillColor(getFillColor(indoorType))); + .strokeWidth(sw) + .fillColor(getFillColor(indoorType)) + .zIndex(getZIndex(indoorType))); drawnPolygons.add(p); } } else if ("MultiLineString".equals(geoType) @@ -283,7 +368,7 @@ private void drawFloorShapes(int floorIndex) { Polyline pl = gMap.addPolyline(new PolylineOptions() .addAll(line) .color(getStrokeColor(indoorType)) - .width(6f)); + .width("wall".equals(indoorType) ? 6f : 4f)); drawnPolylines.add(pl); } } @@ -300,6 +385,27 @@ private void clearDrawnShapes() { drawnPolylines.clear(); } + /** + * Returns the z-index for a given indoor feature type so that fills render + * underneath structural elements (walls always draw on top). + * + * @param indoorType the indoor_type property value + * @return z-index float + */ + private float getZIndex(String indoorType) { + if (indoorType == null) return 1f; + switch (indoorType) { + case "wall": return 3f; // always on top + case "stairs": + case "lift": + case "elevator": return 2f; // navigation features above room fills + case "room": return 1f; + case "corridor": + case "hallway": return 1f; + default: return 0f; + } + } + /** * Returns the stroke colour for a given indoor feature type. * @@ -307,9 +413,19 @@ private void clearDrawnShapes() { * @return ARGB colour value */ private int getStrokeColor(String indoorType) { - if ("wall".equals(indoorType)) return WALL_STROKE; - if ("room".equals(indoorType)) return ROOM_STROKE; - return DEFAULT_STROKE; + if (indoorType == null) return DEFAULT_STROKE; + switch (indoorType) { + case "wall": return WALL_STROKE; + case "room": return ROOM_STROKE; + case "corridor": + case "hallway": return CORRIDOR_STROKE; + case "stairs": + case "staircase": return STAIRS_STROKE; + case "lift": + case "elevator": return LIFT_STROKE; + case "unknown": return UNKNOWN_STROKE; + default: return DEFAULT_STROKE; + } } /** @@ -319,8 +435,92 @@ private int getStrokeColor(String indoorType) { * @return ARGB colour value */ private int getFillColor(String indoorType) { - if ("room".equals(indoorType)) return ROOM_FILL; - return Color.TRANSPARENT; + if (indoorType == null) return DEFAULT_FILL; + switch (indoorType) { + case "wall": return Color.TRANSPARENT; + case "room": return ROOM_FILL; + case "corridor": + case "hallway": return CORRIDOR_FILL; + case "stairs": + case "staircase": return STAIRS_FILL; + case "lift": + case "elevator": return LIFT_FILL; + case "unknown": return UNKNOWN_FILL; + default: return DEFAULT_FILL; + } + } + + private List normalizeFloorOrder( + List input) { + if (input == null || input.isEmpty()) { + return input; + } + + List ordered = new ArrayList<>(input); + Collections.sort(ordered, (a, b) -> { + Integer floorA = logicalFloorFromDisplayName(a == null ? null : a.getDisplayName()); + Integer floorB = logicalFloorFromDisplayName(b == null ? null : b.getDisplayName()); + + if (floorA != null && floorB != null) { + return Integer.compare(floorA, floorB); + } + if (floorA != null) { + return -1; + } + if (floorB != null) { + return 1; + } + return 0; + }); + return ordered; + } + + private int findFloorIndexForLogicalFloor(int logicalFloor) { + if (currentFloorShapes == null || currentFloorShapes.isEmpty()) { + return -1; + } + for (int i = 0; i < currentFloorShapes.size(); i++) { + FloorplanApiClient.FloorShapes floorShapes = currentFloorShapes.get(i); + Integer candidate = logicalFloorFromDisplayName( + floorShapes == null ? null : floorShapes.getDisplayName()); + if (candidate != null && candidate == logicalFloor) { + return i; + } + } + return -1; + } + + private Integer logicalFloorFromDisplayName(String displayName) { + if (displayName == null) { + return null; + } + + String normalized = displayName.trim().toUpperCase(Locale.US).replace(" ", ""); + if (normalized.isEmpty()) { + return null; + } + + if ("LG".equals(normalized) || "L".equals(normalized) || "LOWERGROUND".equals(normalized)) { + return -1; + } + if ("G".equals(normalized) || "GF".equals(normalized) + || "GROUND".equals(normalized) || "GROUNDFLOOR".equals(normalized)) { + return 0; + } + + if (normalized.startsWith("F") || normalized.startsWith("L")) { + normalized = normalized.substring(1); + } + + Matcher matcher = FLOOR_NUMBER_PATTERN.matcher(normalized); + if (matcher.matches()) { + try { + return Integer.parseInt(normalized); + } catch (NumberFormatException ignored) { + return null; + } + } + return null; } /** diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java index 9765b044..115d369c 100644 --- a/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/PdrProcessing.java @@ -141,7 +141,7 @@ public PdrProcessing(Context context) { */ public float[] updatePdr(long currentStepEnd, List accelMagnitudeOvertime, float headingRad) { if (accelMagnitudeOvertime == null || accelMagnitudeOvertime.size() < MIN_REQUIRED_SAMPLES) { - return new float[]{this.positionX, this.positionY}; // Return current position without update + return new float[]{this.positionX, this.positionY}; // - TODO - temporary solution of the empty list issue } @@ -268,7 +268,7 @@ private float weibergMinMax(List accelMagnitude) { * @return float array of size 2, with the X and Y coordinates respectively. */ public float[] getPDRMovement() { - float [] pdrPosition= new float[] {positionX,positionY}; + float [] pdrPosition= new float[] {positionX, positionY}; return pdrPosition; } diff --git a/app/src/main/java/com/openpositioning/PositionMe/utils/ThemePreferences.java b/app/src/main/java/com/openpositioning/PositionMe/utils/ThemePreferences.java new file mode 100644 index 00000000..88d18a9e --- /dev/null +++ b/app/src/main/java/com/openpositioning/PositionMe/utils/ThemePreferences.java @@ -0,0 +1,48 @@ +package com.openpositioning.PositionMe.utils; + +import android.content.Context; +import android.content.SharedPreferences; + +import androidx.appcompat.app.AppCompatDelegate; +import androidx.preference.PreferenceManager; + +public final class ThemePreferences { + + public static final String KEY_THEME_MODE = "theme_mode"; + public static final String THEME_LIGHT = "light"; + public static final String THEME_DARK = "dark"; + public static final String THEME_SYSTEM = "system"; + + private ThemePreferences() { + // Utility class + } + + public static void applyThemeFromPreferences(Context context) { + SharedPreferences prefs = PreferenceManager.getDefaultSharedPreferences(context); + String themeMode; + if (prefs.contains(KEY_THEME_MODE)) { + themeMode = prefs.getString(KEY_THEME_MODE, THEME_SYSTEM); + } else if (prefs.contains("dark_mode")) { + // Backward compatibility: migrate old boolean dark_mode if present. + boolean darkModeEnabled = prefs.getBoolean("dark_mode", false); + themeMode = darkModeEnabled ? THEME_DARK : THEME_LIGHT; + prefs.edit().putString(KEY_THEME_MODE, themeMode).apply(); + } else { + // Fresh install or unset preference: follow system by default. + themeMode = THEME_SYSTEM; + prefs.edit().putString(KEY_THEME_MODE, themeMode).apply(); + } + + applyThemeMode(themeMode); + } + + public static void applyThemeMode(String themeMode) { + if (THEME_DARK.equals(themeMode)) { + AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_YES); + } else if (THEME_LIGHT.equals(themeMode)) { + AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_NO); + } else { + AppCompatDelegate.setDefaultNightMode(AppCompatDelegate.MODE_NIGHT_FOLLOW_SYSTEM); + } + } +} diff --git a/app/src/main/res/drawable-night/switch_label_bg.xml b/app/src/main/res/drawable-night/switch_label_bg.xml new file mode 100644 index 00000000..cae8dc14 --- /dev/null +++ b/app/src/main/res/drawable-night/switch_label_bg.xml @@ -0,0 +1,6 @@ + + + + + diff --git a/app/src/main/res/drawable/bg_map_switch_spinner.xml b/app/src/main/res/drawable/bg_map_switch_spinner.xml new file mode 100644 index 00000000..397208c6 --- /dev/null +++ b/app/src/main/res/drawable/bg_map_switch_spinner.xml @@ -0,0 +1,13 @@ + + + + + + + diff --git a/app/src/main/res/drawable/ic_chevron_up.xml b/app/src/main/res/drawable/ic_chevron_up.xml new file mode 100644 index 00000000..0748bbf8 --- /dev/null +++ b/app/src/main/res/drawable/ic_chevron_up.xml @@ -0,0 +1,10 @@ + + + + diff --git a/app/src/main/res/drawable/legend_circle_blue.xml b/app/src/main/res/drawable/legend_circle_blue.xml new file mode 100644 index 00000000..28a8f242 --- /dev/null +++ b/app/src/main/res/drawable/legend_circle_blue.xml @@ -0,0 +1,5 @@ + + + + diff --git a/app/src/main/res/drawable/legend_circle_green.xml b/app/src/main/res/drawable/legend_circle_green.xml new file mode 100644 index 00000000..23820537 --- /dev/null +++ b/app/src/main/res/drawable/legend_circle_green.xml @@ -0,0 +1,5 @@ + + + + diff --git a/app/src/main/res/drawable/legend_circle_orange.xml b/app/src/main/res/drawable/legend_circle_orange.xml new file mode 100644 index 00000000..a57069fe --- /dev/null +++ b/app/src/main/res/drawable/legend_circle_orange.xml @@ -0,0 +1,5 @@ + + + + diff --git a/app/src/main/res/drawable/switch_label_bg.xml b/app/src/main/res/drawable/switch_label_bg.xml new file mode 100644 index 00000000..104eec3d --- /dev/null +++ b/app/src/main/res/drawable/switch_label_bg.xml @@ -0,0 +1,6 @@ + + + + + diff --git a/app/src/main/res/layout-small/fragment_home.xml b/app/src/main/res/layout-small/fragment_home.xml index bd713b67..1964fa93 100644 --- a/app/src/main/res/layout-small/fragment_home.xml +++ b/app/src/main/res/layout-small/fragment_home.xml @@ -80,7 +80,7 @@ app:layout_constraintTop_toBottomOf="@id/mapFragmentContainer" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" - app:layout_constraintBottom_toTopOf="@id/indoorButton"> + app:layout_constraintBottom_toBottomOf="parent"> - - - diff --git a/app/src/main/res/layout/activity_main.xml b/app/src/main/res/layout/activity_main.xml index 0a3ceda2..12e91881 100644 --- a/app/src/main/res/layout/activity_main.xml +++ b/app/src/main/res/layout/activity_main.xml @@ -3,7 +3,6 @@ xmlns:android="http://schemas.android.com/apk/res/android" xmlns:app="http://schemas.android.com/apk/res-auto" xmlns:tools="http://schemas.android.com/tools" - android:theme="@style/Theme.AppCompat.Light.NoActionBar" android:layout_width="match_parent" android:layout_height="match_parent" tools:context=".presentation.activity.MainActivity"> @@ -13,10 +12,10 @@ android:id="@+id/main_toolbar" android:layout_width="0dp" android:layout_height="?attr/actionBarSize" - android:background="?attr/colorPrimary" + android:background="?attr/colorSurface" android:elevation="4dp" android:title="@string/app_name" - android:titleTextColor="@color/md_theme_light_onPrimary" + android:titleTextColor="?attr/colorOnSurface" app:layout_constraintTop_toTopOf="parent" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" diff --git a/app/src/main/res/layout/fragment_home.xml b/app/src/main/res/layout/fragment_home.xml index 661afbb3..2f8e2d4c 100644 --- a/app/src/main/res/layout/fragment_home.xml +++ b/app/src/main/res/layout/fragment_home.xml @@ -81,7 +81,7 @@ app:layout_constraintTop_toBottomOf="@id/mapFragmentContainer" app:layout_constraintStart_toStartOf="parent" app:layout_constraintEnd_toEndOf="parent" - app:layout_constraintBottom_toTopOf="@id/indoorButton"> + app:layout_constraintBottom_toBottomOf="parent"> - - - diff --git a/app/src/main/res/layout/fragment_recording.xml b/app/src/main/res/layout/fragment_recording.xml index 518e8b75..30c64984 100644 --- a/app/src/main/res/layout/fragment_recording.xml +++ b/app/src/main/res/layout/fragment_recording.xml @@ -81,19 +81,25 @@ app:layout_constraintTop_toBottomOf="@id/currentPositionCard" app:layout_constraintBottom_toTopOf="@id/controlLayout" /> - - + - +