// 纯数据层：只描述坐标、速度和命令
// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。

namespace MyParking.Shared
{
    /// <summary>
    /// 二维坐标点，X、Y单位均为米。
    /// </summary>
    public readonly struct Point2D
    {
        public Point2D(double xMeters, double yMeters)
        {
            XMeters = xMeters;
            YMeters = yMeters;
        }

        public double XMeters { get; }

        public double YMeters { get; }

        public static Point2D Zero => new Point2D(0.0, 0.0);
    }

    /// <summary>
    /// 二维局部坐标系在父坐标系中的位姿。
    /// 位置单位为米，朝向单位为弧度，逆时针为正。
    /// 具体父子关系由变量名称说明，例如RadarPoseInBody。
    /// </summary>
    public readonly struct Pose2D
    {
        public Pose2D(
            double xMeters,
            double yMeters,
            double yawRadians)
        {
            XMeters = xMeters;
            YMeters = yMeters;
            YawRadians = yawRadians;
        }

        public double XMeters { get; }

        public double YMeters { get; }

        public double YawRadians { get; }

        public Point2D Position =>
            new Point2D(XMeters, YMeters);

        public static Pose2D Identity =>
            new Pose2D(0.0, 0.0, 0.0);
    }

    /// <summary>
    /// 二维刚体速度。
    /// 线速度单位为m/s，角速度单位为rad/s。
    /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
    /// </summary>
    public readonly struct Twist2D
    {
        public Twist2D(
            double vxMetersPerSecond,
            double vyMetersPerSecond,
            double omegaRadiansPerSecond)
        {
            VxMetersPerSecond = vxMetersPerSecond;
            VyMetersPerSecond = vyMetersPerSecond;
            OmegaRadiansPerSecond = omegaRadiansPerSecond;
        }

        public double VxMetersPerSecond { get; }

        public double VyMetersPerSecond { get; }

        public double OmegaRadiansPerSecond { get; }

        public static Twist2D Zero =>
            new Twist2D(0.0, 0.0, 0.0);
    }

    /// <summary>
    /// 发送给单辆车的车体坐标系速度命令。
    /// </summary>
    public readonly struct ChassisCommand
    {
        public ChassisCommand(
            int vehicleId,
            Twist2D bodyTwist)
        {
            VehicleId = vehicleId;
            BodyTwist = bodyTwist;
        }

        public int VehicleId { get; }

        /// <summary>
        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
        /// </summary>
        public Twist2D BodyTwist { get; }

        /// <summary>
        /// 创建指定车辆的停止命令。
        /// </summary>
        public static ChassisCommand Stop(int vehicleId)
        {
            return new ChassisCommand(
                vehicleId,
                Twist2D.Zero);
        }
    }

    /// <summary>
    /// 单辆车的车体坐标系在车队坐标系中的位姿。
    /// </summary>
    public readonly struct VehicleLayout
    {
        public VehicleLayout(
            int vehicleId,
            Pose2D poseInFleet)
        {
            VehicleId = vehicleId;
            PoseInFleet = poseInFleet;
        }

        public int VehicleId { get; }

        public Pose2D PoseInFleet { get; }
    }

    /// <summary>
    /// 车队整体运动命令，速度分量均在车队坐标系中表达。
    /// </summary>
    public readonly struct FleetMotionCommand
    {
        public FleetMotionCommand(
            Point2D referencePointInFleet,
            Twist2D twistAtReferencePoint)
        {
            ReferencePointInFleet = referencePointInFleet;
            TwistAtReferencePoint = twistAtReferencePoint;
        }

        /// <summary>
        /// 速度命令对应的参考点，也可作为自定义旋转中心。
        /// </summary>
        public Point2D ReferencePointInFleet { get; }

        /// <summary>
        /// 参考点处的车队速度。
        /// </summary>
        public Twist2D TwistAtReferencePoint { get; }

        /// <summary>
        /// 创建绕指定中心原地旋转的车队命令。
        /// </summary>
        public static FleetMotionCommand RotateAround(
            Point2D rotationCenterInFleet,
            double omegaRadiansPerSecond)
        {
            return new FleetMotionCommand(
                rotationCenterInFleet,
                new Twist2D(
                    0.0,
                    0.0,
                    omegaRadiansPerSecond));
        }

        /// <summary>
        /// 创建车队停止命令。
        /// </summary>
        public static FleetMotionCommand Stop()
        {
            return new FleetMotionCommand(
                Point2D.Zero,
                Twist2D.Zero);
        }
    }


}