using UnityEngine;

namespace HexapodSim
{
    // Which Unity axis the robot's forward (+X) maps to. The old scene marks
    // the legs on Unity -Z as "invertForLeftLeg", which makes -X forward.
    public enum ForwardAxis { NegativeX, PositiveX }

    // Conversions between the firmware's body frame (right-handed, mm:
    // +X forward, +Y left, +Z up) and Unity (left-handed, Y up). Everything
    // physical in the simulation is computed in the robot frame and only
    // converted here, in one place.
    public static class RobotFrame
    {
        public static Vector3 ToUnity(Vector3 r, ForwardAxis f) =>
            f == ForwardAxis.NegativeX ? new Vector3(-r.x, r.z, -r.y) : new Vector3(r.x, r.z, r.y);

        public static Vector3 ToRobot(Vector3 u, ForwardAxis f) =>
            f == ForwardAxis.NegativeX ? new Vector3(-u.x, -u.z, u.y) : new Vector3(u.x, u.z, u.y);

        // A rotation expressed in the robot frame, as the equivalent Unity rotation.
        // (The mapping is a reflection, so this is M * R * M^-1, built from two axes.)
        public static Quaternion ToUnity(Quaternion robotRot, ForwardAxis f)
        {
            Vector3 fwd = ToUnity(robotRot * ToRobot(Vector3.forward, f), f);
            Vector3 up = ToUnity(robotRot * ToRobot(Vector3.up, f), f);
            return Quaternion.LookRotation(fwd, up);
        }

        public static float WrapDeg(float d)
        {
            d %= 360f;
            if (d > 180f) d -= 360f;
            if (d < -180f) d += 360f;
            return d;
        }
    }
}