/* Copyright (C) 2013
 * swift Project Community / Contributors
 *
 * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level
 * directory of this distribution. No part of swift project, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 */

#include "blackmisc/geo/coordinategeodetic.h"
#include "blackmisc/logmessage.h"
#include "blackmisc/propertyindex.h"
#include "blackmisc/variant.h"
#include "blackmisc/verify.h"

#include <QByteArray>
#include <QtGlobal>
#include <QStringBuilder>
#include <cmath>

using namespace BlackMisc::Aviation;
using namespace BlackMisc::PhysicalQuantities;
using namespace BlackMisc::Math;

namespace BlackMisc
{
    namespace Geo
    {
        ICoordinateGeodetic::~ICoordinateGeodetic()
        { }

        QString CCoordinateGeodetic::convertToQString(bool i18n) const
        {
            return ICoordinateGeodetic::convertToQString(i18n);
        }

        CCoordinateGeodetic CCoordinateGeodetic::fromWgs84(const QString &latitudeWgs84, const QString &longitudeWgs84, const CAltitude &geodeticHeight)
        {
            const CLatitude  lat = CLatitude::fromWgs84(latitudeWgs84);
            const CLongitude lon = CLongitude::fromWgs84(longitudeWgs84);
            return CCoordinateGeodetic(lat, lon, geodeticHeight);
        }

        const CCoordinateGeodetic &CCoordinateGeodetic::null()
        {
            static const CCoordinateGeodetic n;
            return n;
        }

        CLength calculateGreatCircleDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
        {
            if (coordinate1.isNull() || coordinate2.isNull()) { return CLength::null(); }
            // if (coordinate1.equalNormalVectorDouble(coordinate2)) { return CLength(0, CLengthUnit::defaultUnit()); }
            constexpr float earthRadiusMeters = 6371000.8f;

            const QVector3D v1 = coordinate1.normalVector();
            const QVector3D v2 = coordinate2.normalVector();
            Q_ASSERT_X(std::isfinite(v1.x()) && std::isfinite(v1.y()) && std::isfinite(v1.z()), Q_FUNC_INFO, "Distance calculation: v1 non-finite argument");
            Q_ASSERT_X(std::isfinite(v2.x()) && std::isfinite(v2.y()) && std::isfinite(v2.z()), Q_FUNC_INFO, "Distance calculation: v2 non-finite argument");

            const float d = earthRadiusMeters * std::atan2(QVector3D::crossProduct(v1, v2).length(), QVector3D::dotProduct(v1, v2));

            BLACK_VERIFY_X(!std::isnan(d), Q_FUNC_INFO, "Distance calculation: NaN in result");
            if (std::isnan(d))
            {
                CLogMessage().debug(u"Distance calculation: NaN in result (given arguments %1 %2 %3; %4 %5 %6)") << static_cast<double>(v1.x()) << static_cast<double>(v1.y()) << static_cast<double>(v1.z()) << static_cast<double>(v2.x()) << static_cast<double>(v2.y()) << static_cast<double>(v2.z());
                return CLength::null();
            }
            return { static_cast<double>(d), CLengthUnit::m() };
        }

        CAngle calculateBearing(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
        {
            if (coordinate1.isNull() || coordinate2.isNull()) { return CAngle::null(); }
            // if (coordinate1.equalNormalVectorDouble(coordinate2)) { return CAngle(0, CAngleUnit::defaultUnit()); } // null or 0?
            static const QVector3D northPole { 0, 0, 1 };
            const QVector3D c1 = QVector3D::crossProduct(coordinate1.normalVector(), coordinate2.normalVector());
            const QVector3D c2 = QVector3D::crossProduct(coordinate1.normalVector(), northPole);
            const QVector3D cross = QVector3D::crossProduct(c1, c2);
            const float sinTheta = std::copysign(cross.length(), QVector3D::dotProduct(cross, coordinate1.normalVector()));
            const float cosTheta = QVector3D::dotProduct(c1, c2);
            const float theta = std::atan2(sinTheta, cosTheta);
            return { static_cast<double>(theta), CAngleUnit::rad() };
        }

        double calculateEuclideanDistance(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
        {
            return static_cast<double>(coordinate1.normalVector().distanceToPoint(coordinate2.normalVector()));
        }

        double calculateEuclideanDistanceSquared(const ICoordinateGeodetic &coordinate1, const ICoordinateGeodetic &coordinate2)
        {
            return static_cast<double>((coordinate1.normalVector() - coordinate2.normalVector()).lengthSquared());
        }

        bool ICoordinateGeodetic::equalNormalVectorDouble(const std::array<double, 3> &otherVector) const
        {
            static const double epsilon = std::numeric_limits<double>::epsilon();
            const std::array<double, 3> thisVector = this->normalVectorDouble();
            for (unsigned int i = 0; i < otherVector.size(); i++)
            {
                const double d = thisVector[i] - otherVector[i];
                if (qAbs(d) > epsilon) { return false; }
            }
            return true;
        }

        bool ICoordinateGeodetic::equalNormalVectorDouble(const ICoordinateGeodetic &otherCoordinate) const
        {
            return this->equalNormalVectorDouble(otherCoordinate.normalVectorDouble());
        }

        CLength ICoordinateGeodetic::calculateGreatCircleDistance(const ICoordinateGeodetic &otherCoordinate) const
        {
            return Geo::calculateGreatCircleDistance((*this), otherCoordinate);
        }

        bool ICoordinateGeodetic::isWithinRange(const ICoordinateGeodetic &otherCoordinate, const CLength &range) const
        {
            if (range.isNull()) { return false; }
            const CLength distance = this->calculateGreatCircleDistance(otherCoordinate);
            if (distance.isNull()) { return false; }
            return distance <= range;
        }

        CAngle ICoordinateGeodetic::calculateBearing(const ICoordinateGeodetic &otherCoordinate) const
        {
            return Geo::calculateBearing((*this), otherCoordinate);
        }

        bool ICoordinateGeodetic::canHandleIndex(const CPropertyIndex &index)
        {
            const int i = index.frontCasted<int>();
            return (i >= static_cast<int>(IndexLatitude)) && (i <= static_cast<int>(IndexNormalVector));
        }

        CVariant ICoordinateGeodetic::propertyByIndex(const BlackMisc::CPropertyIndex &index) const
        {
            if (!index.isMyself())
            {
                const ColumnIndex i = index.frontCasted<ColumnIndex>();
                switch (i)
                {
                case IndexLatitude:  return this->latitude().propertyByIndex(index.copyFrontRemoved());
                case IndexLongitude: return this->longitude().propertyByIndex(index.copyFrontRemoved());
                case IndexLatitudeAsString:  return CVariant(this->latitudeAsString());
                case IndexLongitudeAsString: return CVariant(this->longitudeAsString());
                case IndexGeodeticHeight:    return this->geodeticHeight().propertyByIndex(index.copyFrontRemoved());
                case IndexGeodeticHeightAsString: return CVariant(this->geodeticHeightAsString());
                case IndexNormalVector: return CVariant::fromValue(this->normalVector());
                default: break;
                }
            }

            const QString m = QString("no property, index ").append(index.toQString());
            BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
            return CVariant::fromValue(m);
        }

        int ICoordinateGeodetic::comparePropertyByIndex(const CPropertyIndex &index, const ICoordinateGeodetic &compareValue) const
        {
            if (!index.isMyself())
            {
                const ColumnIndex i = index.frontCasted<ColumnIndex>();
                switch (i)
                {
                case IndexLatitude:  return this->latitude().comparePropertyByIndex(index.copyFrontRemoved(), compareValue.latitude());
                case IndexLongitude: return this->longitude().comparePropertyByIndex(index.copyFrontRemoved(), compareValue.longitude());
                case IndexLatitudeAsString:  return this->latitudeAsString().compare(compareValue.latitudeAsString());
                case IndexLongitudeAsString: return this->longitudeAsString().compare(compareValue.longitudeAsString());
                case IndexGeodeticHeight:    return this->geodeticHeight().comparePropertyByIndex(index.copyFrontRemoved(), compareValue.geodeticHeight());
                case IndexGeodeticHeightAsString: return this->geodeticHeightAsString().compare(compareValue.geodeticHeightAsString());
                default: break;
                }
            }

            const QString m = QString("no property, index ").append(index.toQString());
            BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
            return 0;
        }

        QString ICoordinateGeodetic::convertToQString(bool i18n) const
        {
            const CLatitude lat = this->latitude();
            const CLongitude lng = this->longitude();
            return QStringLiteral("Geodetic: {%1/%2, %3/%4, %5}").arg(lat.valueRoundedWithUnit(CAngleUnit::deg(), 6, i18n),
                    lat.valueRoundedWithUnit(CAngleUnit::rad(), 6, i18n),
                    lng.valueRoundedWithUnit(CAngleUnit::deg(), 6, i18n),
                    lng.valueRoundedWithUnit(CAngleUnit::rad(), 6, i18n),
                    this->geodeticHeight().valueRoundedWithUnit(CLengthUnit::ft(), 2, i18n));
        }

        bool ICoordinateGeodetic::isNaNVector() const
        {
            const QVector3D v = this->normalVector();
            return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z());
        }

        bool ICoordinateGeodetic::isNaNVectorDouble() const
        {
            const std::array<double, 3> v = this->normalVectorDouble();
            return std::isnan(v[0]) || std::isnan(v[1]) || std::isnan(v[2]);
        }

        bool ICoordinateGeodetic::isInfVector() const
        {
            const QVector3D v = this->normalVector();
            return std::isinf(v.x()) || std::isinf(v.y()) || std::isinf(v.z());
        }

        bool ICoordinateGeodetic::isInfVectorDouble() const
        {
            const std::array<double, 3> v = this->normalVectorDouble();
            return std::isinf(v[0]) || std::isinf(v[1]) || std::isinf(v[2]);
        }

        bool ICoordinateGeodetic::isValidVectorRange() const
        {
            // inf is out of range, comparing nans is always false
            const std::array<double, 3> v = this->normalVectorDouble();
            return isValidVector(v);
        }

        bool ICoordinateGeodetic::isValidVector(const std::array<double, 3> &v)
        {
            constexpr double l = 1.00001; // because of interpolation
            return v[0] <= l && v[1] <= l && v[2] <= l &&
                   v[0] >= -l && v[1] >= -l && v[2] >= -l;
        }

        int CCoordinateGeodetic::clampVector()
        {
            int c = 0;
            // *INDENT-OFF*
            if (m_x < -1.0) { m_x = -1.0; c++; } else if (m_x > 1.0) { m_x = 1.0; c++; }
            if (m_y < -1.0) { m_y = -1.0; c++; } else if (m_y > 1.0) { m_y = 1.0; c++; }
            if (m_z < -1.0) { m_z = -1.0; c++; } else if (m_z > 1.0) { m_z = 1.0; c++; }
            // *INDENT-ON*
            return c;
        }

        CVariant CCoordinateGeodetic::propertyByIndex(const BlackMisc::CPropertyIndex &index) const
        {
            if (index.isMyself()) { return CVariant::from(*this); }
            return (ICoordinateGeodetic::canHandleIndex(index)) ?
                   ICoordinateGeodetic::propertyByIndex(index) :
                   CValueObject::propertyByIndex(index);
        }

        void CCoordinateGeodetic::setPropertyByIndex(const CPropertyIndex &index, const CVariant &variant)
        {
            if (index.isMyself()) { (*this) = variant.to<CCoordinateGeodetic>(); return; }
            const ICoordinateGeodetic::ColumnIndex i = index.frontCasted<ICoordinateGeodetic::ColumnIndex>();
            switch (i)
            {
            case IndexGeodeticHeight: m_geodeticHeight.setPropertyByIndex(index.copyFrontRemoved(), variant); break;
            case IndexLatitude:  this->setLatitude(variant.value<CLatitude>());   break;
            case IndexLongitude: this->setLongitude(variant.value<CLongitude>()); break;
            case IndexLatitudeAsString:  this->setLatitude(CLatitude::fromWgs84(variant.toQString()));   break;
            case IndexLongitudeAsString: this->setLongitude(CLongitude::fromWgs84(variant.toQString())); break;
            case IndexGeodeticHeightAsString: m_geodeticHeight.parseFromString(variant.toQString()); break;
            case IndexNormalVector: this->setNormalVector(variant.value<QVector3D>()); break;
            default: CValueObject::setPropertyByIndex(index, variant); break;
            }
        }

        int CCoordinateGeodetic::comparePropertyByIndex(const CPropertyIndex &index, const CCoordinateGeodetic &compareValue) const
        {
            return ICoordinateGeodetic::canHandleIndex(index) ?
                   ICoordinateGeodetic::comparePropertyByIndex(index, compareValue) :
                   CValueObject::comparePropertyByIndex(index, compareValue);
        }

        CCoordinateGeodetic::CCoordinateGeodetic(const std::array<double, 3> &normalVector)
        {
            this->setNormalVector(normalVector);
        }

        CCoordinateGeodetic::CCoordinateGeodetic(const CLatitude &latitude, const CLongitude &longitude) : CCoordinateGeodetic(latitude, longitude, CAltitude::null())
        {
            // void
        }

        CCoordinateGeodetic::CCoordinateGeodetic(const CLatitude &latitude, const CLongitude &longitude, const CAltitude &geodeticHeight) :
            m_x(latitude.cos() * longitude.cos()),
            m_y(latitude.cos() * longitude.sin()),
            m_z(latitude.sin()),
            m_geodeticHeight(geodeticHeight)
        {}

        CCoordinateGeodetic::CCoordinateGeodetic(double latitudeDegrees, double longitudeDegrees) :
            CCoordinateGeodetic({ latitudeDegrees, CAngleUnit::deg() }, { longitudeDegrees, CAngleUnit::deg() }, { 0, nullptr }) {}

        CCoordinateGeodetic::CCoordinateGeodetic(double latitudeDegrees, double longitudeDegrees, double heightFeet) :
            CCoordinateGeodetic({ latitudeDegrees, CAngleUnit::deg() }, { longitudeDegrees, CAngleUnit::deg() }, { heightFeet, CLengthUnit::ft() }) {}

        CCoordinateGeodetic::CCoordinateGeodetic(const ICoordinateGeodetic &coordinate) :
            m_geodeticHeight(coordinate.geodeticHeight())
        {
            this->setNormalVector(coordinate.normalVectorDouble());
        }

        CCoordinateGeodetic CCoordinateGeodetic::calculatePosition(const CLength &distance, const CAngle &relBearing) const
        {
            if (this->isNull()) { return CCoordinateGeodetic::null(); }
            if (distance.isNull() || distance.isNegativeWithEpsilonConsidered() || relBearing.isNull()) { return CCoordinateGeodetic::null(); }
            if (distance.isZeroEpsilonConsidered()) { return *this; }

            // http://www.movable-type.co.uk/scripts/latlong.html#destPoint
            // https://stackoverflow.com/a/879531/356726
            // https://www.cosmocode.de/en/blog/gohr/2010-06/29-calculate-a-destination-coordinate-based-on-distance-and-bearing-in-php
            constexpr double earthRadiusMeters = 6371000.8;
            const double startLatRad = this->latitude().value(CAngleUnit::rad());
            const double startLngRad = this->longitude().value(CAngleUnit::rad());
            const double bearingRad  = relBearing.value(CAngleUnit::rad());
            const double distRatio   = distance.value(CLengthUnit::m()) / earthRadiusMeters;

            const double newLatRad = asin(sin(startLatRad) * cos(distRatio) + cos(startLatRad) * sin(distRatio) * cos(bearingRad));
            double newLngRad = 0;

            constexpr double epsilon = 1E-06;
            if (cos(newLatRad) == 0 || qAbs(cos(newLatRad)) < epsilon)
                newLngRad = startLngRad;
            else
            {
                // λ1 + Math.atan2(Math.sin(brng)*Math.sin(d/R)*Math.cos(φ1), Math.cos(d/R)-Math.sin(φ1)*Math.sin(φ2));
                newLngRad = startLngRad + atan2(sin(bearingRad) * sin(distRatio) * cos(startLatRad), cos(distRatio) - sin(startLatRad) * sin(newLatRad));
                newLngRad = fmod(newLngRad + 3 * M_PI, 2 * M_PI) - M_PI; // normalize +-180deg
            }

            CCoordinateGeodetic copy = *this;
            const CLatitude  lat(newLatRad, CAngleUnit::rad());
            const CLongitude lng(newLngRad, CAngleUnit::rad());
            copy.setLatLong(lat, lng);
            return copy;
        }

        CLatitude CCoordinateGeodetic::latitude() const
        {
            return { std::atan2(m_z, std::hypot(m_x, m_y)), CAngleUnit::rad() };
        }

        CLongitude CCoordinateGeodetic::longitude() const
        {
            // in mathematics atan2 of 0,0 is undefined, with IEEE floating-point atan2(0,0) is either 0 or ±180°
            return { std::atan2(m_y, m_x), CAngleUnit::rad() };
        }

        QVector3D CCoordinateGeodetic::normalVector() const
        {
            return
            {
                static_cast<float>(m_x),
                static_cast<float>(m_y),
                static_cast<float>(m_z)
            };
        }

        std::array<double, 3> CCoordinateGeodetic::normalVectorDouble() const
        {
            return { { m_x, m_y, m_z } };
        }

        void CCoordinateGeodetic::setLatitude(const CLatitude &latitude)
        {
            this->setLatLong(latitude, this->longitude());
        }

        void CCoordinateGeodetic::setLatitudeFromWgs84(const QString &wgs)
        {
            this->setLatitude(CLatitude::fromWgs84(wgs));
        }

        void CCoordinateGeodetic::setLongitude(const CLongitude &longitude)
        {
            this->setLatLong(this->latitude(), longitude);
        }

        void CCoordinateGeodetic::setLongitudeFromWgs84(const QString &wgs)
        {
            this->setLongitude(CLongitude::fromWgs84(wgs));
        }

        void CCoordinateGeodetic::setLatLong(const CLatitude &latitude, const CLongitude &longitude)
        {
            m_x = latitude.cos() * longitude.cos();
            m_y = latitude.cos() * longitude.sin();
            m_z = latitude.sin();
        }

        void CCoordinateGeodetic::setLatLongFromWgs84(const QString &latitude, const QString &longitude)
        {
            this->setLatitudeFromWgs84(latitude);
            this->setLongitudeFromWgs84(longitude);
        }

        void CCoordinateGeodetic::setGeodeticHeightToNull()
        {
            this->setGeodeticHeight(CAltitude::null());
        }

        void CCoordinateGeodetic::setNormalVector(const std::array<double, 3> &normalVector)
        {
            Q_ASSERT_X(normalVector.size() == 3, Q_FUNC_INFO, "Wrong vector size");
            m_x = normalVector[0];
            m_y = normalVector[1];
            m_z = normalVector[2];
        }

        CCoordinateGeodetic &CCoordinateGeodetic::switchUnit(const CLengthUnit &unit)
        {
            m_geodeticHeight.switchUnit(unit);
            return *this;
        }

        CLength ICoordinateWithRelativePosition::calculcateAndUpdateRelativeDistance(const ICoordinateGeodetic &position)
        {
            m_relativeDistance = Geo::calculateGreatCircleDistance(*this, position);
            return m_relativeDistance;
        }

        CLength ICoordinateWithRelativePosition::calculcateAndUpdateRelativeDistanceAndBearing(const ICoordinateGeodetic &position)
        {
            m_relativeDistance = Geo::calculateGreatCircleDistance(*this, position);
            m_relativeBearing = Geo::calculateBearing(*this, position);
            return m_relativeDistance;
        }

        CVariant ICoordinateWithRelativePosition::propertyByIndex(const CPropertyIndex &index) const
        {
            if (ICoordinateGeodetic::canHandleIndex(index)) { return ICoordinateGeodetic::propertyByIndex(index); }
            if (!index.isMyself())
            {
                const ColumnIndex i = index.frontCasted<ColumnIndex>();
                switch (i)
                {
                case IndexRelativeBearing: return this->getRelativeBearing().propertyByIndex(index.copyFrontRemoved());
                case IndexRelativeDistance: return this->getRelativeDistance().propertyByIndex(index.copyFrontRemoved());
                default: break;
                }
            }
            const QString m = QString("no property, index ").append(index.toQString());
            BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
            return CVariant::fromValue(m);
        }

        void ICoordinateWithRelativePosition::setPropertyByIndex(const CPropertyIndex &index, const CVariant &variant)
        {
            if (ICoordinateGeodetic::canHandleIndex(index)) { return; }
            if (!index.isMyself())
            {
                const ColumnIndex i = index.frontCasted<ColumnIndex>();
                switch (i)
                {
                case IndexRelativeBearing: m_relativeBearing.setPropertyByIndex(index.copyFrontRemoved(), variant); break;
                case IndexRelativeDistance: m_relativeDistance.setPropertyByIndex(index.copyFrontRemoved(), variant); break;
                default:
                    const QString m = QString("no property, index ").append(index.toQString());
                    BLACK_VERIFY_X(false, Q_FUNC_INFO, qUtf8Printable(m));
                    break;
                }
            }
        }

        int ICoordinateWithRelativePosition::comparePropertyByIndex(const CPropertyIndex &index, const ICoordinateWithRelativePosition &compareValue) const
        {
            if (ICoordinateGeodetic::canHandleIndex(index)) { return ICoordinateGeodetic::comparePropertyByIndex(index, compareValue); }
            if (!index.isMyself())
            {
                const ColumnIndex i = index.frontCasted<ColumnIndex>();
                switch (i)
                {
                case IndexRelativeBearing: return m_relativeBearing.comparePropertyByIndex(index.copyFrontRemoved(), compareValue.getRelativeBearing());
                case IndexRelativeDistance: return m_relativeDistance.comparePropertyByIndex(index.copyFrontRemoved(), compareValue.getRelativeDistance());
                default:
                    const QString m = QString("no property, index ").append(index.toQString());
                    Q_ASSERT_X(false, Q_FUNC_INFO, m.toLocal8Bit().constData());
                    break;
                }
            }
            return 0;
        }

        QString ICoordinateWithRelativePosition::convertToQString(bool i18n) const
        {
            return m_relativeBearing.toQString(i18n) % u' ' %
                   m_relativeDistance.toQString(i18n) % u' ' %
                   ICoordinateGeodetic::convertToQString(i18n);
        }

        ICoordinateWithRelativePosition::ICoordinateWithRelativePosition()
        { }

        bool ICoordinateWithRelativePosition::canHandleIndex(const CPropertyIndex &index)
        {
            if (ICoordinateGeodetic::canHandleIndex(index)) { return true; }
            const int i = index.frontCasted<int>();
            return (i >= static_cast<int>(IndexRelativeDistance)) && (i <= static_cast<int>(IndexRelativeBearing));
        }
    } // namespace
} // namespace