RoboidControl-csharp/Thing.cs

using System.Collections.Generic;
using Passer.LinearAlgebra;

namespace Passer.Control.Core {

    /// <summary>
    /// A thing is the basic building block
    /// </summary>
    public class Thing {

        #region Types

        public enum Type {
            Undeterment,
            // Sensor
            Switch,
            DistanceSensor,
            DirectionalSensor,
            TemperatureSensor,
            // Motor
            ControlledMotor,
            UncontrolledMotor,
            Servo,
            // Other
            Roboid,
            Humanoid,
            ExternalSensor
        };

        #endregion Types

        #region Properties

        public Participant participant;

        public delegate void ChangeHandler();
        public delegate void SphericalHandler(Spherical v);

        public byte networkId;
        public byte id;

        public event ChangeHandler OnParentChanged;
        private Thing _parent;
        public Thing parent {
            get => _parent;
            set {
                if (_parent == value)
                    return;

                if (value == null) {
                    _parent.RemoveChild(this);
                    _parent = null;
                }
                else {
                    value.AddChild(this);
                    OnParentChanged?.Invoke();
                }
            }
        }

        public void AddChild(Thing child) {
            if (children.Find(thing => thing == child) != null)
                return;
            child._parent = this;
            children.Add(child);
        }
        public void RemoveChild(Thing child) {
            children.Remove(child);
        }

        [System.NonSerialized]
        public List<Thing> children = new List<Thing>();
        public byte type;

        public event ChangeHandler OnNameChanged;
        private string _name;
        public string name {
            get => _name;
            set {
                if (_name != value) {
                    _name = value;
                    OnNameChanged?.Invoke();
                }
            }
        }

        public string modelUrl;

        public byte poseUpdated = 0x00;
        public event ChangeHandler OnPositionChanged;
        private Spherical _position;
        public Spherical position {
            get { return _position; }
            set {
                if (_position != value) {
                    _position = value;
                    OnPositionChanged?.Invoke();
                }
            }
        }

        public event ChangeHandler OnOrientationChanged;
        private SwingTwist _orientation;
        public SwingTwist orientation {
            get { return _orientation; }
            set {
                if (_orientation != value) {
                    _orientation = value;
                    OnOrientationChanged?.Invoke();
                }
            }
        }

        public event SphericalHandler OnLinearVelocityChanged;
        private Spherical _linearVelocity;
        public Spherical linearVelocity {
            get => _linearVelocity;
            set {
                if (_linearVelocity != value) {
                    _linearVelocity = value;
                    OnLinearVelocityChanged?.Invoke(_linearVelocity);
                }
            }
        }
        public Spherical angularVelocity;

        #endregion Properties

        #region Init

        public virtual void Init(bool invokeEvent = true) {
            Thing.Add(this, invokeEvent);
        }

        public Thing(bool initialize = true) {
            if (initialize) {
                //this.Init();
                Thing.Add(this);
            }
        }
        public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) {
            this.participant = client;
            this.id = thingId;
            this.type = thingType;
            this.networkId = networkId;
            this.Init();
            Thing.Add(this);
        }

        #endregion Init

        #region Update

        public virtual void Update(float currentTime) {
            // should recurse over children...
        }

        public virtual void ProcessBytes(byte[] bytes) {
            //if (sensor != null)
            //    sensor.ProcessBytes(bytes);
        }

        #endregion Update

        // Experimental

        // public float stressLevel = 0;

        // protected delegate void ReceptorFunc(Sensor sensor);
        // protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) {
        //     sensor.Signaller += (sensor => Receptor(receptor, sensor));
        // }
        // protected void Receptor(ReceptorFunc receptor, Sensor sensor) {
        //     if (sensor.signalStrength <= stressLevel)
        //         return;

        //     receptor(sensor);
        // }

        //---------- All Things

        private static readonly List<Thing> allThings = new();

        public delegate void ThingHandler(Thing t);
        public static event ThingHandler OnNewThing;

        public static void Add(Thing thing, bool invokeEvent = true) {
            Thing foundThing = Get(thing.networkId, thing.id);

            if (foundThing == null) {
                if (thing.id == 0)
                    thing.id = (byte)(allThings.Count + 1);
                allThings.Add(thing);
                if (invokeEvent)
                    OnNewThing?.Invoke(thing);
                UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
            }
        }

        public static Thing Get(byte networkId, byte thingId) {
            Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId));
            return thing;
        }

        private static bool IsThing(Thing thing, byte networkId, byte thingId) {
            if (thing == null)
                return false;
            return (thing.networkId == networkId) && (thing.id == thingId);
        }

        public static void Remove(byte networkId, byte thingId) {
            allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId);
        }

        public static Thing[] GetAllThings() {
            return allThings.ToArray();
        }

        public static void UpdateAll(float currentTime) {
            foreach (Thing thing in allThings) {
                if (thing.parent == null) // update only root things
                    thing.Update(currentTime);
            }
        }

    }
}