"""A simulation-only hinge experiment. Never communicates with hardware."""
import argparse, csv, json, math, time
from pathlib import Path
import mujoco

def main():
    p=argparse.ArgumentParser(description=__doc__)
    p.add_argument('--kp',type=float,default=2.0)
    p.add_argument('--kd',type=float,default=.15)
    p.add_argument('--target',type=float,default=.5,help='Target angle in radians')
    p.add_argument('--seconds',type=float,default=4)
    p.add_argument('--viewer',action='store_true')
    p.add_argument('--out',default='hinge-run.csv')
    a=p.parse_args()
    if not (0<a.seconds<=120 and 0<=a.kp<=100 and 0<=a.kd<=10 and abs(a.target)<=1.2):
        p.error('Use 0<seconds<=120, 0<=kp<=100, 0<=kd<=10, abs(target)<=1.2.')
    model=mujoco.MjModel.from_xml_path(str(Path(__file__).with_name('hinge.xml')))
    data=mujoco.MjData(model)
    data.qpos[0]=-.5
    mujoco.mj_forward(model,data)
    viewer=None
    if a.viewer:
        from mujoco import viewer as mv
        viewer=mv.launch_passive(model,data)
        viewer.cam.distance=1;viewer.cam.lookat[:]=[0,0,.2]
    rows=[]
    for i in range(round(a.seconds/model.opt.timestep)):
        start=time.perf_counter()
        if viewer and not viewer.is_running():break
        q=float(data.qpos[0]);v=float(data.qvel[0])
        requested=a.kp*(a.target-q)-a.kd*v
        applied=max(-1,min(1,requested))
        data.ctrl[0]=applied
        rows.append([float(data.time),q,v,a.target,requested,applied])
        mujoco.mj_step(model,data)
        if not all(math.isfinite(x) for x in [*data.qpos,*data.qvel]):raise RuntimeError('Nonfinite state')
        if viewer:
            viewer.sync();time.sleep(max(0,model.opt.timestep-(time.perf_counter()-start)))
    if viewer:viewer.close()
    out=Path(a.out);out.parent.mkdir(parents=True,exist_ok=True)
    with out.open('w',newline='') as f:
        w=csv.writer(f);w.writerow(['time_s','position_rad','velocity_rad_s','target_rad','requested_torque_Nm','applied_torque_Nm']);w.writerows(rows)
    print(json.dumps({'file':str(out),'steps':len(rows),'final_position_rad':float(data.qpos[0]),'target_rad':a.target,'kp':a.kp,'kd':a.kd,'torque_limit_Nm':1},indent=2))
if __name__=='__main__':main()