using System;
using System.Collections.Generic;

#if UNITY_5_3_OR_NEWER
using Vector3 = UnityEngine.Vector3;
#endif

namespace LinearAlgebra {
    /// <summary>
    /// A spherical vector
    /// </summary>
    /// <remark>This is a struct such that it is a value type and cannot be null 
    public struct Spherical {
        /// <summary>
        /// Create a spherical vector
        /// </summary>
        /// <param name="distance">The distance in meters</param>
        /// <param name="direction">The direction of the vector</param>
        public Spherical(float distance, Direction direction) {
            if (distance > 0) {
                this.distance = distance;
                this.direction = direction;
            }
            else {
                this.distance = -distance;
                this.direction = -direction;
            }
        }

        /// <summary>
        /// Create spherical vector. All given angles are in degrees
        /// </summary>
        /// <param name="distance">The distance in meters</param>
        /// <param name="horizontal">The horizontal angle in degrees</param>
        /// <param name="vertical">The vertical angle in degrees</param>
        /// <returns></returns>
        public static Spherical Degrees(float distance, float horizontal, float vertical) {
            Direction direction = Direction.Degrees(horizontal, vertical);
            Spherical s = new(distance, direction);
            return s;
        }

        public static Spherical Radians(float distance, float horizontal, float vertical) {
            Direction direction = Direction.Radians(horizontal, vertical);
            Spherical s = new(distance, direction);
            return s;
        }

        /// <summary>
        /// The distance in meters
        /// </summary>
        /// @remark The distance should never be negative
        public float distance;
        /// <summary>
        /// The direction of the vector
        /// </summary>
        public Direction direction;

        /// <summary>
        /// A spherical vector with zero degree angles and distance
        /// </summary>
        public readonly static Spherical zero = new(0, Direction.forward);
        /// <summary>
        /// A normalized forward-oriented vector
        /// </summary>
        public readonly static Spherical forward = new(1, Direction.forward);

#if UNITY_5_3_OR_NEWER
        public static Spherical FromVector3(Vector3 v) {
            float distance = v.magnitude;
            Direction direction = Direction.FromVector3(v / distance);
            return new Spherical(distance, direction);
        }

        public readonly Vector3 ToVector3() {
            Vector3 v = this.direction.ToVector3();
            v *= this.distance;
            return v;
        }
#else
        public static Spherical FromVector3(Vector3Float v) {
            float distance = v.magnitude;
            if (distance == 0.0f)
                return Spherical.zero;
            else {
                float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.vertical / distance)) * AngleFloat.Rad2Deg;
                float horizontalAngle = (float)Math.Atan2(v.horizontal, v.depth) * AngleFloat.Rad2Deg;
                return Degrees(distance, horizontalAngle, verticalAngle);
            }
        }

        public readonly Vector3Float ToVector3() {
            // float verticalRad = (AngleFloat.deg90 - this.direction.vertical).inRadians;
            // float horizontalRad = this.direction.horizontal.inRadians;
            // float cosVertical = (float)Math.Cos(verticalRad);
            // float sinVertical = (float)Math.Sin(verticalRad);
            // float cosHorizontal = (float)Math.Cos(horizontalRad);
            // float sinHorizontal = (float)Math.Sin(horizontalRad);

            // float x = this.distance * sinVertical * sinHorizontal;
            // float y = this.distance * cosVertical;
            // float z = this.distance * sinVertical * cosHorizontal;

            // Vector3Float v = new(x, y, z);
            Vector3Float v = this.direction.ToVector3();
            v *= this.distance;
            return v;
        }
#endif

        public override readonly string ToString() {
            return $"Spherical({this.distance}, h: {this.direction.horizontal}, v: {this.direction.vertical})";
        }


        public readonly float magnitude => this.distance;

        public Spherical normalized {
            get {
                Spherical r = new() {
                    distance = 1,
                    direction = this.direction
                };
                return r;
            }
        }

        public static Spherical operator +(Spherical s1, Spherical s2) {
            // let's do it the easy way...
            // using vars to be compatible with both unity (Vector3) and native (Vector3Float)
            var v1 = s1.ToVector3();
            var v2 = s2.ToVector3();
            var v = v1 + v2;
            Spherical r = FromVector3(v);
            return r;
        }

        public static Spherical operator *(Spherical v, float d) {
            Spherical r = new(v.distance * d, v.direction);
            return r;
        }

        public static bool operator ==(Spherical v1, Spherical v2) {
            return (v1.distance == v2.distance && v1.direction == v2.direction);
        }

        public static bool operator !=(Spherical v1, Spherical v2) {
            return (v1.distance != v2.distance || v1.direction != v2.direction);
        }

        public override readonly bool Equals(object o) {
            if (o is Spherical s)
                return this == s;            
            return false;
        }

        public override readonly int GetHashCode() {
            return HashCode.Combine(this.distance, this.direction);
        }

        public static float Distance(Spherical v1, Spherical v2) {
            // Convert degrees to radians
            float thetaARadians = v1.direction.horizontal.inRadians;
            float phiARadians = v1.direction.vertical.inRadians;// DegreesToRadians(phiA);
            float thetaBRadians = v2.direction.horizontal.inRadians; // DegreesToRadians(thetaB);
            float phiBRadians = v2.direction.vertical.inRadians; // DegreesToRadians(phiB);

            // Calculate sine and cosine values
            float sinPhiA = MathF.Sin(phiARadians);
            float cosPhiA = MathF.Cos(phiARadians);
            float sinPhiB = MathF.Sin(phiBRadians);
            float cosPhiB = MathF.Cos(phiBRadians);

            // Calculate the cosine of the difference in azimuthal angles
            float cosThetaDifference = MathF.Cos(thetaARadians - thetaBRadians);

            // Apply the spherical law of cosines
            float distance = MathF.Sqrt(
                v1.distance * v1.distance +
                v2.distance * v2.distance -
                2 * v1.distance * v2.distance * (sinPhiA * sinPhiB * cosThetaDifference + cosPhiA * cosPhiB)
            );

            return distance;
        }

        public static Spherical Average(Spherical v1, Spherical v2) {
            const float EPS = 1e-6f;

            // Angles in radians
            float a1 = v1.direction.horizontal.inRadians;
            float a2 = v2.direction.horizontal.inRadians;
            float e1 = v1.direction.vertical.inRadians;
            float e2 = v2.direction.vertical.inRadians;

            // Fast path: exactly same direction (allowing wrap for azimuth) -> preserve exact angles
            bool sameAz = MathF.Abs(MathF.IEEERemainder(a1 - a2, MathF.PI * 2f)) < EPS;
            bool sameEl = MathF.Abs(e1 - e2) < EPS;
            if (sameAz && sameEl) {
                // Distances may differ; average distance but keep exact angles from v1
                float rAvgExact = 0.5f * (v1.distance + v2.distance);
                return new Spherical(rAvgExact, v1.direction);
            }

            // Horizontal unit-circle sum
            float cx = MathF.Cos(a1) + MathF.Cos(a2);
            float cy = MathF.Sin(a1) + MathF.Sin(a2);

            // Vertical as z = sin(el)
            float z1 = MathF.Sin(e1);
            float z2 = MathF.Sin(e2);
            float cz = z1 + z2;

            // Magnitude of summed unit-direction vectors
            float sumX = cx;
            float sumY = cy;
            float sumZ = cz;
            float magSum = MathF.Sqrt(sumX * sumX + sumY * sumY + sumZ * sumZ);

            // If the two direction unit-vectors cancel (or nearly), return zero distance.
            if (magSum < EPS) {
                return Spherical.Radians(0f, 0f, 0f);
            }

            // Normalized averaged direction components
            float ux = sumX / magSum;
            float uy = sumY / magSum;
            float uz = sumZ / magSum;

            // Compute averaged angles from normalized vector
            float azAvgRad = MathF.Atan2(uy, ux);
            float elAvgRad = MathF.Asin(Float.Clamp(uz, -1f, 1f));

            // Average distance (arithmetic mean)
            float rAvg = 0.5f * (v1.distance + v2.distance);

            return Spherical.Radians(rAvg, azAvgRad, elAvgRad);
        }

        public static Spherical Sum(List<Spherical> vectors) {
            if (vectors == null || vectors.Count == 0)
                throw new ArgumentException("vectors must contain at least one element", nameof(vectors));

#if UNITY_5_3_OR_NEWER
            Vector3 sum = Vector3.zero;
#else
            Vector3Float sum = Vector3Float.zero;
#endif
            foreach (Spherical v in vectors)
                sum += v.ToVector3();

            return FromVector3(sum);
        }


        public static Spherical Average(List<Spherical> vectors) {
            if (vectors == null || vectors.Count == 0)
                throw new ArgumentException("vectors must contain at least one element", nameof(vectors));

#if UNITY_5_3_OR_NEWER
            Vector3 sum = Vector3.zero;
#else
            Vector3Float sum = Vector3Float.zero;
#endif
            int n = 0;
            foreach (Spherical v in vectors) {
                sum += v.ToVector3();
                n++;
            }
            var avg = sum / n;

            // if (avg.sqrMagnitude == 0f)
            //     return new Spherical(0f, new Direction(AngleFloat.Radians(0f), AngleFloat.Radians(0f)));            
            // else
            return FromVector3(avg);
        }

    }
}