# walker_env.py - same 4-legged MuJoCo robot as our CMA-ES vs PPO guide, with the FIXED score
# pip install mujoco cma numpy gymnasium stable-baselines3
import numpy as np, mujoco, gymnasium as gym

LEG = """<body pos="{x} {y} 0"><joint name="hip{i}" type="hinge" axis="0 1 0" range="-60 60"/>
  <geom type="capsule" fromto="0 0 0 0 0 -0.12" size="0.02"/>
  <body pos="0 0 -0.12"><joint name="knee{i}" type="hinge" axis="0 1 0" range="-70 70"/>
    <geom type="capsule" fromto="0 0 0 0 0 -0.12" size="0.018"/></body></body>"""
CORNERS = [(0.15, 0.1), (0.15, -0.1), (-0.15, 0.1), (-0.15, -0.1)]
XML = f"""<mujoco><option timestep="0.004"/>
<default><joint damping="0.5" armature="0.01"/><geom friction="1 0.005 0.0001" density="1000"/>
  <position kp="8" ctrlrange="-1.2 1.2" ctrllimited="true"/></default>
<worldbody><light pos="0 0 3" dir="0 0 -1"/><geom type="plane" size="20 20 0.1" rgba=".5 .5 .5 1"/>
 <body name="torso" pos="0 0 0.3"><freejoint/><geom type="box" size="0.18 0.12 0.03" density="300"/>
  {''.join(LEG.format(x=x, y=y, i=i) for i, (x, y) in enumerate(CORNERS))}
 </body></worldbody>
<actuator>{''.join(f'<position joint="hip{i}"/><position joint="knee{i}"/>' for i in range(4))}</actuator>
</mujoco>"""

MODEL = mujoco.MjModel.from_xml_string(XML)
N = MODEL.nu            # 8 motors
SECONDS = 5.0           # every trial lasts at most 5 simulated seconds
STEPS = int(round(SECONDS / MODEL.opt.timestep))   # 1250 physics steps per trial
MAX_TILT_DEG = 30.0     # NEW: torso may tilt at most 30 degrees from level
COS_MAX = float(np.cos(np.radians(MAX_TILT_DEG)))

def tilt_deg(d):
    """Angle between the torso's 'up' axis and world vertical."""
    return float(np.degrees(np.arccos(np.clip(d.xmat[1][8], -1, 1))))

def tipped(d):
    return d.xmat[1][8] < COS_MAX

def old_score(d):
    """v1 score (the loophole): metres forward, -1 m only if the top faces DOWN at the end."""
    return d.qpos[0] if d.xmat[1][8] > 0 else d.qpos[0] - 1.0

# Two fixed rules. In both, the trial STOPS the moment the torso tilts past 30 degrees.
#   "stop":    score = metres walked forward (+x) when it stops (or at 5 s).
#   "forfeit": a run that tips before 5 s scores -1 m, whatever distance it had covered.
RULES = ("stop", "forfeit")

def final_score(x, tipped_early, rule):
    if rule == "forfeit" and tipped_early:
        return -1.0
    return x

class WalkerEnv(gym.Env):
    """Gymnasium wrapper for PPO. Policy acts every 5 physics steps (50 Hz), max 250 actions.
    Reward per action = forward progress; the episode ends when tipped. With rule "forfeit" the last reward
    cancels everything walked and adds -1, so the episode return always equals final_score()."""
    FRAME_SKIP = 5
    def __init__(self, rule="forfeit"):
        assert rule in RULES
        self.rule = rule
        self.model = MODEL
        self.data = mujoco.MjData(MODEL)
        self.action_space = gym.spaces.Box(-1.0, 1.0, (N,), np.float32)
        self.observation_space = gym.spaces.Box(-np.inf, np.inf, (2 * N + 8,), np.float64)
    def _obs(self):
        d = self.data
        return np.concatenate([d.qpos[7:], d.qvel[6:], d.qpos[3:7], d.qvel[0:3], d.qpos[2:3]])
    def reset(self, seed=None, options=None):
        super().reset(seed=seed)
        mujoco.mj_resetData(self.model, self.data)
        self.t = 0
        return self._obs(), {}
    def step(self, action):
        d = self.data
        x0 = d.qpos[0]
        d.ctrl[:] = 1.2 * np.clip(action, -1, 1)
        for _ in range(self.FRAME_SKIP):
            mujoco.mj_step(self.model, d)
            if tipped(d):
                break
        self.t += 1
        reward = d.qpos[0] - x0
        terminated = tipped(d)
        if terminated and self.rule == "forfeit":
            reward += -1.0 - d.qpos[0]      # return so far becomes -1 m
        truncated = (not terminated) and self.t * self.FRAME_SKIP >= STEPS
        return self._obs(), reward, terminated, truncated, {}
