using UnityEngine;
using GeoSus.Client;
using System;
using System.Collections;

namespace Subsystems
{
    internal class CoroutineHost : MonoBehaviour
    {
        public CoroutineHost() { }
    }
    internal enum GPSState
    {
        Uninitialized,
        Initializing,
        Running,
        Failed
    }
    public static class PositonExtensions
    {
        public static Position ToLocal(this Position position, Position center)
        {
            double latDiff = position.Lat - center.Lat;
            double lonDiff = position.Lon - center.Lon;
            double metersPerDegreeLat = 111320.0;
            double metersPerDegreeLon = 111320.0 * Math.Cos(center.Lat * Math.PI / 180.0);
            float x = (float)(lonDiff * metersPerDegreeLon);
            float z = (float)(latDiff * metersPerDegreeLat);
            return new Position(z, x); 
        }
        public static Vector3 ToLocalVector3(this Position position, Position center)
        {
            return position.ToLocal(center).ToVector3(); //TODO: Implementace v subsystemech
        }
        public static Vector3 ToVector3(this Position position)
        {
            return new Vector3((float)position.Lon, 0, (float)position.Lat); //TODO: Implementace v subsystemech
        }
        public static double DistanceTo(this Vector3 pos, Vector3 other)
        {
            return Math.Sqrt((other.x - pos.x) * (other.x - pos.x) + (other.z - pos.z) * (other.z - pos.z));
        }

    }

    public class GameManager_Input
    {
        private GameClient _gameClient;
        private Position _currentPosition;
        private Position _lastSentPosition;
        private GameObject _player;
        private bool _testMode;
        
        private GPSState _GPSState = GPSState.Uninitialized;
        private float _speed = 0.00001f;
        private Position _mapCenter;
        private CoroutineHost _coroutineHost = new CoroutineHost();
        public GameManager_Input(GameClient gameClient, GameObject player, bool testMode)
        {
            _gameClient = gameClient;
            _player = player;
            _testMode = testMode;
        }
        public void positionCheck()
        {
            try
            {
                if (_gameClient.CurrentLobbyState.Phase == GamePhase.Playing)
                {
                    if (_testMode)
                    {

                        if (_currentPosition == null || _currentPosition == new Position(0, 0))
                        {
                            //Init blok
                            _currentPosition = _gameClient.CurrentLobbyState.MapData.Center;
                            _mapCenter = _gameClient.CurrentLobbyState.MapData.Center;
                            _lastSentPosition = _currentPosition;
                        }

                        TestPlayerPosition();
                    }
                    else
                    {
                        if (_GPSState == GPSState.Uninitialized)
                        {
                            _coroutineHost.StartCoroutine(InitiallizeGPS());
                            return;
                        }
                        else if (_GPSState == GPSState.Initializing)
                        {
                            return;
                        }
                        else if (_GPSState == GPSState.Running)
                        {
                            try
                            {
                                if (_currentPosition != _lastSentPosition)
                                {
                                    _gameClient.UpdatePosition(_currentPosition);
                                    _lastSentPosition = _currentPosition;
                                    _player.transform.position = _currentPosition.ToLocalVector3(_mapCenter);
                                    _player.transform.rotation = Quaternion.Euler(0, (float)CalculateHeading(_lastSentPosition.ToLocalVector3(_mapCenter), _currentPosition.ToLocalVector3(_mapCenter)), 0);
                                }
                            }
                            catch (Exception ex)
                            {
                                Debug.Log(ex);
                            }
                        }
                        else
                        {
                            Debug.Log("GPS failed, trying again...");
                            _GPSState = GPSState.Uninitialized;
                        }
                    }
                }
            }
            catch (NullReferenceException ex) { Debug.Log(ex); }
        }
        private void TestPlayerPosition()
        {
            double x = Input.GetAxis("Horizontal");
            double y = Input.GetAxis("Vertical");
            Debug.Log($"Input: {x}, {y}");
            _currentPosition = new Position( _lastSentPosition.Lat + y * _speed, _lastSentPosition.Lon + x * _speed);
            Debug.Log($"Current Position: {_currentPosition.Lat}, {_currentPosition.Lon}");
            var localCurrent = _currentPosition.ToLocalVector3(_mapCenter);
            Debug.Log($"Local Current Position: {localCurrent}");
            var heading = CalculateHeading(_lastSentPosition.ToLocalVector3(_mapCenter), localCurrent);
            if (heading != null)
            {
                Debug.Log($"Heading: {heading}");
                _player.transform.rotation = Quaternion.Euler(0, (float)heading, 0);
            }
            _player.transform.position = localCurrent;
            try
            {
                if (_currentPosition != _lastSentPosition)
                {
                    _gameClient.UpdatePosition(_currentPosition);
                    _lastSentPosition = _currentPosition;
                }
            }
            catch
            {
                _gameClient.UpdatePosition(_currentPosition);
                _lastSentPosition = _currentPosition;
            }
        }
        private double? CalculateHeading(Vector3 first, Vector3 second)
        {
            double? heading = null;
            if ((first - second).magnitude == 0)
            {
                return null;
            }
            else if (first.x == second.x && first.z < second.z)
            {
                return 0;
            }
            else if (first.x == second.x && first.z > second.z)
            {
                return 180;
            }
            else if (first.x > second.x && first.z == second.z)
            {
                return 270;
            }
            else if (first.x < second.x && first.z == second.z)
            {
                return 90;
            }
            else if (first.x < second.x && first.z < second.z)
            {
                heading = Math.Asin((second.z - first.z) / first.DistanceTo(second));
                return (heading * 180) / Math.PI;
            }
            else if (first.x < second.x && first.z > second.z)
            {
                heading = Math.Asin((second.z - first.z) / first.DistanceTo(second));
                return (heading * 180) / Math.PI + 180;
            }
            else if (first.x > second.x && first.z < second.z)
            {
                heading = Math.Asin((second.z - first.z) / first.DistanceTo(second));
                return (heading * 180) / Math.PI - 90;
            }
            else if (first.x > second.x && first.z > second.z)
            {
                heading = Math.Asin((second.z - first.z) / first.DistanceTo(second));
                return (heading * 180) / Math.PI - 90;
            }
            else
            {
                return heading;
            }
        }
        IEnumerator InitiallizeGPS()
        {
            _GPSState = GPSState.Initializing;
            if (!Input.location.isEnabledByUser)
            {
                Debug.LogError("Location not enabled on device or app does not have permission to access location");
            }
            // Starts the location service.

            float desiredAccuracyInMeters = 10f;
            float updateDistanceInMeters = 10f;

            Input.location.Start(desiredAccuracyInMeters, updateDistanceInMeters);

            // Waits until the location service initializes
            int maxWait = 20;
            while (Input.location.status == LocationServiceStatus.Initializing && maxWait > 0)
            {
                yield return new WaitForSeconds(1);
                maxWait--;
            }

            // If the service didn't initialize in 20 seconds this cancels location service use.
            if (maxWait < 1)
            {
                _GPSState = GPSState.Failed;
                Debug.LogError("Timed out");
                yield break;
            }
            _GPSState = GPSState.Running;
            yield return _coroutineHost.StartCoroutine(GPSService());
        }
        IEnumerator GPSService()
        {
            // Check if the user has location service enabled.


            // If the connection failed this cancels location service use.
            if (Input.location.status == LocationServiceStatus.Failed)
            {
                Debug.LogError("Unable to determine device location");
                yield break;
            }
            else
            {
                // If the connection succeeded, this retrieves the device's current location and displays it in the Console window.
                _currentPosition = new Position(Input.location.lastData.latitude, Input.location.lastData.longitude);
                Debug.Log("Location: " + Input.location.lastData.latitude + " " + Input.location.lastData.longitude + " " + Input.location.lastData.altitude + " " + Input.location.lastData.horizontalAccuracy + " " + Input.location.lastData.timestamp);
                yield return new WaitForSeconds(5f);
            }

            // Stops the location service if there is no need to query location updates continuously.
            yield return _coroutineHost.StartCoroutine(GPSService());
        }
    }
}