using System;
using System.Collections.Generic;
using System.Linq;
using Unity.XR.PXR;
using UnityEngine;


namespace UDESDK
{
    public class UDE_SDK
    {
        private bool IsInitialized = false;
        private Dictionary<string, BleHandDriver> bleHandDriver = new();

        /// <summary>
        /// Bluetooth Initial.
        /// </summary>
        public void UDE_BleInit()
        {
            UDE_API.UDE_Init();
            IsInitialized = true;
        }
        
        /// <summary>
        /// Start Searching Device.
        /// </summary>
        /// <returns></returns>
        public bool UDE_BleStartSearching()
        {
            if(!IsInitialized)
            {
                return false;
            }
            UDE_API.StartEnumDevices();
            return true;
        }

        /// <summary>
        /// Stop Searching Device.
        /// </summary>
        /// <returns></returns>
        public bool UDE_BleStopSearching()
        {
            if (!IsInitialized)
            {
                return false;
            }
            UDE_API.StopEnumDevices();
            return true;
        }

        /// <summary>
        /// Get Bluetooth Devices List.
        /// </summary>
        /// <returns></returns>
        public List<string> UDE_GetBleDeviceList()
        {
            if (!IsInitialized)
            {
                return new List<string>();
            }
            return UDE_API.GetDeviceList();
        }

        /// <summary>
        /// Connecting a Device by Name.
        /// </summary>
        /// <param name="DeviceName">Device Name</param>
        public bool UDE_ConnectCertainDevice(string DeviceName, Action<string, bool> action = null)
        {
            if(string.IsNullOrEmpty(DeviceName) || !IsInitialized)
            {
                return false;
            }

            if (UDE_API.OpenDeviceByName(DeviceName, action))
            {
                bleHandDriver.Add(DeviceName, new BleHandDriver(DeviceName));
                return true;
            }

            return false;
        }

        /// <summary>
        /// Get Connection State of a Device.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public bool UDE_CheckDeviceIsConnect(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !IsInitialized)
            {
                return false;
            }

            return UDE_API.CheckConnectByName(DeviceName);
        }

        /// <summary>
        /// Disconnect a Device.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public bool UDE_DisconnectDevice(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !IsInitialized)
            {
                return false;
            }

            return UDE_API.CloseDeviceByName(DeviceName);
        }

        /// <summary>
        /// Get Device Current Battery Level (1~5)
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public int UDE_GetDeviceBatteryLevel(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !IsInitialized)
            {
                return 0;
            }

            return UDE_API.GetDeviceBatteryByName(DeviceName);
        }

        /// <summary>
        /// Data Update.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public bool UDE_RunningDataUpdate(string DeviceName)
        {
            if(string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            bleHandDriver[DeviceName].Method_update();
            return true;
        }

        private int CalibrationStepIndex = 1;
        public enum CalibrationType
        {
            AdductFin = -2,
            FistFin = -1,
            None, //未标定
            Fist,   //握拳
            Adduct, //并拢
            Stretch, //张开
            Completed //完成
        }
        /// <summary>
        /// Start First Calibration Motion.
        /// </summary>
        /// <returns></returns>
        public bool UDE_CalibrationMotionController(string DeviceName, CalibrationType calibrationType, bool IsStart)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            if ((int)calibrationType < 1) // || (int)calibrationType > 3 允许重复标定
            {
                return false;
            }

            BleHandDriver.CalibrationType type = (BleHandDriver.CalibrationType)calibrationType;

            if (IsStart)
            {
                CalibrationStepIndex = (int)calibrationType;
                return bleHandDriver[DeviceName].StartCalibration(type);
            }
            else
            {
                CalibrationStepIndex++;
                if (CalibrationStepIndex == 4) CalibrationStepIndex = 1;
                return bleHandDriver[DeviceName].StopCalibration(type);
            }
            
        }

        /// <summary>
        /// Get Calibration State, return None if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public CalibrationType UDE_GetCalibrationType(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return 0;
            }

            return (CalibrationType)bleHandDriver[DeviceName].GetCalibrationType();
        }

        /// <summary>
        /// Get Raw Angle Data, return empty list if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public List<double> UDE_GetRawAngleData(string DeviceName, bool Raw = false)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return new List<double>();
            }

            if(Raw)
                return bleHandDriver[DeviceName].GetRawData();
            else
                return bleHandDriver[DeviceName].GetAngleData();
        }

        /// <summary>
        /// Get calibrate offset value, which shows calibration quaility.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public double[] UDE_GetCalibrationInfoOffset(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return new double[0];
            }

            return bleHandDriver[DeviceName].GetCalibrationInfoOffset();
        }

        /// <summary>
        /// Get Bluetooth device data transfer frame rate.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public int UDE_GetDeviceTransferFPS(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return 0;
            }

            return UDE_API.GetTransferFrequency(DeviceName);
        }

        public enum FingerJointType
        {
            Thumb1, Thumb2, Thumb3,
            Index1, Index2, Index3,
            Middle1, Middle2, Middle3,
            Ring1, Ring2, Ring3,
            Pinky1, Pinky2, Pinky3
        }

        /// <summary>
        /// Get Rotation Transfer Of Each Finger, Return null if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public Dictionary<FingerJointType, Vector3> UDE_GetFingerRotations(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return null;
            }

            Vector3 AddRotationTrans(float angle, Axis angleType)
            {
                float angleX = 0;
                float angleY = 0;
                float angleZ = 0;

                switch (angleType)
                {
                    case Axis.x_n:
                        angleX = -angle;
                        break;
                    case Axis.y_n:
                        angleY = -angle;
                        break;
                    case Axis.z_n:
                        angleZ = -angle;
                        break;
                    case Axis.x:
                        angleX = angle;
                        break;
                    case Axis.y:
                        angleY = angle;
                        break;
                    case Axis.z:
                        angleZ = angle;
                        break;
                }

                return new Vector3(angleX, angleY, angleZ);
            }

            var Driver = bleHandDriver[DeviceName];
            var Data = Driver.GetAngleData();
            if(Data.Count == 0)
            {
                return null;
            }

            return new Dictionary<FingerJointType, Vector3>
            {
                { FingerJointType.Thumb3, AddRotationTrans((float)Data[0], (Axis)Driver.Pitch) },
                { FingerJointType.Thumb2, AddRotationTrans((float)Data[1], (Axis)Driver.Pitch) },
                { FingerJointType.Thumb1, AddRotationTrans((float)Data[2] * Driver.coefficient + Driver.Thumb1Offset.z, (Axis)Driver.Pitch)
                                        + AddRotationTrans((float)Data[3] + Driver.Thumb1Offset.y, Axis.y)
                                        + AddRotationTrans((float)Data[20] + Driver.Thumb1Offset.x, Axis.x)
                },

                { FingerJointType.Index3, AddRotationTrans((float)Data[4], (Axis)Driver.Pitch) },
                { FingerJointType.Index2, AddRotationTrans((float)Data[5], (Axis)Driver.Pitch) },
                { FingerJointType.Index1, AddRotationTrans((float)Data[6], (Axis)Driver.Pitch)
                                        + AddRotationTrans((float)Data[7], Axis.y)
                                        + AddRotationTrans((float)Data[21], (Axis)Driver.Roll)
                },

                { FingerJointType.Middle3, AddRotationTrans((float)Data[8],(Axis)Driver.Pitch) },
                { FingerJointType.Middle2, AddRotationTrans((float)Data[9],(Axis)Driver.Pitch) },
                { FingerJointType.Middle1, AddRotationTrans((float)Data[10], (Axis)Driver.Pitch)
                                         + AddRotationTrans((float)Data[11], Axis.y)
                },

                { FingerJointType.Ring3, AddRotationTrans((float)Data[12], (Axis)Driver.Pitch) },
                { FingerJointType.Ring2, AddRotationTrans((float)Data[13], (Axis)Driver.Pitch) },
                { FingerJointType.Ring1, AddRotationTrans((float)Data[14], (Axis)Driver.Pitch)
                                       + AddRotationTrans(-(float)Data[15], (Axis)Driver.Yaw)
                },

                { FingerJointType.Pinky3, AddRotationTrans((float)Data[16], (Axis)Driver.Pitch) },
                { FingerJointType.Pinky2, AddRotationTrans((float)Data[17], (Axis)Driver.Pitch) },
                { FingerJointType.Pinky1, AddRotationTrans((float)Data[18], (Axis)Driver.Pitch)
                                        + AddRotationTrans(-(float)Data[19], (Axis)Driver.Yaw)
                                        + AddRotationTrans(-(float)Data[22], (Axis)Driver.Roll)
                },
            };
        }

        public string UDE_GetAlgorithmVersionName(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return "";
            }

            return bleHandDriver[DeviceName].GetAlgorithmVerName();
        }

        public enum Axis
        {
            x = -3,
            y = -2,
            z = -1,
            x_n,
            y_n,
            z_n
        }

        /// <summary>
        /// Rotate Joint in Specific Order
        /// </summary>
        /// <param name="Angle"></param>
        /// <param name="joint"></param>
        public void UDE_FingerRotate(Transform Joint, Vector3 Angle)
        {
            Joint.Rotate(0,0,Angle.z);
            Joint.Rotate(0,Angle.y,0);
            Joint.Rotate(Angle.x,0,0);
        }

        /// <summary>
        /// Get Three Axis Index order:Pitch, Roll, Yaw. 
        /// Return null if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public Axis[] UDE_GetThreeAxis(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return null;
            }

            var Driver = bleHandDriver[DeviceName];
            return new Axis[3] { (Axis)Driver.Pitch, (Axis)Driver.Roll, (Axis)Driver.Yaw };
        }

        /// <summary>
        /// Set Three Axis Index order:Pitch, Roll, Yaw.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public bool UDE_SetThreeAxis(string DeviceName, Axis[] axes)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.Pitch = (BleHandDriver.Axis)axes[0];
            Driver.Roll = (BleHandDriver.Axis)axes[1];
            Driver.Yaw = (BleHandDriver.Axis)axes[2];
            return true;
        }

        /// <summary>
        /// Get Thumb Coefficient Value, return -1 if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public float UDE_GetThumbCoefficient(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return -1;
            }

            var Driver = bleHandDriver[DeviceName];
            return Driver.coefficient;
        }

        /// <summary>
        /// Set Thumb Coefficient Value, Range[0,1].
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public bool UDE_SetThumbCoefficient(string DeviceName, float value)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            if(value < 0 || value > 1)
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.coefficient = value;
            return true;
        }

        /// <summary>
        /// Get Thumb First Joint Offset Value, return Zero Vector3 if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public Vector3 UDE_GetThumbFirstJointOffset(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return Vector3.zero;
            }

            var Driver = bleHandDriver[DeviceName];
            return Driver.Thumb1Offset;

        }

        /// <summary>
        /// Set Thumb First Joint Offset Value.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        public bool UDE_SetThumbFirstJointOffset(string DeviceName, Vector3 value)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.Thumb1Offset = value;
            return true;
        }

        /// <summary>
        /// Get Data Of Extra Connected Device (Button, Joystick)
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public InputData UDE_GetInputDeviceData(string DeviceName) 
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return new InputData();
            }

            var Driver = bleHandDriver[DeviceName];
            return Driver.GetInputData();
        }

        /// <summary>
        /// Make Jostick Reset to Zero center point 
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public bool UDE_JoystickCenterCalibration(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.CalibrationCenterData();
            return true;
        }

        /// <summary>
        /// Get the Maximum Range Value of Joystick and Make Calibration
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <param name="IsStart">true is start, false is end</param>
        /// <returns></returns>
        public bool UDE_JoystickRangeCalibration(string DeviceName, bool IsStart)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            if(IsStart)
            {
                Driver.StartCalibrationRangeData();
            }
            else
            {
                Driver.StopCalibrationRangeData();
            }

            return true;
        }

        /// <summary>
        /// Get Joystick Dead Zone Value, range[0,1], return -1 if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public float UDE_GetJoystickDeadZone(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return -1;
            }

            var Driver = bleHandDriver[DeviceName];
            return Driver.GetDeadZone();
        }

        /// <summary>
        /// Set Joystick Dead Zone Value, range[0,1], return false if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        public bool UDE_SetJoystickDeadZone(string DeviceName, float value)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            if(value < 0 || value > 1)
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.SetDeadZone(value);
            return true;
        }

        /// <summary>
        /// Get Trigger Button Active Value, range[0,1], return -1 if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public float UDE_GetTriggerActiveValue(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return -1;
            }

            var Driver = bleHandDriver[DeviceName];
            return Driver.GetTriggerValue();
        }

        /// <summary>
        /// Set Trigger Button Active Value, range[0,1], return false if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        public bool UDE_SetTriggerActiveValue(string DeviceName, float value)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            if (value < 0 || value > 1)
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.SetTriggerValue(value);
            return true;
        }

        /// <summary>
        /// Get Grip Button Active Value, range[0,1], return -1 if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public float UDE_GetGripActiveValue(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return -1;
            }

            var Driver = bleHandDriver[DeviceName];
            return Driver.GetGrabValue();
        }

        /// <summary>
        /// Set Grip Button Active Value, range[0,1], return false if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        public bool UDE_SetGripActiveValue(string DeviceName, float value)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            if (value < 0 || value > 1)
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.SetGrabValue(value);
            return true;
        }

        /// <summary>
        /// Get Grip Button Active Value, range[0,1], return -1 if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <returns></returns>
        public float UDE_GetTrackpadActiveValue(string DeviceName)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return -1;
            }

            var Driver = bleHandDriver[DeviceName];
            return Driver.GetTrackpadValue();
        }

        /// <summary>
        /// Set Grip Button Active Value, range[0,1], return false if invalid.
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <param name="value"></param>
        /// <returns></returns>
        public bool UDE_SetTrackpadActiveValue(string DeviceName, float value)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            if (value < 0 || value > 1)
            {
                return false;
            }

            var Driver = bleHandDriver[DeviceName];
            Driver.SetTrackpadValue(value);
            return true;
        }

        /// <summary>
        /// Control the Vibration of extra device, return false if setting value is invlid 
        /// </summary>
        /// <param name="DeviceName"></param>
        /// <param name="PlaceIndex">only available with 1 or 2</param>
        /// <param name="SustainedTenMillionsecond">default minimum is 4</param>
        /// <param name="Strength">default minimum is 1, range[1,7]</param>
        /// <returns></returns>
        public bool UDE_ControlVibration(string DeviceName, int PlaceIndex, int SustainedTenMillionsecond = 4, int Strength = 1)
        {
            if (string.IsNullOrEmpty(DeviceName) || !bleHandDriver.ContainsKey(DeviceName))
            {
                return false;
            }

            if(PlaceIndex != 2 && PlaceIndex != 1)
            {
                return false;
            }

            SustainedTenMillionsecond = SustainedTenMillionsecond < 4 ? 4 : SustainedTenMillionsecond;
            Strength = Strength < 1 ? 1 : Strength;
            Strength = Strength > 7 ? 7 : Strength;

            UDE_API.Pc2Mcu_SendVibration(DeviceName, (byte)PlaceIndex, (byte)SustainedTenMillionsecond, (byte)(Strength + 3));
            return true;
        }
    }

}