using UnityEngine;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Globalization;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Text.RegularExpressions;
using TMPro;

public class Glove_Animator : MonoBehaviour
{
    public enum Glove_Type
    {
        RightGlove = -1,
        LeftGlove = 1
    }

    public Glove_Type GloveType = Glove_Type.LeftGlove;

    [SerializeField, Tooltip("Optional external anchor for elbow")]
    Transform ElbowAnchorTransform;

    [SerializeField, Tooltip("Optional external anchor for palm")]
    Transform PalmAnchorTransform;

    [SerializeField, Tooltip("Optional rotation to compensate drift")]
    Transform PalmDriftTransform;

    [SerializeField, Tooltip("Hand transform")]
    Transform HandTransform;

    [SerializeField, Tooltip("Root transform")]
    Transform RootTransform;

    [SerializeField, Tooltip("Status text object (for debug)")]
    public GameObject StatusText;

    [SerializeField, Tooltip("JSON client address (our own address, 127.0.0.1 or 0.0.0.0, for PC version, not used for Android)")]
    string m_DK3LocalAddress = "127.0.0.1";

    [SerializeField, Tooltip("JSON server address (for PC version only, not used for Android)")]
    string m_DK3ServerAddress = "127.0.0.1";

    [SerializeField, Tooltip("JSON server port (for PC version only, not used for Android)")]
    int m_DK3ServerPort = 53451;

    public bool SensoDataPreferredForTracking = true;
    
    Transform[] bones;
    Quaternion[] bones_default_rotation;
    Vector3[] bones_default_position;
    const int MAX_BONES = 17;

    UdpClient udp;
    IPEndPoint remoteEP;
    IPEndPoint remoteEP2;
    int nextSend = 1000;

    //[SerializeField, Tooltip("Test_cube")]
    //Transform test_cube;

    [SerializeField, Tooltip("Thumb Senso correction")]
    public  Vector3 ThumbSensoCorrection = new(0f, 50f, -35f);

    [SerializeField, Tooltip("Layers for interaction")]
    LayerMask interactableMask; // only what you care about

    [SerializeField, Tooltip("Colliders at fingertips radius")]
    public float SizeColliderFingers = 0.01f;

    [SerializeField, Tooltip("Collider at wrist radius")]
    public float SizeColliderWrist = 0.05f;


    Transform[] Touch_Points;
    const int MAX_Touch_Points = 6;

    Collider[] _hits = new Collider[16];

    [SerializeField, Tooltip("Anchor point for grabbing")]
    public Transform grabAnchor;      // where the object should sit

    [SerializeField, Tooltip("Hand rigid body for moving / grabbing")]
    public Rigidbody handRb;          // kinematic hand root RB (required)

    FixedJoint joint;
    Rigidbody held;

    [SerializeField, Tooltip("Start grabbing by pinch ")] public 
    float GrabbingPinchStart = 0.35f;

    [SerializeField, Tooltip("Stop grabbing by pinch ")] public 
    float GrabbingPinchStop = 0.15f; 
    
    float CurrentPinchForce = 0;
    float CurrentGripForce = 0;

    [SerializeField, Tooltip("Elbow length for stationary simulation of tracking (put 0 for not to use) ")]
    public 
        Vector3 ElbowVector = new(0, 0, 0.4f);
    
    [SerializeField, Tooltip("Elbow rotation")]
    public 
        Vector3 ElbowRotation = new (0, 0, 0);

    [SerializeField, Tooltip("Simulate arm extention for stationary simulation of tracking (put 0 for not to use) ")]
    public float ArmExtentionLength = 0.20f;


    [SerializeField, Tooltip("Thumb OpenXR correction")]
    public
    Vector3 ThumbXRCorrection = new(290f, 240f, 35f);

    [SerializeField, Tooltip("Index OpenXR correction")]
    public
    Vector3 IndexXRCorrection = new(0, 0, 0);

    [SerializeField, Tooltip("Thumb OpenXR correction")]
    public
    Vector3 MiddleXRCorrection = new(0, 0, 0);

    [SerializeField, Tooltip("Thumb OpenXR correction")]
    public
    Vector3 RingXRCorrection = new(0, 0, 0);

    [SerializeField, Tooltip("Thumb OpenXR correction")]
    public
    Vector3 PinkyXRCorrection = new(0, 0, 0);

    public Vector3 CurrentVelocity;
    Vector3 DefaultPosition;


    [SerializeField, Tooltip("Ref to the OpenXR/Oculus hand tracking position")]
    public Transform OpenXRHand;

    public Transform OpenXRHandThumbMeta;
    public Transform OpenXRHandThumb1;
    public Transform OpenXRHandThumb2;
    public Transform OpenXRHandThumb3;

    public Transform OpenXRHandIndexMeta;
    public Transform OpenXRHandIndex1;
    public Transform OpenXRHandIndex2;
    public Transform OpenXRHandIndex3;

    public Transform OpenXRHandMiddleMeta;
    public Transform OpenXRHandMiddle1;
    public Transform OpenXRHandMiddle2;
    public Transform OpenXRHandMiddle3;

    public Transform OpenXRHandRingMeta;
    public Transform OpenXRHandRing1;
    public Transform OpenXRHandRing2;
    public Transform OpenXRHandRing3;

    public Transform OpenXRHandPinkyMeta;
    public Transform OpenXRHandPinky1;
    public Transform OpenXRHandPinky2;
    public Transform OpenXRHandPinky3;

    Quaternion[] bonesXR_default_rotation;

    public TextMeshPro Status;

    void log_udp(string s)
    {

        var bytes = Encoding.UTF8.GetBytes(s);
        udp.Send(bytes, bytes.Length, remoteEP2);

    }


    Vector3[] touch_offset;
    float[] start_time;
    bool[] touch_state;
    Vector3[] collider_spheres = null;
    float[] check_radius;
    int[] haptic_effects;


    void Awake()
    {
        if(handRb==null)
            handRb = HandTransform.GetComponent<Rigidbody>();
        if (handRb == null)
            handRb = RootTransform.GetComponent<Rigidbody>();

        start_time = new float[MAX_Touch_Points];
        touch_state = new bool[MAX_Touch_Points];
        touch_offset = new Vector3[MAX_Touch_Points];
        collider_spheres = new Vector3[MAX_Touch_Points];
        check_radius = new float[MAX_Touch_Points];
        haptic_effects = new int[MAX_Touch_Points];

        DefaultPosition = handRb.position;

        bones = new Transform[MAX_BONES];
        bones_default_rotation = new Quaternion[MAX_BONES];
        bones_default_position = new Vector3[MAX_BONES];
        bonesXR_default_rotation = new Quaternion[25];

        Touch_Points = new Transform[MAX_Touch_Points];

        if(StatusText != null)
            Status = StatusText.GetComponent<TextMeshPro>();

        bones[0] = RootTransform;//.Find("Root");

        bones[1] = bones[0].Find("wrist_r");

        Touch_Points[0] = bones[1];

        bones[2] = bones[1].Find("finger_thumb_0_r");
        bones[3] = bones[2].Find("finger_thumb_1_r");
        bones[4] = bones[3].Find("finger_thumb_2_r");

        Touch_Points[1] = bones[4].Find("finger_thumb_r_end");

        bones[5] = bones[1].Find("finger_index_meta_r/finger_index_0_r");
        bones[6] = bones[5].Find("finger_index_1_r");
        bones[7] = bones[6].Find("finger_index_2_r");

        Touch_Points[2] = bones[7].Find("finger_index_r_end");

        bones[8] = bones[1].Find("finger_middle_meta_r/finger_middle_0_r");
        bones[9] = bones[8].Find("finger_middle_1_r");
        bones[10] = bones[9].Find("finger_middle_2_r");

        Touch_Points[3] = bones[10].Find("finger_middle_r_end");

        bones[11] = bones[1].Find("finger_ring_meta_r/finger_ring_0_r");
        bones[12] = bones[11].Find("finger_ring_1_r");
        bones[13] = bones[12].Find("finger_ring_2_r");

        Touch_Points[4] = bones[13].Find("finger_ring_r_end");

        bones[14] = bones[1].Find("finger_pinky_meta_r/finger_pinky_0_r");
        bones[15] = bones[14].Find("finger_pinky_1_r");
        bones[16] = bones[15].Find("finger_pinky_2_r");

        Touch_Points[5] = bones[16].Find("finger_pinky_r_end");

        for (int i = 0; i < MAX_BONES; ++i)
        {
            bones_default_rotation[i] = bones[i].localRotation;
            bones_default_position[i] = bones[i].localPosition;
        }

        bonesXR_default_rotation[0] = OpenXRHand.localRotation;

        //bonesXR_default_rotation[2] = OpenXRHandThumbMeta.localRotation;
        bonesXR_default_rotation[2] = OpenXRHandThumb1.localRotation;
        bonesXR_default_rotation[3] = OpenXRHandThumb2.localRotation;
        bonesXR_default_rotation[4] = OpenXRHandThumb3.localRotation;

        //bonesXR_default_rotation[6] = OpenXRHandIndexMeta.localRotation;
        bonesXR_default_rotation[5] = OpenXRHandIndex1.localRotation;
        bonesXR_default_rotation[6] = OpenXRHandIndex2.localRotation;
        bonesXR_default_rotation[7] = OpenXRHandIndex3.localRotation;

        //bonesXR_default_rotation[10] = OpenXRHandMiddleMeta.localRotation;
        bonesXR_default_rotation[8] = OpenXRHandMiddle1.localRotation;
        bonesXR_default_rotation[9] = OpenXRHandMiddle2.localRotation;
        bonesXR_default_rotation[10] = OpenXRHandMiddle3.localRotation;

        //bonesXR_default_rotation[14] = OpenXRHandRingMeta.localRotation;
        bonesXR_default_rotation[11] = OpenXRHandRing1.localRotation;
        bonesXR_default_rotation[12] = OpenXRHandRing2.localRotation;
        bonesXR_default_rotation[13] = OpenXRHandRing3.localRotation;

        //bonesXR_default_rotation[18] = OpenXRHandPinkyMeta.localRotation;
        bonesXR_default_rotation[14] = OpenXRHandPinky1.localRotation;
        bonesXR_default_rotation[15] = OpenXRHandPinky2.localRotation;
        bonesXR_default_rotation[16] = OpenXRHandPinky3.localRotation;


        if (OpenXRHand != null)
        {
            LastOpenXRPosition = OpenXRHand.position;
            LastOpenXRRotation = OpenXRHand.rotation;
        }

    }


    void Start()
    {

        //        Debug.LogWarning("name " + this.name);

        var bindAddr = (string.IsNullOrWhiteSpace(m_DK3LocalAddress) || m_DK3LocalAddress == "0.0.0.0")
                ? IPAddress.Any
                : IPAddress.Parse(m_DK3LocalAddress);

        udp = new UdpClient(new IPEndPoint(bindAddr, 0));
        udp.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
        remoteEP = new IPEndPoint(IPAddress.Parse(m_DK3ServerAddress), m_DK3ServerPort);
        remoteEP2 = new IPEndPoint(IPAddress.Parse(m_DK3ServerAddress), 1111);

    }

    Quaternion q_palm;
    Quaternion q_wrist;
    Quaternion q_thumb;
    Quaternion q_thumb2;
    Quaternion q_index;
    Quaternion q_middle;
    Quaternion q_ring;
    Quaternion q_pinky;
    Vector3 palm_lia;

    readonly HashSet<Rigidbody> _touchA = new();
    readonly HashSet<Rigidbody> _touchB = new();
    public int pinchA = 1;                 // e.g. thumb
    public int pinchB = 2;                 // e.g. index

    Transform prevParent;
    bool prevKinematic, prevUseGravity;
    Vector3 prevAnchorPos;
    Quaternion prevAnchorRot;


    float i = 0;
    // Update is called once per frame

    Vector3 Calculate_Finger_1(Vector3 v_orig)
    {
        Vector3 v = v_orig;
        v.x /= (2 + Math.Abs(v.z) / 30); if (v.x < -15) v.x = -15; if (v.x > 15) v.x = 15;
        v.y /= (2 + Math.Abs(v.z) / 30); if (v.y < -20) v.y = -20; if (v.y > 20) v.y = 20;
        if (v.z > 120) v.z -= 360;
        if (v.z < 0) v.z /= 2.2f; else v.x /= 1.2f;
        return v;
    }

    Vector3 Calculate_Finger_2(Vector3 v_orig)
    {
        Vector3 v = v_orig;
        v.x = 0;
        v.y = 0;
        if (v.z > 120) v.z -= 360;
        if (v.z < 0) v.z /= 2.2f; else v.z = 0;
        return v;
    }

    Vector3 Calculate_Finger_3(Vector3 v_orig)
    {
        Vector3 v = v_orig;
        v.x = 0;
        v.y = 0;
        if (v.z > 120) v.z -= 360;
        if (v.z < 0) v.z /= 2.2f; else v.z = 0;
        return v;
    }

    bool Wrist_Updated = false;

    Rigidbody GetRB(Collider c) =>
    c.attachedRigidbody ? c.attachedRigidbody : c.GetComponentInParent<Rigidbody>();

    float minPenetrationFrac = 0.1f;

    Vector3 LastOpenXRPosition = new Vector3(0, 0, 0);
    Quaternion LastOpenXRRotation = new Quaternion(0, 0, 0, 1);

    Quaternion Palm_Correction = new Quaternion(0,0,0,1);

    Vector3 Palm_Velocity = new Vector3(0, 0, 0);

    int no_glove_data_counter = 1000;

    float Y_drift_correction = 0;

    void FixedUpdate()
    {
        bool Use_OpenXR_Position = false;
        Vector3 last_position = handRb.position;
        Vector3 new_position = handRb.position;
        float ftime = Time.fixedDeltaTime;

        bool coordinates_already_aligned = false;

        if (PalmAnchorTransform != null)
        {
            coordinates_already_aligned = true;
            handRb.MovePosition(PalmAnchorTransform.position);
            handRb.MoveRotation(PalmAnchorTransform.rotation);
        } else
        if (ElbowAnchorTransform != null)
        {
            coordinates_already_aligned = true;
            Vector3 position = ElbowAnchorTransform.position + Quaternion.Euler(ElbowRotation) * ElbowAnchorTransform.rotation * ElbowVector;
            Quaternion rotation = ElbowAnchorTransform.rotation * (Quaternion.Inverse(q_wrist) * q_palm);
            handRb.MovePosition(position);
            handRb.MoveRotation(rotation);
        }

        if (OpenXRHand != null && !LastOpenXRPosition.Equals(OpenXRHand.position) && !LastOpenXRRotation.Equals(OpenXRHand.rotation) &&
          OpenXRHand.position.sqrMagnitude > 0 && !coordinates_already_aligned)
        {
            handRb.MovePosition(OpenXRHand.position);
            handRb.MoveRotation(OpenXRHand.rotation);
            Use_OpenXR_Position = true;
            LastOpenXRPosition = OpenXRHand.position;
            LastOpenXRRotation = OpenXRHand.rotation;
            ElbowVector.Set(0, 0, 0);
            new_position = OpenXRHand.position;
        }

        bool glove_data_obtained = Obtain_UDP_Data();
        if (glove_data_obtained) no_glove_data_counter = 0; else ++no_glove_data_counter;

        if (Status != null)
        {
            if (no_glove_data_counter > 100)
            {
                Status.text = "Connecting...";
                Status.color = Color.yellow;
            }
            else
            {
                Status.text = "Connected";
                Status.color = Color.green;
            }
        }


        float K = -1;
        if (GloveType == Glove_Type.RightGlove) K = 1;

        Vector3 a_t1, a_t2, a_t3;
        Vector3 a_i1, a_i2, a_i3;
        Vector3 a_m1, a_m2, a_m3;
        Vector3 a_r1, a_r2, a_r3;
        Vector3 a_p1, a_p2, a_p3;
        a_i1 = new Vector3(0, 0, 0);
        a_m1 = new Vector3(0, 0, 0);
        a_r1 = new Vector3(0, 0, 0);
        a_p1 = new Vector3(0, 0, 0);
        a_t1 = new Vector3(0, 0, 0);

        
        if (glove_data_obtained)
        {
            if (!coordinates_already_aligned)
            {
                if (!Use_OpenXR_Position)
                {
                    if (ElbowVector != null && ElbowVector.sqrMagnitude > 0 && Wrist_Updated)
                    {
                        Vector3 elbow = ElbowVector;
                        Vector3 elbow_angle = EulerSigned(Quaternion.Inverse(q_palm) * q_wrist);
                        float a = elbow_angle.x;
                        float limit = 70;
                        if (a < -limit) a = -limit;
                        if (a > limit) a = limit;
                        a = (a * ArmExtentionLength / limit) + 1;
                        Vector3 arm_offset = Quaternion.Euler(ElbowRotation) * q_wrist * (elbow * a);

                        if(PalmDriftTransform != null && arm_offset.y*arm_offset.y < (arm_offset.x*arm_offset.x + arm_offset.z*arm_offset.z))
                        {                            
                            float r1 = Mathf.Atan2(arm_offset.x, arm_offset.z) * Mathf.Rad2Deg;
                            //Debug.LogWarning("palm atan2 = " + arm_offset + " " + r1);
                            Vector3 palm_r = PalmDriftTransform.rotation.eulerAngles;
                            float r2 = palm_r.y;if (r2 >= 180) r2 -= 360;
                            r2 -= r1;if (r2 < (-180))r2 += 360; if (r2 > (180)) r2 -= 360;
                            Y_drift_correction += (r2 - Y_drift_correction) * 0.005f;
                        }

                        arm_offset = Quaternion.Euler(0, Y_drift_correction, 0) * arm_offset;
                        

                        handRb.MovePosition(DefaultPosition + arm_offset);
                        new_position = DefaultPosition + arm_offset;
                    }
                    handRb.MoveRotation(Quaternion.Euler(0, Y_drift_correction, 0) * q_palm * Palm_Correction);
                }
                else
                {
                    Palm_Correction = Quaternion.Inverse(q_palm) * OpenXRHand.rotation;
                    /*Quaternion q1 = Quaternion.Inverse(q_palm) * OpenXRHand.rotation;
                    Quaternion q2 = Quaternion.Inverse(OpenXRHand.rotation) * q_palm;                
                    string s = "(" + $"pos=({q1.x,8:F4}, {q1.y,8:F4}, {q1.z,8:F4})" + " )";
                    s += "(" + $"pos=({q2.x,8:F4}, {q2.y,8:F4}, {q2.z,8:F4})" + " )";
                    log_udp(s);*/
                }
            }

            Vector3 a_index = EulerSigned(q_index);
            a_i1 = Calculate_Finger_1(a_index);
            a_i2 = Calculate_Finger_2(a_index);
            a_i3 = Calculate_Finger_3(a_index);

            Vector3 a_middle = EulerSigned(q_middle);
            a_m1 = Calculate_Finger_1(a_middle);
            a_m2 = Calculate_Finger_2(a_middle);
            a_m3 = Calculate_Finger_3(a_middle);

            Vector3 a_ring = EulerSigned(q_ring);
            a_r1 = Calculate_Finger_1(a_ring);
            a_r2 = Calculate_Finger_2(a_ring);
            a_r3 = Calculate_Finger_3(a_ring);

            Vector3 a_pinky = EulerSigned(q_pinky);
            a_p1 = Calculate_Finger_1(a_pinky);
            a_p2 = Calculate_Finger_2(a_pinky);
            a_p3 = Calculate_Finger_3(a_pinky);

            bones[5].localRotation = bones_default_rotation[5] * Quaternion.Euler(a_i1);
            bones[6].localRotation = bones_default_rotation[6] * Quaternion.Euler(a_i2);
            bones[7].localRotation = bones_default_rotation[7] * Quaternion.Euler(a_i3);

            bones[8].localRotation = bones_default_rotation[8] * Quaternion.Euler(a_m1);
            bones[9].localRotation = bones_default_rotation[9] * Quaternion.Euler(a_m2);
            bones[10].localRotation = bones_default_rotation[10] * Quaternion.Euler(a_m3);

            bones[11].localRotation = bones_default_rotation[11] * Quaternion.Euler(a_r1);
            bones[12].localRotation = bones_default_rotation[12] * Quaternion.Euler(a_r2);
            bones[13].localRotation = bones_default_rotation[13] * Quaternion.Euler(a_r3);

            bones[14].localRotation = bones_default_rotation[14] * Quaternion.Euler(a_p1);
            bones[15].localRotation = bones_default_rotation[15] * Quaternion.Euler(a_p2);
            bones[16].localRotation = bones_default_rotation[16] * Quaternion.Euler(a_p3);

            Vector3 a_thumb1 = EulerSigned(q_thumb);
            Vector3 a_thumb2 = EulerSigned(q_thumb2);

            //test_cube.rotation = q_thumb;
            Quaternion thumb_add = Quaternion.Euler(ThumbSensoCorrection);  // -15, 60, 0;

            //if(test_cube!=null)thumb_add = test_cube.rotation;

            Quaternion Q2 = thumb_add * Quaternion.Slerp(Quaternion.identity, q_thumb, 0.5f) * Quaternion.Inverse(thumb_add);
            Quaternion Q3 = thumb_add * Quaternion.Slerp(Quaternion.identity, q_thumb, 0.5f) * Quaternion.Inverse(thumb_add);

            string s = GloveType == Glove_Type.RightGlove ? "right " : "left  ";
            s += $"q1=({q_thumb.x,8:F4}, {q_thumb.y,8:F4}, {q_thumb.z,8:F4})  ";
            s += $"q2=({Q2.x,8:F4}, {Q2.y,8:F4}, {Q2.z,8:F4})  ";
            //s += "(" + $"eu=({a.x,8:F4}, {a.y,8:F4}, {a.z,8:F4})";
            log_udp(s);



            bones[2].localRotation = bones_default_rotation[2] * Q2;
            bones[3].localRotation = bones_default_rotation[3] * Q3;
            bones[4].localRotation = bones_default_rotation[4] * Quaternion.Euler(0, 0, a_thumb2.z < 0 ? a_thumb2.z * 1.1f : 0);




            //  left as [0] => 0.944057  [1] => -0.031365   [2] => 0.101649   [3] => -0.312155
            //  right as[0] => 0.950061  [1] =>  0.146567   [2] => -0.180111  [3] => 0.208477

            // left down[0] => 0.885221  [1] => 0.282906    [2] => -0.333845  [3] => -0.157785
            // right dow[0] => 0.921572  [1] => 0.307773    [2] => 0.13108    [3] => -0.196972
//            q_thumb = Fill_Quaternion(payload, thumb_index, GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");
//            q_thumb2 = Fill_Quaternion(payload, thumb2_index, GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");

        }

        if (Use_OpenXR_Position && !SensoDataPreferredForTracking)
        {            

            if (OpenXRHandThumbMeta != null && OpenXRHandThumb1 != null && OpenXRHandThumb2 != null && OpenXRHandThumb3 != null)
            {
                Quaternion q1 = (Quaternion.Inverse(OpenXRHandThumbMeta.rotation) * OpenXRHandThumb1.rotation);
                Quaternion q2 = (Quaternion.Inverse(OpenXRHandThumb1.rotation) * OpenXRHandThumb2.rotation);
                Quaternion q3 = (Quaternion.Inverse(OpenXRHandThumb2.rotation) * OpenXRHandThumb3.rotation);
                bones[2].localRotation = Quaternion.Euler(ThumbXRCorrection) * (new Quaternion(K*q1.z, K * q1.y, -q1.x, q1.w));
                bones[3].localRotation = new Quaternion(K * q2.z, K * q2.y, -q2.x, q2.w);
                bones[4].localRotation = new Quaternion(K * q3.z, K * q3.y, -q3.x, q3.w);

                Vector3 a = q1.eulerAngles;

                string s = GloveType == Glove_Type.RightGlove ? "right " : "left  ";
                s += $"q1=({q1.x,8:F4}, {q1.y,8:F4}, {q1.z,8:F4})  ";
                s += $"q2=({q2.x,8:F4}, {q2.y,8:F4}, {q2.z,8:F4})  ";
                s += "(" + $"eu=({a.x,8:F4}, {a.y,8:F4}, {a.z,8:F4})";
                log_udp(s);
            }

            if (Use_OpenXR_Position && OpenXRHandIndexMeta != null && OpenXRHandIndex1 != null && OpenXRHandIndex2 != null && OpenXRHandIndex3 != null)
            {
                Quaternion q1 = (Quaternion.Inverse(OpenXRHandIndexMeta.rotation) * OpenXRHandIndex1.rotation);
                Quaternion q2 = (Quaternion.Inverse(OpenXRHandIndex1.rotation) * OpenXRHandIndex2.rotation);
                Quaternion q3 = (Quaternion.Inverse(OpenXRHandIndex2.rotation) * OpenXRHandIndex3.rotation);
                bones[5].localRotation = Quaternion.Euler(IndexXRCorrection) * (new Quaternion(K * q1.z, K * q1.y, -q1.x, q1.w));
                bones[6].localRotation = new Quaternion(K * q2.z, K * q2.y, -q2.x, q2.w);
                bones[7].localRotation = new Quaternion(K * q3.z, K * q3.y, -q3.x, q3.w);
                a_i1 = EulerSignedXR(q1);
            }

            if (Use_OpenXR_Position && OpenXRHandMiddleMeta != null && OpenXRHandMiddle1 != null && OpenXRHandMiddle2 != null && OpenXRHandMiddle3 != null)
            {
                Quaternion q1 = (Quaternion.Inverse(OpenXRHandMiddleMeta.rotation) * OpenXRHandMiddle1.rotation);
                Quaternion q2 = (Quaternion.Inverse(OpenXRHandMiddle1.rotation) * OpenXRHandMiddle2.rotation);
                Quaternion q3 = (Quaternion.Inverse(OpenXRHandMiddle2.rotation) * OpenXRHandMiddle3.rotation);
                bones[8].localRotation = Quaternion.Euler(MiddleXRCorrection) * (new Quaternion(K * q1.z, K * q1.y, -q1.x, q1.w));
                bones[9].localRotation = new Quaternion(K * q2.z, K * q2.y, -q2.x, q2.w);
                bones[10].localRotation = new Quaternion(K * q3.z, K * q3.y, -q3.x, q3.w);
                a_m1 = EulerSignedXR(q1);
            }


            if (OpenXRHandRingMeta != null && OpenXRHandRing1 != null && OpenXRHandRing2 != null && OpenXRHandRing3 != null)
            {
                Quaternion q1 = (Quaternion.Inverse(OpenXRHandRingMeta.rotation) * OpenXRHandRing1.rotation);
                Quaternion q2 = (Quaternion.Inverse(OpenXRHandRing1.rotation) * OpenXRHandRing2.rotation);
                Quaternion q3 = (Quaternion.Inverse(OpenXRHandRing2.rotation) * OpenXRHandRing3.rotation);
                bones[11].localRotation = Quaternion.Euler(RingXRCorrection) * (new Quaternion(K * q1.z, K * q1.y, -q1.x, q1.w));
                bones[12].localRotation = new Quaternion(K * q2.z, K * q2.y, -q2.x, q2.w);
                bones[13].localRotation = new Quaternion(K * q3.z, K * q3.y, -q3.x, q3.w);
                a_r1 = EulerSignedXR(q1);
            }

            if (OpenXRHandPinkyMeta != null && OpenXRHandPinky1 != null && OpenXRHandPinky2 != null && OpenXRHandPinky3 != null)
            {
                Quaternion q1 = (Quaternion.Inverse(OpenXRHandPinkyMeta.rotation) * OpenXRHandPinky1.rotation);
                Quaternion q2 = (Quaternion.Inverse(OpenXRHandPinky1.rotation) * OpenXRHandPinky2.rotation);
                Quaternion q3 = (Quaternion.Inverse(OpenXRHandPinky2.rotation) * OpenXRHandPinky3.rotation);
                bones[14].localRotation = Quaternion.Euler(PinkyXRCorrection) * (new Quaternion(K * q1.z, K * q1.y, -q1.x, q1.w));
                bones[15].localRotation = new Quaternion(K * q2.z, K * q2.y, -q2.x, q2.w);
                bones[16].localRotation = new Quaternion(K * q3.z, K * q3.y, -q3.x, q3.w);
                a_p1 = EulerSignedXR(q1);
            }
        }

        if (!last_position.Equals(new_position) && ftime>0) {
            Vector3 current_velocity = (new_position - last_position) / ftime;
            Palm_Velocity += (current_velocity - Palm_Velocity) * 0.5f; 
            Vector3 acceleration = Palm_Correction * palm_lia;
            Palm_Velocity += acceleration * ftime;
            //handRb.velocity = Palm_Velocity;
        }


//        string s = "(" + $"pos=({handRb.velocity.x,8:F4}, {handRb.velocity.y,8:F4}, {handRb.velocity.z,8:F4})  ";
//        s += "(" + $"pos=({palm_lia.x,8:F4}, {palm_lia.y,8:F4}, {palm_lia.z,8:F4})  ";
//        s += "(" + $"pos=({Palm_Velocity.x,8:F4}, {Palm_Velocity.y,8:F4}, {Palm_Velocity.z,8:F4})  ";
//        log_udp(s);

        /*
        var e = q_palm.eulerAngles;
        string s = "(" + $"pos=({e.x,8:F3}, {e.y,8:F3}, {e.z,8:F3})" + " )";
        log_udp(s);
        */

        if (glove_data_obtained || Use_OpenXR_Position)
        {
            float current_pinch_angle = (-a_i1.z) * 2.2f;
            if (current_pinch_angle < 10f) CurrentPinchForce = 0f;
            else
            if (current_pinch_angle > 90f) CurrentPinchForce = 1f;
            else
                CurrentPinchForce = (current_pinch_angle - 10f) / (90f - 10f);

            float current_grab_angle = (-a_m1.z) * 2.2f;
            if (current_grab_angle < 10f) CurrentGripForce = 0f;
            else
            if (current_grab_angle > 90f) CurrentGripForce = 1f;
            else
                CurrentGripForce = (current_grab_angle - 10f) / (90f - 10f);
        }


        //Quaternion.Euler(i, 0f, 0f);

        CurrentVelocity = handRb.velocity;

        prevAnchorPos = grabAnchor.position;
        prevAnchorRot = grabAnchor.rotation;

        if (held != null)
        {
            //held.SendMessage("GripValue", CurrentPinchForce, SendMessageOptions.DontRequireReceiver);

            if (CurrentPinchForce < GrabbingPinchStop)
            {
                CurrentPinchForce = CurrentGripForce = 0; // restore ball
            }

                // usage:
                var mb = held.GetComponent<MonoBehaviour>(); // or a specific script type if you know it
            if (mb) SetPublicFloatByName(mb, "PinchValue", CurrentPinchForce);

            mb = held.GetComponent<MonoBehaviour>(); // or a specific script type if you know it
            if (mb) SetPublicFloatByName(mb, "GripValue", CurrentGripForce);

            if (CurrentPinchForce < GrabbingPinchStop)Release();
            return;
        }

        _touchA.Clear();
        _touchB.Clear();

        for (int i = 0; i < Touch_Points.Length; i++)
        {
            var tip = Touch_Points[i];
            float r = (i == 0) ? SizeColliderWrist : SizeColliderFingers;

            collider_spheres[i] = tip.position;

            int n = Physics.OverlapSphereNonAlloc(
                tip.position, r, _hits, interactableMask, QueryTriggerInteraction.Ignore);

            if (n==0) touch_state[i] = false;
            for (int h = 0; h < n; h++)
            {
                var col = _hits[h];
                if (!TryClosestPoint(col, tip.position, out var cp, out var d)) continue;

                float pen = r - d; // >0 means sphere overlaps
                if (pen <= r * minPenetrationFrac) continue;

                Check_Haptics(i, col);

                var rb = GetRB(col);
                if (!rb) continue;

                if (i == pinchA) _touchA.Add(rb);
                if (i == pinchB) _touchB.Add(rb);
            }
        }

        // Intersection = objects touched by both pinch tips
        Rigidbody candidate = null;
        foreach (var rb in _touchA)
            if (_touchB.Contains(rb)) { candidate = rb; break; }

        if (held == null && candidate != null && CurrentPinchForce > GrabbingPinchStart)
        {
            Grab(candidate);
        }

        FlushHaptic();

    }

    void FlushHaptic()
    {
        for (int i = 0; i < haptic_effects.Length; ++i) {
            if (haptic_effects[i] >= 0)
            {
                string s = "{\"type\":\"vibration\",\"data\":{\"type\":" + ((i + 5) % 6) + ",\"code\":" + haptic_effects[i] + "}}";
                var bytes = Encoding.UTF8.GetBytes(s);
                udp.Send(bytes, bytes.Length, remoteEP);
            }
            haptic_effects[i] = -1;
        }
    }

    void TriggerHapticPulse(int index, int effect)
    {
        haptic_effects[index] = effect;
    }


    void SetPublicFloatByName(Component target, string name, float value)
    {
        var t = target.GetType();
        var f = t.GetField(name, System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.Public);
        if (f != null && f.FieldType == typeof(float)) { f.SetValue(target, value); return; }

        var p = t.GetProperty(name, System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.Public);
        if (p != null && p.PropertyType == typeof(float) && p.CanWrite) p.SetValue(target, value);
    }

   

    static bool IsClosestPointSupported(Collider c)
    {
        if (c is BoxCollider || c is SphereCollider || c is CapsuleCollider) return true;
        if (c is MeshCollider mc) return mc.convex;   // only convex allowed
        return false; // TerrainCollider, WheelCollider, non-convex mesh, etc.
    }

    bool TryClosestPoint(Collider col, Vector3 p, out Vector3 cp, out float dist)
    {
        if (IsClosestPointSupported(col))
        {
            cp = col.ClosestPoint(p);
        }
        else
        {
            // Fallback: AABB closest point (rough but safe for all colliders)
            cp = col.bounds.ClosestPoint(p);
        }
        dist = Vector3.Distance(cp, p);
        return true;
    }



    void OnDestroy()
    {
        udp?.Close();
        udp = null;
    }

    Vector3 EulerSigned(Quaternion q)
    {
        var e = q.eulerAngles;
        if (e.x > 180f) e.x -= 360f;
        if (e.y > 180f) e.y -= 360f;
        if (e.z > 180f) e.z -= 360f;
        return e; // each in [-180, 180]
    }
    Vector3 EulerSignedXR(Quaternion q)
    {
        var e = q.eulerAngles;
        if (e.x > 180f) e.x -= 360f;
        if (e.y > 180f) e.y -= 360f;
        if (e.z > 180f) e.z -= 360f;
        return new Vector3(-e.z,-e.y,-e.x); // each in [-180, 180]
    }

    private int battery_level = -1;
    private string mac_address = "";
    private string glove_connection_state = "Disconnected";

    bool Obtain_UDP_Data()
    {
        
        ++nextSend;
        if (nextSend > 20)
        {
            nextSend = 0;
            var bytes = Encoding.UTF8.GetBytes("{\"type\":\"ping\"}");
            udp.Send(bytes, bytes.Length, remoteEP);
        }

        string payload = "";
        string glove_state = "";

        while (udp != null && udp.Available > 0)
        {
            IPEndPoint from = null;
            var data = udp.Receive(ref from); // safe because Available > 0
            string p = Regex.Replace(Encoding.UTF8.GetString(data), @"\s+", "");
            if (p.IndexOf("\"type\":\"position\"") >= 0) payload = p;
            if(p.IndexOf("\"type\":\"state\"") >= 0) glove_state = p;
        }
/*        if (glove_state.Length > 0)
        {
            int index_mac = glove_state.IndexOf("\"src\":\"");
            int index_mode = glove_state.IndexOf("\"mode\":");
            	$state = '{"src":"ac:4d:16:e8:46:a9","version":"3.1","type":"state","glove":"lh","mode":2,"calibrated":2.0000,"frames_rx":95,"frames_tx":4,"battery":100,"v_b":4149,"v_p":4827,"temperature":25,"hw_info":"05050316 05060316 01030211 00000000 "}';
        }*/

        if (payload.Length < 1) return false;

        int index_data = payload.IndexOf("\"data\":{");if (index_data<0) return false;
        int palm_index = payload.IndexOf("\"palm\":{", index_data); if (palm_index < 0) return false;
        int wrist_index = payload.IndexOf("\"wrist\":{", index_data); if (wrist_index < 0) return false;
        int fingers_index = payload.IndexOf("\"fingers\":[", index_data); if (fingers_index < 0) return false;

        int thumb_index = payload.IndexOf("\"q\":[",fingers_index);
        int thumb2_index = payload.IndexOf("\"quat2\":[", fingers_index);

        int index_finger_index = payload.IndexOf("}", fingers_index)+2;
        int middle_finger_index = payload.IndexOf("}", index_finger_index) + 2;
        int ring_finger_index = payload.IndexOf("}", middle_finger_index) + 2;
        int pinky_finger_index = payload.IndexOf("}", ring_finger_index) + 2;

        if (pinky_finger_index <= 0) return false;


        q_palm = Fill_Quaternion(payload, payload.IndexOf("\"quat\":", palm_index),"XzYW");
        Quaternion q_wrist1 = Fill_Quaternion(payload, payload.IndexOf("\"quat\":", wrist_index), "XzYW");
        if (q_wrist.Equals(q_wrist1)) Wrist_Updated = false; else Wrist_Updated = true;
        q_wrist = q_wrist1;
        palm_lia = Fill_Vector3(payload, payload.IndexOf("\"lia\":", palm_index));
        q_thumb = Fill_Quaternion(payload, thumb_index, GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");
        q_thumb2 = Fill_Quaternion(payload, thumb2_index, GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");
        q_index = Fill_Quaternion(payload, payload.IndexOf("\"quat\":", index_finger_index), GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");
        q_middle = Fill_Quaternion(payload, payload.IndexOf("\"quat\":", middle_finger_index), GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");
        q_ring = Fill_Quaternion(payload, payload.IndexOf("\"quat\":", ring_finger_index), GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");
        q_pinky = Fill_Quaternion(payload, payload.IndexOf("\"quat\":", pinky_finger_index), GloveType == Glove_Type.LeftGlove ? "yZxW" : "YzxW");

        return true;

    }
    
    Quaternion Fill_Quaternion(string payload, int index, string order) {
        int len = 4;
        float[] d = new float[len];
        int index_start = payload.IndexOf("[",index)+1;
        for (int i = 0; i < len; ++i)
        {
            int index_end = payload.IndexOf(i == len - 1 ? "]" : ",", index_start);
            float.TryParse(payload.Substring(index_start, index_end - index_start), NumberStyles.Float, CultureInfo.InvariantCulture, out d[i]);
            index_start = index_end + 1;
        }

        float[] q = new float[len];
        for (int i = 0; i < len; ++i)
        {
            int o = order[i];
            if (o == 'X') q[i] = d[1];
            else
                if (o == 'x') q[i] = -d[1];
            else
                if (o == 'Y') q[i] = d[2];
            else
                if (o == 'y') q[i] = -d[2];
            else
                if (o == 'Z') q[i] = d[3];
            else
                if (o == 'z') q[i] = -d[3];
            else
                if (o == 'W') q[i] = d[0];
            else
                if (o == 'w') q[i] = -d[0];
        }

        return new Quaternion(q[0], q[1], q[2], q[3]).normalized;
    }

    Vector3 Fill_Vector3(string payload, int index)
    {
        int index_1 = payload.IndexOf("[", index) + 1;
        int index_2 = payload.IndexOf(",", index_1) + 1;
        int index_3 = payload.IndexOf(",", index_2) + 1;
        int index_4 = payload.IndexOf("]", index_3) + 1;

        float.TryParse(payload.Substring(index_1, index_2 - index_1 - 1), NumberStyles.Float, CultureInfo.InvariantCulture, out var x);
        float.TryParse(payload.Substring(index_2, index_3 - index_2 - 1), NumberStyles.Float, CultureInfo.InvariantCulture, out var y);
        float.TryParse(payload.Substring(index_3, index_4 - index_3 - 1), NumberStyles.Float, CultureInfo.InvariantCulture, out var z);
        
        return new Vector3(x, z, y);
    }

    public void Grab1(Rigidbody targetRb)
    {
        if (!targetRb) return;

        if (joint) { Destroy(joint); joint = null; }   // drop any old joint

        // Optional: snap target so its GrabPoint matches grabAnchor
        Transform gp = GetBestGrabPoint(targetRb.transform, grabAnchor.position);
        Quaternion rotDelta = grabAnchor.rotation * Quaternion.Inverse(gp.rotation);
        targetRb.MoveRotation(rotDelta * targetRb.rotation);
        targetRb.MovePosition(targetRb.position + (grabAnchor.position - gp.position));

        held = targetRb;

        // remember state
        prevParent = targetRb.transform.parent;
        prevKinematic = targetRb.isKinematic;
        prevUseGravity = targetRb.useGravity;

        // stick it to the hand
        targetRb.isKinematic = true;
        targetRb.useGravity = false;
        //targetRb.velocity = Vector3.zero;
        //targetRb.angularVelocity = Vector3.zero;

        targetRb.transform.SetParent(grabAnchor, true);
        targetRb.transform.SetPositionAndRotation(grabAnchor.position, grabAnchor.rotation);
    }

    public void Grab(Rigidbody targetRb)
    {
        if (!targetRb) return;
        if (joint) { Destroy(joint); joint = null; }

        // 1) choose grab point on the object (marker or root)
        Transform gp = GetBestGrabPoint(targetRb.transform, grabAnchor.position);
        if (!gp) return;

        if (targetRb.isKinematic) return; 

        // 2) remember previous state for Release()
        prevParent = targetRb.transform.parent;
        prevKinematic = targetRb.isKinematic;
        prevUseGravity = targetRb.useGravity;
        held = targetRb;

        // 3) freeze physics while held
        targetRb.velocity = Vector3.zero;
        targetRb.angularVelocity = Vector3.zero;
        targetRb.isKinematic = true;
        targetRb.useGravity = false;

        // 4) SNAP by local math: make gp == anchor (both position & rotation)
        //    Assumes gp.localScale == (1,1,1)
        Vector3 gpLocalPos = gp.localPosition;
        Quaternion gpLocalRot = gp.localRotation;

        // parent to anchor WITHOUT keeping world pose (we'll set local TRS)
        targetRb.transform.SetParent(grabAnchor, false);

        // local so that: anchor * (objectLocal) * (gpLocal) == anchor
        targetRb.transform.localRotation = Quaternion.Inverse(gpLocalRot);
        targetRb.transform.localPosition = -(targetRb.transform.localRotation * gpLocalPos);

        // (Optional) if you want a twist offset in hand space:
        // targetRb.transform.localRotation = handTwistOffset * Quaternion.Inverse(gpLocalRot);
    }



    public void Release()
    {
        if (!held) return;

        // compute throw from anchor motion (fallback to handRb if you prefer)
        float dt = Mathf.Max(Time.deltaTime, 1e-6f);
        Vector3 throwVel = (grabAnchor.position - prevAnchorPos) / dt;

        Quaternion dq = grabAnchor.rotation * Quaternion.Inverse(prevAnchorRot);
        dq.ToAngleAxis(out float aDeg, out Vector3 axis);
        if (aDeg > 180f) aDeg -= 360f;
        Vector3 throwAngVel = axis * (aDeg * Mathf.Deg2Rad / dt);

        // unstick & restore physics
        held.transform.SetParent(prevParent, true);
        held.isKinematic = prevKinematic;
        held.useGravity = prevUseGravity;

        held.velocity = Palm_Velocity + Vector3.up*0.2f; // handRb ? handRb.velocity : throwVel;
        held.angularVelocity = handRb ? handRb.angularVelocity : throwAngVel;

        //held.velocity += Vector3.up;

        held = null;
    }

    Transform GetBestGrabPoint(Transform root, Vector3 handPos)
    {
        var byName = root.Find("GrabPoint");
        if (byName) return byName;
        // Final fallback: object’s root
        return null; // root;
    }

    void AlignToAnchor(Transform target, Transform grabPoint, Transform anchor)
    {
        // rotate so gp’s orientation matches anchor
        var rotDelta = anchor.rotation * Quaternion.Inverse(grabPoint.rotation);
        target.rotation = rotDelta * target.rotation;

        // then translate so gp’s position matches anchor
        var posDelta = anchor.position - grabPoint.position;
        target.position += posDelta;
    }

    void Check_Haptics(int i, Collider col)
    {
        GameObject hitGameObject = col.gameObject;
        Senso_Haptic_Material hapticMaterial = hitGameObject.GetComponent<Senso_Haptic_Material>();

        float ct = Time.time;

        if (hapticMaterial != null)
        {
            AudioSource audioSource_touch = hapticMaterial.SoundWhenTouch;
            AudioSource audioSource_stroke = hapticMaterial.SoundWhenStroke;

            if (!touch_state[i])
            {
                touch_state[i] = true;
                touch_offset[i] = hitGameObject.transform.position - collider_spheres[i];
                start_time[i] = ct;
                if (audioSource_touch) audioSource_touch.Play();
                if (hapticMaterial.VibrationWhenTouch != Senso_Haptic_Material.Vibro_Effect.VibroNone)
                {
                    TriggerHapticPulse(i, (int)hapticMaterial.VibrationWhenTouch);
                }
            }
            else
            {
                Vector3 new_offset = hitGameObject.transform.position - collider_spheres[i];
                Vector3 offset_diff = new_offset - touch_offset[i];
                if (offset_diff.magnitude > hapticMaterial.DistanceToRepeatStrokeEffect ||
                    (hapticMaterial.TimeIntervalToRepeatStrokeEffect > 0 && start_time[i] + hapticMaterial.TimeIntervalToRepeatStrokeEffect < ct))
                {
                    touch_offset[i] = hitGameObject.transform.position - collider_spheres[i];
                    start_time[i] = ct;
                    if (audioSource_stroke) audioSource_stroke.Play();
                    if (hapticMaterial.VibrationWhenTouch != Senso_Haptic_Material.Vibro_Effect.VibroNone)
                    {
                        TriggerHapticPulse(i, (int)hapticMaterial.VibrationWhenStroke);
                    }
                }

            }

            //Debug.Log("Touched object " + hitGameObject.name + " amp=" + hapticMaterial.amplitude + " freq=" + hapticMaterial.frequency + " dur="+ hapticMaterial.durationSeconds);
            //hand.TriggerHapticPulse(hapticMaterial.durationSeconds, hapticMaterial.frequency, hapticMaterial.amplitude);
            // Here you can add your logic to trigger different haptic effects based on material type
        }




    }



}
