using LinearAlgebra;

namespace RoboidControl {

    /// @brief A thing which can move itself using a differential drive system
    ///
    /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
    public class DifferentialDrive : Thing {
        /*
        /// <summary>
        /// Create a differential drive without communication abilities
        /// </summary
        /// <param name="invokeEvent">Invoke a OnNewThing event when the thing has been created</param>
        public DifferentialDrive() : base(Type.DifferentialDrive) { }
        
        /// <summary>
        /// Create a differential drive for a participant
        /// </summary>
        /// <param name="owner">The owning participant</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 DifferentialDrive(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.DifferentialDrive, thingId, invokeEvent) {
            // Motor leftWheel = new(this) {
            //     name = "Left Wheel"
            // };
            // Motor rightWheel = new(this) {
            //     name = "Right Wheel"
            // };
            // SetMotors(leftWheel, rightWheel);
            // sendBinary = true;
            // owner.Send(new BinaryMsg(owner.networkId, this));
            // this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id));
        }
        */

        /// <summary>
        /// Create a new child differential drive
        /// </summary>
        /// <param name="parent">The parent thing</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 DifferentialDrive(Thing parent) : base(Type.DifferentialDrive, parent) { }

        /// @brief Configures the dimensions of the drive
        /// @param wheelDiameter The diameter of the wheels in meters
        /// @param wheelSeparation The distance between the wheels in meters
        ///
        /// These values are used to compute the desired wheel speed from the set
        /// linear and angular velocity.
        /// @sa SetLinearVelocity SetAngularVelocity
        public void SetDriveDimensions(float wheelDiameter, float wheelSeparation = 0) {
            this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2;
            this.rpsToMs = wheelDiameter * Angle.pi;

            if (wheelSeparation > 0) {
                wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation;
                float distance = wheelSeparation / 2;
                if (this.leftWheel != null)
                    this.leftWheel.position = new Spherical(distance, Direction.left);
                if (this.rightWheel != null)
                    this.rightWheel.position = new Spherical(distance, Direction.right);
            }
        }

        /// @brief Congures the motors for the wheels
        /// @param leftWheel The motor for the left wheel
        /// @param rightWheel The motor for the right wheel
        public void SetMotors(Motor leftWheel, Motor rightWheel) {
            // float distance = this.wheelSeparation / 2;

            this.leftWheel = leftWheel;
            this.leftWheel.parent = this;
            // if (this.leftWheel != null)
            //     this.leftWheel.position = new Spherical(distance, Direction.left);

            this.rightWheel = rightWheel;
            this.rightWheel.parent= this;
            // if (this.rightWheel != null)
            //     this.rightWheel.position = new Spherical(distance, Direction.right);

            owner.Send(new BinaryMsg(owner.networkId, this));
            this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
        }

        /// @brief Directly specify the speeds of the motors
        /// @param speedLeft The speed of the left wheel in degrees per second.
        /// Positive moves the robot in the forward direction.
        /// @param speedRight The speed of the right wheel in degrees per second.
        /// Positive moves the robot in the forward direction.
        public void SetWheelVelocity(float speedLeft, float speedRight) {
            if (this.leftWheel != null) {
                this.leftWheel.targetSpeed = speedLeft;
                //this.leftWheel.angularVelocity = new Spherical(speedLeft, Direction.left);
            }
            if (this.rightWheel != null) {
                this.rightWheel.targetSpeed = speedRight;
                //this.rightWheel.angularVelocity = new Spherical(speedRight, Direction.right);
            }
        }
        /// @brief Directly specify the speeds of the motors
        /// @param speedLeft The speed of the left wheel in degrees per second.
        /// Positive moves the robot in the forward direction.
        /// @param speedRight The speed of the right wheel in degrees per second.
        /// Positive moves the robot in the forward direction.
        public void SetWheelAngularVelocity(float angularSpeedLeft, float angularSpeedRight) {
            // This only works when the motor is a motor with encoder
            if (this.leftWheel is ControlledMotor leftMotor)
                leftMotor.targetAngularSpeed = angularSpeedLeft;
            if (this.rightWheel is ControlledMotor rightMotor)
                rightMotor.targetAngularSpeed = angularSpeedRight;
        }

        /// @copydoc RoboidControl::Thing::Update(unsigned long)
        public override void Update(ulong currentMs, bool recursive = true) {
            if (this.linearVelocityUpdated) {
                // this assumes forward velocity only....
                float linearVelocity = this.linearVelocity.distance;

                Spherical angularVelocity = this.angularVelocity;
                float angularSpeed = angularVelocity.distance * Angle.Deg2Rad;
                // Determine the rotation direction
                if (angularVelocity.direction.horizontal < 0)
                    angularSpeed = -angularSpeed;

                // This assumes that the left wheel position has Direction.left
                // and the right wheel position has Direction.Right...
                float speedLeft = (linearVelocity + angularSpeed * this.leftWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg;
                float speedRight = (linearVelocity - angularSpeed * this.rightWheel.position.distance) / this.wheelRadius * Angle.Rad2Deg;
                this.SetWheelVelocity(speedLeft, speedRight);
            }
        }

        /// @brief The radius of a wheel in meters
        private float _wheelRadius = 0.0f;
        public float wheelRadius { get => _wheelRadius; }
        /// @brief The distance between the wheels in meters
        // private float _wheelSeparation = 0.0f;
        // public float wheelSeparation { get => _wheelSeparation; }

        /// @brief Convert revolutions per second to meters per second
        protected float rpsToMs = 1.0f;

        /// @brief The left wheel
        public Motor leftWheel = null;
        /// @brief The right wheel
        public Motor rightWheel = null;

        bool sendBinary = false;
        public override byte[] GenerateBinary() {
            if (!sendBinary)
                return System.Array.Empty<byte>();

            byte[] data = new byte[4];
            byte ix = 0;
            data[ix++] = leftWheel.id;
            data[ix++] = rightWheel.id;
            LowLevelMessages.SendFloat16(data, ref ix, wheelRadius);
            //LowLevelMessages.SendFloat16(data, ref ix, wheelSeparation);
            sendBinary = false;
            return data;
        }

        public override void ProcessBinary(byte[] data) {
            byte ix = 0;
            byte leftWheelId = data[ix++];
            this.leftWheel = this.owner.Get(leftWheelId) as Motor;
            byte rightWheelId = data[ix++];
            this.rightWheel = this.owner.Get(rightWheelId) as Motor;
            this._wheelRadius = LowLevelMessages.ReceiveFloat16(data, ref ix);
            //this._wheelSeparation = LowLevelMessages.ReceiveFloat16(data, ref ix);
            this.updateQueue.Enqueue(new CoreEvent(BinaryMsg.Id));
        }
    };

}  // namespace RoboidControl