namespace RoboidControl {

    /// @brief A motor with speed control
    /// It uses a feedback loop from an encoder to regulate the speed
    /// The speed is measured in revolutions per second.
    public class ControlledMotor : Motor {
        public ControlledMotor(Thing parent, RelativeEncoder encoder) : base(parent) {
            this.encoder = encoder;
         }
        // Upgrade an existing motor with an encoder
        public ControlledMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) {
            this.motor = motor;
            this.encoder = encoder;
         }

        readonly RelativeEncoder encoder;
        readonly Motor motor;

        public override float targetSpeed {
            get {
                if (this.motor != null)
                    return this.motor.targetSpeed;
                else
                    return base.targetSpeed;
            }
            set {
                if (this.motor != null)
                    this.motor.targetSpeed = value;
                else
                    base.targetSpeed = value;
            }
        }

        private float _targetAngularSpeed; // In degrees per second
        public virtual float targetAngularSpeed {
            get => _targetAngularSpeed;
            set {
                if (value != this._targetAngularSpeed) {
                    this._targetAngularSpeed = value;
                    updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
                    owner.Send(new BinaryMsg(this));
                }
            }
        }
        /// @brief Get the actual speed from the encoder
        /// @return The speed in angle per second
        public virtual float actualAngularSpee {
            get => encoder.angularSpeed;
        }

        float pidP = 0.1F;
        float pidD = 0.0F;
        //float pidI = 0.0F;

        public override void Update(ulong currentTimeMs, bool recurse = false) {
            float actualSpeed = this.encoder.angularSpeed;
            // Simplified rotation direction, shouldbe improved
            // This goes wrong when the target speed is inverted and the motor axcle is still turning in the old direction
            if (this.targetAngularSpeed < 0)
                actualSpeed = -actualSpeed;

            float deltaTime = (currentTimeMs - this.lastUpdateTime) / 1000.0f;

            float error = this.targetAngularSpeed - actualSpeed;
            float errorChange = (error - lastError) * deltaTime;

            float delta = error * pidP + errorChange * pidD;

            // Update the motor speed
            if (motor != null)
                motor.targetSpeed += delta;
            else
                base.targetSpeed += delta;
            this.lastUpdateTime = currentTimeMs;
        }

        ulong lastUpdateTime = 0;
        float lastError = 0;

        // enum Direction { Forward = 1, Reverse = -1 };
        // Direction rotationDirection;
    };

} // namespace RoboidControl