using Passer.LinearAlgebra;

namespace RoboidControl {

    /// <summary>
    /// Message to communicate the pose of the thing
    /// </summary>
    /// The pose is in local space relative to the parent. If there is not parent (the thing is a root thing), the pose will be in world space.
    public class PoseMsg : IMessage {
        /// <summary>
        /// The message ID
        /// </summary>
        public const byte Id = 0x10;
        /// <summary>
        /// The length of the message
        /// </summary>
        public const byte length = 4 + 4 + 4;
        /// <summary>
        /// The network ID of the thing
        /// </summary>
        public byte networkId;
        /// <summary>
        /// The ID of the thing
        /// </summary>
        public byte thingId;

        /// <summary>
        /// Bit pattern stating which pose components are available
        /// </summary>
        public byte poseType;
        /// <summary>
        /// Bit pattern for a pose with position
        /// </summary>
        public const byte Pose_Position = 0x01;
        /// <summary>
        /// Bit pattern for a pose with orientation
        /// </summary>
        public const byte Pose_Orientation = 0x02;
        /// <summary>
        /// Bit pattern for a pose with linear velocity
        /// </summary>
        public const byte Pose_LinearVelocity = 0x04;
        /// <summary>
        /// Bit pattern for a pose with angular velocity
        /// </summary>
        public const byte Pose_AngularVelocity = 0x08;

        /// <summary>
        /// The position of the thing in local space in meters
        /// </summary>
        public Spherical position = Spherical.zero;
        /// <summary>
        /// The orientation of the thing in local space
        /// </summary>
        public SwingTwist orientation = SwingTwist.zero;
        /// <summary>
        /// The linear velocity of the thing in local space in meters per second
        /// </summary>
        public Spherical linearVelocity = Spherical.zero;
        /// <summary>
        /// The angular velocity of the thing in local space
        /// </summary>
        public Spherical angularVelocity = Spherical.zero;

        /// <summary>
        /// Create a new message for sending
        /// </summary>
        /// <param name="networkId">The network ID of the thing</param>
        /// <param name="thingId">The ID of the thing</param>
        /// <param name="position">The position of the thing in local space in meters</param>
        /// <param name="orientation">The orientation of the thing in local space</param>
        public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation, Spherical linearVelocity = null, Spherical angularVelocity = null) {
            this.networkId = networkId;
            this.thingId = thingId;

            this.poseType = 0;
            if (this.position != null)
                this.poseType |= Pose_Position;
            if (this.orientation != null)
                this.poseType |= Pose_Orientation;
            if (this.linearVelocity != null)
                this.poseType |= Pose_LinearVelocity;
            if (this.angularVelocity != null)
                this.poseType |= Pose_AngularVelocity;

            this.position = position;
            this.orientation = orientation;
            this.linearVelocity = linearVelocity;
            this.angularVelocity = angularVelocity;
        }
        /// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
        public PoseMsg(byte[] buffer) : base(buffer) {
            byte ix = 1;
            this.networkId = buffer[ix++];
            this.thingId = buffer[ix++];

            this.poseType = buffer[ix++];
            this.position = null;
            this.orientation = null;
            this.linearVelocity = null;
            this.angularVelocity = null;
            if ((this.poseType & Pose_Position) != 0)
                this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
            if ((this.poseType & Pose_Orientation) != 0)
                this.orientation = SwingTwist.FromQuat32(LowLevelMessages.ReceiveQuat32(buffer, ref ix));
            if ((this.poseType & Pose_LinearVelocity) != 0)
                this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
            if ((this.poseType & Pose_AngularVelocity) != 0)
                this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
        }

        /// @copydoc Passer::RoboidControl::IMessage::Serialize
        public override byte Serialize(ref byte[] buffer) {
            byte ix = 0;
            buffer[ix++] = PoseMsg.Id;
            buffer[ix++] = this.networkId;
            buffer[ix++] = this.thingId;
            buffer[ix++] = this.poseType;

            if ((poseType & Pose_Position) != 0)
                LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
            if ((poseType & Pose_Orientation) != 0)
                LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
            if ((poseType & Pose_LinearVelocity) != 0)
                LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
            if ((poseType & Pose_AngularVelocity) != 0)
                LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
            return ix;
        }

    }

}