// SPDX-License-Identifier: LGPL-2.1-or-later
//
// SPDX-FileCopyrightText: 2010 Dennis Nienhüser <nienhueser@kde.org>
//

#include "RoutingPoint.h"

#include <QTextStream>

#include <cmath>

namespace Marble
{

RoutingPoint::RoutingPoint( qreal lon, qreal lat ) :
        m_lon( lon), m_lonRad( lon * M_PI / 180.0 ),
        m_lat( lat ), m_latRad( lat * M_PI / 180.0 )
{
    // nothing to do
}

qreal RoutingPoint::lon() const
{
    return m_lon;
}

qreal RoutingPoint::lat() const
{
    return m_lat;
}

// Code based on https://www.movable-type.co.uk/scripts/latlong.html
qreal RoutingPoint::bearing( const RoutingPoint &other ) const
{
    qreal deltaLon = other.m_lonRad - m_lonRad;
    qreal y = sin( deltaLon ) * cos( other.m_latRad );
    qreal x = cos( m_latRad ) * sin( other.m_latRad ) -
              sin( m_latRad ) * cos( other.m_latRad ) * cos( deltaLon );
    return atan2( y, x );
}

// From MarbleMath::distanceSphere
qreal RoutingPoint::distance( const RoutingPoint &other ) const
{
    qreal h1 = sin( 0.5 * ( other.m_latRad - m_latRad ) );
    qreal h2 = sin( 0.5 * ( other.m_lonRad - m_lonRad ) );
    qreal d = h1 * h1 + cos( m_latRad ) * cos( other.m_latRad ) * h2 * h2;

    return 6378137.0 * 2.0 * atan2( sqrt( d ), sqrt( 1.0 - d ) );
}

QTextStream& operator<<( QTextStream& stream, const RoutingPoint &p )
{
    stream << "(" << p.lon() << ", " << p.lat() << ")";
    return stream;
}

} // namespace Marble
