//
//  VisionSqueeze.cpp
//  interface/src
//
//  Created by Seth Alves on 2019-3-13.
//  Copyright 2019 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 "VisionSqueeze.h"

#include <glm/gtx/matrix_decompose.hpp>
#include <glm/gtc/epsilon.hpp>

#include <display-plugins/hmd/HmdDisplayPlugin.h>

#include "Application.h"

VisionSqueeze::VisionSqueeze() :
    _visionSqueezeEnabled(_visionSqueezeEnabledSetting.get()),
    _visionSqueezeRatioX(_visionSqueezeRatioXSetting.get()),
    _visionSqueezeRatioY(_visionSqueezeRatioYSetting.get()),
    _visionSqueezeUnSqueezeDelay(_visionSqueezeUnSqueezeDelaySetting.get()),
    _visionSqueezeUnSqueezeSpeed(_visionSqueezeUnSqueezeSpeedSetting.get()),
    _visionSqueezeTransition(_visionSqueezeTransitionSetting.get()),
    _visionSqueezePerEye(_visionSqueezePerEyeSetting.get()),
    _visionSqueezeGroundPlaneY(_visionSqueezeGroundPlaneYSetting.get()),
    _visionSqueezeSpotlightSize(_visionSqueezeSpotlightSizeSetting.get()),
    _visionSqueezeTurningXFactor(_visionSqueezeTurningXFactorSetting.get()),
    _visionSqueezeTurningYFactor(_visionSqueezeTurningYFactorSetting.get()) {
}

void VisionSqueeze::setVisionSqueezeEnabled(bool value) {
    if (value != _visionSqueezeEnabled) {
        _visionSqueezeEnabled = value;
        _visionSqueezeEnabledSetting.set(_visionSqueezeEnabled);
    }
}

void VisionSqueeze::setVisionSqueezeRatioX(float value) {
    if (value != _visionSqueezeRatioX) {
        _visionSqueezeRatioX = value;
        _visionSqueezeRatioXSetting.set(_visionSqueezeRatioX);
    }
}

void VisionSqueeze::setVisionSqueezeRatioY(float value) {
    if (value != _visionSqueezeRatioY) {
        _visionSqueezeRatioY = value;
        _visionSqueezeRatioYSetting.set(_visionSqueezeRatioY);
    }
}

void VisionSqueeze::setVisionSqueezeUnSqueezeDelay(float value) {
    if (value != _visionSqueezeUnSqueezeDelay) {
        _visionSqueezeUnSqueezeDelay = value;
        _visionSqueezeUnSqueezeDelaySetting.set(_visionSqueezeUnSqueezeDelay);
    }
}

void VisionSqueeze::setVisionSqueezeUnSqueezeSpeed(float value) {
    if (value != _visionSqueezeUnSqueezeSpeed) {
        _visionSqueezeUnSqueezeSpeed = value;
        _visionSqueezeUnSqueezeSpeedSetting.set(_visionSqueezeUnSqueezeSpeed);
    }
}

void VisionSqueeze::setVisionSqueezeTransition(float value) {
    if (value != _visionSqueezeTransition) {
        _visionSqueezeTransition = value;
        _visionSqueezeTransitionSetting.set(_visionSqueezeTransition);
    }
}

void VisionSqueeze::setVisionSqueezePerEye(int value) {
    if (value != _visionSqueezePerEye) {
        _visionSqueezePerEye = value;
        _visionSqueezePerEyeSetting.set(_visionSqueezePerEye);
    }
}

void VisionSqueeze::setVisionSqueezeGroundPlaneY(float value) {
    if (value != _visionSqueezeGroundPlaneY) {
        _visionSqueezeGroundPlaneY = value;
        _visionSqueezeGroundPlaneYSetting.set(_visionSqueezeGroundPlaneY);
    }
}

void VisionSqueeze::setVisionSqueezeSpotlightSize(float value) {
    if (value != _visionSqueezeSpotlightSize) {
        _visionSqueezeSpotlightSize = value;
        _visionSqueezeSpotlightSizeSetting.set(_visionSqueezeSpotlightSize);
    }
}

void VisionSqueeze::setVisionSqueezeTurningXFactor(float value) {
    if (value != _visionSqueezeTurningXFactor) {
        _visionSqueezeTurningXFactor = value;
        _visionSqueezeTurningXFactorSetting.set(_visionSqueezeTurningXFactor);
    }
}

void VisionSqueeze::setVisionSqueezeTurningYFactor(float value) {
    if (value != _visionSqueezeTurningYFactor) {
        _visionSqueezeTurningYFactor = value;
        _visionSqueezeTurningYFactorSetting.set(_visionSqueezeTurningYFactor);
    }
}

void VisionSqueeze::updateVisionSqueeze(const glm::mat4& sensorToWorldMatrix, float deltaTime) {

    const float SENSOR_TO_WORLD_TRANS_EPSILON = 0.0001f;
    const float SENSOR_TO_WORLD_TRANS_Y_EPSILON = 0.01f;
    const float SENSOR_TO_WORLD_TRANS_ITS_A_TELEPORT_SQUARED = 2.0f;
    const float SENSOR_TO_WORLD_ROT_EPSILON = 0.000005f;
    const float SENSOR_TO_WORLD_ROT_ITS_A_SNAP_TURN = 0.99f;
    const float VISION_SQUEEZE_TP_LOCKOUT = 0.1f; // seconds

    glm::vec3 scale;
    glm::quat rotation;
    glm::vec3 translation;
    glm::vec3 skew;
    glm::vec4 perspective;
    glm::decompose(sensorToWorldMatrix, scale, rotation, translation, skew, perspective);

    if (!_visionSqueezeEnabled) {
        _squeezeVision = false;
        _squeezeVisionTurning = false;
    } else if (_visionSqueezeLockout > 0.0f) {
        _visionSqueezeLockout -= deltaTime;
    } else {
        _squeezeVision = false;
        _squeezeVisionTurning = false;
        glm::vec3 absTransDelta = glm::abs(translation - _prevTranslation);
        float rotDot = fabsf(glm::dot(rotation, _prevRotation));

        // if the avatar has just teleported or snap-turned, briefly disable triggering of vision-squeeze
        if (glm::length2(translation - _prevTranslation) > SENSOR_TO_WORLD_TRANS_ITS_A_TELEPORT_SQUARED ||
            rotDot < SENSOR_TO_WORLD_ROT_ITS_A_SNAP_TURN) {
            _visionSqueezeLockout = VISION_SQUEEZE_TP_LOCKOUT;
            _squeezeVision = true;
            _squeezeVisionTurning = true;
        } else if (rotDot < 1.0f - SENSOR_TO_WORLD_ROT_EPSILON) {
            _squeezeVision = true;
            _squeezeVisionTurning = true;
        } else if (absTransDelta.x > SENSOR_TO_WORLD_TRANS_EPSILON ||
                   absTransDelta.y > SENSOR_TO_WORLD_TRANS_Y_EPSILON ||
                   absTransDelta.z > SENSOR_TO_WORLD_TRANS_EPSILON) {
            _squeezeVision = true;
            _squeezeVisionTurning = false;
        }
    }

    _prevTranslation = translation;
    _prevRotation = rotation;

    static quint64 lastSqueezeTime = 0;
    quint64 now = usecTimestampNow();
    static float visionSqueezeX = 0.0f; // 0.0 -- unobstructed, 1.0 -- fully blocked
    static float visionSqueezeY = 0.0f; // 0.0 -- unobstructed, 1.0 -- fully blocked

    if (_squeezeVision) {
        float ratioX = getVisionSqueezeRatioX();
        float ratioY = getVisionSqueezeRatioY();

        if (ratioX >= 0.0f) {
            if (_squeezeVisionTurning) {
                ratioX += (1.0f - ratioX) * getVisionSqueezeTurningXFactor();
            }
            float newVisionSqueezeX = ratioX;
            if (newVisionSqueezeX >= visionSqueezeX) {
                lastSqueezeTime = now;
                visionSqueezeX = newVisionSqueezeX;
            }
        } else {
            visionSqueezeX = -1.0f;
        }

        if (ratioY >= 0.0f) {
            float newVisionSqueezeY = ratioY;
            if (newVisionSqueezeY >= visionSqueezeY) {
                lastSqueezeTime = now;
                visionSqueezeY = newVisionSqueezeY;
            }
        } else {
            visionSqueezeY = -1.0f;
        }
    }

    float unsqueezeAmount = deltaTime * getVisionSqueezeUnSqueezeSpeed();
    if (now - lastSqueezeTime > getVisionSqueezeUnSqueezeDelay() * USECS_PER_SECOND) {
        visionSqueezeX -= unsqueezeAmount;
        if (visionSqueezeX < 0.0f) {
            visionSqueezeX = -1.0f;
        }
        visionSqueezeY -= unsqueezeAmount;
        if (visionSqueezeY < 0.0f) {
            visionSqueezeY = -1.0f;
        }
    }

    std::shared_ptr<HmdDisplayPlugin> hmdDisplayPlugin =
        std::dynamic_pointer_cast<HmdDisplayPlugin>(qApp->getActiveDisplayPlugin());
    if (hmdDisplayPlugin) {
        hmdDisplayPlugin->updateVisionSqueezeParameters(visionSqueezeX, visionSqueezeY,
                                                        getVisionSqueezeTransition(),
                                                        getVisionSqueezePerEye(),
                                                        getVisionSqueezeGroundPlaneY(),
                                                        getVisionSqueezeSpotlightSize());
    }
}