# walker_env.py - the SAME 4-legged MuJoCo robot for both CMA-ES and PPO
# 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 5 simulated seconds
STEPS = int(round(SECONDS / MODEL.opt.timestep))   # 1250 physics steps per trial

def score(d):
    """Shared fitness: metres walked forward (+x), minus 1 m if the robot ends upside down."""
    upright = d.xmat[1][8] > 0
    return d.qpos[0] if upright else d.qpos[0] - 1.0

class WalkerEnv(gym.Env):
    """Gymnasium wrapper for PPO. Policy acts every 5 physics steps (50 Hz), 250 actions per trial.
    Reward per action = forward progress, so the episode return equals score() exactly."""
    FRAME_SKIP = 5
    def __init__(self):
        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
        # joint angles, joint speeds, torso orientation (quat), torso velocity, torso height
        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)
        self.t += 1
        reward = d.qpos[0] - x0
        truncated = self.t * self.FRAME_SKIP >= STEPS
        if truncated and d.xmat[1][8] <= 0:
            reward -= 1.0
        return self._obs(), reward, False, truncated, {}
