import display
import machine
import math

proc initHal*(stepdeg: float): Hal =
  return Hal(
    steprads: stepdeg / 360 * math.PI,
    current: MachineState(
      lpos: 0, rpos: 0, ldir: CCW, rdir: CCW, lenable: false, renable: false))

proc enable*(hal: var Hal, left: bool, right: bool) =
  hal.current.lenable = left
  hal.current.renable = right
  displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))

proc direction*(hal: var Hal, left: Dir, right: Dir) =
  hal.current.ldir = left
  hal.current.rdir = right
  displayPush(DisplayRequest(kind: UPDATE, newstate: hal.current))

proc sendMotion*(hal: var Hal, m: Motion): MachineState =
  if m.ldir != hal.current.ldir or m.rdir != hal.current.rdir:
    hal.direction(m.ldir, m.rdir)
  let ltime = float(m.ln) / float(m.lhz)
  let rtime = float(m.rn) / float(m.rhz)
  let time = max(ltime, rtime)
  let frames = int(ceil(time * frameRate))
  let s = hal.current

  for frame in 0..frames:
    let t = frame / frameRate
    let lticks = if s.lenable: min(m.ln, uint32(t * float(m.lhz))) else: 0
    let rticks = if s.renable: min(m.rn, uint32(t * float(m.rhz))) else: 0
    let ldist = (if s.ldir == CCW: 1.0 else: -1.0) * hal.steprads * float(lticks)
    let rdist = (if s.rdir == CCW: 1.0 else: -1.0) * hal.steprads * float(rticks)

    let step = MachineState(
      lpos: s.lpos + ldist, rpos: s.rpos + rdist,
      ldir: s.ldir, rdir: s.rdir,
      lenable: s.lenable, renable: s.renable)
    displayPush(DisplayRequest(kind: UPDATE, newstate: step))
    hal.current = step
  return hal.current