using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.IO;
using System.Net;
using System.Net.Sockets;
using UnityEngine;
using System.Runtime.Serialization;


#if hNW_ROBOID

using Passer.LinearAlgebra;

namespace Passer.Humanoid {

    public partial class HumanoidPlayer : MonoBehaviour, IHumanoidNetworking {

        [SerializeField]
        protected float _sendRate = 25;
        public float sendRate {
            get { return _sendRate; }
        }

        [SerializeField]
        protected bool _createLocalRemotes = false;
        public bool createLocalRemotes {
            get { return _createLocalRemotes; }
            set { _createLocalRemotes = value; }
        }

        public int nssPort = 7681;
        protected UdpClient udpClient;
        protected IPEndPoint endPoint;
        protected object obj;
        protected AsyncCallback receiveCallback;
        ConcurrentQueue<HumanoidNetworking.IMessage> messageQueue = new ConcurrentQueue<HumanoidNetworking.IMessage>();

        public byte networkId;

        #region Dummy Interface

        public void Send(bool b) { }
        public void Send(byte b) { }
        public void Send(int x) { }
        public void Send(float f) { }
        public void Send(Vector3 v) { }
        public void Send(Quaternion q) { }

        public bool ReceiveBool() {
            return false;
        }
        public byte ReceiveByte() {
            return 0;
        }
        public int ReceiveInt() {
            return 0;
        }
        public float ReceiveFloat() {
            return 0;
        }
        public Vector3 ReceiveVector3() {
            return Vector3.zero;
        }
        public Quaternion ReceiveQuaternion() {
            return Quaternion.identity;
        }

        public ulong GetObjectIdentity(GameObject obj) {
            return 0;
        }
        public GameObject GetGameObject(ulong objIdentity) {
            return this.gameObject;
        }

        public void InstantiateHumanoid(HumanoidControl humanoid) {
            if (debug <= HumanoidNetworking.DebugLevel.Info)
                DebugLog("Send Instantiate Humanoid " + humanoid.humanoidId);

            HumanoidNetworking.InstantiateHumanoid instantiateHumanoid = new HumanoidNetworking.InstantiateHumanoid(humanoid);
            if (createLocalRemotes)
                this.Receive(instantiateHumanoid);
        }
        public void DestroyHumanoid(HumanoidControl humanoid) { }
        public void Grab(HandTarget handTarget, GameObject obj, bool rangeCheck, HandTarget.GrabType grabType = HandTarget.GrabType.HandGrab) { }
        public void LetGo(HandTarget handTarget) { }
        public void ChangeAvatar(HumanoidControl humanoid, string avatarPrefabName, string possessionLocation = null) {
            if (debug <= HumanoidNetworking.DebugLevel.Info)
                Debug.Log(humanoid.nwId + ": Change Avatar: " + avatarPrefabName);

            HumanoidNetworking.ChangeAvatar changeAvatar = new HumanoidNetworking.ChangeAvatar(humanoid, avatarPrefabName, possessionLocation);
            if (createLocalRemotes)
                this.Receive(changeAvatar);
        }

        public void SyncTrackingSpace(HumanoidControl humanoid) { }
        public void ReenableNetworkSync(GameObject obj) { }
        public void DisableNetworkSync(GameObject obj) { }

        public void DebugLog(string s) {
            Debug.Log(s);
        }
        public void DebugWarning(string s) {
            Debug.LogWarning(s);
        }
        public void DebugError(string s) {
            Debug.LogError(s);
        }

        public HumanoidNetworking.Smoothing smoothing => HumanoidNetworking.Smoothing.None;

        private bool _isLocal;
        public bool isLocal => _isLocal; // may need to implement this

        public ulong nwId => 0;

        public HumanoidNetworking.HumanoidPose lastHumanoidPose { get => null; set { return; } }

        public List<HumanoidControl> humanoids { get; set; }

        #endregion Dummy Interface

        protected virtual void Awake() {
            mInstance = this;

            GameObject.DontDestroyOnLoad(this.gameObject);
            humanoids = HumanoidNetworking.FindLocalHumanoids();

            for (int i = 0; i < humanoids.Count; i++) {
                HumanoidControl humanoid = humanoids[i];
                if (humanoid.isRemote)
                    continue;

                humanoid.humanoidNetworking = this;
            }

            endPoint = new IPEndPoint(IPAddress.Any, nssPort + 3);
            udpClient = new UdpClient(endPoint);
            receiveCallback = new AsyncCallback(Receiver);
            udpClient.BeginReceive(receiveCallback, obj);
        }

        protected void OnApplicationQuit() {
            if (udpClient != null) {
                udpClient.Client.Close();
                udpClient.Close();
                udpClient = null;
            }
        }

        #region Update

        float lastSend;

        protected virtual void LateUpdate() {
            while (messageQueue.TryDequeue(out HumanoidNetworking.IMessage msg)) {
                ProcessMessage(msg);
            }

            if (Time.time > lastSend + 1 / sendRate) {
                if (humanoids != null) {
                    foreach (HumanoidControl humanoid in humanoids) {
                        if (!humanoid.isRemote) {
                            UpdateHumanoidPose(humanoid);
                        }
                    }
                }
                lastSend = Time.time;
            }
        }

        protected virtual void ProcessMessage(HumanoidNetworking.IMessage msg) {
            switch (msg) {
                case NetworkIdMsg networkId:
                    ProcessNetworkId(networkId);
                    break;
            }
        }

        void Receiver(IAsyncResult result) {
            // Take care of buffered messages after the client has been closed
            if (udpClient == null || udpClient.Client.Connected == false)
                return;

            byte[] data = (udpClient.EndReceive(result, ref endPoint));

            ReceivePacket(data);

            udpClient.BeginReceive(receiveCallback, obj);
        }

        void ReceivePacket(byte[] data) {
            byte msgId = data[0];
            switch (msgId) {
                case 0xA1: // (161) Network Id
                    this.ReceiveNetworkId(data); break;
            }

        }

        #endregion Update

        #region NetworkId

        public class NetworkIdMsg : HumanoidNetworking.IMessage {
            public const byte msgId = 0xA1;
            public byte networkId = 0;

            public NetworkIdMsg(byte networkId) {
                this.networkId = networkId;
            }
            public NetworkIdMsg(byte[] data) : base(data) { }

            public override byte[] Serialize() {
                MemoryStream ms = new();
                BinaryWriter bw = new(ms);
                bw.Write(msgId);
                bw.Write(this.networkId); // Thing Id
                byte[] data = ms.ToArray();
                return data;
            }
            public override void Deserialize(byte[] data) {
                base.Deserialize(data);
                this.networkId = (byte)data[1];
            }
        }

        public void ReceiveNetworkId(byte[] data) {
            NetworkIdMsg msg = new(data);
            this.networkId = msg.networkId;

            messageQueue.Enqueue(msg);
        }

        private void ProcessNetworkId(NetworkIdMsg msg) {
            GameObject networkingObj = this.GetGameObject(msg.networkId);
            if (networkingObj == null) {
                if (this.debug <= HumanoidNetworking.DebugLevel.Error)
                    this.DebugLog("Could not find Networking for Instantiate Humanoid " + msg.networkId + "/0");
                return;
            }
            HumanoidControl remoteHumanoid = humanoids[0]; //FindRemoteHumanoid(humanoids, 0);
            if (remoteHumanoid == null) {
                if (debug <= HumanoidNetworking.DebugLevel.Warning)
                    DebugLog("Remote Humanoid " + msg.networkId + "/" + 0 + "not found");
                return;
            }

            SendThing(remoteHumanoid);
            SendName(remoteHumanoid, 0, "Humanoid");
            SendModel(remoteHumanoid, "https://gitlab.passervr.com/passer/models/humanoid/-/raw/main/MakeHumanPasserMedium.glb?ref_type=heads&inline=false");

            SendSubThing(remoteHumanoid, remoteHumanoid.hipsTarget.hips);
            SendSubThing(remoteHumanoid, remoteHumanoid.hipsTarget.spine);
            SendSubThing(remoteHumanoid, remoteHumanoid.hipsTarget.chest);
            SendSubThing(remoteHumanoid, remoteHumanoid.headTarget.neck);
            SendSubThing(remoteHumanoid, remoteHumanoid.headTarget.head);

            SendSubThing(remoteHumanoid, remoteHumanoid.leftHandTarget.upperArm);
            SendSubThing(remoteHumanoid, remoteHumanoid.leftHandTarget.forearm);
            SendSubThing(remoteHumanoid, remoteHumanoid.leftHandTarget.hand);

            SendSubThing(remoteHumanoid, remoteHumanoid.rightHandTarget.upperArm);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightHandTarget.forearm);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightHandTarget.hand);

            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.upperLeg);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.lowerLeg);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.foot);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.toes);

            SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.upperLeg);
            SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.lowerLeg);
            SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.foot);
            SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.toes);

            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.upperLeg);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.lowerLeg);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.foot);
            SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.toes);
        }

        #endregion

        #region Thing

        protected class ThingMsg : HumanoidNetworking.IMessage {
            private const byte msgId = 0x80;
            public const byte length = 3;
            public byte thingId;
            public byte thingType;

            public ThingMsg(byte thingId, byte thingType) {
                this.thingId = thingId;
                this.thingType = thingType;
            }
            public ThingMsg(byte[] data) : base(data) { }

            public override byte[] Serialize() {
                byte[] data = new byte[3];
                data[0] = msgId;
                data[1] = thingId;
                data[2] = thingType;
                return data;
            }
            public override void Deserialize(byte[] data) {
                try {
                    uint ix = 0;
                    thingId = data[ix++];
                    thingType = data[ix++];
                }
                catch { }
            }
        }

        public void SendThing(HumanoidControl humanoid) {
            if (debug <= HumanoidNetworking.DebugLevel.Debug)
                Debug.Log("Send Thing " + humanoid.humanoidId + " nwId: " + humanoid.nwId);

            ThingMsg thingMsg = new(0, 1);

            if (udpClient != null) {
                byte[] data = thingMsg.Serialize();
                udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
            }
        }

        #endregion Thing

        #region SubThing

        protected class SubThingMsg : HumanoidNetworking.IMessage {
            private const byte msgId = 0x12; // (18)
            public const byte length = 3 + 4;
            public byte thingId;
            public byte parentId;
            readonly Vector3 position;

            public SubThingMsg(byte thingId, byte parentId, Vector3 position) {
                this.thingId = thingId;
                this.parentId = parentId;
                this.position = position;
            }
            public SubThingMsg(byte[] data) : base(data) { }

            public override byte[] Serialize() {
                byte[] data = new byte[7];
                data[0] = msgId;
                data[1] = thingId;
                data[2] = parentId;

                data[3] = 0;
                data[4] = 0;
                data[5] = 0;
                data[6] = 0;
                return data;
            }
            //public override void Deserialize(byte[] data) {
            //    try {
            //        uint ix = 0;
            //        thingId = data[ix++];
            //        thingType = data[ix++];
            //    }
            //    catch { }
            //}
        }

        public void SendSubThing(HumanoidControl humanoid, HumanoidTarget.TargetedBone bone) {
            if (debug <= HumanoidNetworking.DebugLevel.Debug)
                Debug.Log("Send SubThing " + humanoid.humanoidId + " nwId: " + humanoid.nwId);

            SubThingMsg thingMsg;
            if (bone.parent != null)
                thingMsg = new((byte)bone.boneId, (byte)bone.parent.boneId, bone.bone.transform.position);
            else
                thingMsg = new((byte)bone.boneId, 0, bone.bone.transform.position);

            if (udpClient != null) {
                byte[] data = thingMsg.Serialize();
                udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
            }

            SendName(humanoid, (byte)bone.boneId, bone.bone.transform.name);
        }

        #endregion 

        #region Name

        public class NameMsg : HumanoidNetworking.IMessage {
            public const byte msgId = 0x91; // (145) Name
            public byte networkId = 0;
            public byte thingId;
            public byte len;
            public string name;

            public NameMsg(byte thingId, string name) {
                this.thingId = thingId;
                this.name = name;
            }
            public NameMsg(byte[] data) : base(data) { }

            public override byte[] Serialize() {
                MemoryStream ms = new();
                BinaryWriter bw = new(ms);
                bw.Write(msgId); // 145 Name Msg
                bw.Write((byte)thingId); // Thing Id
                bw.Write((byte)name.Length);

                for (int ix = 0; ix < name.Length; ix++)
                    bw.Write(name[ix]);

                byte[] data = ms.ToArray();
                return data;
            }
            public override void Deserialize(byte[] data) {
                uint ix = 0;
                this.thingId = data[ix++];
                int strlen = data[ix++];
                this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen);
            }
        }

        public void SendName(HumanoidControl humanoid, byte thingId, string name) {
            if (debug <= HumanoidNetworking.DebugLevel.Debug)
                Debug.Log("Send Name " + humanoid.humanoidId + " nwId: " + humanoid.nwId);

            NameMsg nameMsg = new(thingId, name); // humanoid.name;

            if (udpClient != null) {
                byte[] data = nameMsg.Serialize();
                udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
            }
        }

        #endregion

        #region Model

        class ModelUrlMsg : HumanoidNetworking.IMessage {
            public const byte msgId = 0x90; // (144) Model URL
            public byte objectId;
            public Vector3 position;
            public float scale;
            public string url;

            public ModelUrlMsg(string url) {
                this.url = url;
            }
            public ModelUrlMsg(byte[] data) : base(data) { }

            public override byte[] Serialize() {
                MemoryStream ms = new();
                BinaryWriter bw = new(ms);
                bw.Write(msgId); // 145 Name Msg
                bw.Write((byte)0x00); // Thing Id

                bw.Write((byte)0x00); // Dummy position
                bw.Write((byte)0x00);
                bw.Write((byte)0x00);
                bw.Write((byte)0x00);

                SendFloat16(bw, 1.0f);

                bw.Write((byte)url.Length);

                for (int ix = 0; ix < url.Length; ix++)
                    bw.Write(url[ix]);

                byte[] data = ms.ToArray();
                return data;
            }
            //public override void Deserialize(byte[] data) {
            //    uint ix = 0;
            //    this.objectId = data[ix++];
            //    //this.position = ReceiveVector3(data, ref ix);
            //    this.position = ReceiveSpherical(data, ref ix);
            //    this.scale = ReceiveFloat16(data, ref ix);
            //    int strlen = data[ix++];
            //    url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen);
            //}
        }

        public void SendModel(HumanoidControl humanoid, string url) {
            if (debug <= HumanoidNetworking.DebugLevel.Debug)
                Debug.Log("Send Name " + humanoid.humanoidId + " nwId: " + humanoid.nwId);

            ModelUrlMsg msg = new(url); // humanoid.name;

            if (udpClient != null) {
                byte[] data = msg.Serialize();
                udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
            }
        }

        #endregion Model

        #region Pose

        [Serializable]
        public class RoboidBonePose : HumanoidNetworking.HumanoidPose {
            private readonly byte msgId = 0x10;
            readonly byte boneId;
            readonly Quat32 boneOrientation;

            public RoboidBonePose(HumanoidTarget.TargetedBone targetedBone, bool isRoot = false) {
                this.boneId = (byte)targetedBone.boneId;
                if (isRoot) {
                    this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation);
                }
                else
                    this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation);    
            }
            
            public override byte[] Serialize() {
                byte[] data = new byte[11];
                data[0] = msgId;
                data[1] = boneId;
                data[2] = 0x03; // Pose_orientation

                //data[3][4][5][6] are 0

                data[7] = boneOrientation.qx;
                data[8] = boneOrientation.qy;
                data[9] = boneOrientation.qz;
                data[10] = boneOrientation.qw;
                return data;
            }
        }


        //[Serializable]
        //public class RoboidPose : HumanoidNetworking.HumanoidPose {
        //    readonly Quat32 headOrientation;

        //    public RoboidPose(HumanoidControl humanoid) {
        //        headOrientation = new Quat32(humanoid.headTarget.head.bone.transform.rotation);
        //    }

        //    public override byte[] Serialize() {
        //        MemoryStream ms = new();
        //        BinaryWriter bw = new(ms);
        //        bw.Write((byte)0x10); // PoseMsg
        //        bw.Write((byte)this.head.boneId); // Thing Id
        //        bw.Write((byte)0x03); // Pose_Orientation

        //        bw.Write((byte)0x00); // Dummy data
        //        bw.Write((byte)0x00);
        //        bw.Write((byte)0x00);
        //        bw.Write((byte)0x00);

        //        SendQuat32(bw, headOrientation);

        //        byte[] data = ms.ToArray();
        //        return data;
        //    }
        //}

        public virtual void UpdateHumanoidPose(HumanoidControl humanoid) {
            if (debug <= HumanoidNetworking.DebugLevel.Debug)
                Debug.Log("Send Pose Humanoid " + humanoid.humanoidId + " nwId: " + humanoid.nwId);

            //RoboidPose humanoidPose = new(humanoid);
            //if (createLocalRemotes)
            //    this.Receive(humanoidPose);

            //if (udpClient != null) {
            //    byte[] data = humanoidPose.Serialize();
            //    udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
            //}
            SendBone(humanoid.hipsTarget.hips, true);
            SendBone(humanoid.hipsTarget.spine);
            SendBone(humanoid.hipsTarget.chest);
            SendBone(humanoid.headTarget.neck);
            SendBone(humanoid.headTarget.head);

            SendBone(humanoid.leftHandTarget.upperArm);
            SendBone(humanoid.leftHandTarget.forearm);
            SendBone(humanoid.leftHandTarget.hand);

            SendBone(humanoid.rightHandTarget.upperArm);
            SendBone(humanoid.rightHandTarget.forearm);
            SendBone(humanoid.rightHandTarget.hand);

            SendBone(humanoid.leftFootTarget.upperLeg);
            SendBone(humanoid.leftFootTarget.lowerLeg);
            SendBone(humanoid.leftFootTarget.foot);

            SendBone(humanoid.rightFootTarget.upperLeg);
            SendBone(humanoid.rightFootTarget.lowerLeg);
            SendBone(humanoid.rightFootTarget.foot);
        }

        private void SendBone(HumanoidTarget.TargetedBone bone, bool isRoot = false) {
            RoboidBonePose bonePose = new(bone, isRoot);
            byte[] buffer = bonePose.Serialize();
            udpClient.Send(buffer, buffer.Length, "127.0.0.1", nssPort);
        }

        #endregion Pose

        #region Send

        static void SendQuat32(BinaryWriter bw, Quat32 q) {
            bw.Write((byte)q.qx);
            bw.Write((byte)q.qy);
            bw.Write((byte)q.qz);
            bw.Write((byte)q.qw);
        }

        static void SendFloat16(BinaryWriter bw, float f) {
            bw.Write((byte)0x3C); // Dummy value 1
            bw.Write((byte)0x00);
        }

        #endregion Send
    }
}
#endif