/*****************************************************************************************
 *                                                                                       *
 * OpenSpace                                                                             *
 *                                                                                       *
 * Copyright (c) 2014-2018                                                               *
 *                                                                                       *
 * 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/space/translation/keplertranslation.h>

#include <openspace/documentation/verifier.h>
#include <openspace/util/spicemanager.h>

#include <openspace/util/updatestructures.h>

#include <glm/gtx/transform.hpp>

#include <math.h>

namespace {

template <typename T, typename Func>
T solveIteration(Func function, T x0, const T& err = 0.0, int maxIterations = 100) {
    T x = 0;
    T x2 = x0;

    for (int i = 0; i < maxIterations; ++i) {
        x = x2;
        x2 = function(x);
        if (std::abs(x2 - x) < err) {
            return x2;
        }
    }

    return x2;
}

    static const openspace::properties::Property::PropertyInfo EccentricityInfo = {
        "Eccentricity",
        "Eccentricity",
        "This value determines the eccentricity, that is the deviation from a perfect "
        "sphere, for this orbit. Currently, hyperbolic orbits using Keplerian elements "
        "are not supported."
    };

    static const openspace::properties::Property::PropertyInfo SemiMajorAxisInfo = {
        "SemiMajorAxis",
        "Semi-major axis",
        "This value determines the semi-major axis, that is the distance of the object "
        "from the central body in kilometers (semi-major axis = average of periapsis and "
        "apoapsis)."
    };

    static const openspace::properties::Property::PropertyInfo InclinationInfo = {
        "Inclination",
        "Inclination",
        "This value determines the degrees of inclination, or the angle of the orbital "
        "plane, relative to the reference plane, on which the object orbits around the "
        "central body."
    };

    static const openspace::properties::Property::PropertyInfo AscendingNodeInfo = {
        "AscendingNode",
        "Right ascension of ascending Node",
        "This value determines the right ascension of the ascending node in degrees, "
        "that is the location of position along the orbit where the inclined plane and "
        "the horizonal reference plane intersect."
    };

    static const openspace::properties::Property::PropertyInfo ArgumentOfPeriapsisInfo = {
        "ArgumentOfPeriapsis",
        "Argument of Periapsis",
        "This value determines the argument of periapsis in degrees, that is the "
        "position on the orbit that is closest to the orbiting body."
    };

    static const openspace::properties::Property::PropertyInfo MeanAnomalyAtEpochInfo = {
        "MeanAnomaly",
        "Mean anomaly at epoch",
        "This value determines the mean anomaly at the epoch in degrees, which "
        "determines the initial location of the object along the orbit at epoch."
    };

    static const openspace::properties::Property::PropertyInfo EpochInfo = {
        "Epoch",
        "Epoch",
        "This value determines the epoch for which the initial location is defined in "
        "the form of YYYY MM DD HH:mm:ss."
    };

    static const openspace::properties::Property::PropertyInfo PeriodInfo = {
        "Period",
        "Orbit period",
        "Specifies the orbital period (in seconds)."
    };
} // namespace

namespace openspace {

KeplerTranslation::RangeError::RangeError(std::string off)
    : ghoul::RuntimeError("Value '" + off + "' out of range", "KeplerTranslation")
    , offender(std::move(off))
{}

documentation::Documentation KeplerTranslation::Documentation() {
    using namespace openspace::documentation;
    return {
        "Kepler Translation",
        "space_transform_kepler",
        {
            {
                "Type",
                new StringEqualVerifier("KeplerTranslation"),
                Optional::No
            },
            {
                EccentricityInfo.identifier,
                new DoubleInRangeVerifier(0.0, 1.0),
                Optional::No,
                EccentricityInfo.description
            },
            {
                SemiMajorAxisInfo.identifier,
                new DoubleVerifier,
                Optional::No,
                SemiMajorAxisInfo.description
            },
            {
                InclinationInfo.identifier,
                new DoubleInRangeVerifier(0.0, 360.0),
                Optional::No,
                InclinationInfo.description
            },
            {
                AscendingNodeInfo.identifier,
                new DoubleInRangeVerifier(0.0, 360.0),
                Optional::No,
                AscendingNodeInfo.description
            },
            {
                ArgumentOfPeriapsisInfo.identifier,
                new DoubleInRangeVerifier(0.0, 360.0),
                Optional::No,
                ArgumentOfPeriapsisInfo.description
            },
            {
                MeanAnomalyAtEpochInfo.identifier,
                new DoubleInRangeVerifier(0.0, 360.0),
                Optional::No,
                MeanAnomalyAtEpochInfo.description
            },
            {
                EpochInfo.identifier,
                new StringVerifier,
                Optional::No,
                EpochInfo.description
            },
            {
                PeriodInfo.identifier,
                new DoubleGreaterVerifier(0.0),
                Optional::No,
                PeriodInfo.description
            },
        }
    };
}

KeplerTranslation::KeplerTranslation()
    : Translation()
    , _eccentricity(EccentricityInfo, 0.0, 0.0, 1.0)
    , _semiMajorAxis(SemiMajorAxisInfo, 0.0, 0.0, 1e6)
    , _inclination(InclinationInfo, 0.0, 0.0, 360.0)
    , _ascendingNode(AscendingNodeInfo, 0.0, 0.0, 360.0)
    , _argumentOfPeriapsis(ArgumentOfPeriapsisInfo, 0.0, 0.0, 360.0)
    , _meanAnomalyAtEpoch(MeanAnomalyAtEpochInfo, 0.0, 0.0, 360.0)
    , _epoch(EpochInfo, 0.0, 0.0, 1e9)
    , _period(PeriodInfo, 0.0, 0.0, 1e6)
    , _orbitPlaneDirty(true)
{
    auto update = [this]() {
        _orbitPlaneDirty = true;
        requireUpdate();
    };

    // Only the eccentricity, semimajor axis, inclination, and location of ascending node
    // invalidate the shape of the orbit. The other parameters only determine the location
    // the spacecraft on that orbit
    _eccentricity.onChange(update);
    addProperty(_eccentricity);

    _semiMajorAxis.onChange(update);
    addProperty(_semiMajorAxis);

    _inclination.onChange(update);
    addProperty(_inclination);

    _ascendingNode.onChange(update);
    addProperty(_ascendingNode);

    _argumentOfPeriapsis.onChange(update);
    addProperty(_argumentOfPeriapsis);

    addProperty(_meanAnomalyAtEpoch);
    addProperty(_epoch);
    addProperty(_period);
}

KeplerTranslation::KeplerTranslation(const ghoul::Dictionary& dictionary)
    : KeplerTranslation()
{
    documentation::testSpecificationAndThrow(
        Documentation(),
        dictionary,
        "KeplerTranslation"
    );

    setKeplerElements(
        dictionary.value<double>(EccentricityInfo.identifier),
        dictionary.value<double>(SemiMajorAxisInfo.identifier),
        dictionary.value<double>(InclinationInfo.identifier),
        dictionary.value<double>(AscendingNodeInfo.identifier),
        dictionary.value<double>(ArgumentOfPeriapsisInfo.identifier),
        dictionary.value<double>(MeanAnomalyAtEpochInfo.identifier),
        dictionary.value<double>(PeriodInfo.identifier),
        dictionary.value<std::string>(EpochInfo.identifier)
    );
}

double KeplerTranslation::eccentricAnomaly(double meanAnomaly) const {
    // Compute the eccentric anomaly (the location of the spacecraft taking the
    // eccentricity of the orbit into account) using different solves for the regimes in
    // which they are most efficient

    if (_eccentricity == 0.0) {
        // In a circular orbit, the eccentric anomaly = mean anomaly
        return meanAnomaly;
    }
    else if (_eccentricity < 0.2) {
        auto solver = [this, &meanAnomaly](double x) -> double {
            // For low eccentricity, using a first order solver sufficient
            return meanAnomaly + _eccentricity * sin(x);
        };
        return solveIteration(solver, meanAnomaly, 0.0, 5);
    }
    else if (_eccentricity < 0.9) {
        auto solver = [this, &meanAnomaly](double x) -> double {
            double e = _eccentricity;
            return x + (meanAnomaly + e * sin(x) - x) / (1.0 - e * cos(x));
        };
        return solveIteration(solver, meanAnomaly, 0.0, 6);
    }
    else if (_eccentricity < 1.0) {
        auto sign = [](double val) -> double {
            return val > 0.0 ? 1.0 : ((val < 0.0) ? -1.0 : 0.0);
        };
        double e = meanAnomaly + 0.85 * _eccentricity * sign(sin(meanAnomaly));

        auto solver = [this, &meanAnomaly, &sign](double x) -> double {
            double s = _eccentricity * sin(x);
            double c = _eccentricity * cos(x);
            double f = x - s - meanAnomaly;
            double f1 = 1 - c;
            double f2 = s;
            return x + (-5 * f / (f1 + sign(f1) *
                sqrt(std::abs(16 * f1 * f1 - 20 * f * f2))));
        };
        return solveIteration(solver, e, 0.0, 8);
    }
    else {
        ghoul_assert(false, "Eccentricity must not be >= 1.0");
        LERRORC("KeplerTranslation", "Eccentricity must not be >= 1.0");
        return 0.0;
    }
}

glm::dvec3 KeplerTranslation::position(const Time& time) const {
    if (_orbitPlaneDirty) {
        computeOrbitPlane();
        _orbitPlaneDirty = false;
    }

    double t = time.j2000Seconds() - _epoch;
    double meanMotion = 2.0 * glm::pi<double>() / _period;
    double meanAnomaly = glm::radians(_meanAnomalyAtEpoch.value()) + t * meanMotion;
    double e = eccentricAnomaly(meanAnomaly);

    // Use the eccentric anomaly to compute the actual location
    double a = _semiMajorAxis / (1.0 - _eccentricity) * 1000.0;
    glm::dvec3 p = {
        a * (cos(e) - _eccentricity),
        a * sqrt(1.0 - _eccentricity * _eccentricity) * sin(e),
        0.0
    };
    return _orbitPlaneRotation * p;
}

void KeplerTranslation::computeOrbitPlane() const {
    // We assume the following coordinate system:
    // z = axis of rotation
    // x = pointing towards the first point of Aries
    // y completes the righthanded coordinate system

    // Perform three rotations:
    // 1. Around the z axis to place the location of the ascending node
    // 2. Around the x axis (now aligned with the ascending node) to get the correct
    // inclination
    // 3. Around the new z axis to place the closest approach to the correct location

    const glm::vec3 ascendingNodeAxisRot = { 0.f, 0.f, 1.f };
    const glm::vec3 inclinationAxisRot = { 1.f, 0.f, 0.f };
    const glm::vec3 argPeriapsisAxisRot = { 0.f, 0.f, 1.f };

    const double asc = glm::radians(_ascendingNode.value());
    const double inc = glm::radians(_inclination.value());
    const double per = glm::radians(_argumentOfPeriapsis.value());

    _orbitPlaneRotation =
        glm::rotate(asc, glm::dvec3(ascendingNodeAxisRot)) *
        glm::rotate(inc, glm::dvec3(inclinationAxisRot)) *
        glm::rotate(per, glm::dvec3(argPeriapsisAxisRot));

    notifyObservers();
    _orbitPlaneDirty = false;
}

void KeplerTranslation::setKeplerElements(double eccentricity, double semiMajorAxis,
                                          double inclination, double ascendingNode,
                                          double argumentOfPeriapsis,
                                          double meanAnomalyAtEpoch,
                                          double orbitalPeriod, const std::string& epoch)
{
    setKeplerElements(
        eccentricity,
        semiMajorAxis,
        inclination,
        ascendingNode,
        argumentOfPeriapsis,
        meanAnomalyAtEpoch,
        orbitalPeriod,
        SpiceManager::ref().ephemerisTimeFromDate(epoch)
    );
}

void KeplerTranslation::setKeplerElements(double eccentricity, double semiMajorAxis,
                                          double inclination, double ascendingNode,
                                          double argumentOfPeriapsis,
                                          double meanAnomalyAtEpoch,
                                          double orbitalPeriod, double epoch)
{
    auto isInRange = [](double val, double min, double max) -> bool {
        return val >= min && val <= max;
    };

    if (isInRange(eccentricity, 0.0, 1.0)) {
        _eccentricity = eccentricity;
    }
    else {
        throw RangeError("Eccentricity");
    }

    _semiMajorAxis = semiMajorAxis;

    if (isInRange(inclination, 0.0, 360.0)) {
        _inclination = inclination;
    }
    else {
        throw RangeError("Inclination");
    }

    if (isInRange(_ascendingNode, 0.0, 360.0)) {
        _ascendingNode = ascendingNode;
    }
    else {
        throw RangeError("Ascending Node");
    }
    if (isInRange(_argumentOfPeriapsis, 0.0, 360.0)) {
        _argumentOfPeriapsis = argumentOfPeriapsis;
    }
    else {
        throw RangeError("Argument of Periapsis");
    }

    if (isInRange(_meanAnomalyAtEpoch, 0.0, 360.0)) {
        _meanAnomalyAtEpoch = meanAnomalyAtEpoch;
    }
    else {
        throw RangeError("Mean anomaly at epoch");
    }

    _period = orbitalPeriod;
    _epoch = epoch;

    computeOrbitPlane();
}

} // namespace openspace