using System;
using System.Collections.Generic;
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;
            // Other
            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

        /// <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)
                InvokeNewThing(this);
        }

        /// <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;
        }

        // /// <summary>
        // /// Create a new thing for the given participant
        // /// </summary>
        // /// <param name="owner">The participant owning the thing</param>
        // /// <param name="networkId">The network ID of the thing</param>
        // /// <param name="thingId">The ID of the thing</param>
        // /// <param name="thingType">The type of thing</param>
        // public Thing(Participant owner, byte networkId, byte thingId, byte thingType = 0) {
        //     this.owner = owner;
        //     this.id = thingId;
        //     this.type = thingType;
        //     this.networkId = networkId;
        //     // Console.Write($"New thing added to {owner}");
        //     this.owner.Add(this);
        //     InvokeNewThing(this);
        // }

        /// <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;
                    OnNameChanged?.Invoke();
                }
            }
        }
        /// <summary>
        /// Event which is triggered when the name changes
        /// </summary>
        public event ChangeHandler OnNameChanged = delegate { };
        public bool nameChanged = false;

        /// <summary>
        /// An URL pointing to the location where a model of the thing can be found
        /// </summary>
        public string modelUrl = "";

#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;
            }
        }


        /// <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;
                    //OnPositionChanged?.Invoke();
                }
            }
        }
        /// <summary>
        /// Event triggered when the pose has changed
        /// </summary>
        public event ChangeHandler OnPoseChanged = delegate { };
        /// <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;
                    //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;
                    OnLinearVelocityChanged?.Invoke(_linearVelocity);
                }
            }
        }
        /// <summary>
        /// Event triggered when the linear velocity has changed
        /// </summary>
        public event SphericalHandler OnLinearVelocityChanged = delegate { };
        /// <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;
                    OnAngularVelocityChanged?.Invoke(_angularVelocity);
                }
            }
        }
        /// <summary>
        /// Event triggered when the angular velocity has changed
        /// </summary>
        public event SphericalHandler OnAngularVelocityChanged = delegate { };
        /// <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) {
            if (this.positionUpdated || this.orientationUpdated)
                OnPoseChanged?.Invoke();
            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 delegate void ChangeHandler();
        public delegate void SphericalHandler(Spherical v);
        public delegate void ThingHandler(Thing t);

        /// <summary>
        /// Event triggered when a new thing has been created
        /// </summary>
        public static event ThingHandler OnNewThing = delegate { };
        /// <summary>
        /// Trigger the creation for the given thing
        /// </summary>
        /// <param name="thing">The created thing</param>
        public static void InvokeNewThing(Thing thing) {
            OnNewThing?.Invoke(thing);
        }

        #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) {
        }

    }
}