using System;
using System.Collections.Generic;
using System.Collections.Concurrent;
using LinearAlgebra;

namespace RoboidControl {

    /// <summary>
    /// A thing is the primitive building block
    /// </summary>
    [Serializable]
    public class Thing {

        /// <summary>
        /// Predefined thing types
        /// </summary>
        public static class Type {
            public const byte Undetermined = 0x00;
            // sensor
            public const byte Switch = 0x01;
            public const byte DistanceSensor = 0x02;
            public const byte DirectionalSensor = 0x03;
            public const byte TemperatureSensor = 0x04;
            public const byte TouchSensor = 0x05;
            // Motor
            public const byte ControlledMotor = 0x06;
            public const byte UncontrolledMotor = 0x07;
            public const byte Servo = 0x08;
            public const byte RelativeEncoder = 0x19;
            // Other
            public const byte Root = 0x10;
            public const byte Roboid = 0x09;
            public const byte Humanoid = 0x0A;
            public const byte ExternalSensor = 0x0B;
            public const byte Animator = 0x0C;
            public const byte DifferentialDrive = 0x0D;
        }

        #region Init

        private Thing(Participant owner) {
            this.type = Type.Root;

            this.positionUpdated = true;
            this.orientationUpdated = true;
            this.hierarchyChanged = true;

            this.owner = owner;
            this.parent = null;
        }

        public static Thing CreateRoot(Participant owner) {
            return new Thing(owner);
        }

        static Thing localRoot {
            get {
                Participant participant = Participant.localParticipant;
                return participant.root;
            }
        }

        /*
        /// <summary>
        /// Create a new thing without communication abilities
        /// </summary>
        /// <param name="thingType">The type of thing (can use \ref RoboidControl::Thing::Type "Thing.Type")</param>
        /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
        public Thing(byte thingType = Type.Undetermined, bool invokeEvent = true) : this(ParticipantUDP.Isolated(), thingType, 0, invokeEvent) {
        }

        /// <summary>
        /// Create a new thing for a participant
        /// </summary>
        /// <param name="owner">The owning participant</param>
        /// <param name="thingType">The type of thing (can use \ref RoboidControl::Thing::Type "Thing.Type")</param>
        /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
        /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
        public Thing(Participant owner, byte thingType = Type.Undetermined, byte thingId = 0, bool invokeEvent = true) {
            this.owner = owner;
            this.id = thingId;
            this.type = thingType;
            if (this.owner != null)
                this.owner.Add(this);
            if (invokeEvent) {
                Participant.UpdateEvent e = new() {
                    messageId = ThingMsg.id,
                    thing = this
                };
                this.owner.updateQueue.Enqueue(e);
            }
        }
        */

        /// <summary>
        /// Create a new child thing
        /// </summary>
        /// <param name="parent">The parent thing</param>
        /// <param name="thingType">The type of thing (can use \ref RoboidControl::Thing::Type "Thing.Type")</param>
        /// <param name="thingId">The ID of the thing, leave out or set to zero to generate an ID</param>
        /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
        /// <note>The owner will be the same as the owner of the parent thing</note>
        /*
        public Thing(Thing parent, byte thingType = Type.Undetermined, byte thingId = 0, bool invokeEvent = true) : this(parent.owner, thingType, thingId, invokeEvent) {
            this.parent = parent;
        }
        */
        public Thing(Thing parent = default) {
            this.type = Type.Undetermined;

            this.positionUpdated = true;
            this.orientationUpdated = true;
            this.hierarchyChanged = true;

            if (parent == default)
                this.parent = Participant.localParticipant.root;
            else
                this.parent = parent;

            this.owner = this.parent.owner;
            this.owner.Add(this, true);

            Participant.UpdateEvent e = new() {
                messageId = ThingMsg.id,
                thing = this
            };
            this.owner.updateQueue.Enqueue(e);
        }

        // public static Thing CreateRemote(Participant owner, byte thingId) {
        //     Thing remoteThing = new(owner.root) {
        //         id = thingId
        //     };
        //     return remoteThing;
        // }

        /// <summary>
        /// Function which can be used to create components in external engines.
        /// </summary>
        /// Currently this is used to create GameObjects in Unity
        public virtual void CreateComponent() {
#if UNITY_5_3_OR_NEWER            
            this.component = Unity.Thing.Create(this);
            this.component.core = this;
#endif            
        }

        #endregion Init

        public bool terminate = false;

        #region Properties

        /// <summary>
        /// The participant owning this thing
        /// </summary>
        public Participant owner = null;
        /// <summary>
        /// The ID of this thing
        /// </summary>
        public byte id = 0;

        /// <summary>
        /// The type of this thing. 
        /// </summary>
        /// This can be either a \ref RoboidControl::Thing::Type "Thing.Type" or a byte value for custom types.
        public byte type = Type.Undetermined;

        private string _name = "";
        /// <summary>
        /// The name of the thing
        /// </summary>
        public virtual string name {
            get => _name;
            set {
                if (_name != value) {
                    _name = value;
                    nameChanged = true;
                    this.updateQueue.Enqueue(new CoreEvent(NameMsg.Id));
                }
            }
        }
        public bool nameChanged = false;

        private string _modelUrl = "";
        /// <summary>
        /// An URL pointing to the location where a model of the thing can be found
        /// </summary>
        public string modelUrl {
            get => _modelUrl;
            set {
                if (_modelUrl != value) {
                    _modelUrl = value;
                    this.updateQueue.Enqueue(new CoreEvent(ModelUrlMsg.Id));
                }
            }
        }

#if UNITY_5_3_OR_NEWER
        /// <summary>
        /// A reference to the representation of the thing in Unity
        /// </summary>
        [NonSerialized]
        public Unity.Thing component = null;
#endif

        #endregion Properties

        #region Hierarchy

        private Thing _parent;
        /// <summary>
        /// The parent of this thing
        /// </summary>
        public Thing parent {
            get => _parent;
            set {
                if (_parent == value)
                    return;

                if (value == null) {
                    _parent?.RemoveChild(this);
                    _parent = null;
                }
                else {
                    value.AddChild(this);
                }
                this.hierarchyChanged = true;
                this.updateQueue.Enqueue(new CoreEvent(ThingMsg.id));
            }
        }


        /// <summary>
        /// The children of this thing
        /// </summary>
        [NonSerialized]
        protected List<Thing> children = new();

        /// <summary>
        /// Add a child Thing to this Thing
        /// </summary>
        /// <param name="child">The Thing which should become a child</param>
        /// @remark When the Thing is already a child, it will not be added again
        public void AddChild(Thing child) {
            if (children.Find(thing => thing == child) != null)
                return;

            child._parent = this;
            children.Add(child);
        }
        /// <summary>
        /// Remove the given thing as a child of this thing
        /// </summary>
        /// <param name="child">The child to remove</param>
        /// <returns>True when the child was present or false when it was not found</returns>
        public bool RemoveChild(Thing child) {
            return children.Remove(child);
        }

        /// <summary>
        /// Get a child by thing Id
        /// </summary>
        /// <param name="thingId">The thing ID to find</param>
        /// <param name="recurse">Look recursively through all descendants</param>
        /// <returns></returns>
        Thing GetChild(byte thingId, bool recurse = false) {
            foreach (Thing child in this.children) {
                if (child == null)
                    continue;

                if (child.id == thingId)
                    return child;
                if (recurse) {
                    Thing foundChild = child.GetChild(thingId, recurse);
                    if (foundChild != null)
                        return foundChild;
                }
            }
            return null;
        }
        /// <summary>
        /// Find a child by name
        /// </summary>
        /// <param name="name">The name of the child thing</param>
        /// <param name="recurse">Look recursively through all descendants</param>
        /// <returns>The found thing or null when nothing is found</returns>
        Thing FindChild(string name, bool recurse = true) {
            foreach (Thing child in this.children) {
                if (child == null)
                    continue;

                if (child.name == name)
                    return child;

                if (recurse) {
                    Thing foundChild = child.FindChild(name, recurse);
                    if (foundChild != null)
                        return foundChild;
                }
            }
            return null;
        }

        /// <summary>
        /// Indicator that the hierarchy of the thing has changed
        /// </summary>
        public bool hierarchyChanged = true;

        #endregion Hierarchy

        #region Pose

        private Spherical _position = Spherical.zero;
        /// <summary>
        /// The position of the thing in local space, in meters.
        /// </summary>
        public Spherical position {
            get { return _position; }
            set {
                if (_position != value) {
                    _position = value;
                    positionUpdated = true;
                    updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
                }
            }
        }
        /// <summary>
        /// Boolean indicating that the thing has an updated position
        /// </summary>
        public bool positionUpdated = false;

        private SwingTwist _orientation = SwingTwist.zero;
        /// <summary>
        /// The orientation of the thing in local space
        /// </summary>
        public SwingTwist orientation {
            get { return _orientation; }
            set {
                if (_orientation != value) {
                    _orientation = value;
                    orientationUpdated = true;
                    updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
                    //OnOrientationChanged?.Invoke();
                }
            }
        }
        /// <summary>
        /// Boolean indicating the thing has an updated orientation
        /// </summary>
        public bool orientationUpdated = false;

        private Spherical _linearVelocity = Spherical.zero;
        /// <summary>
        /// The linear velocity of the thing in local space in meters per second
        /// </summary>
        public Spherical linearVelocity {
            get => _linearVelocity;
            set {
                if (_linearVelocity != value) {
                    _linearVelocity = value;
                    linearVelocityUpdated = true;
                    updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
                }
            }
        }
        /// <summary>
        /// Boolean indicating the thing has an updated linear velocity
        /// </summary>
        public bool linearVelocityUpdated = false;

        private Spherical _angularVelocity = Spherical.zero;
        /// <summary>
        /// The angular velocity of the thing in local space in degrees per second
        /// </summary>
        public Spherical angularVelocity {
            get => _angularVelocity;
            set {
                if (_angularVelocity != value) {
                    _angularVelocity = value;
                    angularVelocityUpdated = true;
                    updateQueue.Enqueue(new CoreEvent(PoseMsg.Id));
                }
            }
        }
        /// <summary>
        /// Boolean indicating the thing has an updated angular velocity
        /// </summary>
        public bool angularVelocityUpdated = false;

        #endregion Pose

        #region Update

        /// <summary>
        /// Get the current time in milliseconds
        /// </summary>
        /// <returns>The current time in milliseconds</returns>
        public static ulong GetTimeMs() {
#if UNITY_5_3_OR_NEWER
            return (ulong)(UnityEngine.Time.time * 1000);
#else  
            return TimeManager.GetCurrentTimeMilliseconds();
#endif
        }

        /// <summary>
        /// Update de state of the thing
        /// </summary>
        /// <param name="recurse">When true, this will Update the descendants recursively</param>
        public void Update(bool recurse = false) {
            Update(GetTimeMs(), recurse);
        }
        // #endif
        /// <summary>
        /// Update this thing
        /// </summary>
        /// <param name="currentTime">he current clock time in milliseconds; if this is zero, the current time is retrieved automatically</param>
        /// <param name="recurse">When true, this will Update the descendants recursively</param>
        public virtual void Update(ulong currentTimeMs, bool recurse = false) {
            this.positionUpdated = false;
            this.orientationUpdated = false;
            this.linearVelocityUpdated = false;
            this.angularVelocityUpdated = false;
            this.hierarchyChanged = false;

            // should recurse over children...
            if (recurse) {
                for (byte childIx = 0; childIx < this.children.Count; childIx++) {
                    Thing child = this.children[childIx];
                    if (child == null)
                        continue;
                    child.Update(currentTimeMs, recurse);
                }
            }
        }

        public class CoreEvent {
            public CoreEvent(int messageId) {
                this.messageId = messageId;
            }
            public int messageId; // see the communication messages
        };
        public ConcurrentQueue<CoreEvent> updateQueue = new();

        #endregion Update

        /// <summary>
        /// Function used to generate binary data for this thing
        /// </summary>
        /// <returns>A byte array with the binary data</returns>
        /// @sa Passer::RoboidControl::BinaryMsg
        public virtual byte[] GenerateBinary() { return Array.Empty<byte>(); }

        /// <summary>
        /// Function used to process binary data received for this thing
        /// </summary>
        /// <param name="bytes">The binary data to process</param>
        public virtual void ProcessBinary(byte[] bytes) {
        }

    }
}