using System.Collections.Generic;
using TMPro;
using UDESDK;
using UnityEngine;
using UnityEngine.UI;

public class BleManager : MonoBehaviour
{
    public static UDE_SDK _SDK = new();
    public string MyselfTest;
    public string LeftDeviceName { get; private set; }
    public string RightDeviceName { get; private set; }
    public string LeftAlgorithmVer { get; private set; }
    public string RightAlgorithmVer { get; private set; }
    public bool LeftConnectState;
    public bool RightConnectState;

    [Header("Finger Joints")]
    [Space]
    public HandDriver[] handDrivers;
    private Transform[] LeftHands;
    private Vector3[] LeftHandsVec3;
    private Transform[] RightHands;
    private Vector3[] RightHandsVec3;
    [HideInInspector]
    public List<double> LeftRawData;
    [HideInInspector]
    public List<double> LeftRawAngleData;
    [HideInInspector]
    public List<double> RightRawData;
    [HideInInspector]
    public List<double> RightRawAngleData;

    public double[] LeftCalibrationOffset { get; private set; }
    public double[] RightCalibrationOffset { get; private set; }

    public TextMeshProUGUI[] FPS;
    public Text DebugText;

    private UDE_SDK.FingerJointType[] JointOrder =
    {
        UDE_SDK.FingerJointType.Thumb1, UDE_SDK.FingerJointType.Thumb2, UDE_SDK.FingerJointType.Thumb3,
        UDE_SDK.FingerJointType.Index1, UDE_SDK.FingerJointType.Index2, UDE_SDK.FingerJointType.Index3,
        UDE_SDK.FingerJointType.Middle1, UDE_SDK.FingerJointType.Middle2, UDE_SDK.FingerJointType.Middle3,
        UDE_SDK.FingerJointType.Ring1, UDE_SDK.FingerJointType.Ring2, UDE_SDK.FingerJointType.Ring3,
        UDE_SDK.FingerJointType.Pinky1, UDE_SDK.FingerJointType.Pinky2, UDE_SDK.FingerJointType.Pinky3
    };

    private Dictionary<UDE_SDK.FingerJointType, Quaternion> LeftHandDefault = new();
    private Dictionary<UDE_SDK.FingerJointType, Quaternion> RightHandDefault = new();

    [HideInInspector]
    public bool UsingAndroidService;
    void Start()
    {
        gameObject.SetActive(false);
#if !UNITY_EDITOR
        gameObject.SetActive(true);
        handDrivers[0].UsingNetwork = false;
        handDrivers[1].UsingNetwork = false;
#endif
        if (UsingAndroidService) return;
        _SDK.UDE_BleInit();
        _SDK.UDE_BleStartSearching();

        LeftHands = new Transform[15]
        {
            handDrivers[0].Thumb1,
            handDrivers[0].Thumb2,
            handDrivers[0].Thumb3,
            handDrivers[0].Index1,
            handDrivers[0].Index2,
            handDrivers[0].Index3,
            handDrivers[0].Middle1,
            handDrivers[0].Middle2,
            handDrivers[0].Middle3,
            handDrivers[0].Ring1,
            handDrivers[0].Ring2,
            handDrivers[0].Ring3,
            handDrivers[0].Pinky1,
            handDrivers[0].Pinky2,
            handDrivers[0].Pinky3
        };
        LeftHandsVec3 = new Vector3[15]
        {
            handDrivers[0].thumb1,
            handDrivers[0].thumb2,
            handDrivers[0].thumb3,
            handDrivers[0].index1,
            handDrivers[0].index2,
            handDrivers[0].index3,
            handDrivers[0].middle1,
            handDrivers[0].middle2,
            handDrivers[0].middle3,
            handDrivers[0].ring1,
            handDrivers[0].ring2,
            handDrivers[0].ring3,
            handDrivers[0].pinky1,
            handDrivers[0].pinky2,
            handDrivers[0].pinky3
        };

        RightHands = new Transform[15]
        {
            handDrivers[1].Thumb1,
            handDrivers[1].Thumb2,
            handDrivers[1].Thumb3,
            handDrivers[1].Index1,
            handDrivers[1].Index2,
            handDrivers[1].Index3,
            handDrivers[1].Middle1,
            handDrivers[1].Middle2,
            handDrivers[1].Middle3,
            handDrivers[1].Ring1,
            handDrivers[1].Ring2,
            handDrivers[1].Ring3,
            handDrivers[1].Pinky1,
            handDrivers[1].Pinky2,
            handDrivers[1].Pinky3
        };
        RightHandsVec3 = new Vector3[15]
        {
            handDrivers[1].thumb1,
            handDrivers[1].thumb2,
            handDrivers[1].thumb3,
            handDrivers[1].index1,
            handDrivers[1].index2,
            handDrivers[1].index3,
            handDrivers[1].middle1,
            handDrivers[1].middle2,
            handDrivers[1].middle3,
            handDrivers[1].ring1,
            handDrivers[1].ring2,
            handDrivers[1].ring3,
            handDrivers[1].pinky1,
            handDrivers[1].pinky2,
            handDrivers[1].pinky3
        };

        for (int i = 0; i < JointOrder.Length; ++i)
        {
            LeftHandDefault.Add(JointOrder[i], LeftHands[i].localRotation);
            RightHandDefault.Add(JointOrder[i], RightHands[i].localRotation);
        }
    }

    bool finishConnect = false;
    bool NeedCD = false;
    float CDTime = 3;
    float FirstSearching = 6;

    public bool NeedChooseConnect = false;
    public List<string> DevicesList { get; private set; }
    public string LeftSpecificConnect = "";
    public string RightSpecificConnect = "";

    [HideInInspector]
    public bool FinJoyCalibration = true;
    public bool FinishCalibration { get; private set; } = false;
    void Update()
    {
        if (UsingAndroidService) return;

        DebugText.text = "";
        DevicesList = _SDK.UDE_GetBleDeviceList();

        LeftConnectState = _SDK.UDE_CheckDeviceIsConnect(LeftDeviceName);
        RightConnectState = _SDK.UDE_CheckDeviceIsConnect(RightDeviceName);
        if(FPS != null && FPS.Length == 2)
        {
            FPS[0].text = "FPS " + _SDK.UDE_GetDeviceTransferFPS(LeftDeviceName);
            FPS[1].text = "FPS " + _SDK.UDE_GetDeviceTransferFPS(RightDeviceName);
        }

        if (NeedCD)
        {
            CDTime -= Time.deltaTime;
            NeedCD = !(CDTime <= 0);
        }

        //DebugText.text = "Count " + DevicesList.Count;
        if (!finishConnect)
        {
            if(DevicesList.Count <= 2)
            {
                NeedChooseConnect = false;
                foreach (string deviceName in DevicesList)
                {
                    if (deviceName[^1] == 'L' && string.IsNullOrEmpty(LeftDeviceName) && deviceName.Contains(MyselfTest) && !NeedCD)
                    {
                        LeftDeviceName = deviceName;
                        _SDK.UDE_ConnectCertainDevice(LeftDeviceName);
                        NeedCD = true;
                    }
                    else if (deviceName[^1] == 'R' && string.IsNullOrEmpty(RightDeviceName) && deviceName.Contains(MyselfTest) && !NeedCD)
                    {
                        RightDeviceName = deviceName;
                        _SDK.UDE_ConnectCertainDevice(RightDeviceName);
                        NeedCD = true;
                    }
                }
            }
            else
            {
                NeedChooseConnect = true;
                if (string.IsNullOrEmpty(LeftDeviceName) && !string.IsNullOrEmpty(LeftSpecificConnect) && !NeedCD)
                {
                    LeftDeviceName = LeftSpecificConnect;
                    _SDK.UDE_ConnectCertainDevice(LeftDeviceName);
                    NeedCD = true;
                }
                else if(string.IsNullOrEmpty(RightDeviceName) && !string.IsNullOrEmpty(RightSpecificConnect) && !NeedCD)
                {
                    RightDeviceName = RightSpecificConnect;
                    _SDK.UDE_ConnectCertainDevice(RightDeviceName);
                    NeedCD = true;
                }
            }

            if (DevicesList.Count >= 2 && !string.IsNullOrEmpty(LeftDeviceName) && !string.IsNullOrEmpty(RightDeviceName))
            {
                _SDK.UDE_BleStopSearching();
                finishConnect = true;
                NeedChooseConnect = false;
            }
        }

        //DebugText.text = "finishConnect " + finishConnect + "/" + LeftDeviceName + "/" + RightDeviceName;

        if (LeftConnectState)
        {
            _SDK.UDE_RunningDataUpdate(LeftDeviceName);
            var DataDict = _SDK.UDE_GetFingerRotations(LeftDeviceName);
            DebugText.text += LeftDeviceName + "[" + _SDK.UDE_GetCalibrationType(LeftDeviceName) + "]:" + _SDK.UDE_GetRawAngleData(LeftDeviceName, true);
            LeftRawData = _SDK.UDE_GetRawAngleData(LeftDeviceName, true);
            LeftRawAngleData = _SDK.UDE_GetRawAngleData(LeftDeviceName);
            LeftAlgorithmVer = _SDK.UDE_GetAlgorithmVersionName(LeftDeviceName);
            if (DataDict != null)
            {
                int index = 0;
                foreach (var joint in JointOrder)
                {
                    LeftHands[index].localRotation = LeftHandDefault[joint];
                    LeftHandsVec3[index] = DataDict[joint];
                    _SDK.UDE_FingerRotate(LeftHands[index++], DataDict[joint]);
                }
                handDrivers[0].GetVec3Value(LeftHandsVec3);
                
            }
        }
        if (RightConnectState)
        {
            _SDK.UDE_RunningDataUpdate(RightDeviceName);
            var DataDict = _SDK.UDE_GetFingerRotations(RightDeviceName);
            DebugText.text += RightDeviceName + "[" + _SDK.UDE_GetCalibrationType(RightDeviceName) + "]:" + _SDK.UDE_GetRawAngleData(RightDeviceName, true);
            RightRawData = _SDK.UDE_GetRawAngleData(RightDeviceName, true);
            RightRawAngleData = _SDK.UDE_GetRawAngleData(RightDeviceName);
            RightAlgorithmVer = _SDK.UDE_GetAlgorithmVersionName(RightDeviceName);
            if (DataDict != null)
            {
                int index = 0;
                foreach (var joint in JointOrder)
                {
                    RightHands[index].localRotation = RightHandDefault[joint];
                    RightHandsVec3[index] = DataDict[joint];
                    _SDK.UDE_FingerRotate(RightHands[index++], DataDict[joint]);
                }
                handDrivers[1].GetVec3Value(RightHandsVec3);
            }
        }

        if (_SDK.UDE_GetCalibrationType(LeftDeviceName) == UDE_SDK.CalibrationType.Completed && _SDK.UDE_GetCalibrationType(RightDeviceName) == UDE_SDK.CalibrationType.Completed && !FinishCalibration)
        {
            Invoke(nameof(GetCalibrationOffset), 1f);
        }
    }

    public void Calibration(int step)
    {
        switch(step)
        {
            case 0:
                _SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Fist, true);
                _SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Fist, true);
                FinishCalibration = false;
                FinJoyCalibration = false;
                break;
            case 1:
                _SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Fist, false);
                _SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Fist, false);
                _SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Adduct, true);
                _SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Adduct, true);
                break;
            case 2:
                _SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Adduct, false);
                _SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Adduct, false);
                _SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Stretch, true);
                _SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Stretch, true);
                break;
            case 3:
                _SDK.UDE_CalibrationMotionController(LeftDeviceName, UDE_SDK.CalibrationType.Stretch, false);
                _SDK.UDE_CalibrationMotionController(RightDeviceName, UDE_SDK.CalibrationType.Stretch, false);
                _SDK.UDE_JoystickCenterCalibration(LeftDeviceName);
                _SDK.UDE_JoystickCenterCalibration(RightDeviceName);
                Invoke(nameof(GetCalibrationOffset), 1f);
                break;
        }
    }

    public void JoyRangeCalibration(bool control)
    {
        _SDK.UDE_JoystickRangeCalibration(LeftDeviceName, control);
        _SDK.UDE_JoystickRangeCalibration(RightDeviceName, control);
        FinJoyCalibration = !control;
    }

    void GetCalibrationOffset()
    {
        FinishCalibration = true;
        LeftCalibrationOffset = _SDK.UDE_GetCalibrationInfoOffset(LeftDeviceName);
        RightCalibrationOffset = _SDK.UDE_GetCalibrationInfoOffset(RightDeviceName);
    }
}