using LinearAlgebra;

namespace RoboidControl {

    public class Motor : Thing {
        public Motor(Thing parent) : base(parent) {
            this.type = Type.UncontrolledMotor;
            this.name = "Motor";
        }

        /// @brief Motor turning direction
        public enum Direction {
            Clockwise = 1,
            CounterClockwise = -1
        };
        /// @brief The forward turning direction of the motor
        public Direction direction = Direction.Clockwise;

        protected float currentTargetSpeed = 0;

        private float _targetSpeed = 0;
        /// <summary>
        /// The speed between -1 (full reverse), 0 (stop) and 1 (full forward)
        /// </summary>
        public virtual float targetSpeed {
            get => _targetSpeed;
            set {
                if (value != _targetSpeed) {
                    _targetSpeed = Float.Clamp(value, -1, 1);
                    updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
                    owner.Send(new BinaryMsg(this));
                }
            }
        }

        public override byte[] GenerateBinary() {
            byte[] data = new byte[1];
            byte ix = 0;
            data[ix++] = (byte)(this.targetSpeed * 127);
            return data;
        }
        public override void ProcessBinary(byte[] data) {
            this.targetSpeed = (float)data[0] / 127;
        }
    }

}