// teleop.ts: the source of a port of Hello Robot's stretch_body, offered under the GNU Lesser General Public License v3.0.
//
// This is the file as the page at joosep.parts/stretch is built from it. It is a work based on stretch_body 0.7.31
// (https://github.com/hello-robot/stretch_body, gamepad_teleop.py and gamepad_joints.py), Copyright 2020-2024 Hello Robot
// Inc., which is free software under the GNU Lesser General Public License v3.0. The changes are the translation into
// TypeScript and the differences listed in the header of the code below. As a work based on it, this file is offered
// under the same licence, and without any warranty. The page's script holds this code compiled into it.
//
// Licence texts, served next to this file: stretch_body-LICENSE.txt (Hello Robot's notice), LGPL-3.0.txt and GPL-3.0.txt
// (the GNU General Public License v3.0, which the LGPL adds to).
//
// Hello Robot's gamepad teleop, ported for the browser: a gamepad state in, the calls stretch_body would make out.
//
// The port follows stretch_body 0.7.31 (master @92c9db0) `GamePadTeleop.command_robot_joints`, `do_motion`,
// `manage_robot_stow` and the `gamepad_joints` command classes line for line (stretch_body/gamepad_teleop.py and
// gamepad_joints.py), for a Stretch 3 with the Dex Wrist 3 and the SG3 gripper (`eoa_wrist_dw3_tool_sg3`). It is the
// same port as the simulator's Python one: names, constants and the order of the calls are the original's, so the
// files can be laid side by side. The parity check replays stretch_body's own call logs through it, cycle by cycle.
//
//   PadState     the Joy layout of Hello Robot's gamepad driver (6 axes, 14 buttons); `makePad` clamps and quantises it as a pad
//                reports it, `fromJoy` is the driver's conversion on the way in
//   Teleop       step(state, status, t) -> Command[]; every Command is a call the original makes on the robot object,
//                with the stretch_body object path as target ('lift', 'arm', 'base', 'end_of_arm.wrist_yaw',
//                'end_of_arm.wrist_pitch', 'end_of_arm.wrist_roll', 'end_of_arm.stretch_gripper', 'head.head_pan',
//                'head.head_tilt', 'robot') and the keyword arguments under the names of the stretch_body method
//   JointStatus  what the original reads back from the robot; the firmware fills it before every cycle
//
// Calls a step can return (a_r is null where the original passes None: the joint's default acceleration):
//   lift, arm                    set_velocity(v_m, a_m)          move_to(x_m)  (the precision jog)
//   base                         set_velocity(v_m, w_r, a)       translate_by(x_m)   rotate_by(x_r)  (precision)
//   end_of_arm.wrist_yaw|wrist_pitch|wrist_roll, head.head_pan|head_tilt     set_velocity(v_r, a_r)
//   end_of_arm.stretch_gripper   move_by(delta_pct, v_r, a_r)    quick_stop()
//   robot                        stow()   home()
//
// Where the port differs from the original (as the simulator's Python port does, for the same reasons):
//   * Time is the `t` passed to `step` (simulated seconds); the original reads time.time and time.perf_counter.
//   * The stow runs in the firmware: Y held 2 s emits Command('robot', 'stow', {}) once and the teleop then ignores the
//     pad until JointStatus.stowing is false again. The command stands for stow_robot: wrist_yaw.set_motion_params to its
//     default (2.0 rad/s, 3.0 rad/s^2), then robot.stow().
//   * Beeps are not commands: `beeps` counts those requested by the last step; the end-of-stow beep is counted in the step
//     that sees the stow finish.
//   * Start homes a robot that is not homed (do_motion): the beep, then Command('robot', 'home', {}), which stands for
//     robot.home(). The original blocks in that call; here the homing runs in simulated time outside the teleop and the
//     teleop reads JointStatus.homed and .homing: until the robot is homed no joint is commanded (the original's not-homed
//     branch commands nothing either), and the step that sees the homing over counts robot.home's closing beep, also after
//     a hand-over that started the teleop anew in the middle of the homing. A status without `homed` is homed.
//   * Back (shutdown), the stick clicks and the ring button do nothing.
//   * The Y timer tests `=== null` where the original tests truthiness, so a simulated clock at exactly 0 works.

export const AXIS_NAMES = ['left_stick_x', 'left_stick_y', 'right_stick_x', 'right_stick_y', 'left_trigger', 'right_trigger'] as const
export const BUTTON_NAMES = ['left_stick_click', 'right_stick_click', 'a', 'b', 'x', 'y', 'lb', 'rb', 'back', 'start',
  'dpad_down', 'dpad_up', 'dpad_left', 'dpad_right'] as const
export type ButtonName = typeof BUTTON_NAMES[number]
export const AXIS: Record<typeof AXIS_NAMES[number], number> = Object.fromEntries(AXIS_NAMES.map((n, i) => [n, i])) as never
export const BUTTON: Record<ButtonName, number> = Object.fromEntries(BUTTON_NAMES.map((n, i) => [n, i])) as never
export const N_AXES = 6
export const N_BUTTONS = 14

export const STICK_NORM = 32768.0
export const TRIGGER_NORM = 255.0

export const STOW_POSE: Record<string, number> = { head_pan: 0.0, head_tilt: 0.0, lift: 0.3, arm: 0.0, wrist_pitch: -0.62, wrist_roll: 0.0,
  wrist_yaw: 3.0, stretch_gripper: 0.0 }

// robot_params_SE3.py (RP): base motion max vel 0.3, default vel 0.12, max accel 0.3 (619-634); lift max accel 0.3,
// default accel 0.2 (910-939); arm default vel 0.14, default accel 0.14, max accel 0.4 (583-612); stretch_gripper motion
// max accel 20 and vel 20; the Dynamixel joints' default vel and slow and max accelerations (189-293, 668-739).
export const BASE_MAX_VEL = 0.3, BASE_DEFAULT_VEL = 0.12, BASE_MAX_ACCEL = 0.3
export const LIFT_MAX_ACCEL = 0.3, LIFT_DEFAULT_ACCEL = 0.2
export const ARM_DEFAULT_VEL = 0.14, ARM_DEFAULT_ACCEL = 0.14, ARM_MAX_ACCEL = 0.4
export const GRIPPER_MAX_ACCEL = 20, GRIPPER_MAX_VEL = 20
export const DXL_PARAMS: Record<string, { default_vel: number; slow_accel: number; max_accel: number }> = {
  wrist_yaw: { default_vel: 2.0, slow_accel: 1.5, max_accel: 10 },
  wrist_pitch: { default_vel: 2.0, slow_accel: 4.0, max_accel: 10.0 },
  wrist_roll: { default_vel: 2.0, slow_accel: 4.0, max_accel: 12 },
  head_pan: { default_vel: 3.0, slow_accel: 4.0, max_accel: 14.0 },
  head_tilt: { default_vel: 3.0, slow_accel: 4.0, max_accel: 14.0 },
}

/** One call on the stretch_body robot object, as GamePadTeleop and the gamepad_joints classes make it. */
export interface Command {
  target: string
  method: string
  kwargs: Record<string, number | null>
}

/** The pad as `sensor_msgs/Joy` carries it.
 *  axes     0 left stick x (right +), 1 left stick y (up +), 2 right stick x, 3 right stick y, 4 left trigger (0 to 1),
 *           5 right trigger
 *  buttons  0 left stick click, 1 right stick click, 2 A, 3 B, 4 X, 5 Y, 6 LB, 7 RB, 8 Back, 9 Start, 10 D-pad down,
 *           11 D-pad up, 12 D-pad left, 13 D-pad right (0 or 1) */
export interface PadState {
  readonly axes: readonly number[]
  readonly buttons: readonly number[]
}

export const NEUTRAL: PadState = Object.freeze({ axes: Object.freeze([0, 0, 0, 0, 0, 0]), buttons: Object.freeze(new Array(14).fill(0)) })

function clampAxis(i: number, v: number, quantise: boolean): number {
  v = Number(v)
  if (v !== v) return 0.0
  if (i >= 4) {
    v = Math.min(1.0, Math.max(0.0, v))
    return quantise ? Math.min(255, Math.max(0, Math.floor(v * TRIGGER_NORM + 0.5))) / TRIGGER_NORM : v
  }
  v = Math.min(1.0, Math.max(-1.0, v))
  if (!quantise) return v
  const raw = Math.floor(v * STICK_NORM + 0.5)
  const [lo, hi] = i % 2 === 0 ? [-32768, 32767] : [-32767, 32768]
  return Math.min(hi, Math.max(lo, raw)) / STICK_NORM
}

/** GamepadState.make: clamps to the ranges and, by default, rounds to the device lattice (sticks in steps of 1/32768,
 *  triggers of 1/255; a stick reaches 32767/32768 at the right and at the bottom, 1.0 at the left and at the top, as the
 *  Xbox 360 pad reports it). A button counts when truthy. */
export function makePad(axes?: ArrayLike<number> | null, buttons?: ArrayLike<number | boolean> | null, quantise = true): PadState {
  const a = axes ?? [0, 0, 0, 0, 0, 0]
  const b = buttons ?? new Array(14).fill(0)
  if (a.length !== N_AXES || b.length !== N_BUTTONS) throw new Error('a pad state has 6 axes and 14 buttons')
  return { axes: Array.from({ length: N_AXES }, (_, i) => clampAxis(i, a[i] as number, quantise)),
           buttons: Array.from({ length: N_BUTTONS }, (_, i) => (b[i] ? 1 : 0)) }
}

/** stretch_ros2's gamepad_conversion on the way into the driver (Hello Robot's stretch_ros2): an axis outside its range
 *  reads 0 and a button counts only at exactly 1. */
export function fromJoy(axes: ArrayLike<number>, buttons: ArrayLike<number>): PadState {
  if (axes.length !== N_AXES || buttons.length !== N_BUTTONS) throw new Error('a Joy pad state has 6 axes and 14 buttons')
  return { axes: Array.from({ length: N_AXES }, (_, i) => { const v = axes[i]; return (i >= 4 ? 0.0 : -1.0) <= v && v <= 1.0 ? v : 0.0 }),
           buttons: Array.from({ length: N_BUTTONS }, (_, i) => (buttons[i] === 1 ? 1 : 0)) }
}

export function isNeutral(s: PadState | null): boolean {
  if (!s) return true
  for (const v of s.axes) if (v !== 0) return false
  for (const v of s.buttons) if (v) return false
  return true
}

/** The dict GamePadController.get_state returns, which the original's command_robot_joints reads. */
interface ControllerState {
  left_stick_x: number; left_stick_y: number; right_stick_x: number; right_stick_y: number
  left_stick_button_pressed: boolean; right_stick_button_pressed: boolean
  bottom_button_pressed: boolean; top_button_pressed: boolean; left_button_pressed: boolean; right_button_pressed: boolean
  left_shoulder_button_pressed: boolean; right_shoulder_button_pressed: boolean
  select_button_pressed: boolean; start_button_pressed: boolean
  left_trigger_pulled: number; right_trigger_pulled: number
  bottom_pad_pressed: boolean; top_pad_pressed: boolean; left_pad_pressed: boolean; right_pad_pressed: boolean
}

function toControllerState(s: PadState): ControllerState {
  const a = s.axes, b = s.buttons
  return {
    left_stick_x: a[0], left_stick_y: a[1], right_stick_x: a[2], right_stick_y: a[3],
    left_stick_button_pressed: !!b[0], right_stick_button_pressed: !!b[1],
    bottom_button_pressed: !!b[2], top_button_pressed: !!b[5], left_button_pressed: !!b[4], right_button_pressed: !!b[3],
    left_shoulder_button_pressed: !!b[6], right_shoulder_button_pressed: !!b[7],
    select_button_pressed: !!b[8], start_button_pressed: !!b[9],
    left_trigger_pulled: a[4], right_trigger_pulled: a[5],
    bottom_pad_pressed: !!b[10], top_pad_pressed: !!b[11], left_pad_pressed: !!b[12], right_pad_pressed: !!b[13],
  }
}

const ZERO_CONTROLLER_STATE = toControllerState(NEUTRAL)

/** What the teleop reads back from the robot, in stretch_body's units: lift height and arm extension in metres (arm 0 is
 *  retracted), speeds in m/s. `stowing` is true from the step after the firmware receives the stow command until its stow
 *  sequence has finished. `homed` is robot.is_homed() (robot.py 538-547); left out, the robot is homed. `homing` is true from
 *  the step after robot.home() was called until the homing has ended. */
export interface JointStatus {
  lift_pos: number
  lift_vel: number
  arm_pos: number
  arm_vel: number
  stowing: boolean
  homed?: boolean
  homing?: boolean
}

export const REST_STATUS: JointStatus = { lift_pos: 0.3, lift_vel: 0.0, arm_pos: 0.0, arm_vel: 0.0, stowing: false }

export function stowedStatus(s: JointStatus): boolean {
  return s.arm_pos < STOW_POSE.arm + 0.1 && s.lift_pos < STOW_POSE.lift + 0.05
}

export function mapToRange(value: number, newMin: number, newMax: number): number {
  value = Math.max(0, Math.min(1, value))
  return (value - 0) * (newMax - newMin) / (1 - 0) + newMin
}

export function toParabolaTransform(x: number): number {
  if (x < 0) return -1 * (Math.abs(x) * Math.abs(x))
  return x * x
}

// ---------- the slice of the stretch_body robot object the command classes touch ----------
class Robot {
  sink: Command[] = []
  t = 0.0
  readonly baseStatus = { x: 0.0, y: 0.0, theta: 0.0 }
  lift = { pos: STOW_POSE.lift, vel: 0.0 }
  arm = { pos: STOW_POSE.arm, vel: 0.0 }

  load(s: JointStatus, t: number): void {
    this.t = t
    this.lift = { pos: s.lift_pos, vel: s.lift_vel }
    this.arm = { pos: s.arm_pos, vel: s.arm_vel }
  }

  take(): Command[] {
    const out = this.sink
    this.sink = []
    return out
  }

  joint(name: 'lift' | 'arm'): { pos: number; vel: number } {
    return name === 'lift' ? this.lift : this.arm
  }

  getStowPos(joint: string): number {
    return STOW_POSE[joint]
  }

  // calls
  prismaticSetVelocity(target: 'lift' | 'arm', v_m: number, a_m: number): void { this.sink.push({ target, method: 'set_velocity', kwargs: { v_m, a_m } }) }
  prismaticMoveTo(target: 'lift' | 'arm', x_m: number): void { this.sink.push({ target, method: 'move_to', kwargs: { x_m } }) }
  baseSetVelocity(v_m: number, w_r: number, a: number): void { this.sink.push({ target: 'base', method: 'set_velocity', kwargs: { v_m, w_r, a } }) }
  baseTranslateBy(x_m: number): void { this.sink.push({ target: 'base', method: 'translate_by', kwargs: { x_m } }) }
  baseRotateBy(x_r: number): void { this.sink.push({ target: 'base', method: 'rotate_by', kwargs: { x_r } }) }
  chainSetVelocity(chain: 'end_of_arm' | 'head', joint: string, v_r: number, a_r: number | null): void {
    this.sink.push({ target: `${chain}.${joint}`, method: 'set_velocity', kwargs: { v_r, a_r } })
  }
  gripperMoveBy(delta_pct: number, v_r: number, a_r: number): void {
    this.sink.push({ target: 'end_of_arm.stretch_gripper', method: 'move_by', kwargs: { delta_pct, v_r, a_r } })
  }
  gripperQuickStop(): void { this.sink.push({ target: 'end_of_arm.stretch_gripper', method: 'quick_stop', kwargs: {} }) }
  stow(): void { this.sink.push({ target: 'robot', method: 'stow', kwargs: {} }) }
  home(): void { this.sink.push({ target: 'robot', method: 'home', kwargs: {} }) }
}

// ---------- gamepad_joints ----------
class CommandBase {
  dead_zone = 0.0001
  _prev_set_vel_ts: number
  max_linear_vel = BASE_MAX_VEL
  max_rotation_vel = 1.90241
  normal_linear_vel = BASE_DEFAULT_VEL
  normal_rotation_vel = this.max_rotation_vel * 0.4
  arm_extended_rotation_vel = this.max_rotation_vel * 0.2
  precision_mode = false
  fast_base_mode = false
  acc = BASE_MAX_ACCEL
  start_pos: number[] | null = null
  start_theta: number | null = null
  target_position: number[] | null = null
  target_theta: number | null = null
  precision_max_linear_vel = 0.05
  precision_max_rot_vel = 0.3

  constructor(t0 = 0.0) {
    this._prev_set_vel_ts = t0
  }

  command_stick_to_motion(x: number, y: number, robot: Robot): void {
    if (Math.abs(x) < this.dead_zone) x = 0
    if (Math.abs(y) < this.dead_zone) y = 0

    x = toParabolaTransform(x)

    if (!this.precision_mode) {
      this.start_pos = null
      this.start_theta = null
      const [v_m, w_r] = this._process_stick_to_vel(x, y, robot)
      robot.baseSetVelocity(v_m, w_r, this.acc)
      this._prev_set_vel_ts = robot.t
    } else {
      if (this.start_pos === null) {
        this.start_pos = [robot.baseStatus.x, robot.baseStatus.y]
        this.start_theta = robot.baseStatus.theta
        this.target_position = this.start_pos
        this.target_theta = this.start_theta
      }
      let yv = mapToRange(Math.abs(y), 0, this.precision_max_linear_vel)
      let xv = mapToRange(Math.abs(x), 0, this.precision_max_rot_vel)
      if (x < 0) xv = -1 * xv
      if (y < 0) yv = -1 * yv
      if (Math.abs(x) > Math.abs(y)) this._step_precision_rotate(xv, robot)
      else this._step_precision_translate(yv, robot)
      this._prev_set_vel_ts = robot.t
    }
  }

  stop_motion(robot: Robot): void {
    robot.baseSetVelocity(0, 0, this.acc)
  }

  is_fastbase_safe(robot: Robot): boolean {
    const arm = robot.arm.pos < (robot.getStowPos('arm') + 0.1)
    const lift = robot.lift.pos < (robot.getStowPos('lift') + 0.05)
    return arm && lift
  }

  _process_stick_to_vel(x: number, y: number, robot: Robot): [number, number] {
    let max_linear_vel = this.normal_linear_vel
    let max_rotation_vel = this.normal_rotation_vel
    if (this.fast_base_mode && this.is_fastbase_safe(robot)) {
      max_linear_vel = this.max_linear_vel
      max_rotation_vel = this.max_rotation_vel
    }
    if (!(robot.arm.pos < (robot.getStowPos('arm') + 0.1))) max_rotation_vel = this.arm_extended_rotation_vel
    let v_m = mapToRange(Math.abs(y), 0, max_linear_vel)
    if (y < 0) v_m = -1 * v_m
    x = -1 * x
    let w_r = mapToRange(Math.abs(x), 0, max_rotation_vel)
    if (x < 0) w_r = -1 * w_r
    return [v_m, w_r]
  }

  _step_precision_rotate(xv: number, robot: Robot): void {
    const elapsed_time = robot.t - this._prev_set_vel_ts
    const desired_theta_change = xv * elapsed_time
    if (Math.abs(xv) >= 0) robot.baseRotateBy(-1 * desired_theta_change)
  }

  _step_precision_translate(yv: number, robot: Robot): void {
    const elapsed_time = robot.t - this._prev_set_vel_ts
    const desired_position_change = yv * elapsed_time
    if (Math.abs(yv) >= 0) robot.baseTranslateBy(desired_position_change)
  }
}

class CommandPrismatic {
  dead_zone = 0.0001
  _prev_set_vel_ts: number | null = null
  precision_mode = false
  start_pos: number | null = null
  target_position: number | null = null
  precision_max_vel = 0.04
  stopped_for_precision = false

  constructor(readonly name: 'lift' | 'arm', readonly max_linear_vel: number, readonly acc: number, readonly button_accel: number,
              readonly precision_kp: number) {}

  command_stick_to_motion(x: number, robot: Robot): void {
    const joint = robot.joint(this.name)
    if (Math.abs(x) < this.dead_zone) x = 0
    if (!this.precision_mode) {
      this.start_pos = null
      this.stopped_for_precision = false
      const v_m = this._process_stick_to_vel(x)
      robot.prismaticSetVelocity(this.name, v_m, this.acc)
      this._prev_set_vel_ts = robot.t
    } else {
      if (Math.abs(joint.vel) > 0.001 && !this.stopped_for_precision) {
        this.stop_motion(robot)
      } else {
        this.stopped_for_precision = true
        if (this.start_pos === null) {
          this.start_pos = joint.pos
          this.target_position = this.start_pos
        }
      }
      const r = this.precision_max_vel
      let xv = mapToRange(Math.abs(x), 0, r)
      if (x < 0) xv = -1 * xv
      if (this.stopped_for_precision) this._step_precision_move(xv, robot)
    }
  }

  stop_motion(robot: Robot): void {
    robot.prismaticSetVelocity(this.name, 0, this.acc)
  }

  _process_stick_to_vel(x: number): number {
    let v_m = mapToRange(Math.abs(x), 0, this.max_linear_vel)
    if (x < 0) v_m = -1 * v_m
    return v_m
  }

  _step_precision_move(xv: number, robot: Robot): void {
    const joint = robot.joint(this.name)
    const current_position = joint.pos
    // the original subtracts None here (a TypeError) if precision is the very first mode; the mode flags lag a cycle, so
    // the first cycle always runs the standard branch and sets the clock
    const elapsed_time = robot.t - (this._prev_set_vel_ts as number)
    const desired_position_change = xv * elapsed_time
    this.target_position = (this.target_position as number) + desired_position_change
    const position_error = this.target_position - current_position
    const x_des = this.precision_kp * position_error
    robot.prismaticMoveTo(this.name, (this.start_pos as number) + x_des)
    this._prev_set_vel_ts = robot.t
  }
}

class CommandDxlJoint {
  readonly params: { default_vel: number; slow_accel: number; max_accel: number }
  dead_zone = 0.001
  _prev_set_vel_ts: number | null = null
  readonly max_vel: number
  precision_mode = false
  readonly acc: number | null
  precision_scale_down = 0.05

  constructor(readonly name: string, max_vel: number | null, acc_type: 'slow' | null) {
    this.params = DXL_PARAMS[name]
    this.max_vel = max_vel ? max_vel : this.params.default_vel
    this.acc = acc_type ? this.params.slow_accel : null
  }

  private setVelocity(robot: Robot, v: number, acc: number | null): void {
    if (this.name.includes('wrist')) robot.chainSetVelocity('end_of_arm', this.name, v, acc)
    if (this.name.includes('head')) robot.chainSetVelocity('head', this.name, v, acc)
  }

  command_stick_to_motion(x: number, robot: Robot): void {
    if (Math.abs(x) < this.dead_zone) x = 0
    let acc = this.acc
    if (x === 0) acc = this.params.max_accel
    let v = this._process_stick_to_vel(x)
    if (this.precision_mode) v = v * this.precision_scale_down
    this.setVelocity(robot, v, acc)
    this._prev_set_vel_ts = robot.t
  }

  command_button_to_motion(direction: number, robot: Robot): void {
    let vel = this.max_vel
    if (this.precision_mode) vel = vel * this.precision_scale_down
    if (direction === 1) this.setVelocity(robot, vel, this.acc)
    else if (direction === -1) this.setVelocity(robot, -1 * vel, this.acc)
    this._prev_set_vel_ts = robot.t
  }

  stop_motion(robot: Robot): void {
    this.setVelocity(robot, 0, this.params.max_accel)
  }

  _process_stick_to_vel(x: number): number {
    x = -1 * x
    let v = mapToRange(Math.abs(x), 0, this.max_vel)
    if (x < 0) v = -1 * v
    return v
  }
}

class CommandGripperPosition {
  readonly name = 'stretch_gripper'
  gripper_rotate_pct = 60.0
  gripper_accel = GRIPPER_MAX_ACCEL
  gripper_vel = GRIPPER_MAX_VEL
  precision_mode = false
  stop_reqd = false

  open_gripper(robot: Robot): void {
    let pct = this.gripper_rotate_pct
    if (this.precision_mode) pct = this.gripper_rotate_pct * 0.05
    robot.gripperMoveBy(pct, this.gripper_vel, this.gripper_accel)
    this.stop_reqd = true
  }

  close_gripper(robot: Robot): void {
    let pct = this.gripper_rotate_pct
    if (this.precision_mode) pct = this.gripper_rotate_pct * 0.05
    robot.gripperMoveBy(-pct, this.gripper_vel, this.gripper_accel)
    this.stop_reqd = true
  }

  stop_gripper(robot: Robot): void {
    if (this.stop_reqd) {
      robot.gripperQuickStop()
      this.stop_reqd = false
    }
  }
}

/** GamePadTeleop for one robot. `step` is `do_motion` plus the push: one call per teleop cycle. */
export class Teleop {
  precision_mode = false
  fast_base_mode = false
  controller_state: ControllerState = ZERO_CONTROLLER_STATE
  _i = 0
  beeps = 0
  dexwrist_ctrl_switch = true
  skip_x_button = false
  time_since_dexwrist_switch: number | null = null
  _last_fn_btn_press: number | null = null
  currently_stowing = false
  /** robot.home() was called and the robot is not homed yet (the original is blocked in that call meanwhile) */
  currently_homing = false
  private robot = new Robot()
  base_command!: CommandBase
  lift_command!: CommandPrismatic
  arm_command!: CommandPrismatic
  wirst_yaw_command!: CommandDxlJoint
  head_pan_command!: CommandDxlJoint
  head_tilt_command!: CommandDxlJoint
  gripper!: CommandGripperPosition
  wrist_pitch_command!: CommandDxlJoint
  wrist_roll_command!: CommandDxlJoint

  constructor(t0 = 0.0) {
    this.reset(t0)
  }

  reset(t0 = 0.0): void {
    this.precision_mode = false
    this.fast_base_mode = false
    this.robot = new Robot()
    this.controller_state = ZERO_CONTROLLER_STATE
    this._i = 0
    this.beeps = 0
    this.base_command = new CommandBase(t0)
    this.lift_command = new CommandPrismatic('lift', 0.13, LIFT_MAX_ACCEL, LIFT_DEFAULT_ACCEL, 0.5)
    this.arm_command = new CommandPrismatic('arm', ARM_DEFAULT_VEL, ARM_MAX_ACCEL, ARM_DEFAULT_ACCEL, 0.6)
    this.wirst_yaw_command = new CommandDxlJoint('wrist_yaw', 1.5, 'slow')
    this.head_pan_command = new CommandDxlJoint('head_pan', null, null)
    this.head_tilt_command = new CommandDxlJoint('head_tilt', null, 'slow')
    this.gripper = new CommandGripperPosition()
    this.wrist_pitch_command = new CommandDxlJoint('wrist_pitch', 1, 'slow')
    this.wrist_roll_command = new CommandDxlJoint('wrist_roll', null, null)
    this.dexwrist_ctrl_switch = true
    this.skip_x_button = false
    this.time_since_dexwrist_switch = null
    this._last_fn_btn_press = null
    this.currently_stowing = false
    this.currently_homing = false
  }

  /** the D-pad drives the head (X toggled it there) */
  get dpadOnHead(): boolean {
    return !this.dexwrist_ctrl_switch
  }

  /** lift and arm have stopped and are being jogged by position */
  get precisionLive(): boolean {
    return this.lift_command.stopped_for_precision && this.arm_command.stopped_for_precision
  }

  /** RT is held and the arm and lift are stowed enough for the fast base (as of the last step) */
  get fastLive(): boolean {
    return this.fast_base_mode && this.base_command.is_fastbase_safe(this.robot)
  }

  get stowing(): boolean {
    return this.currently_stowing
  }

  get homing(): boolean {
    return this.currently_homing
  }

  /** Run one teleop cycle at simulated time `t` and return the calls it makes, in the original's order. `state` null is
   *  the original's no-input case (no dongle and no Joy message): every joint is told to stop. A robot that is not homed
   *  gets no call but robot.home(), on Start (do_motion). */
  step(state: PadState | null, status: JointStatus, t: number): Command[] {
    const robot = this.robot
    robot.load(status, t)
    this.beeps = 0
    if (this.currently_stowing && !status.stowing) {
      this.currently_stowing = false
      this.beeps += 1
    }
    const homed = status.homed !== false
    if (status.homing) this.currently_homing = true
    if (this.currently_homing && !status.homing) {
      // robot.home() has returned (robot.py 649 beeps whether or not every joint found its stop)
      this.currently_homing = false
      this.beeps += 1
    }
    this._i = this._i + 1
    this._update_state(state)
    this._update_modes()
    if (!homed && this.controller_state.start_button_pressed && !this.currently_homing) {
      this.do_single_beep()
      this.home_robot(robot)
    }
    if (homed && !this.currently_stowing) {
      if (state !== null) this.command_robot_joints(robot)
      else this._safety_stop(robot)
    }
    return robot.take()
  }

  private home_robot(robot: Robot): void {
    this.currently_homing = true
    robot.home()
  }

  private _update_state(state: PadState | null): void {
    this.controller_state = state === null ? ZERO_CONTROLLER_STATE : toControllerState(state)
  }

  private _update_modes(): void {
    this.arm_command.precision_mode = this.precision_mode
    this.lift_command.precision_mode = this.precision_mode
    this.base_command.precision_mode = this.precision_mode
    this.base_command.fast_base_mode = this.fast_base_mode
    this.wirst_yaw_command.precision_mode = this.precision_mode
    this.gripper.precision_mode = this.precision_mode
    this.wrist_pitch_command.precision_mode = this.precision_mode
    this.wrist_roll_command.precision_mode = this.precision_mode
    this.head_pan_command.precision_mode = this.precision_mode
    this.head_tilt_command.precision_mode = this.precision_mode
  }

  private command_robot_joints(robot: Robot): void {
    const cs = this.controller_state
    this.precision_mode = cs.left_trigger_pulled > 0.9
    this.fast_base_mode = cs.right_trigger_pulled > 0.9

    const dxl_zero_vel_set_division_factor = 3

    if (cs.right_shoulder_button_pressed) this.wirst_yaw_command.command_button_to_motion(-1, robot)
    else if (cs.left_shoulder_button_pressed) this.wirst_yaw_command.command_button_to_motion(1, robot)
    else if (this._i % dxl_zero_vel_set_division_factor === 0) this.wirst_yaw_command.stop_motion(robot)
    if (!this.dexwrist_ctrl_switch) {
      if (cs.top_pad_pressed) this.head_tilt_command.command_button_to_motion(1, robot)
      else if (cs.bottom_pad_pressed) this.head_tilt_command.command_button_to_motion(-1, robot)
      else if (this._i % dxl_zero_vel_set_division_factor === 0) this.head_tilt_command.stop_motion(robot)

      if (cs.left_pad_pressed) this.head_pan_command.command_button_to_motion(1, robot)
      else if (cs.right_pad_pressed) this.head_pan_command.command_button_to_motion(-1, robot)
      else if (this._i % dxl_zero_vel_set_division_factor === 0) this.head_pan_command.stop_motion(robot)

      this.wrist_pitch_command.stop_motion(robot)
      this.wrist_roll_command.stop_motion(robot)
    } else {
      if (cs.top_pad_pressed) this.wrist_pitch_command.command_button_to_motion(1, robot)
      else if (cs.bottom_pad_pressed) this.wrist_pitch_command.command_button_to_motion(-1, robot)
      else if (this._i % dxl_zero_vel_set_division_factor === 0) this.wrist_pitch_command.stop_motion(robot)

      if (cs.left_pad_pressed) this.wrist_roll_command.command_button_to_motion(-1, robot)
      else if (cs.right_pad_pressed) this.wrist_roll_command.command_button_to_motion(1, robot)
      else if (this._i % dxl_zero_vel_set_division_factor === 0) this.wrist_roll_command.stop_motion(robot)

      this.head_pan_command.stop_motion(robot)
      this.head_tilt_command.stop_motion(robot)
    }

    this.arm_command.command_stick_to_motion(cs.right_stick_x, robot)
    this.lift_command.command_stick_to_motion(cs.right_stick_y, robot)
    this.base_command.command_stick_to_motion(cs.left_stick_x, cs.left_stick_y, robot)

    if (cs.right_button_pressed) this.gripper.open_gripper(robot)
    else if (cs.bottom_button_pressed) this.gripper.close_gripper(robot)
    else this.gripper.stop_gripper(robot)

    if (cs.left_button_pressed && !this.skip_x_button) {
      this.dexwrist_ctrl_switch = !this.dexwrist_ctrl_switch
      this.do_single_beep()
      this.skip_x_button = true
      this.time_since_dexwrist_switch = robot.t
    }
    if (this.skip_x_button) {
      if ((robot.t - (this.time_since_dexwrist_switch as number)) > 0.5) this.skip_x_button = false
    }

    this.manage_robot_stow(robot, cs.top_button_pressed)
  }

  private do_single_beep(): void {
    this.beeps += 1
  }

  private manage_robot_stow(robot: Robot, button_state: boolean): void {
    if (button_state) {
      if (this._last_fn_btn_press === null) this._last_fn_btn_press = robot.t
      if (robot.t - this._last_fn_btn_press >= 2 && !this.currently_stowing) {
        this.do_single_beep()
        this._last_fn_btn_press = null
        this.stow_robot(robot)
      }
    } else {
      this._last_fn_btn_press = null
    }
  }

  private stow_robot(robot: Robot): void {
    this.currently_stowing = true
    robot.stow()
  }

  private _safety_stop(robot: Robot): void {
    this.wirst_yaw_command.command_stick_to_motion(0, robot)
    this.arm_command.command_stick_to_motion(0, robot)
    this.lift_command.command_stick_to_motion(0, robot)
    this.head_pan_command.command_stick_to_motion(0, robot)
    this.head_tilt_command.command_stick_to_motion(0, robot)
    this.base_command.command_stick_to_motion(0, 0, robot)
    this.wrist_pitch_command.command_stick_to_motion(0, robot)
    this.wrist_roll_command.command_stick_to_motion(0, robot)
  }
}