//
//  AngularConstraint.cpp
//  interface/src/renderer
//
//  Created by Andrew Meadows on 2014.05.30
//  Copyright 2014 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 <glm/gtx/norm.hpp>

#include "AngularConstraint.h"
#include "SharedUtil.h"

// helper function
/// \param angle radian angle to be clamped within angleMin and angleMax
/// \param angleMin minimum value
/// \param angleMax maximum value
/// \return value between minAngle and maxAngle closest to angle
float clampAngle(float angle, float angleMin, float angleMax) {
    float minDistance = angle - angleMin;
    float maxDistance = angle - angleMax;
    if (maxDistance > 0.0f) {
        minDistance = glm::min(minDistance, angleMin + TWO_PI - angle);
        angle = (minDistance < maxDistance) ? angleMin : angleMax;
    } else if (minDistance < 0.0f) {
        maxDistance = glm::max(maxDistance, angleMax - TWO_PI - angle);
        angle = (minDistance > maxDistance) ? angleMin : angleMax;
    }
    return angle;
}

// static 
AngularConstraint* AngularConstraint::newAngularConstraint(const glm::vec3& minAngles, const glm::vec3& maxAngles) {
    float minDistance2 = glm::distance2(minAngles, glm::vec3(-PI, -PI, -PI));
    float maxDistance2 = glm::distance2(maxAngles, glm::vec3(PI, PI, PI));
    if (minDistance2 < EPSILON && maxDistance2 < EPSILON) {
        // no constraint
        return NULL;
    }
    // count the zero length elements
    glm::vec3 rangeAngles = maxAngles - minAngles;
    int pivotIndex = -1;
    int numZeroes = 0;
    for (int i = 0; i < 3; ++i) {
        if (rangeAngles[i] < EPSILON) {
            ++numZeroes;
        } else {
            pivotIndex = i;
        }
    }
    if (numZeroes == 2) {
        // this is a hinge
        int forwardIndex = (pivotIndex + 1) % 3;
        glm::vec3 forwardAxis(0.0f);
        forwardAxis[forwardIndex] = 1.0f;
        glm::vec3 rotationAxis(0.0f);
        rotationAxis[pivotIndex] = 1.0f;
        return new HingeConstraint(forwardAxis, rotationAxis, minAngles[pivotIndex], maxAngles[pivotIndex]);
    } else if (numZeroes == 0) {
        // approximate the angular limits with a cone roller
        // we assume the roll is about z
        glm::vec3 middleAngles = 0.5f * (maxAngles + minAngles);
        glm::quat yaw = glm::angleAxis(middleAngles[1], glm::vec3(0.0f, 1.0f, 0.0f));
        glm::quat pitch = glm::angleAxis(middleAngles[0], glm::vec3(1.0f, 0.0f, 0.0f));
        glm::vec3 coneAxis = pitch * yaw * glm::vec3(0.0f, 0.0f, 1.0f);
        // the coneAngle is half the average range of the two non-roll rotations
        glm::vec3 range = maxAngles - minAngles;
        float coneAngle = 0.25f * (range[0] + range[1]);
        return new ConeRollerConstraint(coneAngle, coneAxis, minAngles.z, maxAngles.z);
    }
    return NULL;
}

bool AngularConstraint::softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction) {
    glm::quat clampedTarget = targetRotation;
    bool clamped = clamp(clampedTarget);
    if (clamped) {
        // check if oldRotation is also clamped
        glm::quat clampedOld = oldRotation;
        bool clamped2 = clamp(clampedOld);
        if (clamped2) {
            // oldRotation is already beyond the constraint
            // we clamp again midway between targetRotation and clamped oldPosition
            clampedTarget = glm::shortMix(clampedOld, targetRotation, mixFraction);
            // and then clamp that
            clamp(clampedTarget);
        }
        // finally we mix targetRotation with the clampedTarget
        targetRotation = glm::shortMix(clampedTarget, targetRotation, mixFraction);
    }
    return clamped;
}

HingeConstraint::HingeConstraint(const glm::vec3& forwardAxis, const glm::vec3& rotationAxis, float minAngle, float maxAngle)
        : _minAngle(minAngle), _maxAngle(maxAngle) {
    assert(_minAngle < _maxAngle);
    // we accept the rotationAxis direction
    assert(glm::length(rotationAxis) > EPSILON);
    _rotationAxis = glm::normalize(rotationAxis);
    // but we compute the final _forwardAxis
    glm::vec3 otherAxis = glm::cross(_rotationAxis, forwardAxis);
    assert(glm::length(otherAxis) > EPSILON);
    _forwardAxis = glm::normalize(glm::cross(otherAxis, _rotationAxis));
}

// virtual 
bool HingeConstraint::clamp(glm::quat& rotation) const {
    glm::vec3 forward = rotation * _forwardAxis;
    forward -= glm::dot(forward, _rotationAxis) * _rotationAxis;
    float length = glm::length(forward);
    if (length < EPSILON) {
        // infinite number of solutions ==> choose the middle of the contrained range
        rotation = glm::angleAxis(0.5f * (_minAngle + _maxAngle), _rotationAxis);
        return true;
    }
    forward /= length;
    float sign = (glm::dot(glm::cross(_forwardAxis, forward), _rotationAxis) > 0.0f ? 1.0f : -1.0f);
    //float angle = sign * acos(glm::dot(forward, _forwardAxis) / length);
    float angle = sign * acos(glm::dot(forward, _forwardAxis));
    glm::quat newRotation = glm::angleAxis(clampAngle(angle, _minAngle, _maxAngle), _rotationAxis);
    if (fabsf(1.0f - glm::dot(newRotation, rotation)) > EPSILON * EPSILON) {
        rotation = newRotation;
        return true;
    }
    return false;
}

bool HingeConstraint::softClamp(glm::quat& targetRotation, const glm::quat& oldRotation, float mixFraction) {
    // the hinge works best without a soft clamp
    return clamp(targetRotation);
}

ConeRollerConstraint::ConeRollerConstraint(float coneAngle, const glm::vec3& coneAxis, float minRoll, float maxRoll)
        : _coneAngle(coneAngle), _minRoll(minRoll), _maxRoll(maxRoll) {
    assert(_maxRoll >= _minRoll);
    float axisLength = glm::length(coneAxis);
    assert(axisLength > EPSILON);
    _coneAxis = coneAxis / axisLength;
}

// virtual 
bool ConeRollerConstraint::clamp(glm::quat& rotation) const {
    bool applied = false;
    glm::vec3 rotatedAxis = rotation * _coneAxis;
    glm::vec3 perpAxis = glm::cross(rotatedAxis, _coneAxis);
    float perpAxisLength = glm::length(perpAxis);
    if (perpAxisLength > EPSILON) {
        perpAxis /= perpAxisLength;
        // enforce the cone
        float angle = acosf(glm::dot(rotatedAxis, _coneAxis));
        if (angle > _coneAngle) {
            rotation = glm::angleAxis(angle - _coneAngle, perpAxis) * rotation;
            rotatedAxis = rotation * _coneAxis;
            applied = true;
        }
    } else {
        // the rotation is 100% roll
        // there is no obvious perp axis so we must pick one
        perpAxis = rotatedAxis;
        // find the first non-zero element:
        float iValue = 0.0f;
        int i = 0;
        for (i = 0; i < 3; ++i) {
            if (fabsf(perpAxis[i]) > EPSILON) {
                iValue = perpAxis[i];
                break;
            }
        }
        assert(i != 3);
        // swap or negate the next element
        int j = (i + 1) % 3;
        float jValue = perpAxis[j];
        if (fabsf(jValue - iValue) > EPSILON) {
            perpAxis[i] = jValue;
            perpAxis[j] = iValue;
        } else {
            perpAxis[i] = -iValue;
        }
        perpAxis = glm::cross(perpAxis, rotatedAxis);
        perpAxisLength = glm::length(perpAxis);
        assert(perpAxisLength > EPSILON);
        perpAxis /= perpAxisLength;
    }
    // measure the roll
    // NOTE: perpAxis is perpendicular to both _coneAxis and rotatedConeAxis, so we can 
    // rotate it again and we'll end up with an something that has only been rolled.
    glm::vec3 rolledPerpAxis = rotation * perpAxis;
    float sign = glm::dot(rotatedAxis, glm::cross(perpAxis, rolledPerpAxis)) > 0.0f ? 1.0f : -1.0f;
    float roll = sign * angleBetween(rolledPerpAxis, perpAxis);
    if (roll < _minRoll || roll > _maxRoll) {
        float clampedRoll = clampAngle(roll, _minRoll, _maxRoll);
        rotation = glm::normalize(glm::angleAxis(clampedRoll - roll, rotatedAxis) * rotation);
        applied = true;
    }
    return applied;
}