/*
 *	Yubeshi GPS Parser
 *
 *	This software is distributed under a zlib-style license.
 *	See license.txt for more information.
 */

using System;
using System.Collections.Generic;
using System.Text;

namespace Yubeshi
{
    public class EcefCoordinate
    {
        #region fields
        private const double a = Constants.SemiMajorAxisA;
        private const double b = Constants.SemiMajorAxisB;
        private const double e1sq = Constants.FirstEccentricitySquared;
        private const double e2sq = Constants.SecondEccentricitySquared;
        private const double pi = Constants.Pi;
        #endregion

        #region constructors

        public EcefCoordinate(double x, double y, double z)
            : this(x, y, z, Double.NaN)
        {
        }

        public EcefCoordinate(double x, double y, double z, double accuracy)
        {
            X = x;
            Y = y;
            Z = z;
            Accuracy = accuracy;
        }

        /// <summary>
        ///     
        /// </summary>
        /// <param name="x">ECEF X coordinate in cm</param>
        /// <param name="y">ECEF Y coordinate in cm</param>
        /// <param name="z">ECEF Z coordinate in cm</param>
        /// <param name="accuracy">3D Position Accuracy Estimate in cm</param>
        public EcefCoordinate(int x, int y, int z, uint accuracy)
            : this(x * 0.01, y * 0.01, z * 0.01, accuracy * 0.01)
        {
        }
        #endregion

        #region properties
        public double X
        {
            get;
            set;
        }

        public double Y
        {
            get;
            set;
        }

        public double Z
        {
            get;
            set;
        }

        public double Accuracy
        {
            get;
            set;
        }
        #endregion

        #region public methods
        public GeodeticCoordinate ToGeodeticCoordinate()
        {
            // approximation
            double lambda = Math.Atan2(Y, X);
            double p = Math.Sqrt(X * X + Y * Y);
            double theta = Math.Atan2(Z * a, p * b);
            double sin = Math.Sin(theta);
            double cos = Math.Cos(theta);

            double phi = Math.Atan2(Z + e2sq * b * sin * sin * sin,
                                            p - e1sq * a * cos * cos * cos);
            double sinPhi = Math.Sin(phi);
            double khi = Math.Sqrt(1.0 - e1sq * sinPhi * sinPhi);
            double n = a / khi;
            double h = p / Math.Cos(phi) - n;
            return new GeodeticCoordinate(phi * 180 / pi, lambda * 180 / pi, h);
        }
        #endregion
    }
}