/*****************************************************************************************
 *                                                                                       *
 * OpenSpace                                                                             *
 *                                                                                       *
 * Copyright (c) 2014-2016                                                               *
 *                                                                                       *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
 * software and associated documentation files (the "Software"), to deal in the Software *
 * without restriction, including without limitation the rights to use, copy, modify,    *
 * merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
 * permit persons to whom the Software is furnished to do so, subject to the following   *
 * conditions:                                                                           *
 *                                                                                       *
 * The above copyright notice and this permission notice shall be included in all copies *
 * or substantial portions of the Software.                                              *
 *                                                                                       *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
 * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
 * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
 ****************************************************************************************/

#include <modules/globebrowsing/geometry/geodetic2.h>

#include <modules/globebrowsing/geometry/angle.h>

namespace openspace {
namespace globebrowsing {

Geodetic2::Geodetic2(double latitude, double longitude)
    : lat(latitude)
    , lon(longitude)
{}

Geodetic2::Geodetic2(const Geodetic2& p)
    : Geodetic2(p.lat, p.lon)
{}

glm::dvec2 Geodetic2::toLonLatVec2() const {
    return glm::dvec2(lon, lat);
}

bool Geodetic2::operator==(const Geodetic2& other) const {
    return lat == other.lat && lon == other.lon;
}

Geodetic2 Geodetic2::operator+(const Geodetic2& other) const {
    return Geodetic2(lat + other.lat, lon + other.lon);
}

Geodetic2 Geodetic2::operator-(const Geodetic2& other) const {
    return Geodetic2(lat - other.lat, lon - other.lon);
}

Geodetic2 Geodetic2::operator*(double scalar) const {
    return Geodetic2(lat * scalar, lon * scalar);
}

Geodetic2 Geodetic2::operator/(double scalar) const {
    return Geodetic2(lat / scalar, lon / scalar);
}

GeodeticPatch::GeodeticPatch(double centerLat, double centerLon, double halfSizeLat,
                             double halfSizeLon)
    : _center(Geodetic2(centerLat, centerLon))
    , _halfSize(Geodetic2(halfSizeLat, halfSizeLon))
{}

GeodeticPatch::GeodeticPatch(const Geodetic2& center, const Geodetic2& halfSize)
    : _center(center)
    , _halfSize(halfSize)
{}

GeodeticPatch::GeodeticPatch(const GeodeticPatch& patch)
    : _center(patch._center)
    , _halfSize(patch._halfSize)
{}

GeodeticPatch::GeodeticPatch(const TileIndex& tileIndex) {
    double deltaLat = (2 * glm::pi<double>()) / ((double)(1 << tileIndex.level));
    double deltaLon = (2 * glm::pi<double>()) / ((double)(1 << tileIndex.level));
    Geodetic2 nwCorner(glm::pi<double>() / 2 - deltaLat * tileIndex.y, -glm::pi<double>() + deltaLon * tileIndex.x);
    _halfSize = Geodetic2(deltaLat / 2, deltaLon / 2);
    _center = Geodetic2(nwCorner.lat - _halfSize.lat, nwCorner.lon + _halfSize.lon);
}

void GeodeticPatch::setCenter(const Geodetic2& center) {
    _center = center;
}

void GeodeticPatch::setHalfSize(const Geodetic2& halfSize) {
    _halfSize = halfSize;
}

double GeodeticPatch::maximumTileLevel() const {
    // Numerator is just pi, not 2*pi, since we are dealing with HALF sizes
    return log2(glm::pi<double>() / glm::min(_halfSize.lat, _halfSize.lon));
}

double GeodeticPatch::minimumTileLevel() const {
    // Numerator is just pi, not 2*pi, since we are dealing with HALF sizes
    return log2(glm::pi<double>() / glm::max(_halfSize.lat, _halfSize.lon));
}

const Geodetic2& GeodeticPatch::center() const {
    return _center;
}

const Geodetic2& GeodeticPatch::halfSize() const {
    return _halfSize;
}

Geodetic2 GeodeticPatch::size() const {
    return Geodetic2(2 * _halfSize.lat, 2 * _halfSize.lon);
}

Geodetic2 GeodeticPatch::getCorner(Quad q) const {
    switch (q) {
        case NORTH_WEST: return Geodetic2(maxLat(), minLon());// northWestCorner();
        case NORTH_EAST: return Geodetic2(maxLat(), maxLon());// northEastCorner();
        case SOUTH_WEST: return Geodetic2(minLat(), minLon());// southWestCorner();
        case SOUTH_EAST: return Geodetic2(minLat(), maxLon());// southEastCorner();
    }
}

Geodetic2 GeodeticPatch::getSize() const {
    return _halfSize * 2;
}

double GeodeticPatch::minLat() const {
    return _center.lat - _halfSize.lat;
}

double GeodeticPatch::maxLat() const {
    return _center.lat + _halfSize.lat;
}

double GeodeticPatch::minLon() const {
    return _center.lon - _halfSize.lon;
}

double GeodeticPatch::maxLon() const {
    return _center.lon + _halfSize.lon;
}
    
bool GeodeticPatch::contains(const Geodetic2& p) const {
    Geodetic2 diff = _center - p;
    return glm::abs(diff.lat) <= _halfSize.lat && glm::abs(diff.lon) <= _halfSize.lon;
}

double GeodeticPatch::edgeLatitudeNearestEquator() const {
    return _center.lat + _halfSize.lat * (isNorthern() ? -1 : 1);
}

double GeodeticPatch::isNorthern() const {
    return _center.lat > 0.0;
}

Geodetic2 GeodeticPatch::clamp(const Geodetic2& p) const {
    using Ang = Angle<double>;

    // Convert to Angles for normalization
    Ang centerLat = Ang::fromRadians(_center.lat);
    Ang centerLon = Ang::fromRadians(_center.lon);
    Ang pointLat = Ang::fromRadians(p.lat);
    Ang pointLon = Ang::fromRadians(p.lon);

    // Normalize w.r.t. the center in order for the clamping to done correctly
    //
    // Example: 
    //    centerLat = 0 deg, halfSize.lat = 10 deg, pointLat = 330 deg
    //        --> Just clamping pointLat would be clamp(330, -10, 10) = 10 // WRONG!
    //    Instead, if we first normalize 330 deg around 0, we get -30 deg
    //        --> clamp(-30, -10, 10) = -10 // CORRECT!
    pointLat.normalizeAround(centerLat);
    pointLon.normalizeAround(centerLon);

    return Geodetic2(
        glm::clamp(pointLat.asRadians(), minLat(), maxLat()),
        glm::clamp(pointLon.asRadians(), minLon(), maxLon())
    );
}

Geodetic2 GeodeticPatch::closestCorner(const Geodetic2& p) const {
    using Ang = Angle<double>;

    // LatLon vector from patch center to the point
    Geodetic2 centerToPoint = p - _center;
    
    // Normalize the difference angles to be centered around 0.
    Ang latDiff = Ang::fromRadians(centerToPoint.lat).normalizeAround(Ang::ZERO);
    Ang lonDiff = Ang::fromRadians(centerToPoint.lon).normalizeAround(Ang::ZERO);
        
    // If latDiff > 0 
    //    --> point p is north of the patch center 
    //    --> the closest corner to the point must be a northern one
    //    --> set the corner's latitude coordinate to center.lat + halfSize.lat
    // else 
    //    --> set corner's latidude coordinate to center.lat - halfSize.lat
    double cornerLat = _center.lat + _halfSize.lat * (latDiff > Ang::ZERO ? 1 : -1);

    // We then assigned the corner's longitude coordinate in a similar fashion
    double cornerLon = _center.lon + _halfSize.lon * (lonDiff > Ang::ZERO ? 1 : -1);

    return Geodetic2(cornerLat, cornerLon);
}

Geodetic2 GeodeticPatch::closestPoint(const Geodetic2& p) const {
    // This method finds the closest point on the patch, to the provided
    // point p. As we are deali ng with latitude-longitude patches, distance in this 
    // context refers to great-circle distance.
    // (https://en.wikipedia.org/wiki/Great-circle_distance)
    //
    // This uses a simple clamping approach to find the closest point on the 
    // patch. A naive castesian clamp is not sufficient for this purpose, 
    // as illustrated with an example below.

    // Example: (degrees are used for latidude, longitude)
    //    patchCenter = (0,0), patchHalfSize = (45,45), point = (5, 170)
    //    Note, the point and the patch are on opposite sides of the sphere
    //
    //    cartesian clamp: 
    //       --> clampedPointLat = clamp(5, -45, 45) = 5
    //       --> clampedPointLon = clamp(170, -45, 45) = 45
    //       --> result: (5, 45)
    //       --> closest point is actually (45, 45) 
    //       --> The error is significant
    // 
    // This method simply adds an extra clamp on the latitude in these cases. In the 
    // above example, that would be the following:
    //       --> clampedPointLat = clamp(180 - 5, -45, 45) = 45
    // 
    // Just doing this actually makes points returned from this methods being the
    // true closest point, great-circle distance-wise. 
    

    using Ang = Angle<double>;

    // Convert to Angles for normalization
    Ang centerLat = Ang::fromRadians(_center.lat);
    Ang centerLon = Ang::fromRadians(_center.lon);
    Ang pointLat = Ang::fromRadians(p.lat);
    Ang pointLon = Ang::fromRadians(p.lon);

    // Normalize point with respect to center. This is done because the point 
    // will later be clamped. See LatLonPatch::clamp(const LatLon&) for explanation
    pointLat.normalizeAround(centerLat);
    pointLon.normalizeAround(centerLon);

    // Calculate the longitud difference between center and point. We normalize around 
    // zero because we want the "shortest distance" difference, i.e the difference 
    // should be in the interval [-180 deg, 180 deg]
    Ang centerToPointLon = (centerLon - pointLon).normalizeAround(Ang::ZERO);

    // Calculate the longitudinal distance to the closest patch edge 
    Ang longitudeDistanceToClosestPatchEdge = centerToPointLon.abs() - Ang::fromRadians(_halfSize.lon);

    // If the longitude distance to the closest patch edge is larger than 90 deg
    // the latitude will have to be clamped to its closest corner, as explained in
    // the example above.
    double clampedLat = longitudeDistanceToClosestPatchEdge > Ang::QUARTER  ?
        clampedLat = glm::clamp((Ang::HALF - pointLat).normalizeAround(centerLat).asRadians(), minLat(), maxLat()) :
        clampedLat = glm::clamp(pointLat.asRadians(), minLat(), maxLat());

    // Longitude is just clamped normally
    double clampedLon = glm::clamp(pointLon.asRadians(), minLon(), maxLon());

    return Geodetic2(clampedLat, clampedLon);
}

} // namespace globebrowsing
} // namespace openspace
