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

def run(genes, rule):
    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)
    d = mujoco.MjData(MODEL)
    for _ in range(STEPS):
        d.ctrl[:] = amp * np.sin(2 * np.pi * freq * d.time + phase) + offset
        mujoco.mj_step(MODEL, 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
    rule = sys.argv[3] if len(sys.argv) > 3 else "forfeit"
    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()
        dist = [run(g, rule) for g in pop]
        es.tell(pop, [-x for x in dist])
        champ = run(es.result.xbest, rule)
        steps = gen * len(pop) * STEPS    # budget counted as full 5 s trials (same as v1)
        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_{rule}_seed{seed}.npy", es.result.xbest)
    json.dump(log, open(f"cmaes_{rule}_seed{seed}.json", "w"))
