using System;
using System.Collections.Generic;
using UnityEngine;

using LastPositionItem = OnlineMapsLocationService.LastPositionItem;

/// <summary>
/// Smoothes changes in GPS location
/// </summary>
public class SmoothGPSMovement: MonoBehaviour
{
    /// <summary>
    /// Number of seconds to correct position
    /// </summary>
    public float correctionTime = 0.5f;

    /// <summary>
    /// Should the map position be updated?
    /// </summary>
    public bool updateMapPosition = true;

    /// <summary>
    /// Smooth changes in compass values
    /// </summary>
    public bool lerpCompassValue = true;

    /// <summary>
    /// Reference to Location Service
    /// </summary>
    private OnlineMapsLocationService locationService;

    /// <summary>
    /// Current speed (km/h)
    /// </summary>
    private float speed;

    /// <summary>
    /// Compass true heading (degree)
    /// </summary>
    private float compass;

    /// <summary>
    /// Last known location
    /// </summary>
    private OnlineMapsVector2d lastKnownLocation;

    /// <summary>
    /// Current simulated location
    /// </summary>
    private OnlineMapsVector2d currentLocation;

    /// <summary>
    /// Correction vector
    /// </summary>
    private OnlineMapsVector2d correction;

    /// <summary>
    /// Reference to the marker
    /// </summary>
    private OnlineMapsMarker marker;

    /// <summary>
    /// List of last positions
    /// </summary>
    private List<LastPositionItem> lastPositions;

    /// <summary>
    /// The maximum number of items in the list of last positions
    /// </summary>
    private int maxPositionCount = 3;

    /// <summary>
    /// Progress of correction
    /// </summary>
    private float correctionProgress;

    /// <summary>
    /// Smoothed compass value (degree)
    /// </summary>
    private float smoothedCompass;

    private void Start()
    {
        locationService = OnlineMapsLocationService.instance;
        if (locationService == null)
        {
            Debug.LogError("Cannot find Location Service");
            Destroy(this);
            return;
        }

        // Subscribe to location events
        locationService.OnLocationInited += OnLocationInited;
        locationService.OnLocationChanged += OnLocationChanged;
        locationService.OnCompassChanged += OnCompassChanged;
    }

    private void OnLocationInited()
    {
        // Create the marker and store initial values
        marker = OnlineMapsMarkerManager.CreateItem(locationService.position, "Smoothed Marker");
        lastKnownLocation = currentLocation = locationService.position;
    }

    private void OnCompassChanged(float newValue)
    {
        // Update compass value
        compass = newValue * 360;

        // If the marker rotation should not smooth, update the rotation
        if (!lerpCompassValue && marker != null) marker.rotation = newValue;
    }

    private void OnLocationChanged(Vector2 newLocation)
    {
        // Save a new location
        lastKnownLocation = newLocation;

        // Calculating the correction vector
        correction = lastKnownLocation - currentLocation;

        // Update current speed
        UpdateSpeed();

        // Calculate a distance between new and old locations
        double dx, dy;
        OnlineMapsUtils.DistanceBetweenPoints(newLocation.x, newLocation.y, currentLocation.x, currentLocation.y, out dx, out dy);
        double d = Math.Sqrt(dx * dx + dy * dy);

        // If the distance is too long or the speed is too low, update the location
        if (d > 0.01 || speed < 1)
        {
            currentLocation = lastKnownLocation;
            correction = OnlineMapsVector2d.zero;
        }

        // Reset correction progress
        correctionProgress = 0;
    }

    private void Update()
    {
        if (speed < 1 || marker == null) return;

        // Smooth changes of compass
        if (lerpCompassValue)
        {
            if (compass - smoothedCompass > 180) smoothedCompass += 360;
            else if (compass - smoothedCompass < -180) smoothedCompass -= 360;

            if (Math.Abs(compass - smoothedCompass) >= float.Epsilon)
            {
                if (Mathf.Abs(compass - smoothedCompass) < 0.003f) smoothedCompass = compass;
                else smoothedCompass = Mathf.Lerp(smoothedCompass, compass, 0.02f);

                marker.rotationDegree = smoothedCompass;
            }
        }

        // Find the expected location
        float coveredDistance = Time.deltaTime * speed / 3600f;
        double lng, lat;
        OnlineMapsUtils.GetCoordinateInDistance(currentLocation.x, currentLocation.y, coveredDistance, compass, out lng, out lat);
        currentLocation.x = lng;
        currentLocation.y = lat;

        // If correction is required, do it
        if (correctionProgress < 1 && correction.SqrMagnitude() > 0)
        {
            float nextCorrectionProgress = correctionProgress + Time.deltaTime / correctionTime;
            if (nextCorrectionProgress > 1) nextCorrectionProgress = 1;
            
            float correctionDelta = nextCorrectionProgress - correctionProgress;
            currentLocation.x += correction.x * correctionDelta;
            currentLocation.y += correction.y * correctionDelta;

            correctionProgress = nextCorrectionProgress;
        }

        // Set marker position
        marker.SetPosition(lng, lat);

        // Set map position
        if (updateMapPosition) OnlineMaps.instance.SetPosition(lng, lat);
    }

    /// <summary>
    /// Update speed
    /// </summary>
    public void UpdateSpeed()
    {
        if (lastPositions == null) lastPositions = new List<LastPositionItem>();

        lastPositions.Add(new LastPositionItem((float)lastKnownLocation.x, (float)lastKnownLocation.y, Time.time));
        while (lastPositions.Count > maxPositionCount) lastPositions.RemoveAt(0);

        if (lastPositions.Count < 2)
        {
            speed = 0;
            return;
        }

        LastPositionItem p1 = lastPositions[0];
        LastPositionItem p2 = lastPositions[lastPositions.Count - 1];

        double dx, dy;
        OnlineMapsUtils.DistanceBetweenPoints(p1.lng, p1.lat, p2.lng, p2.lat, out dx, out dy);
        double distance = Math.Sqrt(dx * dx + dy * dy);
        double time = (p2.timestamp - p1.timestamp) / 3600;
        speed = Mathf.Abs((float)(distance / time));
    }
}