using Oculus.Interaction;
using System;
using UnityEngine;
using UnityEngine.Events;

public class CustomOneGrabTransformer : MonoBehaviour, ITransformer
{
    public enum Axis
    {
        Right = 0,
        Up = 1,
        Forward = 2
    }

    [SerializeField, Optional]
    private Transform _pivotTransform = null;

    public Transform Pivot => _pivotTransform != null ? _pivotTransform : transform;

    public UnityEvent MinAngleReachedEvent;
    public UnityEvent MaxAngleReachedEvent;

    [SerializeField]
    private Axis _rotationAxis = Axis.Up;

    /// <summary>
    /// The axis that this transformer will rotate around.
    /// </summary>
    public Axis RotationAxis => _rotationAxis;

    /// <summary>
    /// Constraints that determine the minimum and maximum rotation angle for this transormer.
    /// </summary>
    [Serializable]
    public class OneGrabRotateConstraints
    {
        public FloatConstraint MinAngle;
        public FloatConstraint MaxAngle;
    }

    [SerializeField]
    private OneGrabRotateConstraints _constraints =
        new OneGrabRotateConstraints()
        {
            MinAngle = new FloatConstraint(),
            MaxAngle = new FloatConstraint()
        };

    /// <summary>
    /// The current <see cref="OneGrabRotateConstraints"/> that are being used by this instance.
    /// </summary>
    public OneGrabRotateConstraints Constraints
    {
        get
        {
            return _constraints;
        }

        set
        {
            _constraints = value;
        }
    }
    private bool hitMin = true;
    private bool hitMax = false;
    private float _relativeAngle = 0.0f;
    private float _constrainedRelativeAngle = 0.0f;

    private IGrabbable _grabbable;
    private Vector3 _grabPositionInPivotSpace;
    private Pose _transformPoseInPivotSpace;

    private Pose _worldPivotPose;
    private Vector3 _previousVectorInPivotSpace;

    private Quaternion _localRotation;
    private float _startAngle = 0;


    /// <summary>
    /// Implementation of <see cref="ITransformer.Initialize"/>; for details, please refer to the related documentation
    /// provided for that interface.
    /// </summary>
    public void Initialize(IGrabbable grabbable)
    {
        _grabbable = grabbable;
    }

    /// <summary>
    /// Computes the pose of the rotational pivot, in world space.
    /// </summary>
    /// <returns>The world space pose of the rotational pivot.</returns>
    public Pose ComputeWorldPivotPose()
    {
        if (_pivotTransform != null)
        {
            return _pivotTransform.GetPose();
        }

        var targetTransform = _grabbable.Transform;

        Vector3 worldPosition = targetTransform.position;
        Quaternion worldRotation = targetTransform.parent != null
            ? targetTransform.parent.rotation * _localRotation
            : _localRotation;

        return new Pose(worldPosition, worldRotation);
    }

    /// <summary>
    /// Implementation of <see cref="ITransformer.BeginTransform"/>; for details, please refer to the related documentation
    /// provided for that interface.
    /// </summary>
    public void BeginTransform()
    {
        var grabPoint = _grabbable.GrabPoints[0];
        var targetTransform = _grabbable.Transform;

        if (_pivotTransform == null)
        {
            _localRotation = targetTransform.localRotation;
        }

        Vector3 localAxis = Vector3.zero;
        localAxis[(int)_rotationAxis] = 1f;

        _worldPivotPose = ComputeWorldPivotPose();
        Vector3 rotationAxis = _worldPivotPose.rotation * localAxis;

        Quaternion inverseRotation = Quaternion.Inverse(_worldPivotPose.rotation);

        Vector3 grabDelta = grabPoint.position - _worldPivotPose.position;
        // The initial delta must be non-zero between the pivot and grab location for rotation
        if (Mathf.Abs(grabDelta.magnitude) < 0.001f)
        {
            Vector3 localAxisNext = Vector3.zero;
            localAxisNext[((int)_rotationAxis + 1) % 3] = 0.001f;
            grabDelta = _worldPivotPose.rotation * localAxisNext;
        }

        _grabPositionInPivotSpace =
            inverseRotation * grabDelta;

        Vector3 worldPositionDelta =
            inverseRotation * (targetTransform.position - _worldPivotPose.position);

        Quaternion worldRotationDelta = inverseRotation * targetTransform.rotation;
        _transformPoseInPivotSpace = new Pose(worldPositionDelta, worldRotationDelta);

        Vector3 initialOffset = _worldPivotPose.rotation * _grabPositionInPivotSpace;
        Vector3 initialVector = Vector3.ProjectOnPlane(initialOffset, rotationAxis);
        _previousVectorInPivotSpace = Quaternion.Inverse(_worldPivotPose.rotation) * initialVector;

        _startAngle = _constrainedRelativeAngle;
        _relativeAngle = _startAngle;

        float parentScale = targetTransform.parent != null ? targetTransform.parent.lossyScale.x : 1f;
        _transformPoseInPivotSpace.position /= parentScale;


    }

    /// <summary>
    /// Implementation of <see cref="ITransformer.UpdateTransform"/>; for details, please refer to the related documentation
    /// provided for that interface.
    /// </summary>
    public void UpdateTransform()
    {
        var grabPoint = _grabbable.GrabPoints[0];
        var targetTransform = _grabbable.Transform;

        Vector3 localAxis = Vector3.zero;
        localAxis[(int)_rotationAxis] = 1f;
        _worldPivotPose = ComputeWorldPivotPose();
        Vector3 rotationAxis = _worldPivotPose.rotation * localAxis;

        // Project our positional offsets onto a plane with normal equal to the rotation axis
        Vector3 targetOffset = grabPoint.position - _worldPivotPose.position;
        Vector3 targetVector = Vector3.ProjectOnPlane(targetOffset, rotationAxis);

        Vector3 previousVectorInWorldSpace =
            _worldPivotPose.rotation * _previousVectorInPivotSpace;

        // update previous
        _previousVectorInPivotSpace = Quaternion.Inverse(_worldPivotPose.rotation) * targetVector;

        float signedAngle =
            Vector3.SignedAngle(previousVectorInWorldSpace, targetVector, rotationAxis);

        _relativeAngle += signedAngle;

        _constrainedRelativeAngle = _relativeAngle;
        if (Constraints.MinAngle.Constrain)
        {
            //_constrainedRelativeAngle = Mathf.Max(_constrainedRelativeAngle, Constraints.MinAngle.Value);
            if (_constrainedRelativeAngle < Constraints.MinAngle.Value)
            {
                _constrainedRelativeAngle = Constraints.MinAngle.Value;
                if (!hitMin&&hitMax)
                {
                    hitMin = true;
                    hitMax = false;
                    MinAngleReachedEvent?.Invoke();
                }
            }
        }
        if (Constraints.MaxAngle.Constrain)
        {
            //_constrainedRelativeAngle = Mathf.Min(_constrainedRelativeAngle, Constraints.MaxAngle.Value);
            if (_constrainedRelativeAngle > Constraints.MaxAngle.Value)
            {
                _constrainedRelativeAngle = Constraints.MaxAngle.Value;
                if (!hitMax&&hitMin)
                {
                    hitMax = true;
                    hitMin = false;
                    MaxAngleReachedEvent?.Invoke();
                }
            }
        }
        Quaternion deltaRotation = Quaternion.AngleAxis(_constrainedRelativeAngle - _startAngle, rotationAxis);

        float parentScale = targetTransform.parent != null ? targetTransform.parent.lossyScale.x : 1f;
        Pose transformDeltaInWorldSpace =
            new Pose(
                _worldPivotPose.rotation * (parentScale * _transformPoseInPivotSpace.position),
                _worldPivotPose.rotation * _transformPoseInPivotSpace.rotation);

        Pose transformDeltaRotated = new Pose(
            deltaRotation * transformDeltaInWorldSpace.position,
            deltaRotation * transformDeltaInWorldSpace.rotation);

        targetTransform.position = _worldPivotPose.position + transformDeltaRotated.position;
        targetTransform.rotation = transformDeltaRotated.rotation;
    }

    /// <summary>
    /// Implementation of <see cref="ITransformer.EndTransform"/>; for details, please refer to the related documentation
    /// provided for that interface.
    /// </summary>
    public void EndTransform() { }

    // Start is called once before the first execution of Update after the MonoBehaviour is created

}
