using Toybox.System as System;
using Toybox.WatchUi as WatchUi;
using Toybox.Graphics as Graphics;
using Toybox.Position as Position;
using Toybox.Lang as Lang;

class MyWaypointsApp extends WatchUi.AppBase {

    private var mainView;

    function initialize() {
        WatchUi.AppBase.initialize();
    }

    function onStart(state) {
        // Push our main view that handles location + bearing logic
        mainView = new WaypointsMainView();
        WatchUi.pushView(mainView);
    }

    function onStop(state) {
        // Clean up if needed
    }
}

// The main view that displays “bearing to next waypoint”
class WaypointsMainView extends WatchUi.View {

    // Hardcoded route: an array of lat/lon dictionaries
    private var routePoints = [
        { :lat => 40.7128, :lon => -74.0060 }, // example: New York
        { :lat => 39.9526, :lon => -75.1652 }, // example: Philadelphia
        { :lat => 38.9072, :lon => -77.0369 }, // example: DC
    ];

    private var waypointIndex = 0; // Start with the first route point
    private var currentBearing = 0; // Bearing in degrees
    private var distanceThreshold = 0.01; // ~0.01 km or some radius you want

    private var locationListener;

    function initialize() {
        WatchUi.View.initialize();

        // Listen to user’s location
        locationListener = new MyLocationListener(self);
        // EITHER (A) older approach
        Position.enableLocationEvents(locationListener);
        // OR (B) newer approach if your device supports it:
        // Position.setEnabled(true);
        // Position.setListener(locationListener);
        Position.start();
    }

    function onShow() as Void {
        WatchUi.requestUpdate();
    }

    function onHide() as Void {
        // Stop location to save battery
        Position.disableLocationEvents();
        Position.stop();
    }

    function onUpdate(dc as Graphics.Dc) as Void {
        dc.clear();
        dc.setColor(Graphics.COLOR_WHITE);
        dc.setFont(Graphics.FONT_LARGE);

        // If we've reached the end of our routePoints
        if (waypointIndex >= routePoints.size()) {
            dc.drawText(dc.getWidth()/2,
                        dc.getHeight()/2,
                        Graphics.FONT_LARGE,
                        "Route Completed!",
                        Graphics.TEXT_JUSTIFY_CENTER);
            return;
        }

        // Display current bearing and the name or index of the next waypoint
        var text = "Next WP " + waypointIndex + "\n" +
                   "Bearing: " + Lang.formatNumber(currentBearing,0) + "°";

        dc.drawText(dc.getWidth()/2,
                    dc.getHeight()/2,
                    Graphics.FONT_LARGE,
                    text,
                    Graphics.TEXT_JUSTIFY_CENTER);
    }

    // Called by the location listener when user’s lat/lon changes
    function onUserLocation(lat as Number, lon as Number) as Void {

        // If we’re done
        if (waypointIndex >= routePoints.size()) {
            return;
        }

        var targetWp = routePoints[waypointIndex];
        var wpLat = targetWp[:lat];
        var wpLon = targetWp[:lon];

        // Compute the bearing
        currentBearing = calculateBearing(lat, lon, wpLat, wpLon);

        // Check distance to see if we’re “close enough” to move on
        var distKm = distanceBetween(lat, lon, wpLat, wpLon);
        if (distKm <= distanceThreshold) {
            // Move to next waypoint
            waypointIndex++;
        }

        WatchUi.requestUpdate();
    }

    // Haversine formula or simpler approach for distance in km
    function distanceBetween(lat1, lon1, lat2, lon2) as Number {
        var R = 6371; // Earth radius in km
        var dLat = toRadians(lat2 - lat1);
        var dLon = toRadians(lon2 - lon1);
        var a = Math.sin(dLat/2)*Math.sin(dLat/2)
              + Math.cos(toRadians(lat1)) * Math.cos(toRadians(lat2))
                * Math.sin(dLon/2)*Math.sin(dLon/2);
        var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
        return R * c;
    }

    // Basic formula for bearing from (lat1,lon1) to (lat2,lon2)
    function calculateBearing(lat1, lon1, lat2, lon2) as Number {
        var phi1 = toRadians(lat1);
        var phi2 = toRadians(lat2);
        var dLon = toRadians(lon2 - lon1);

        var y = Math.sin(dLon) * Math.cos(phi2);
        var x = Math.cos(phi1)*Math.sin(phi2)
              - Math.sin(phi1)*Math.cos(phi2)*Math.cos(dLon);
        var brng = Math.atan2(y, x); // -pi..+pi
        var deg = toDegrees(brng);   // Convert to 0..360
        return (deg + 360) mod 360;
    }

    function toRadians(deg as Number) as Number {
        return deg * Math.PI / 180;
    }

    function toDegrees(rad as Number) as Number {
        return rad * 180 / Math.PI;
    }
}

// The location listener
class MyLocationListener extends Position.LocationListener {
    private var parentView;

    function initialize(pv as WaypointsMainView) {
        Position.LocationListener.initialize();
        parentView = pv;
    }

    function onLocation(loc as Position.Location) as Void {
        if (loc != null && loc.valid) {
            parentView.onUserLocation(loc.latitude, loc.longitude);
        } else {
            System.println("Invalid location.");
        }
    }

    function onStateChange(newState as Position.State) as Void {
        // E.g. handle transitions
    }
}