stack-chan

import PWM from 'embedded:io/pwm'
import {
  type MotionCompletion,
  type MotionDurationSeconds,
  type MotionResultCallback,
  motionDurationSecondsToMilliseconds,
} from 'motion-controller'
import type { Maybe, Rotation } from 'stackchan-util'
import Timer from 'timer'

const INTERVAL = 16.5

class Servo {
  #pwm: PWM
  #min: number
  #max: number

  constructor(param: { pin: number; min?: number; max?: number }) {
    const TAREGET_PERIOD = 18
    this.#pwm = new PWM({
      pin: param.pin,
      hz: Math.floor(1000 / TAREGET_PERIOD),
    })

    const min = param.min ?? 500
    const max = param.max ?? 2400
    const range = 2 ** this.#pwm.resolution - 1
    this.#min = Math.ceil((min / 1000 / TAREGET_PERIOD) * range)
    this.#max = Math.floor((max / 1000 / TAREGET_PERIOD) * range)
  }

  write(degrees: number): void {
    const value = Math.round(this.#min + (degrees / 180) * (this.#max - this.#min))
    this.#pwm.write(value)
  }
}

function easeInOutSine(ratio) {
  return -(Math.cos(Math.PI * ratio) - 1) / 2
}

type PWMServoDriverProps = {
  pwmPan?: number
  pwmTilt?: number
  offsetPan?: number
  offsetTilt?: number
}
export class PWMServoDriver {
  _pan
  _tilt
  _panRef
  _tiltRef
  _driveHandler
  _range
  _offsetPan
  _offsetTilt
  #rotation: Rotation = { y: 0, p: 0, r: 0 }
  #rotationResult: Maybe<Rotation> = { success: true, value: this.#rotation }

  constructor(param: PWMServoDriverProps = {}) {
    const pwmPan = param.pwmPan ?? 5
    const pwmTilt = param.pwmTilt ?? 2
    this._pan = new Servo({
      pin: pwmPan,
      min: 500,
      max: 2400,
    })
    this._tilt = new Servo({
      pin: pwmTilt,
      min: 500,
      max: 2400,
    })
    this._panRef = {
      current: 0,
    }
    this._tiltRef = {
      current: 0,
    }
    this._offsetPan = param.offsetPan ?? 0
    this._offsetTilt = param.offsetTilt ?? 0
  }

  setTorque(_torque: boolean, callback?: MotionCompletion): void {
    // We cannot change torque via Stack-chan board for now.
    // torque keeps on while 5V supplied.
    callback?.()
  }

  applyRotation(rotation: Rotation, time: MotionDurationSeconds = 0.5, callback?: MotionCompletion): void {
    trace(`applyPose: ${JSON.stringify(rotation)}\n`)
    if (this._driveHandler != null) {
      trace('clearing\n')
      Timer.clear(this._driveHandler)
      this._driveHandler = null
    }
    const startPan = this._panRef.current
    const startTilt = this._tiltRef.current
    const diffPan = (rotation.y * 180) / Math.PI - startPan
    const diffTilt = (rotation.p * 180) / Math.PI - startTilt
    let cnt = 0
    const numFrame = motionDurationSecondsToMilliseconds(time) / INTERVAL
    this._driveHandler = Timer.repeat(() => {
      if (cnt >= numFrame) {
        Timer.clear(this._driveHandler)
        this._driveHandler = null
      }
      const ratio = easeInOutSine(cnt / numFrame)
      const p = startPan + diffPan * ratio
      const t = startTilt + diffTilt * ratio
      const writingPan = Math.max(Math.min(p + 90, 170), 10) + this._offsetPan
      const writingTilt = Math.max(Math.min(t + 90, 100), 65) + this._offsetTilt
      this._pan.write(writingPan)
      this._tilt.write(writingTilt)
      this._panRef.current = p
      this._tiltRef.current = t
      cnt += 1
    }, INTERVAL)
    callback?.()
  }

  getRotation(callback: MotionResultCallback<Maybe<Rotation>>): void {
    this.#rotation.y = (Math.PI * this._panRef.current) / 180
    this.#rotation.p = (Math.PI * this._tiltRef.current) / 180
    this.#rotation.r = 0.0
    callback(this.#rotationResult)
  }
}