using System;
using System.Collections;
using System.Collections.Generic;
using System.Globalization;
using System.Linq;
using MVR.FileManagementSecure;
using SimpleJSON;
using UnityEngine;

/// <summary>
/// Snug
/// By Acidbubbles
/// Adjust hand alignement and position to match your actual body
/// Source: https://github.com/acidbubbles/vam-wrist
/// Note: See FixedUpdate for the implementation of the algorithm
/// </summary>
public class Snug : MVRScript {
    private const float _baseCueSize = 0.35f;
    private const string _saveExt = "snugprofile";
    private const string _saveFolder = "Saves\\snugprofiles";

    private readonly List<GameObject> _cues = new List<GameObject>();
    private readonly List<ControllerAnchorPoint> _anchorPoints = new List<ControllerAnchorPoint>();
    private bool _ready, _loaded;
    private bool _leftHandActive, _rightHandActive;
    private GameObject _leftHandTarget, _rightHandTarget;
    private JSONStorableBool _possessHandsJSON;
    private JSONStorableFloat _falloffJSON;
    private JSONStorableFloat _handOffsetXJSON, _handOffsetYJSON, _handOffsetZJSON;
    private JSONStorableFloat _handRotateXJSON, _handRotateYJSON, _handRotateZJSON;
    private FreeControllerV3 _personLHandController, _personRHandController;
    private Transform _leftAutoSnapPoint, _rightAutoSnapPoint;
    private Vector3 _palmToWristOffset;
    private Vector3 _handRotateOffset;
    private JSONStorableBool _showVisualCuesJSON;
    private JSONStorableStringChooser _selectedAnchorsJSON;
    private JSONStorableFloat _anchorVirtScaleXJSON, _anchorVirtScaleZJSON;
    private JSONStorableFloat _anchorVirtOffsetXJSON, _anchorVirtOffsetYJSON, _anchorVirtOffsetZJSON;
    private JSONStorableFloat _anchorPhysOffsetXJSON, _anchorPhysOffsetYJSON, _anchorPhysOffsetZJSON;
    private JSONStorableFloat _anchorPhysScaleXJSON, _anchorPhysScaleZJSON;
    private JSONStorableBool _anchorActiveJSON;
    private readonly Vector3[] _lHandVisualCueLinePoints = new Vector3[VisualCueLineIndices.Count];
    private readonly Vector3[] _rHandVisualCueLinePoints = new Vector3[VisualCueLineIndices.Count];
    private LineRenderer _lHandVisualCueLine, _rHandVisualCueLine;
    private readonly List<GameObject> _lHandVisualCueLinePointIndicators = new List<GameObject>();
    private readonly List<GameObject> _rHandVisualCueLinePointIndicators = new List<GameObject>();
    private static class VisualCueLineIndices {
        public static int Anchor = 0;
        public static int Hand = 1;
        public static int Controller = 2;
        public static int Count = 3;
    }

    public override void Init() {
        try {
            if (containingAtom.type != "Person") {
                SuperController.LogError("Snug can only be applied on Person atoms.");
                return;
            }

            InitPossessHandsUI();
            InitVisualCuesUI();
            InitArmForRecordUI();
            InitDisableSelectionUI();
            CreateSpacer().height = 10f;
            InitPresetUI();
            CreateSpacer().height = 10f;
            InitHandsSettingsUI();

            InitAnchors();
            InitAnchorsUI();
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(Init)}: {exc}");
        }

        StartCoroutine(DeferredInit());
    }

    private void InitPossessHandsUI() {
        var s = SuperController.singleton;

        Transform touchObjectLeft = null;
        if (s.isOVR)
            touchObjectLeft = s.touchObjectLeft;
        else if (s.isOpenVR)
            touchObjectLeft = s.viveObjectLeft;
        if (touchObjectLeft != null)
            _leftAutoSnapPoint = touchObjectLeft.GetComponent<Possessor>().autoSnapPoint;
        _personLHandController = containingAtom.freeControllers.FirstOrDefault(fc => fc.name == "lHandControl");
        if (_personLHandController == null) throw new NullReferenceException($"Could not find the lHandControl controller. Controllers: {string.Join(", ", containingAtom.freeControllers.Select(fc => fc.name).ToArray())}");
        _leftHandTarget = new GameObject($"{containingAtom.gameObject.name}.lHandController.snugTarget");
        var leftHandRigidBody = _leftHandTarget.AddComponent<Rigidbody>();
        leftHandRigidBody.isKinematic = true;
        leftHandRigidBody.detectCollisions = false;

        Transform touchObjectRight = null;
        if (s.isOVR)
            touchObjectRight = s.touchObjectRight;
        else if (s.isOpenVR)
            touchObjectRight = s.viveObjectRight;
        if (touchObjectRight != null)
            _rightAutoSnapPoint = touchObjectRight.GetComponent<Possessor>().autoSnapPoint;
        _personRHandController = containingAtom.freeControllers.FirstOrDefault(fc => fc.name == "rHandControl");
        if (_personRHandController == null) throw new NullReferenceException($"Could not find the rHandControl controller. Controllers: {string.Join(", ", containingAtom.freeControllers.Select(fc => fc.name).ToArray())}");
        _rightHandTarget = new GameObject($"{containingAtom.gameObject.name}.rHandController.snugTarget");
        var rightHandRigidBody = _rightHandTarget.AddComponent<Rigidbody>();
        rightHandRigidBody.isKinematic = true;
        rightHandRigidBody.detectCollisions = false;

        _possessHandsJSON = new JSONStorableBool("Possess Hands", false, (bool val) => {
            if (!_ready) return;
            if (val) {
                if (_leftAutoSnapPoint != null)
                    _leftHandActive = true;
                if (_rightAutoSnapPoint != null)
                    _rightHandActive = true;
                StartCoroutine(DeferredActivateHands());
            } else {
                if (_leftHandActive) {
                    CustomReleaseHand(_personLHandController);
                    _leftHandActive = false;
                }
                if (_rightHandActive) {
                    CustomReleaseHand(_personRHandController);
                    _rightHandActive = false;
                }
            }
        }) { isStorable = false };
        RegisterBool(_possessHandsJSON);
        CreateToggle(_possessHandsJSON);
    }

    private void InitVisualCuesUI() {
        _showVisualCuesJSON = new JSONStorableBool("Show Visual Cues", true, (bool val) => {
            if (!_ready) return;
            if (val)
                CreateVisualCues();
            else
                DestroyVisualCues();
        });
        RegisterBool(_showVisualCuesJSON);
        CreateToggle(_showVisualCuesJSON);
    }

    private void InitArmForRecordUI() {
        CreateButton("Arm hands for record").button.onClick.AddListener(() => {
            SuperController.singleton.ArmAllControlledControllersForRecord();
            _personLHandController.GetComponent<MotionAnimationControl>().armedForRecord = true;
            _personRHandController.GetComponent<MotionAnimationControl>().armedForRecord = true;
        });
    }

    private struct FreeControllerV3Snapshot { public bool canGrabPosition; public bool canGrabRotation; }
    private readonly Dictionary<FreeControllerV3, FreeControllerV3Snapshot> _previousState = new Dictionary<FreeControllerV3, FreeControllerV3Snapshot>();
    private void InitDisableSelectionUI() {
        var disableSelectionJSON = new JSONStorableBool("Make controllers unselectable", false, (bool val) => {
            if (val) {
                foreach (var fc in containingAtom.freeControllers) {
                    if (fc == _personLHandController || fc == _personRHandController) continue;
                    if (!fc.canGrabPosition && !fc.canGrabRotation) continue;
                    if (!fc.name.EndsWith("Control")) continue;
                    var state = new FreeControllerV3Snapshot { canGrabPosition = fc.canGrabPosition, canGrabRotation = fc.canGrabRotation };
                    _previousState[fc] = state;
                    fc.canGrabPosition = false;
                    fc.canGrabRotation = false;
                }
            } else {
                foreach (var kvp in _previousState) {
                    kvp.Key.canGrabPosition = kvp.Value.canGrabPosition;
                    kvp.Key.canGrabRotation = kvp.Value.canGrabRotation;
                }
                _previousState.Clear();
            }
        });
        CreateToggle(disableSelectionJSON);
    }

    private void InitPresetUI() {
        var loadPresetUI = CreateButton("Load Preset", false);
        loadPresetUI.button.onClick.AddListener(() => {
            FileManagerSecure.CreateDirectory(_saveFolder);
            var shortcuts = FileManagerSecure.GetShortCutsForDirectory(_saveFolder);
            SuperController.singleton.GetMediaPathDialog((string path) => {
                if (string.IsNullOrEmpty(path)) return;
                JSONClass jc = (JSONClass)LoadJSON(path);
                RestoreFromJSON(jc);
                SyncSelectedAnchorJSON("");
                SyncHandsOffset();
            }, _saveExt, _saveFolder, false, true, false, null, false, shortcuts);
        });

        var savePresetUI = CreateButton("Save Preset", false);
        savePresetUI.button.onClick.AddListener(() => {
            FileManagerSecure.CreateDirectory(_saveFolder);
            var fileBrowserUI = SuperController.singleton.fileBrowserUI;
            fileBrowserUI.SetTitle("Save colliders preset");
            fileBrowserUI.fileRemovePrefix = null;
            fileBrowserUI.hideExtension = false;
            fileBrowserUI.keepOpen = false;
            fileBrowserUI.fileFormat = _saveExt;
            fileBrowserUI.defaultPath = _saveFolder;
            fileBrowserUI.showDirs = true;
            fileBrowserUI.shortCuts = null;
            fileBrowserUI.browseVarFilesAsDirectories = false;
            fileBrowserUI.SetTextEntry(true);
            fileBrowserUI.Show((string path) => {
                fileBrowserUI.fileFormat = null;
                if (string.IsNullOrEmpty(path)) return;
                if (!path.ToLower().EndsWith($".{_saveExt}")) path += $".{_saveExt}";
                var jc = GetJSON();
                jc.Remove("id");
                SaveJSON(jc, path);
            });
            fileBrowserUI.ActivateFileNameField();
        });
    }

    private void InitHandsSettingsUI() {
        _falloffJSON = new JSONStorableFloat("Effect Falloff", 0.3f, UpdateHandsOffset, 0f, 5f, false) { isStorable = false };
        CreateSlider(_falloffJSON, false);

        _handOffsetXJSON = new JSONStorableFloat("Hand Offset X", 0f, UpdateHandsOffset, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_handOffsetXJSON, false);
        _handOffsetYJSON = new JSONStorableFloat("Hand Offset Y", 0f, UpdateHandsOffset, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_handOffsetYJSON, false);
        _handOffsetZJSON = new JSONStorableFloat("Hand Offset Z", 0f, UpdateHandsOffset, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_handOffsetZJSON, false);

        _handRotateXJSON = new JSONStorableFloat("Hand Rotate X", 0f, UpdateHandsOffset, -25f, 25f, false) { isStorable = false };
        CreateSlider(_handRotateXJSON, false);
        _handRotateYJSON = new JSONStorableFloat("Hand Rotate Y", 0f, UpdateHandsOffset, -25f, 25f, false) { isStorable = false };
        CreateSlider(_handRotateYJSON, false);
        _handRotateZJSON = new JSONStorableFloat("Hand Rotate Z", 0f, UpdateHandsOffset, -25f, 25f, false) { isStorable = false };
        CreateSlider(_handRotateZJSON, false);
    }

    private void InitAnchors() {
        _anchorPoints.Add(new ControllerAnchorPoint {
            Label = "Head",
            RigidBody = containingAtom.rigidbodies.First(rb => rb.name == "head"),
            VirtualOffset = new Vector3(0, 0.2f, 0),
            VirtualScale = new Vector3(1, 1, 1),
            PhysicalOffset = new Vector3(0, 0, 0),
            PhysicalScale = new Vector3(1, 1, 1),
            Active = true,
            Locked = true
        });
        _anchorPoints.Add(new ControllerAnchorPoint {
            Label = "Lips",
            RigidBody = containingAtom.rigidbodies.First(rb => rb.name == "LipTrigger"),
            VirtualOffset = new Vector3(0, 0, -0.07113313f),
            VirtualScale = new Vector3(0.3830788f, 1f, 0.5201911f),
            PhysicalOffset = new Vector3(0, 0, 0),
            PhysicalScale = new Vector3(1, 1, 1),
            Active = true
        });
        _anchorPoints.Add(new ControllerAnchorPoint {
            Label = "Chest",
            RigidBody = containingAtom.rigidbodies.First(rb => rb.name == "chest"),
            VirtualOffset = new Vector3(0, 0.0682705f, 0.04585214f),
            VirtualScale = new Vector3(0.8217609f, 1, 0.8080147f),
            PhysicalOffset = new Vector3(0, 0, 0),
            PhysicalScale = new Vector3(1, 1, 1),
            Active = true
        });
        _anchorPoints.Add(new ControllerAnchorPoint {
            Label = "Abdomen",
            RigidBody = containingAtom.rigidbodies.First(rb => rb.name == "abdomen"),
            VirtualOffset = new Vector3(0, 0.0770329f, 0.04218798f),
            VirtualScale = new Vector3(0.7670161f, 1, 0.5064289f),
            PhysicalOffset = new Vector3(0, 0, 0),
            PhysicalScale = new Vector3(1, 1, 1),
            Active = true
        });
        _anchorPoints.Add(new ControllerAnchorPoint {
            Label = "Hips",
            RigidBody = containingAtom.rigidbodies.First(rb => rb.name == "hip"),
            VirtualOffset = new Vector3(0, -0.08762675f, -0.009161186f),
            VirtualScale = new Vector3(1.041181f, 1, 0.8080372f),
            PhysicalOffset = new Vector3(0, 0, 0),
            PhysicalScale = new Vector3(1, 1, 1),
            Active = true
        });
        _anchorPoints.Add(new ControllerAnchorPoint {
            Label = "Ground (Control)",
            RigidBody = containingAtom.rigidbodies.First(rb => rb.name == "object"),
            VirtualOffset = new Vector3(0, 0, 0),
            VirtualScale = new Vector3(1, 1, 1),
            PhysicalOffset = new Vector3(0, 0, 0),
            PhysicalScale = new Vector3(1, 1, 1),
            Active = true,
            Locked = true
        });
    }

    private void InitAnchorsUI() {
        _selectedAnchorsJSON = new JSONStorableStringChooser("Selected Anchor", _anchorPoints.Select(a => a.Label).ToList(), "Chest", "Anchor", SyncSelectedAnchorJSON) { isStorable = false };
        CreateScrollablePopup(_selectedAnchorsJSON, true);

        _anchorVirtScaleXJSON = new JSONStorableFloat("Virtual Scale X", 1f, UpdateAnchor, 0.01f, 3f, true) { isStorable = false };
        CreateSlider(_anchorVirtScaleXJSON, true);
        _anchorVirtScaleZJSON = new JSONStorableFloat("Virtual Scale Z", 1f, UpdateAnchor, 0.01f, 3f, true) { isStorable = false };

        CreateSlider(_anchorVirtScaleZJSON, true);
        _anchorVirtOffsetXJSON = new JSONStorableFloat("Virtual Offset X", 0f, UpdateAnchor, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_anchorVirtOffsetXJSON, true);
        _anchorVirtOffsetYJSON = new JSONStorableFloat("Virtual Offset Y", 0f, UpdateAnchor, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_anchorVirtOffsetYJSON, true);
        _anchorVirtOffsetZJSON = new JSONStorableFloat("Virtual Offset Z", 0f, UpdateAnchor, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_anchorVirtOffsetZJSON, true);

        _anchorPhysScaleXJSON = new JSONStorableFloat("Physical Scale X", 1f, UpdateAnchor, 0.01f, 3f, true) { isStorable = false };
        CreateSlider(_anchorPhysScaleXJSON, true);
        _anchorPhysScaleZJSON = new JSONStorableFloat("Physical Scale Z", 1f, UpdateAnchor, 0.01f, 3f, true) { isStorable = false };
        CreateSlider(_anchorPhysScaleZJSON, true);

        _anchorPhysOffsetXJSON = new JSONStorableFloat("Physical Offset X", 0f, UpdateAnchor, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_anchorPhysOffsetXJSON, true);
        _anchorPhysOffsetYJSON = new JSONStorableFloat("Physical Offset Y", 0f, UpdateAnchor, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_anchorPhysOffsetYJSON, true);
        _anchorPhysOffsetZJSON = new JSONStorableFloat("Physical Offset Z", 0f, UpdateAnchor, -0.2f, 0.2f, true) { isStorable = false };
        CreateSlider(_anchorPhysOffsetZJSON, true);

        _anchorActiveJSON = new JSONStorableBool("Anchor Active", true, (bool _) => UpdateAnchor(0)) { isStorable = false };
        CreateToggle(_anchorActiveJSON, true);
    }

    private IEnumerator DeferredActivateHands() {
        yield return new WaitForSeconds(0f);
        if (_leftHandActive)
            CustomPossessHand(_personLHandController, _leftHandTarget);
        if (_rightHandActive)
            CustomPossessHand(_personRHandController, _rightHandTarget);
    }

    private static void CustomPossessHand(FreeControllerV3 controllerV3, GameObject target) {
        controllerV3.PossessMoveAndAlignTo(target.transform);
        controllerV3.SelectLinkToRigidbody(target.GetComponent<Rigidbody>());
        controllerV3.canGrabPosition = false;
        controllerV3.canGrabRotation = false;
        controllerV3.possessed = true;
        controllerV3.possessable = false;
        (controllerV3.GetComponent<HandControl>() ?? controllerV3.GetComponent<HandControlLink>().handControl).possessed = true;
    }

    private static void CustomReleaseHand(FreeControllerV3 controllerV3) {
        controllerV3.RestorePreLinkState();
        // TODO: This should return to the previous state
        controllerV3.canGrabPosition = true;
        controllerV3.canGrabRotation = true;
        controllerV3.possessed = false;
        controllerV3.possessable = true;
        (controllerV3.GetComponent<HandControl>() ?? controllerV3.GetComponent<HandControlLink>().handControl).possessed = false;
    }

    private IEnumerator DeferredInit() {
        yield return new WaitForEndOfFrame();
        try {
            if (!_loaded) containingAtom.RestoreFromLast(this);
            OnEnable();
            SyncSelectedAnchorJSON("");
            SyncHandsOffset();
            if (_showVisualCuesJSON.val) CreateVisualCues();
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(DeferredInit)}: {exc}");
        }
    }

    #region Load / Save

    public override JSONClass GetJSON(bool includePhysical = true, bool includeAppearance = true, bool forceStore = false) {
        var json = base.GetJSON(includePhysical, includeAppearance, forceStore);

        try {
            json["hands"] = new JSONClass{
                {"offset", SerializeVector3(_palmToWristOffset)},
                {"rotation", SerializeVector3(_handRotateOffset)}
            };
            var anchors = new JSONClass();
            foreach (var anchor in _anchorPoints) {
                anchors[anchor.Label] = new JSONClass
                {
                    {"virOffset", SerializeVector3(anchor.VirtualOffset)},
                    {"virScale", SerializeVector2(new Vector3(anchor.VirtualScale.x, anchor.VirtualScale.z))},
                    {"phyOffset", SerializeVector3(anchor.PhysicalOffset)},
                    {"phyScale", SerializeVector2(new Vector2(anchor.PhysicalScale.x, anchor.PhysicalScale.z))},
                    {"active", anchor.Active ? "true" : "false"},
                };
            }
            json["anchors"] = anchors;
            needsStore = true;
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(GetJSON)}:  {exc}");
        }

        return json;
    }

    private static string SerializeVector3(Vector3 v) { return $"{v.x.ToString(CultureInfo.InvariantCulture)},{v.y.ToString(CultureInfo.InvariantCulture)},{v.z.ToString(CultureInfo.InvariantCulture)}"; }
    private static string SerializeVector2(Vector2 v) { return $"{v.x.ToString(CultureInfo.InvariantCulture)},{v.y.ToString(CultureInfo.InvariantCulture)}"; }

    public override void RestoreFromJSON(JSONClass jc, bool restorePhysical = true, bool restoreAppearance = true, JSONArray presetAtoms = null, bool setMissingToDefault = true) {
        base.RestoreFromJSON(jc, restorePhysical, restoreAppearance, presetAtoms, setMissingToDefault);

        try {
            var handsJSON = jc["hands"];
            if (handsJSON != null) {
                _palmToWristOffset = DeserializeVector3(handsJSON["offset"], _palmToWristOffset);
                _handRotateOffset = DeserializeVector3(handsJSON["rotation"], _handRotateOffset);
            }

            var anchorsJSON = jc["anchors"];
            if (anchorsJSON != null) {
                foreach (var anchor in _anchorPoints) {
                    var anchorJSON = anchorsJSON[anchor.Label];
                    if (anchorJSON == null) continue;
                    anchor.VirtualOffset = DeserializeVector3(anchorJSON["virOffset"], anchor.VirtualOffset);
                    anchor.VirtualScale = DeserializeVector2AsFlatScale(anchorJSON["virScale"], anchor.VirtualScale);
                    anchor.PhysicalOffset = DeserializeVector3(anchorJSON["phyOffset"], anchor.PhysicalOffset);
                    anchor.PhysicalScale = DeserializeVector2AsFlatScale(anchorJSON["phyScale"], anchor.PhysicalScale);
                    anchor.Active = anchorJSON["active"]?.Value != "false";
                    anchor.Update();
                }
            }

            _loaded = true;
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(RestoreFromJSON)}: {exc}");
        }
    }

    private static Vector3 DeserializeVector3(string v, Vector3 defaultValue) {
        if (string.IsNullOrEmpty(v)) return defaultValue;
        var s = v.Split(',');
        return new Vector3(float.Parse(s[0], CultureInfo.InvariantCulture), float.Parse(s[1], CultureInfo.InvariantCulture), float.Parse(s[2], CultureInfo.InvariantCulture));
    }
    private static Vector3 DeserializeVector2AsFlatScale(string v, Vector3 defaultValue) {
        if (string.IsNullOrEmpty(v)) return defaultValue;
        var s = v.Split(',');
        return new Vector3(float.Parse(s[0], CultureInfo.InvariantCulture), 1f, float.Parse(s[1], CultureInfo.InvariantCulture));
    }

    #endregion

    private void SyncSelectedAnchorJSON(string _) {
        if (!_ready) return;
        var anchor = _anchorPoints.FirstOrDefault(a => a.Label == _selectedAnchorsJSON.val);
        if (anchor == null) throw new NullReferenceException($"Could not find the selected anchor {_selectedAnchorsJSON.val}");
        _anchorVirtScaleXJSON.valNoCallback = anchor.VirtualScale.x;
        _anchorVirtScaleZJSON.valNoCallback = anchor.VirtualScale.z;
        _anchorVirtOffsetXJSON.valNoCallback = anchor.VirtualOffset.x;
        _anchorVirtOffsetYJSON.valNoCallback = anchor.VirtualOffset.y;
        _anchorVirtOffsetZJSON.valNoCallback = anchor.VirtualOffset.z;
        _anchorPhysScaleXJSON.valNoCallback = anchor.PhysicalScale.x;
        _anchorPhysScaleZJSON.valNoCallback = anchor.PhysicalScale.z;
        _anchorPhysOffsetXJSON.valNoCallback = anchor.PhysicalOffset.x;
        _anchorPhysOffsetYJSON.valNoCallback = anchor.PhysicalOffset.y;
        _anchorPhysOffsetZJSON.valNoCallback = anchor.PhysicalOffset.z;
        _anchorActiveJSON.valNoCallback = anchor.Active;
    }

    private void UpdateAnchor(float _) {
        if (!_ready) return;
        var anchor = _anchorPoints.FirstOrDefault(a => a.Label == _selectedAnchorsJSON.val);
        if (anchor == null) throw new NullReferenceException($"Could not find the selected anchor {_selectedAnchorsJSON.val}");
        anchor.VirtualScale = new Vector3(_anchorVirtScaleXJSON.val, 1f, _anchorVirtScaleZJSON.val);
        anchor.VirtualOffset = new Vector3(_anchorVirtOffsetXJSON.val, _anchorVirtOffsetYJSON.val, _anchorVirtOffsetZJSON.val);
        anchor.PhysicalScale = new Vector3(_anchorPhysScaleXJSON.val, 1f, _anchorPhysScaleZJSON.val);
        anchor.PhysicalOffset = new Vector3(_anchorPhysOffsetXJSON.val, _anchorPhysOffsetYJSON.val, _anchorPhysOffsetZJSON.val);
        if (anchor.Locked && !_anchorActiveJSON.val) {
            _anchorActiveJSON.valNoCallback = true;
        } else if (anchor.Active != _anchorActiveJSON.val) {
            anchor.Active = _anchorActiveJSON.val;
            anchor.Update();
        }
        anchor.Update();
    }

    private void SyncHandsOffset() {
        if (!_ready) return;
        _handOffsetXJSON.valNoCallback = _palmToWristOffset.x;
        _handOffsetYJSON.valNoCallback = _palmToWristOffset.y;
        _handOffsetZJSON.valNoCallback = _palmToWristOffset.z;
        _handRotateXJSON.valNoCallback = _handRotateOffset.x;
        _handRotateYJSON.valNoCallback = _handRotateOffset.y;
        _handRotateZJSON.valNoCallback = _handRotateOffset.z;
    }

    private void UpdateHandsOffset(float _) {
        if (!_ready) return;
        _palmToWristOffset = new Vector3(_handOffsetXJSON.val, _handOffsetYJSON.val, _handOffsetZJSON.val);
        _handRotateOffset = new Vector3(_handRotateXJSON.val, _handRotateYJSON.val, _handRotateZJSON.val);
    }

    #region Update

    public void Update() {
        if (!_ready) return;
        try {
            UpdateCueLine(_lHandVisualCueLine, _lHandVisualCueLinePoints, _lHandVisualCueLinePointIndicators);
            UpdateCueLine(_rHandVisualCueLine, _rHandVisualCueLinePoints, _rHandVisualCueLinePointIndicators);
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(Update)}: {exc}");
            OnDisable();
        }
    }

    private static void UpdateCueLine(LineRenderer line, Vector3[] points, IList<GameObject> indicators) {
        if (line != null) {
            line.SetPositions(points);
            for (var i = 0; i < points.Length; i++)
                indicators[i].transform.position = points[i];
        }
    }

    public void FixedUpdate() {
        if (!_ready) return;
        try {
            if (_leftHandActive || _showVisualCuesJSON.val && _leftAutoSnapPoint != null)
                ProcessHand(_leftHandTarget, SuperController.singleton.leftHand, _leftAutoSnapPoint, new Vector3(_palmToWristOffset.x * -1f, _palmToWristOffset.y, _palmToWristOffset.z), new Vector3(_handRotateOffset.x, -_handRotateOffset.y, -_handRotateOffset.z), _lHandVisualCueLinePoints);
            if (_rightHandActive || _showVisualCuesJSON.val && _rightAutoSnapPoint != null)
                ProcessHand(_rightHandTarget, SuperController.singleton.rightHand, _rightAutoSnapPoint, _palmToWristOffset, _handRotateOffset, _rHandVisualCueLinePoints);
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(FixedUpdate)}: {exc}");
            OnDisable();
        }
    }

    private void ProcessHand(GameObject handTarget, Transform physicalHand, Transform autoSnapPoint, Vector3 palmToWristOffset, Vector3 handRotateOffset, Vector3[] visualCueLinePoints) {
        // Base position
        var position = physicalHand.position;
        var snapOffset = autoSnapPoint.localPosition;

        // Find the anchor over and under the controller
        ControllerAnchorPoint lower = null;
        ControllerAnchorPoint upper = null;
        foreach (var anchorPoint in _anchorPoints) {
            if (!anchorPoint.Active) continue;
            var anchorPointPos = anchorPoint.GetWorldPosition();
            if (position.y > anchorPointPos.y && (lower == null || anchorPointPos.y > lower.GetWorldPosition().y)) {
                lower = anchorPoint;
            } else if (position.y < anchorPointPos.y && (upper == null || anchorPointPos.y < upper.GetWorldPosition().y)) {
                upper = anchorPoint;
            }
        }

        if (lower == null)
            lower = upper;
        else if (upper == null)
            upper = lower;

        // Find the weight of both anchors (closest = strongest effect)
        var upperPosition = upper.GetWorldPosition();
        var lowerPosition = lower.GetWorldPosition();
        var yUpperDelta = upperPosition.y - position.y;
        var yLowerDelta = position.y - lowerPosition.y;
        var totalDelta = yLowerDelta + yUpperDelta;
        var upperWeight = totalDelta == 0 ? 1f : yLowerDelta / totalDelta;
        var lowerWeight = 1f - upperWeight;
        var lowerRotation = lower.RigidBody.transform.rotation;
        var upperRotation = upper.RigidBody.transform.rotation;
        var anchorPosition = Vector3.Lerp(upperPosition, lowerPosition, lowerWeight);
        var anchorRotation = Quaternion.Lerp(upperRotation, lowerRotation, lowerWeight);
        // TODO: Is this useful?
        anchorPosition.y = position.y;
        visualCueLinePoints[VisualCueLineIndices.Anchor] = anchorPosition;

        // Determine the falloff (closer = stronger, fades out with distance)
        // TODO: Even better to use closest point on ellipse, but not necessary.
        var distance = Mathf.Abs(Vector3.Distance(anchorPosition, position));
        var physicalCueSize = Vector3.Lerp(Vector3.Scale(upper.VirtualScale, upper.PhysicalScale), Vector3.Scale(lower.VirtualScale, lower.PhysicalScale), lowerWeight) * (_baseCueSize / 2f);
        var physicalCueDistanceFromCenter = Mathf.Max(physicalCueSize.x, physicalCueSize.z);
        // TODO: Check both x and z to determine a falloff relative to both distances
        var falloff = _falloffJSON.val > 0 ? 1f - (Mathf.Clamp(distance - physicalCueDistanceFromCenter, 0, _falloffJSON.val) / _falloffJSON.val) : 1f;

        // Calculate the controller offset based on the physical scale/offset of anchors
        var physicalScale = Vector3.Lerp(upper.PhysicalScale, lower.PhysicalScale, lowerWeight);
        var physicalOffset = Vector3.Lerp(upper.PhysicalOffset, lower.PhysicalOffset, lowerWeight);
        var baseOffset = position - anchorPosition;
        var resultOffset = Quaternion.Inverse(anchorRotation) * baseOffset;
        resultOffset = new Vector3(resultOffset.x / physicalScale.x, resultOffset.y / physicalScale.y, resultOffset.z / physicalScale.z) - physicalOffset;
        resultOffset = anchorRotation * resultOffset;
        var resultPosition = anchorPosition + Vector3.Lerp(baseOffset, resultOffset, falloff);
        visualCueLinePoints[VisualCueLineIndices.Hand] = resultPosition;

        // Apply the hands adjustments
        var resultRotation = autoSnapPoint.rotation * Quaternion.Euler(handRotateOffset);
        resultPosition += resultRotation * (snapOffset + palmToWristOffset);

        // Do the displacement
        var rb = handTarget.GetComponent<Rigidbody>();
        rb.MovePosition(resultPosition);
        rb.MoveRotation(resultRotation);

        visualCueLinePoints[VisualCueLineIndices.Controller] = physicalHand.transform.position;

        // SuperController.singleton.ClearMessages();
        // SuperController.LogMessage($"y {position.y:0.00} btwn {lower.RigidBody.name} y {yLowerDelta:0.00} w {lowerWeight:0.00} and {upper.RigidBody.name} y {yUpperDelta:0.00} w {upperWeight: 0.00}");
        // SuperController.LogMessage($"dist {distance:0.00}/{physicalCueDistanceFromCenter:0.00} falloff {falloff:0.00}");
        // SuperController.LogMessage($"rot {upperRotation.eulerAngles} psca {physicalScale} poff {physicalOffset} base {baseOffset} res {resultOffset}");
    }

    #endregion

    #region Lifecycle

    public void OnEnable() {
        try {
            _ready = true;
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(OnEnable)}: {exc}");
        }
    }

    public void OnDisable() {
        try {
            _ready = false;
            DestroyVisualCues();
            Destroy(_rightHandTarget);
        } catch (Exception exc) {
            SuperController.LogError($"{nameof(Snug)}.{nameof(OnDisable)}: {exc}");
        }
    }

    public void OnDestroy() {
        OnDisable();
    }

    #endregion

    #region Debuggers

    private void CreateVisualCues() {
        DestroyVisualCues();

        _lHandVisualCueLine = CreateHandVisualCue(_lHandVisualCueLinePointIndicators);
        _rHandVisualCueLine = CreateHandVisualCue(_rHandVisualCueLinePointIndicators);

        foreach (var anchorPoint in _anchorPoints) {
            anchorPoint.VirtualCue = new ControllerAnchorPointVisualCue(anchorPoint.RigidBody.transform, Color.gray);
            anchorPoint.Update();

            anchorPoint.PhysicalCue = new ControllerAnchorPointVisualCue(anchorPoint.RigidBody.transform, Color.white);
            anchorPoint.Update();
        }
    }

    private LineRenderer CreateHandVisualCue(List<GameObject> visualCueLinePointIndicators) {
        var lineGo = new GameObject();
        var line = VisualCuesHelper.CreateLine(lineGo, Color.yellow, 0.002f, VisualCueLineIndices.Count, true);
        _cues.Add(lineGo);

        for (var i = 0; i < VisualCueLineIndices.Count; i++) {
            var p = VisualCuesHelper.CreatePrimitive(null, PrimitiveType.Cube, Color.yellow);
            p.transform.localScale = new Vector3(0.02f, 0.02f, 0.02f);
            visualCueLinePointIndicators.Add(p);
        }

        return line;
    }

    private void DestroyVisualCues() {
        _lHandVisualCueLine = null;
        _rHandVisualCueLine = null;
        foreach (var p in _lHandVisualCueLinePointIndicators)
            Destroy(p);
        _lHandVisualCueLinePointIndicators.Clear();
        foreach (var p in _rHandVisualCueLinePointIndicators)
            Destroy(p);
        _rHandVisualCueLinePointIndicators.Clear();
        foreach (var debugger in _cues) {
            Destroy(debugger);
        }
        _cues.Clear();
        foreach (var anchor in _anchorPoints) {
            Destroy(anchor.VirtualCue?.gameObject);
            anchor.VirtualCue = null;
            Destroy(anchor.PhysicalCue?.gameObject);
            anchor.PhysicalCue = null;
        }
    }

    #endregion

    private class ControllerAnchorPoint {
        public string Label { get; set; }
        public Rigidbody RigidBody { get; set; }
        public Vector3 PhysicalOffset { get; set; }
        public Vector3 PhysicalScale { get; set; }
        public Vector3 VirtualOffset { get; set; }
        public Vector3 VirtualScale { get; set; }
        public ControllerAnchorPointVisualCue VirtualCue { get; set; }
        public ControllerAnchorPointVisualCue PhysicalCue { get; set; }
        public bool Active { get; set; }
        public bool Locked { get; set; }

        public Vector3 GetWorldPosition() {
            return RigidBody.transform.position + RigidBody.transform.rotation * (VirtualOffset + PhysicalOffset);
        }

        public void Update() {
            if (VirtualCue != null) {
                VirtualCue.gameObject.SetActive(Active);
                VirtualCue.Update(VirtualOffset, VirtualScale);
            }
            if (PhysicalCue != null) {
                PhysicalCue.gameObject.SetActive(Active);
                PhysicalCue.Update(VirtualOffset + PhysicalOffset, Vector3.Scale(VirtualScale, PhysicalScale));
            }
        }
    }

    private class ControllerAnchorPointVisualCue : IDisposable {
        private const float _width = 0.005f;
        public readonly GameObject gameObject;
        private readonly Transform _xAxis;
        private readonly Transform _zAxis;
        private readonly Transform _frontHandle;
        private readonly Transform _leftHandle;
        private readonly Transform _rightHandle;
        private readonly LineRenderer _ellipse;

        public ControllerAnchorPointVisualCue(Transform parent, Color color) {
            var go = new GameObject();
            _xAxis = VisualCuesHelper.CreatePrimitive(go.transform, PrimitiveType.Cube, Color.red).transform;
            _zAxis = VisualCuesHelper.CreatePrimitive(go.transform, PrimitiveType.Cube, Color.blue).transform;
            _frontHandle = VisualCuesHelper.CreatePrimitive(go.transform, PrimitiveType.Cube, color).transform;
            _leftHandle = VisualCuesHelper.CreatePrimitive(go.transform, PrimitiveType.Cube, color).transform;
            _rightHandle = VisualCuesHelper.CreatePrimitive(go.transform, PrimitiveType.Cube, color).transform;
            _ellipse = VisualCuesHelper.CreateEllipse(go, color, _width);
            if (_ellipse == null) throw new NullReferenceException("Boom");
            gameObject = go;
            gameObject.transform.parent = parent;
            gameObject.transform.localPosition = Vector3.zero;
            gameObject.transform.localRotation = Quaternion.identity;
        }

        public void Update(Vector3 offset, Vector3 scale) {
            gameObject.transform.localPosition = offset;

            var size = new Vector2(_baseCueSize * scale.x, _baseCueSize * scale.z);
            _xAxis.localScale = new Vector3(size.x - _width * 2, _width * 0.25f, _width * 0.25f);
            _zAxis.localScale = new Vector3(_width * 0.25f, _width * 0.25f, size.y - _width * 2);
            _frontHandle.localScale = new Vector3(_width * 3, _width * 3, _width * 3);
            _frontHandle.transform.localPosition = Vector3.forward * size.y / 2;
            _leftHandle.localScale = new Vector3(_width * 3, _width * 3, _width * 3);
            _leftHandle.transform.localPosition = Vector3.left * size.x / 2;
            _rightHandle.localScale = new Vector3(_width * 3, _width * 3, _width * 3);
            _rightHandle.transform.localPosition = Vector3.right * size.x / 2;
            if (_ellipse == null) throw new NullReferenceException("Bam");
            VisualCuesHelper.DrawEllipse(_ellipse, new Vector2(size.x / 2f, size.y / 2f));
        }

        public void Dispose() {
            Destroy(gameObject);
        }
    }

    private static class VisualCuesHelper {
        public static GameObject Cross(Color color) {
            var go = new GameObject();
            var size = 0.2f; var width = 0.005f;
            CreatePrimitive(go.transform, PrimitiveType.Cube, Color.red).transform.localScale = new Vector3(size, width, width);
            CreatePrimitive(go.transform, PrimitiveType.Cube, Color.green).transform.localScale = new Vector3(width, size, width);
            CreatePrimitive(go.transform, PrimitiveType.Cube, Color.blue).transform.localScale = new Vector3(width, width, size);
            CreatePrimitive(go.transform, PrimitiveType.Sphere, color).transform.localScale = new Vector3(size / 8f, size / 8f, size / 8f);
            foreach (var c in go.GetComponentsInChildren<Collider>()) Destroy(c);
            return go;
        }

        public static GameObject CreatePrimitive(Transform parent, PrimitiveType type, Color color) {
            var go = GameObject.CreatePrimitive(type);
            go.GetComponent<Renderer>().material = new Material(Shader.Find("Sprites/Default")) { color = color, renderQueue = 4000 };
            foreach (var c in go.GetComponents<Collider>()) { c.enabled = false; Destroy(c); }
            go.transform.parent = parent;
            return go;
        }

        public static LineRenderer CreateLine(GameObject go, Color color, float width, int points, bool useWorldSpace) {
            var line = go.AddComponent<LineRenderer>();
            line.useWorldSpace = useWorldSpace;
            line.material = new Material(Shader.Find("Sprites/Default")) { renderQueue = 4000 };
            line.widthMultiplier = width;
            line.colorGradient = new Gradient {
                colorKeys = new[] { new GradientColorKey(color, 0f), new GradientColorKey(color, 1f) }
            };
            line.positionCount = points;
            return line;
        }

        public static LineRenderer CreateEllipse(GameObject go, Color color, float width, int resolution = 32) {
            var line = CreateLine(go, color, width, resolution, false);
            return line;
        }

        public static void DrawEllipse(LineRenderer line, Vector2 radius) {
            for (int i = 0; i <= line.positionCount; i++) {
                var angle = i / (float)line.positionCount * 2.0f * Mathf.PI;
                Quaternion pointQuaternion = Quaternion.AngleAxis(90, Vector3.right);
                Vector3 pointPosition;

                pointPosition = new Vector3(radius.x * Mathf.Cos(angle), radius.y * Mathf.Sin(angle), 0.0f);
                pointPosition = pointQuaternion * pointPosition;

                line.SetPosition(i, pointPosition);
            }
            line.loop = true;
        }
    }
}