# cmaes_walker.py - CMA-ES evolves an open-loop sine gait (25 numbers), forfeit rule, with or without domain randomization
import sys, time, json, numpy as np, mujoco, cma
from walker_env import MODEL, N, STEPS, tipped, final_score, random_world

def run(genes, rule="forfeit", world=None):
    """One 5 s trial. world=None -> the default robot (friction 1.0, level start); else (model, data) from random_world()."""
    amp, phase, offset = genes[:N], genes[N:2*N], genes[2*N:3*N]
    freq = np.clip(1.5 + genes[-1], 0.5, 3.0)
    m, d = world if world is not None else (MODEL, mujoco.MjData(MODEL))
    for _ in range(STEPS):
        d.ctrl[:] = amp * np.sin(2 * np.pi * freq * d.time + phase) + offset
        mujoco.mj_step(m, d)
        if tipped(d):                 # trial ends when the torso tilts past 30 degrees
            return final_score(d.qpos[0], True, rule)
    return final_score(d.qpos[0], False, rule)

if __name__ == "__main__":
    seed = int(sys.argv[1]) if len(sys.argv) > 1 else 1
    gens = int(sys.argv[2]) if len(sys.argv) > 2 else 100
    mode = sys.argv[3] if len(sys.argv) > 3 else "random"      # "fixed" = last week's training, "random" = domain randomization
    # ("fixedmean" = control run: fixed world, but keep the search mean like "random" does)
    rule = "forfeit"
    rng = np.random.default_rng(seed)
    es = cma.CMAEvolutionStrategy(np.zeros(3 * N + 1), 0.5, {"popsize": 24, "seed": seed, "verbose": -9})
    t0, log = time.process_time(), []
    for gen in range(1, gens + 1):
        pop = es.ask()
        # random mode: every candidate gets ONE trial in its own random world (same budget as fixed mode)
        dist = [run(g, rule, random_world(rng) if mode == "random" else None) for g in pop]
        es.tell(pop, [-x for x in dist])
        # with noisy scores the luckiest candidate (xbest) is not the best gait -> keep the search mean instead
        champ_genes = es.result.xbest if mode == "fixed" else es.result.xfavorite
        champ = run(champ_genes, rule)            # logged on the default robot
        steps = gen * len(pop) * STEPS
        log.append(dict(physics_steps=steps, trials=gen * len(pop), seconds=round(time.process_time() - t0, 1), distance=champ))
        if gen % 10 == 0:
            print(f"gen {gen:3d}  trials {gen*len(pop):5d}  champion {champ:5.2f} m  cpu {time.process_time()-t0:6.1f} s", flush=True)
    np.save(f"cmaes_{mode}_seed{seed}.npy", champ_genes)
    json.dump(log, open(f"cmaes_{mode}_seed{seed}.json", "w"))
