import { ConnectionManager, TaskRegistry } from '@things-factory/integration-base'
import { access } from '@things-factory/utils'
import { getTrackingWorkspace } from './get-tracking-workspace'

const debug = require('debug')('aidoop:robotics:ws-get-object-state')

async function TrackingWorkspaceGetObjectState(step, { logger, data, domain }) {
  var {
    connection,
    params: { accessor }
  } = step

  var object = ConnectionManager.getConnectionInstanceByName(domain, connection) || {}
  if (!object) {
    throw new Error(`no connection : ${connection}`)
  }

  var endpoint = accessor ? access(accessor, data) : {}
  logger.info(`accessor: ${accessor}, endpoint: ${endpoint}`)

  var workspace = getTrackingWorkspace(domain)
  var { engine } = workspace
  var { trackingStorage } = engine

  var objectState = trackingStorage.getObjectState(endpoint)
  // test if pose is available or return null
  if (
    isNaN(objectState.pose.x) ||
    isNaN(objectState.pose.y) ||
    isNaN(objectState.pose.z) ||
    isNaN(objectState.pose.u) ||
    isNaN(objectState.pose.v) ||
    isNaN(objectState.pose.w)
  )
    objectState = null

  return {
    data: objectState
  }
}

TrackingWorkspaceGetObjectState.parameterSpec = [
  {
    type: 'scenario-step-input',
    label: 'accessor',
    name: 'accessor'
  }
]

TrackingWorkspaceGetObjectState.help = 'robotics/task/tracking-workspace-get-object-state'

TaskRegistry.registerTaskHandler('tracking-ws-get-object-state', TrackingWorkspaceGetObjectState)
