using System;
using Windows.UI.Xaml.Media.Media3D;

namespace Petzold.Windows8.VectorDrawing
{
    public struct HorizontalCoordinate
    {
        public HorizontalCoordinate(double azimuth, double altitude, double tilt)
            : this()
        {
            this.Azimuth = azimuth;
            this.Altitude = altitude;
            this.Tilt = tilt;
        }

        public HorizontalCoordinate(double azimuth, double altitude)
            : this(azimuth, altitude, 0)
        {
        }

        // Na wschód od północy
        public double Azimuth { private set; get; }

        public double Altitude { private set; get; }

        public double Tilt { private set; get; }

        public static HorizontalCoordinate FromVector(Vector3 vector)
        {
            double altitude = 180 * Math.Asin(vector.Z) / Math.PI;
            double azimuth = 180 * Math.Atan2(vector.X, vector.Y) / Math.PI;

            return new HorizontalCoordinate(azimuth, altitude);
        }

        public static HorizontalCoordinate FromMotionMatrix(Matrix3D matrix)
        {
            // Odwrócenie macierzy
            matrix.Invert();

            // Transformacja (0, 0, -1) -- wektory wychodzący z obiektywu
            Vector3 zAxisTransformed = Vector3.Transform(-Vector3.UnitZ, matrix);

            // Odczyt współrzędnych horyzontalnych
            HorizontalCoordinate horzCoord = FromVector(zAxisTransformed);

            // Określenie teoretycznych HorizontalCoordinate dla przekształconego wektora +Y
            //     jeżeli urządzenie jest trzymane pionowo
            double yUprightAltitude = 0;
            double yUprightAzimuth = 0;

            if (horzCoord.Altitude > 0)
            {
                yUprightAltitude = 90 - horzCoord.Altitude;
                yUprightAzimuth = 180 + horzCoord.Azimuth;
            }
            else
            {
                yUprightAltitude = 90 + horzCoord.Altitude;
                yUprightAzimuth = horzCoord.Azimuth;
            }
            Vector3 yUprightVector = 
                new HorizontalCoordinate(yUprightAzimuth, yUprightAltitude).ToVector();

            // Określenie rzeczywistego przekształconego wektora +Y
            Vector3 yAxisTransformed = Vector3.Transform(Vector3.UnitY, matrix);

            // Pobranie kąta pomiędzy wektorem +Y a rzeczywistym, przekształconym wektorem +Y
            double dotProduct = Vector3.Dot(yUprightVector, yAxisTransformed);
            Vector3 crossProduct = Vector3.Cross(yUprightVector, yAxisTransformed);
            crossProduct = crossProduct.Normalized;

            // Czasami dotProduct jest nieco większy od 1, co
            // generuje wyjątek w obliczeniach angleBetween , więc ....
            dotProduct = Math.Min(dotProduct, 1);
            double angleBetween = 180 * Vector3.Dot(zAxisTransformed, crossProduct) 
                                      * Math.Acos(dotProduct) / Math.PI;
            horzCoord.Tilt = angleBetween;

            return horzCoord;
        }

        public Vector3 ToVector()
        {
            double x = Math.Cos(Math.PI * this.Altitude / 180) * 
                       Math.Sin(Math.PI * this.Azimuth / 180);
            double y = Math.Cos(Math.PI * this.Altitude / 180) * 
                       Math.Cos(Math.PI * this.Azimuth / 180);
            double z = Math.Sin(Math.PI * this.Altitude / 180);

            return new Vector3((float)x, (float)y, (float)z);
        }

        public override string ToString()
        {
            return String.Format("Azy: {0} Wys: {1} Poch: {2}", 
                                 this.Azimuth, this.Altitude, this.Tilt);
        }
    }
}