//
//  SwingTwistConstraint.cpp
//
//  Copyright 2015 High Fidelity, Inc.
//
//  Distributed under the Apache License, Version 2.0.
//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//

#include "SwingTwistConstraint.h"

#include <algorithm>
#include <math.h>

#include <GeometryUtil.h>
#include <GLMHelpers.h>
#include <NumericalConstants.h>
#include "AnimUtil.h"


const float MIN_MINDOT = -0.999f;
const float MAX_MINDOT = 1.0f;

const int LAST_CLAMP_LOW_BOUNDARY = -1;
const int LAST_CLAMP_NO_BOUNDARY = 0;
const int LAST_CLAMP_HIGH_BOUNDARY = 1;

SwingTwistConstraint::SwingLimitFunction::SwingLimitFunction() {
    _minDots.push_back(-1.0f);
    _minDots.push_back(-1.0f);

    _minDotIndexA = -1;
    _minDotIndexB = -1;
}

// In order to support the dynamic adjustment to swing limits we require
// that minDots have a minimum number of elements:
const int MIN_NUM_DOTS = 8;

void SwingTwistConstraint::SwingLimitFunction::setMinDots(const std::vector<float>& minDots) {
    int numDots = (int)minDots.size();
    _minDots.clear();
    if (numDots == 0) {
        // push multiple copies of MIN_MINDOT
        for (int i = 0; i < MIN_NUM_DOTS; ++i) {
            _minDots.push_back(MIN_MINDOT);
        }
        // push one more for cyclic boundary conditions
        _minDots.push_back(MIN_MINDOT);
    } else {
        // for minimal fidelity in the dynamic adjustment we expand the swing limit data until
        // we have enough data points
        int trueNumDots = numDots;
        int numFiller = 0;
        while(trueNumDots < MIN_NUM_DOTS) {
            numFiller++;
            trueNumDots += numDots;
        }
        _minDots.reserve(trueNumDots);

        for (int i = 0; i < numDots; ++i) {
            // push the next value
            _minDots.push_back(glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT));

            if (numFiller > 0) {
                // compute endpoints of line segment
                float nearDot = glm::clamp(minDots[i], MIN_MINDOT, MAX_MINDOT);
                int k = (i + 1) % numDots;
                float farDot = glm::clamp(minDots[k], MIN_MINDOT, MAX_MINDOT);

                // fill the gap with interpolated values
                for (int j = 0; j < numFiller; ++j) {
                    float delta = (float)(j + 1) / float(numFiller + 1);
                    _minDots.push_back((1.0f - delta) * nearDot + delta * farDot);
                }
            }
        }
        // push the first value to the back to for cyclic boundary conditions
        _minDots.push_back(_minDots[0]);
    }
    _minDotIndexA = -1;
    _minDotIndexB = -1;
}

/// \param angle radian angle to update
/// \param minDotAdjustment minimum dot limit at that angle
void SwingTwistConstraint::SwingLimitFunction::dynamicallyAdjustMinDots(float theta, float minDotAdjustment) {
    // What does "dynamic adjustment" mean?
    //
    // Consider a limitFunction that looks like this:
    //
    //  1+
    //   |                valid space
    //   |
    //   +-----+-----+-----+-----+-----+-----+-----+-----+
    //   |
    //   |                invalid space
    //  0+------------------------------------------------
    //   0          pi/2         pi         3pi/2       2pi
    //   theta --->
    //
    // If we wanted to modify the envelope to accept a single invalid point X
    // then we would need to modify neighboring values A and B accordingly:
    //
    //  1+          adjustment for X at some thetaX
    //   |              |
    //   |              |
    //   +-----+.       V       .+-----+-----+-----+-----+
    //   |        -           -
    //   |         ' A--X--B '
    //  0+------------------------------------------------
    //   0          pi/2         pi         3pi/2       2pi
    //
    // The code below computes the values of A and B such that the line between them
    // passes through the point X, and we get reasonable interpolation for nearby values
    // of theta.  The old AB values are saved for later restore.

    if (_minDotIndexA > -1) {
        // retstore old values
        _minDots[_minDotIndexA] = _minDotA;
        _minDots[_minDotIndexB] = _minDotB;

        // handle cyclic boundary conditions
        int lastIndex = (int)_minDots.size() - 1;
        if (_minDotIndexA == 0) {
            _minDots[lastIndex] = _minDotA;
        } else if (_minDotIndexB == lastIndex) {
            _minDots[0] = _minDotB;
        }
    }

    // extract the positive normalized fractional part of the theta
    float integerPart;
    float normalizedAngle = modff(theta / TWO_PI, &integerPart);
    if (normalizedAngle < 0.0f) {
        normalizedAngle += 1.0f;
    }

    // interpolate between the two nearest points in the curve
    float delta = modff(normalizedAngle * (float)(_minDots.size() - 1), &integerPart);
    int indexA = (int)(integerPart);
    int indexB = (indexA + 1) % _minDots.size();
    float interpolatedDot = _minDots[indexA] * (1.0f - delta) + _minDots[indexB] * delta;

    if (minDotAdjustment < interpolatedDot) {
        // minDotAdjustment is outside the existing bounds so we must modify

        // remember the indices
        _minDotIndexA = indexA;
        _minDotIndexB = indexB;

        // save the old minDots
        _minDotA = _minDots[_minDotIndexA];
        _minDotB = _minDots[_minDotIndexB];

        // compute replacement values to _minDots that will provide a line segment
        // that passes through minDotAdjustment while balancing the distortion between A and B.
        // Note: the derivation of these formulae is left as an exercise to the reader.
        float twiceUndershoot = 2.0f * (minDotAdjustment - interpolatedDot);
        _minDots[_minDotIndexA] -= twiceUndershoot * (delta + 0.5f) * (delta - 1.0f);
        _minDots[_minDotIndexB] -= twiceUndershoot * delta * (delta - 1.5f);

        // handle cyclic boundary conditions
        int lastIndex = (int)_minDots.size() - 1;
        if (_minDotIndexA == 0) {
            _minDots[lastIndex] = _minDots[_minDotIndexA];
        } else if (_minDotIndexB == lastIndex) {
            _minDots[0] = _minDots[_minDotIndexB];
        }
    } else {
        // minDotAdjustment is inside bounds so there is nothing to do
        _minDotIndexA = -1;
        _minDotIndexB = -1;
    }
}

float SwingTwistConstraint::SwingLimitFunction::getMinDot(float theta) const {
    // extract the positive normalized fractional part of theta
    float integerPart;
    float normalizedTheta = modff(theta / TWO_PI, &integerPart);
    if (normalizedTheta < 0.0f) {
        normalizedTheta += 1.0f;
    }

    // interpolate between the two nearest points in the cycle
    float fractionPart = modff(normalizedTheta * (float)(_minDots.size() - 1), &integerPart);
    int i = (int)(integerPart);
    int j = (i + 1) % _minDots.size();
    return _minDots[i] * (1.0f - fractionPart) + _minDots[j] * fractionPart;
}

SwingTwistConstraint::SwingTwistConstraint() :
        RotationConstraint(),
        _swingLimitFunction(),
        _minTwist(-PI),
        _maxTwist(PI),
        _lastTwistBoundary(LAST_CLAMP_NO_BOUNDARY)
{
}

void SwingTwistConstraint::setSwingLimits(std::vector<float> minDots) {
    _swingLimitFunction.setMinDots(minDots);
}

void SwingTwistConstraint::setSwingLimits(const std::vector<glm::vec3>& swungDirections) {
    struct SwingLimitData {
        SwingLimitData() : _theta(0.0f), _minDot(1.0f) {}
        SwingLimitData(float theta, float minDot) : _theta(theta), _minDot(minDot) {}
        float _theta;
        float _minDot;
        bool operator<(const SwingLimitData& other) const { return _theta < other._theta; }
    };
    std::vector<SwingLimitData> limits;

    int numLimits = (int)swungDirections.size();
    limits.reserve(numLimits);

    // compute the limit pairs: <theta, minDot>
    for (int i = 0; i < numLimits; ++i) {
        float directionLength = glm::length(swungDirections[i]);
        if (directionLength > EPSILON) {
            glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungDirections[i]);
            float theta = atan2f(-swingAxis.z, swingAxis.x);
            if (theta < 0.0f) {
                theta += TWO_PI;
            }
            limits.push_back(SwingLimitData(theta, swungDirections[i].y / directionLength));
        }
    }

    std::vector<float> minDots;
    numLimits = (int)limits.size();
    if (numLimits == 0) {
        // trivial case: nearly free constraint
        std::vector<float> minDots;
        _swingLimitFunction.setMinDots(minDots);
    } else if (numLimits == 1) {
        // trivial case: uniform conical constraint
        std::vector<float> minDots;
        minDots.push_back(limits[0]._minDot);
        _swingLimitFunction.setMinDots(minDots);
    } else {
        // interesting case: potentially non-uniform constraints

        // sort limits by theta
        std::sort(limits.begin(), limits.end());

        // extrapolate evenly distributed limits for fast lookup table
        float deltaTheta = TWO_PI / (float)(numLimits);
        int rightIndex = 0;
        for (int i = 0; i < numLimits; ++i) {
            float theta = (float)i * deltaTheta;
            int leftIndex = (rightIndex - 1 + numLimits) % numLimits;
            while (rightIndex < numLimits && theta > limits[rightIndex]._theta) {
                leftIndex = rightIndex++;
            }

            if (leftIndex == numLimits - 1) {
                // we straddle the boundary
                rightIndex = 0;
            }

            float rightTheta = limits[rightIndex]._theta;
            float leftTheta = limits[leftIndex]._theta;
            if (leftTheta > rightTheta) {
                // we straddle the boundary, but we need to figure out which way to stride
                // in order to keep theta between left and right
                if (leftTheta > theta) {
                    leftTheta -= TWO_PI;
                } else {
                    rightTheta += TWO_PI;
                }
            }

            // blend between the left and right minDots to get the value that corresponds to this theta
            float rightWeight = (theta - leftTheta) / (rightTheta - leftTheta);
            minDots.push_back((1.0f - rightWeight) * limits[leftIndex]._minDot + rightWeight * limits[rightIndex]._minDot);
        }
    }
    _swingLimitFunction.setMinDots(minDots);
}

void SwingTwistConstraint::setTwistLimits(float minTwist, float maxTwist) {
    // NOTE: min/maxTwist angles should be in the range [-PI, PI]
    _minTwist = glm::min(minTwist, maxTwist);
    _maxTwist = glm::max(minTwist, maxTwist);

    _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
    _twistAdjusted = false;
}

// private
float SwingTwistConstraint::handleTwistBoundaryConditions(float twistAngle) const {
    // adjust measured twistAngle according to clamping history
    switch (_lastTwistBoundary) {
        case LAST_CLAMP_LOW_BOUNDARY:
            // clamp to min
            if (twistAngle > _maxTwist) {
                twistAngle -= TWO_PI;
            }
            break;
        case LAST_CLAMP_HIGH_BOUNDARY:
            // clamp to max
            if (twistAngle < _minTwist) {
                twistAngle += TWO_PI;
            }
            break;
        default: // LAST_CLAMP_NO_BOUNDARY
            // clamp to nearest boundary
            float midBoundary = 0.5f * (_maxTwist + _minTwist + TWO_PI);
            if (twistAngle > midBoundary) {
                // lower boundary is closer --> phase down one cycle
                twistAngle -= TWO_PI;
            } else if (twistAngle < midBoundary - TWO_PI) {
                // higher boundary is closer --> phase up one cycle
                twistAngle += TWO_PI;
            }
            break;
    }
    return twistAngle;
}

bool SwingTwistConstraint::apply(glm::quat& rotation) const {
    // decompose the rotation into first twist about yAxis, then swing about something perp
    // NOTE: rotation = postRotation * referenceRotation
    glm::quat postRotation = rotation * glm::inverse(_referenceRotation);
    glm::quat swingRotation, twistRotation;
    swingTwistDecomposition(postRotation, Vectors::UNIT_Y, swingRotation, twistRotation);
    // NOTE: postRotation = swingRotation * twistRotation

    // compute raw twistAngle
    float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));
    glm::vec3 twistedX = twistRotation * Vectors::UNIT_X;
    twistAngle *= copysignf(1.0f, glm::dot(glm::cross(Vectors::UNIT_X, twistedX), Vectors::UNIT_Y));

    bool somethingClamped = false;
    if (_minTwist != _maxTwist) {
        // twist limits apply --> figure out which limit we're hitting, if any
        twistAngle = handleTwistBoundaryConditions(twistAngle);

        // clamp twistAngle
        float clampedTwistAngle = glm::clamp(twistAngle, _minTwist, _maxTwist);
        somethingClamped = (twistAngle != clampedTwistAngle);

        // remember twist's clamp boundary history
        if (somethingClamped) {
            _lastTwistBoundary = (twistAngle > clampedTwistAngle) ? LAST_CLAMP_HIGH_BOUNDARY : LAST_CLAMP_LOW_BOUNDARY;
            twistAngle = clampedTwistAngle;
        } else {
            _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
        }
    }

    // clamp the swing
    // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
    glm::vec3 swungY = swingRotation * Vectors::UNIT_Y;
    glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungY);
    float axisLength = glm::length(swingAxis);
    if (axisLength > EPSILON) {
        // The limit of swing is a function of "theta" which can be computed from the swingAxis
        // (which is in the constraint's ZX plane).
        float theta = atan2f(-swingAxis.z, swingAxis.x);
        float minDot = _swingLimitFunction.getMinDot(theta);
        if (glm::dot(swungY, Vectors::UNIT_Y) < minDot) {
            // The swing limits are violated so we extract the angle from midDot and
            // use it to supply a new rotation.
            swingAxis /= axisLength;
            swingRotation = glm::angleAxis(acosf(minDot), swingAxis);
            somethingClamped = true;
        }
    }

    if (somethingClamped) {
        // update the rotation
        twistRotation = glm::angleAxis(twistAngle, Vectors::UNIT_Y);
        rotation = swingRotation * twistRotation * _referenceRotation;
        return true;
    }
    return false;
}

void SwingTwistConstraint::dynamicallyAdjustLimits(const glm::quat& rotation) {
    glm::quat postRotation = rotation * glm::inverse(_referenceRotation);
    glm::quat swingRotation, twistRotation;

    swingTwistDecomposition(postRotation, Vectors::UNIT_Y, swingRotation, twistRotation);

    { // adjust swing limits
        glm::vec3 swungY = swingRotation * Vectors::UNIT_Y;
        glm::vec3 swingAxis = glm::cross(Vectors::UNIT_Y, swungY);
        float theta = atan2f(-swingAxis.z, swingAxis.x);
        if (glm::isnan(theta)) {
            // atan2f() will only return NaN if either of its arguments is NaN, which can only
            // happen if we've been given a bad rotation.  Since a NaN value here could potentially
            // cause a crash (we use the value of theta to compute indices into a std::vector)
            // we specifically check for this case.
            theta = 0.0f;
            swungY.y = 1.0f;
        }
        _swingLimitFunction.dynamicallyAdjustMinDots(theta, swungY.y);
    }

    // restore twist limits
    if (_twistAdjusted) {
        _minTwist = _oldMinTwist;
        _maxTwist = _oldMaxTwist;
        _twistAdjusted = false;
    }

    if (_minTwist != _maxTwist) {
        // compute twistAngle
        float twistAngle = 2.0f * acosf(fabsf(twistRotation.w));
        glm::vec3 twistedX = twistRotation * Vectors::UNIT_X;
        twistAngle *= copysignf(1.0f, glm::dot(glm::cross(Vectors::UNIT_X, twistedX), Vectors::UNIT_Y));
        twistAngle = handleTwistBoundaryConditions(twistAngle);

        if (twistAngle < _minTwist || twistAngle > _maxTwist) {
            // expand twist limits
            _twistAdjusted = true;
            _oldMinTwist = _minTwist;
            _oldMaxTwist = _maxTwist;
            if (twistAngle < _minTwist) {
                _minTwist = twistAngle;
            } else if (twistAngle > _maxTwist) {
                _maxTwist = twistAngle;
            }
        }
    }
}

void SwingTwistConstraint::clearHistory() {
    _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
}

glm::quat SwingTwistConstraint::computeCenterRotation() const {
    const size_t NUM_TWIST_LIMITS = 2;
    const size_t NUM_MIN_DOTS = getMinDots().size();
    std::vector<glm::quat> swingLimits;
    swingLimits.reserve(NUM_MIN_DOTS);

    glm::quat twistLimits[NUM_TWIST_LIMITS];
    if (_minTwist != _maxTwist) {
        // to ensure that twists do not flip the center rotation, we devide twist angle by 2.
        twistLimits[0] = glm::angleAxis(_minTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);
        twistLimits[1] = glm::angleAxis(_maxTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);
    }
    const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1);
    float theta = 0.0f;
    for (size_t i = 0; i < NUM_MIN_DOTS - 1; i++, theta += D_THETA) {
        // compute swing rotation from theta and phi angles.
        float phi = acos(getMinDots()[i]);
        float cos_phi = getMinDots()[i];
        float sin_phi = sinf(phi);
        glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, -sin_phi * sinf(theta));

        // to ensure that swings > 90 degrees do not flip the center rotation, we devide phi / 2
        glm::quat swing = glm::angleAxis(phi / 2, glm::normalize(glm::cross(Vectors::UNIT_Y, swungAxis)));
        swingLimits.push_back(swing);
    }
    glm::quat averageSwing = averageQuats(swingLimits.size(), &swingLimits[0]);
    glm::quat averageTwist = averageQuats(2, twistLimits);
    return averageSwing * averageTwist * _referenceRotation;
}