using UnityEngine;
using GeoSus.Client;
using System;
namespace Subsystems
{
    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 float _speed = 0.00001f;
        private Position _mapCenter;
        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
                    {
                        //TODO: Real GPS
                    }
                }
            }
            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;
            }
        }
    }
}