// 车体、运动、车队坐标系之间的转换
using System;

namespace MyParking.Shared
{
    /// <summary>
    /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
    /// 坐标系采用X向前、Y向左、逆时针为正的右手系。
    /// </summary>
    public static class FrameTransform2D
    {
        /// <summary>
        /// 将角度归一化到[-π, π)范围。
        /// </summary>
        public static double NormalizeAngle(double angleRadians)
        {
            return AngleMath.NormalizeRadians(angleRadians);
        }

        /// <summary>
        /// 计算从current到target的最短角度差。
        /// 返回正值表示逆时针旋转。
        /// </summary>
        public static double ShortestAngleDifference(
            double targetRadians,
            double currentRadians)
        {
            return AngleMath.ShortestDifferenceRadians(
                targetRadians,
                currentRadians);
        }

        /// <summary>
        /// 将源坐标系中的点变换到目标坐标系。
        /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
        /// </summary>
        public static Point2D TransformPoint(
            Pose2D sourcePoseInTarget,
            Point2D pointInSource)
        {
            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
            var sin = Math.Sin(sourcePoseInTarget.YawRadians);

            return new Point2D(
                sourcePoseInTarget.XMeters +
                cos * pointInSource.XMeters -
                sin * pointInSource.YMeters,

                sourcePoseInTarget.YMeters +
                sin * pointInSource.XMeters +
                cos * pointInSource.YMeters);
        }

        /// <summary>
        /// 将目标坐标系中的点反向变换到源坐标系。
        /// </summary>
        public static Point2D InverseTransformPoint(
            Pose2D sourcePoseInTarget,
            Point2D pointInTarget)
        {
            var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;

            var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;

            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
            var sin = Math.Sin(sourcePoseInTarget.YawRadians);

            return new Point2D(
                cos * dx + sin * dy,
                -sin * dx + cos * dy);
        }

        /// <summary>
        /// 将源坐标系中的向量旋转到目标坐标系。
        /// 向量没有位置，因此不叠加平移量。
        /// </summary>
        public static Point2D TransformVector(
            Pose2D sourcePoseInTarget,
            Point2D vectorInSource)
        {
            var cos = Math.Cos(sourcePoseInTarget.YawRadians);
            var sin = Math.Sin(sourcePoseInTarget.YawRadians);

            return new Point2D(
                cos * vectorInSource.XMeters -
                sin * vectorInSource.YMeters,

                sin * vectorInSource.XMeters +
                cos * vectorInSource.YMeters);
        }

        /// <summary>
        /// 组合两级坐标变换。
        /// parentFromMiddle表示middle在parent中的位姿；
        /// middleFromChild表示child在middle中的位姿；
        /// 返回child在parent中的位姿。
        /// </summary>
        public static Pose2D Compose(
            Pose2D parentFromMiddle,
            Pose2D middleFromChild)
        {
            var childPositionInParent = TransformPoint(
                parentFromMiddle,
                middleFromChild.Position);

            return new Pose2D(
                childPositionInParent.XMeters,
                childPositionInParent.YMeters,
                NormalizeAngle(
                    parentFromMiddle.YawRadians +
                    middleFromChild.YawRadians));
        }

        /// <summary>
        /// 对坐标变换求逆。
        /// 输入child在parent中的位姿，返回parent在child中的位姿。
        /// </summary>
        public static Pose2D Inverse(Pose2D childPoseInParent)
        {
            var cos = Math.Cos(childPoseInParent.YawRadians);
            var sin = Math.Sin(childPoseInParent.YawRadians);

            return new Pose2D(
                -cos * childPoseInParent.XMeters -
                sin * childPoseInParent.YMeters,

                sin * childPoseInParent.XMeters -
                cos * childPoseInParent.YMeters,

                NormalizeAngle(
                    -childPoseInParent.YawRadians));
        }


        /// <summary>
        /// 将源坐标系中的位姿变换到目标坐标系。
        /// </summary>
        public static Pose2D TransformPose(
            Pose2D sourcePoseInTarget,
            Pose2D poseInSource)
        {
            return Compose(sourcePoseInTarget, poseInSource);
        }

        /// <summary>
        /// 转换同一物理参考点处的速度表达坐标系。
        /// 只旋转线速度，角速度保持不变。
        /// </summary>
        public static Twist2D TransformTwistAtSamePoint(
            Pose2D sourcePoseInTarget,
            Twist2D twistInSource)
        {
            var linearVelocityInTarget = TransformVector(
                sourcePoseInTarget,
                new Point2D(
                    twistInSource.VxMetersPerSecond,
                    twistInSource.VyMetersPerSecond));

            return new Twist2D(
                linearVelocityInTarget.XMeters,
                linearVelocityInTarget.YMeters,
                twistInSource.OmegaRadiansPerSecond);
        }
    }
}