import * as THREE from 'three'
import {
  AmrErrorCode,
  AmrMsg, AmrMsg10010, AmrMsg10050, AmrMsg10060, AmrMsg10110, AmrMsg10120, AmrMsg20010,
  AmrMsg20011, AmrMsg20020, AmrMsg20050, AmrMsg20060, AmrMsg20100,
  AmrMsg20147,
  AmrMsg20148, AmrMsg20149, AmrMsg20150, AmrMsg20250,
  type CEventId,
  type COperationType,
  type CPickMode,
  type CTaskMode, CurBatteryData,
  type LogicDirection,
  TaskCompletedData,
  TaskModeChangeData,
  TaskStatusChangeData,
  TaskTypeChangeData
} from '@/core/manager/amr/AmrMessageDefine'
import {worldModel} from '@/core/manager/WorldModel'
import Viewport from '@/core/engine/Viewport'
import {Euler} from 'three/src/math/Euler'
import gsap from 'gsap'
import {MeshWrap} from "@/core/manager/InstanceMeshManager";
import {getRenderer} from "@/core/manager/ModuleManager";


type CStepTaskType = 'MOVE' | 'MOVE_BACKWARD' | 'ROTATION' | 'LOAD' | 'UNLOAD' | 'CHARGE'

export interface StepTask {
  SeqNo: number;
  StepTaskType: CStepTaskType;
  OperationType: 0 | 1 | 2 | 3 | 4 | 5 | 135 | 136;
  PickMode: 0 | 1 | 2 | 3 | 4 | 5 | 6;
  X: number;
  Y: number;
  Speed: number;
  EndDirection: 0 | 1 | 2 | 3 | 15;
  ChargeLocation: number;
  GoodsSlotHeight: number;
  position: THREE.Vector3;
  isCompleted: boolean;
}

export default class PtrObject extends THREE.Object3D {

  public item: ItemJson
  public readonly viewport: Viewport

  public currentLogicX: number = -1
  public currentLogicY: number = -1
  public currentDirection: LogicDirection = 15
  public currentOrientation: number = 0
  public sendMessageQueue: AmrMsg<any>[] = []

  public Battery: number = 100

  public vehicleId: number

  public clock = new THREE.Clock()

  override AGVModel = ''
  override AGVFnModel = ''
  override rotationSpeed: number = 0
  override showForkSpeed: number = 0
  override upForkSpeed: number = 0

  private boxBody: any = null
  private __toPos: THREE.Vector3 = null

  private currentStepTaskList: StepTask[] = []
  private runningStepTask: StepTask = null
  public runningStepTaskList: StepTask[] = []

  // 移动动画
  public travelAnimation: core.Tween = null
  // 旋转动画
  public rotationAnimation: core.Tween = null
  // 动作动画
  public actionAnimation: core.Tween = null

  // 心跳间隔 UInt32 单位: s
  private heartBeatInterval: number = 0
  // 小车所有上报消息重试间隔(未收到应答消息时重发消息)  UInt32 单位: s
  private mqRetryInterval: number = 3

  private __TaskMode: CTaskMode = 0
  private __OperationType: COperationType = 0
  private __TaskStatus: CEventId = 0
  private __PickMode: CPickMode = 0

  private agvStatusVo: AgvStatusVo;

  get TaskMode(): CTaskMode {
    return this.__TaskMode
  }

  set TaskMode(value: CTaskMode) {
    if (this.__TaskMode != value) {
      const info = new TaskModeChangeData(this.__TaskMode, value)
      const msg = new AmrMsg20011(this.vehicleId, info)
      msg.TaskMode = value
      this.send20011(msg)
    }
    this.__TaskMode = value
  }

  get OperationType(): COperationType {
    return this.__OperationType
  }

  set OperationType(value: COperationType) {
    if (this.__OperationType != value) {
      const info = new TaskTypeChangeData(this.__OperationType, value)
      const msg = new AmrMsg20011(this.vehicleId, info)
      msg.TaskMode = this.TaskMode
      this.send20011(msg)
    }
    this.__OperationType = value
  }

  get TaskStatus(): CEventId {
    return this.__TaskStatus
  }

  set TaskStatus(value: CEventId) {
    if (this.__TaskStatus != value) {

      if (value != 1 && value != 4 && value != 8) {
        const info = new TaskStatusChangeData(this.OperationType)
        const msg = new AmrMsg20011(this.vehicleId, info)
        msg.TaskMode = this.TaskMode
        msg.EventId = value
        this.send20011(msg)
      } else if (value == 4) {

        const info = new TaskCompletedData(this.OperationType)
        info.Battery = this.Battery
        info.OperationResult = 0
        info.CurLogicX = this.currentLogicX
        info.CurLogicY = this.currentLogicY
        info.CurX = this.currentLogicX
        info.CurY = this.currentLogicY
        info.CurDirection = this.currentDirection
        info.CurOrientation = this.currentOrientation
        const msg = new AmrMsg20011(this.vehicleId, info)
        msg.TaskMode = this.TaskMode
        this.send20011(msg)
      }

    }
  }

  get PickMode(): CPickMode {
    return this.__PickMode
  }

  set PickMode(value: CPickMode) {
    if (this.__PickMode !== value) {
      this.__PickMode = value
    }
  }

  private bootTime: number = 0

  private heartBeatTimeCount: number = 0
  private mqRetryTimeCount: number = 0

  constructor(item: ItemJson, viewport: Viewport) {
    super()
    this.viewport = viewport
    this.item = item
    this.vehicleId = parseInt(item.id)

    this.viewport.addFrameTimerCallback(this.item.id, this.onFrameTimer.bind(this))
    if (!worldModel.state.runState.isVirtual) {
      this.bootForShow()
    }
    window.agv3 = this
  }

  protected onFrameTimer() {
    if (!worldModel.state.runState.isVirtual) {
      return
    }
    const delta = this.clock.getDelta()
    this.mqRetryTimeCount += delta
    if (this.mqRetryInterval > 0 && this.mqRetryTimeCount >= this.mqRetryInterval) {
      this.mqRetryTimeCount = 0
      // 在此处理消息重试
      if (this.sendMessageQueue.length > 0) {
        this.sendMessage(this.sendMessageQueue[0])
      }
    }
    this.heartBeatTimeCount += delta
    if (this.heartBeatInterval > 0 && this.heartBeatTimeCount >= this.heartBeatInterval) {
      this.heartBeatTimeCount = 0
      // 在此处发送心跳报文
      this.sendHeartBeat()
    }
  }

  // 开机
  boot() {
    this.bootTime = Date.now()
    this.computeLogicXYAndDirection()

    const boxShape = new this.viewport.ammoModel.btBoxShape(new Ammo.btVector3(0.5, 0.5, 0.5))

    const mass = 1
    const localInertia = new this.viewport.ammoModel.btVector3(0, 0, 0)
    boxShape.calculateLocalInertia(mass, localInertia)

    const boxTransform = new this.viewport.ammoModel.btTransform()
    boxTransform.setIdentity()
    boxTransform.setOrigin(new this.viewport.ammoModel.btVector3(this.position.x, this.position.y, this.position.z))

    const boxMotionState = new this.viewport.ammoModel.btDefaultMotionState(boxTransform)
    const rbInfo = new this.viewport.ammoModel.btRigidBodyConstructionInfo(mass, boxMotionState, boxShape, localInertia)
    this.boxBody = new this.viewport.ammoModel.btRigidBody(rbInfo)
    this.viewport.physicsWorld.addRigidBody(this.boxBody)
  }

  bootForShow() {

    this.boot()
    this.subscribeMessage('/wcs_server/' + this.item.id)
    this.subscribeMessage('/agv_robot/status')

  }

  bootForMonitor() {

    this.boot()
    this.subscribeMessage('/wcs_server/' + this.vehicleId)
    this.send20147()
    setTimeout(() => {
      this.send20149()
      this.TaskMode = 1
      // 检查当前所在位置和方向 根据车当前所在的xz坐标获取地标
      setTimeout(() => {
        this.sendCurrentPositionAndDirection()
        setTimeout(() => {
          this.send20150()
        }, 1000)
      }, 1000)
    }, 2000)

  }

  // 关机
  shutdown() {
    if (!worldModel.state.runState.isVirtual) {
      return
    }
    const content = new AmrMsg20148(this.vehicleId)
    // 电量
    content.Battery = 100
    content.CreateMonoTime = Date.now() - this.bootTime
    content.Uptime = content.CreateMonoTime
    const m20148 = new AmrMsg<AmrMsg20148>(content)
    this.sendMessage(m20148)
  }

  // 开机上报
  send20147() {
    const content = new AmrMsg20147(this.vehicleId)
    content.AGVModel = this.AGVModel
    content.AGVFnModel = this.AGVFnModel
    // 电量
    content.Battery = 100
    content.CreateMonoTime = Date.now() - this.bootTime
    const m20147 = new AmrMsg<AmrMsg20147>(content)
    this.sendMessage(m20147)
  }

  // 主程序启动上报
  send20149() {
    const content = new AmrMsg20149(this.vehicleId)
    content.AGVModel = this.AGVModel
    content.AGVFnModel = this.AGVFnModel
    // 电量
    content.Battery = 100
    content.CreateMonoTime = Date.now() - this.bootTime
    const m20149 = new AmrMsg<AmrMsg20149>(content)
    this.sendMessage(m20149)
  }

  // 上线上报
  send20150() {
    const content = new AmrMsg20150(this.vehicleId)
    content.AGVModel = this.AGVModel
    content.AGVFnModel = this.AGVFnModel
    content.Battery = 100
    content.CreateMonoTime = Date.now() - this.bootTime
    const m20150 = new AmrMsg<AmrMsg20150>(content)
    this.sendMessage(m20150)
  }

  // 上报当前位姿，地标和方向
  sendCurrentPositionAndDirection() {
    if (this.currentLogicX <= 0 || this.currentLogicY <= 0) {
      // 当前车辆所在位置未找到
      const content = new AmrMsg20250(this.vehicleId)
      content.Duration = 0
      content.ErrCode = 5
      content.ErrCodeName = AmrErrorCode[5].ErrCodeName
      content.ErrEvtType = 1
      content.ErrLevel = 14
      content.ErrLifecycle = 2
      content.CreateMonoTime = Date.now() - this.bootTime
      const m20250 = new AmrMsg<AmrMsg20250>(content)
      this.sendMessage(m20250)
    } else {
      // 发送正常地标信息
      const content = new AmrMsg20020(this.vehicleId)
      content.CurDirection = this.currentDirection
      content.CurOrientation = this.currentOrientation
      content.CurLogicX = this.currentLogicX
      content.CurLogicY = this.currentLogicY
      content.CurX = this.currentLogicX
      content.CurY = this.currentLogicY
      content.CreateMonoTime = Date.now() - this.bootTime
      const m20020 = new AmrMsg<AmrMsg20020>(content)
      this.sendMessage(m20020)
    }
  }

  send20010() {
    const content: AmrMsg20010 = new AmrMsg20010(this.vehicleId)
    content.CurX = this.currentLogicX
    content.CurY = this.currentLogicY
    content.CurDirection = this.currentDirection
    content.OperationType = this.OperationType
    content.Battery = this.Battery
    content.OperationResult = 0
    content.Summary = {
      ActuatorsData: [
        {
          MechNo: 1,
          Name: 'Mech1',
          PickMode: this.PickMode
        }
      ]
    }


    const m20010 = new AmrMsg<AmrMsg20010>(content)
    this.sendMessage(m20010)
  }

  send20011(content: AmrMsg20011<any>) {
    const m20011 = new AmrMsg<AmrMsg20011<any>>(content)
    this.sendMessage(m20011)
  }

  send20020(content: AmrMsg20020) {
    const m20020 = new AmrMsg<AmrMsg20020>(content)
    this.sendMessage(m20020)
  }

  send20060() {
    const content = new AmrMsg20060(this.vehicleId)
    content.CreateMonoTime = Date.now() - this.bootTime
    content.CurBattery = new CurBatteryData()
    content.CurLogicX = this.currentLogicX
    content.CurLogicY = this.currentLogicY
    content.CurOrientation = this.currentOrientation
    content.CurX = this.position.x
    content.CurY = this.position.z
    content.X = this.position.x
    content.Y = this.position.z
    const m20060 = new AmrMsg<AmrMsg20060>(content)
    this.sendMessage(m20060)
  }

  send20250(content: AmrMsg20250) {
    this.sendMessage(new AmrMsg<AmrMsg20250>(content))
  }

  subscribeMessage(topic: string) {
    worldModel.envManager.client.subscribe(topic, {qos: 0})
  }

  sendMessage(msg: AmrMsg<any>) {
    if (!worldModel.state.runState.isVirtual) {
      return
    }
    // console.log('send message:', JSON.stringify(msg))
    if (this.sendMessageQueue.indexOf(msg) < 0) {
      this.sendMessageQueue.push(msg)
    }
    if (this.sendMessageQueue.length <= 0) {
      this.mqRetryTimeCount = 0
    }
    worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(msg))
    this.heartBeatTimeCount = 0
  }

  sendHeartBeat() {
    if (!worldModel.state.runState.isVirtual) {
      return
    }
    const content = new AmrMsg20100(this.vehicleId)
    content.Temperature = {Battery: this.Battery}
    const m20100 = new AmrMsg<AmrMsg20100>(content)
    worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(m20100))
  }

  sendAck(seqNo: number, vehicleId: number) {
    if (!worldModel.state.runState.isVirtual) {
      return
    }
    const msg20050 = new AmrMsg20050(seqNo, vehicleId)
    const ack = new AmrMsg<AmrMsg20050>(msg20050)
    this.heartBeatTimeCount = 0
    worldModel.envManager.client.publish('/agv_robot/status', JSON.stringify(ack))
  }

  /*==========RCS消息处理============*/

  // 处理任务
  handle10010Message(data: AmrMsg10010) {
    if (this.currentStepTaskList.length > 0) {
      const tindex = this.currentStepTaskList.length
      const lt = this.currentStepTaskList[tindex - 1]
      if (this.runningStepTask.OperationType == 0 && this.runningStepTask.X == data.StartX && this.runningStepTask.Y == data.StartY) {
        // this.currentStepTaskList = []
        this.makeStepTask(data)
        this.executeTask()
      } else if (lt.OperationType == 0 && lt.X == data.StartX && lt.Y == data.StartY) {
        this.makeStepTask(data)
      } else {
        // 此处应该有错误处理
        console.log('handle10010Message:', data)
      }
    } else {
      this.makeStepTask(data)
      this.executeTask()
    }
  }

  handle10050Message(data: AmrMsg<AmrMsg10050>) {
    if (this.sendMessageQueue.length > 0 && data.content.SeqNo === this.sendMessageQueue[0].content.SeqNo) {
      this.mqRetryTimeCount = 0
      this.sendMessageQueue.shift()
    }
  }

  handle10060Message(data: AmrMsg<AmrMsg10060>) {
    this.mqRetryInterval = data.content.MqRetryTime
    this.heartBeatInterval = data.content.HeartBeat
    this.TaskMode = 0
  }

  // 处理状态查询
  handle10110Message(data: AmrMsg<AmrMsg10110>) {
    this.send20060()
  }

  // 取消任务
  handle10120Message(data: AmrMsg<AmrMsg10120>) {

  }


  handle20020Message(data: AmrMsg20020) {
    const p = Model.getPositionByLogicXY(data.CurLogicX, data.CurLogicY) as THREE.Vector3
    this.position.set(p.x, 0, p.z)
  }

  handle20060Message(data: AmrMsg20060) {
    const p = Model.getPositionByLogicXY(data.CurLogicX, data.CurLogicY) as THREE.Vector3
    this.position.set(p.x, 0, p.z)
  }

  /*==========真车消息处理============*/

  // 计算逻辑方向
  computeLogicXYAndDirection() {
    let ra = this.rotation.y
    while (ra > Math.PI * 2) {
      ra -= Math.PI * 2
    }
    while (ra < 0) {
      ra += Math.PI * 2
    }
    const ddra = Math.PI / 180
    if (ra >= Math.PI * 2 - ddra || ra <= ddra) {
      this.currentDirection = 0

    } else if (ra >= Math.PI / 2 - ddra && ra <= Math.PI / 2 + ddra) {
      this.currentDirection = 3

    } else if (ra >= Math.PI - ddra && ra <= Math.PI + ddra) {
      this.currentDirection = 2

    } else if (ra >= Math.PI / 2 * 3 - ddra && ra <= Math.PI / 2 * 3 + ddra) {
      this.currentDirection = 1
    } else {
      this.currentDirection = 15
    }

    const pointItem = Model.getItemByXYZ(this.position.x, this.position.y, this.position.z)
    if (!pointItem || !pointItem.logicX || !pointItem.logicY) {
      this.currentLogicX = -1
      this.currentLogicY = -1
    } else {
      this.currentLogicX = pointItem.logicX
      this.currentLogicY = pointItem.logicY
    }
    this.currentOrientation = this.getAmrOrientation(this.rotation.y)
  }

  makeStepTask(data: AmrMsg10010) {
    console.log('makeStepTask:', data)
    let currentStepTask: StepTask = this.runningStepTask
    if (currentStepTask == null) {
      if (this.currentStepTaskList && this.currentStepTaskList.length > 0) {
        currentStepTask = this.currentStepTaskList[this.currentStepTaskList.length - 1]
      } else {
        currentStepTask = {
          SeqNo: 0,
          StepTaskType: 'MOVE',
          OperationType: 0,
          PickMode: 0,
          X: this.currentLogicX,
          Y: this.currentLogicY,
          Speed: 1000,
          EndDirection: this.currentDirection,
          ChargeLocation: 0,
          GoodsSlotHeight: 0,
          position: this.position,
          isCompleted: true
        }
      }

    }
    let endDirection = currentStepTask.EndDirection

    const linkCount = data.Link?.length || 0
    if (linkCount > 0) {
      let prevLink = {X: data.StartX, Y: data.StartY, Speed: 1000}
      for (let i = 0; i < data.Link.length; i++) {
        const link = data.Link[i]
        if ((currentStepTask.X == link.X && currentStepTask.Y == link.Y)
          || (currentStepTask.X != link.X && currentStepTask.Y != link.Y)) {
          prevLink = link
          continue
        } else if (currentStepTask.X < link.X) {
          if (link.Speed > 0) {
            endDirection = 0
          } else {
            endDirection = 2
          }
        } else if (currentStepTask.X > link.X) {
          if (link.Speed > 0) {
            endDirection = 2
          } else {
            endDirection = 0
          }
        } else if (currentStepTask.Y < link.Y) {
          if (link.Speed > 0) {
            endDirection = 1
          } else {
            endDirection = 3
          }
        } else if (currentStepTask.Y > link.Y) {
          if (link.Speed > 0) {
            endDirection = 3
          } else {
            endDirection = 1
          }
        }

        if (endDirection != currentStepTask.EndDirection) {
          const item = this.viewport.entityManager.findItemByLogicXY(prevLink.X, prevLink.Y)
          if (item.dt?.agvRotation && item.dt?.agvRotation?.length > 0) {
            const stepTask: StepTask = {
              SeqNo: data.SeqNo,
              StepTaskType: 'ROTATION',
              OperationType: 0,
              PickMode: 0,
              X: prevLink.X,
              Y: prevLink.Y,
              Speed: prevLink.Speed,
              EndDirection: endDirection,
              ChargeLocation: data.ChargeLocation,
              GoodsSlotHeight: data.GoodsSlotHeight,
              position: Model.getPositionByLogicXY(prevLink.X, prevLink.Y) as THREE.Vector3,
              isCompleted: false
            }
            currentStepTask = stepTask
            this.currentStepTaskList.push(stepTask)
          }
        }

        const stepTask: StepTask = {
          SeqNo: data.SeqNo,
          StepTaskType: link.Speed > 0 ? 'MOVE' : 'MOVE_BACKWARD',
          OperationType: 0,
          PickMode: 0,
          X: link.X,
          Y: link.Y,
          Speed: link.Speed,
          EndDirection: endDirection,
          ChargeLocation: data.ChargeLocation,
          GoodsSlotHeight: data.GoodsSlotHeight,
          position: Model.getPositionByLogicXY(link.X, link.Y) as THREE.Vector3,
          isCompleted: false
        }
        currentStepTask = stepTask
        this.currentStepTaskList.push(stepTask)
        prevLink = link
      }
    }

    // 如果没有link 或者当前link的XY是任务结束点位
    if (linkCount <= 0
      || (linkCount > 0 && data.Link[linkCount - 1].X == data.EndX && data.Link[linkCount - 1].Y == data.EndY)) {

      if (data.OperationType == 0 && data.EndDirection >= 0 && data.EndDirection <= 3) {
        endDirection = data.EndDirection
      } else if (data.OperationType == 3 && data.ChargeDirection >= 0 && data.ChargeDirection <= 3) {
        endDirection = data.ChargeDirection
      } else if (data.OperationType == 4 && data.GoodsSlotDirection >= 0 && data.GoodsSlotDirection <= 3) {
        if (data.GoodsSlotDirection == 3) {
          endDirection = 0
        } else {
          endDirection = (data.GoodsSlotDirection + 1) as LogicDirection
        }
      }

      const item = this.viewport.entityManager.findItemByLogicXY(data.EndX, data.EndY)

      if (endDirection != currentStepTask.EndDirection) {
        // 如果此处不能转弯，忽略结束方向 等待后续任务
        if (!item.dt?.agvRotation || item.dt?.agvRotation?.length <= 0) {
          debugger
          return
        }
        const stepTask: StepTask = {
          SeqNo: data.SeqNo,
          StepTaskType: 'ROTATION',
          OperationType: 0,
          PickMode: 0,
          X: data.EndX,
          Y: data.EndY,
          Speed: currentStepTask.Speed,
          EndDirection: endDirection,
          ChargeLocation: data.ChargeLocation,
          GoodsSlotHeight: data.GoodsSlotHeight,
          position: Model.getPositionByLogicXY(data.EndX, data.EndY) as THREE.Vector3,
          isCompleted: false
        }
        this.currentStepTaskList.push(stepTask)
      }

      if (data.OperationType == 3) {

        const stepTask: StepTask = {
          SeqNo: data.SeqNo,
          StepTaskType: 'CHARGE',
          OperationType: 3,
          PickMode: 0,
          X: data.EndX,
          Y: data.EndY,
          Speed: currentStepTask.Speed,
          EndDirection: endDirection,
          ChargeLocation: data.ChargeLocation,
          GoodsSlotHeight: data.GoodsSlotHeight,
          position: Model.getPositionByLogicXY(data.EndX, data.EndY) as THREE.Vector3,
          isCompleted: false
        }
        this.currentStepTaskList.push(stepTask)

      } else if (data.OperationType == 4) {

        const stepTask: StepTask = {
          SeqNo: data.SeqNo,
          StepTaskType: data.PickMode == 1 ? 'LOAD' : 'UNLOAD',
          OperationType: 4,
          PickMode: data.PickMode,
          X: data.EndX,
          Y: data.EndY,
          Speed: currentStepTask.Speed,
          EndDirection: endDirection,
          ChargeLocation: data.ChargeLocation,
          GoodsSlotHeight: data.GoodsSlotHeight,
          position: Model.getPositionByLogicXY(data.EndX, data.EndY) as THREE.Vector3,
          isCompleted: false
        }
        this.currentStepTaskList.push(stepTask)

      } else {

      }
    }


    console.log(JSON.stringify(this.currentStepTaskList))

  }

  executeTask() {
    if (this.runningStepTaskList.length > 0) {
      this.TaskMode = 2
    }
    while (this.currentStepTaskList.length > 0) {
      const stepTask = this.currentStepTaskList[0]
      if (this.runningStepTask) {
        if ((stepTask.StepTaskType == 'MOVE' || stepTask.StepTaskType == 'MOVE_BACKWARD')
          && this.rotationAnimation == null && this.actionAnimation == null
          && stepTask.EndDirection == this.runningStepTask.EndDirection
          && (stepTask.Speed > 0) == (this.runningStepTask.Speed > 0)) {
          this.runningStepTask = stepTask
          this.currentStepTaskList.shift()
          this.runningStepTaskList.push(stepTask)
          console.log('移动')
          this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection, stepTask.Speed / 1000)

        } else {
          break
        }
      } else {
        this.runningStepTask = stepTask
        this.currentStepTaskList.shift()
        this.runningStepTaskList.push(stepTask)
        if (stepTask.StepTaskType == 'MOVE' || stepTask.StepTaskType == 'MOVE_BACKWARD') {
          console.log('移动')
          this.addTravel(stepTask.X, stepTask.Y, stepTask.EndDirection, stepTask.Speed / 1000)
        } else if (stepTask.StepTaskType == 'ROTATION') {
          console.log('转动')
          this.addRotation(stepTask.EndDirection)
        } else if (stepTask.StepTaskType == 'LOAD') {
          console.log('取货')
          this.addLoad(stepTask.GoodsSlotHeight / 1000, this.agvStatusVo.bizLpn)
        } else if (stepTask.StepTaskType == 'UNLOAD') {
          console.log('放货')
          this.addUnload(stepTask.GoodsSlotHeight / 1000, this.agvStatusVo.bizLpn)
        }
      }
    }
  }

  onActionCompleted() {
    setTimeout(() => {
      this.runningStepTaskList = []
      this.runningStepTask = null
      this.computeLogicXYAndDirection()
      // 当前所有动作执行完毕
      if (this.currentStepTaskList.length <= 0) {
        this.send20010()
      }
      this.PickMode = 0
      this.OperationType = 0
      this.TaskMode = 0
      this.executeTask()
    }, 2000)
  }

  /*==========动作处理============*/

  // 转
  addRotation(direction: number): Promise<void> {
    this.OperationType = 0
    this.PickMode = 0
    this.TaskMode = 2
    let rad = 0
    switch (direction) {
      case 1:
        rad = Math.PI / 2 * 3
        break
      case 2:
        rad = Math.PI
        break
      case 3:
        rad = Math.PI / 2
        break
      default:
        rad = 0
    }
    const quat1 = new THREE.Quaternion().setFromEuler(this.rotation)
    const euler: Euler = new Euler(this.rotation.x, rad, this.rotation.z)
    const quat2 = new THREE.Quaternion().setFromEuler(euler)
    const angleDiff = quat1.angleTo(quat2)
    console.log(rad, this.rotation.y, angleDiff)
    const tr = this.rotation.y + angleDiff
    let time = Math.abs(angleDiff) / this.rotationSpeed
    const duration = time

    return new Promise(resolve => {
      this.rotationAnimation = gsap.to(this.rotation, {
        y: rad,
        duration,
        ease: 'none',
        onComplete: () => {
          resolve()
          this.rotationAnimation = null
          this.onActionCompleted()
        }
      })
    })
  }

  keepSpeed: any
  force: any
  reverseForce: any

  // 走
  addTravel(logicX: number, logicY: number, direction: number, speed: number = 1): Promise<void> {


    this.OperationType = 0
    this.PickMode = 0
    this.TaskMode = 2
    const pos = Model.getPositionByLogicXY(logicX, logicY)
    this.__toPos = pos as THREE.Vector3
    //加速度
    const a = 0.4

    if (this.position.x < this.__toPos.x || this.position.z < this.__toPos.z) {
      speed = Math.abs(speed)
    } else if (this.position.x > this.__toPos.x || this.position.z > this.__toPos.z) {
      speed = 0 - Math.abs(speed)
    }

    // 运动参数
    const accelForce = speed > 0 ? a : (-a)
    let currentPhase = 'accelerate'

    this.keepSpeed = new this.viewport.ammoModel.btVector3(0, 0, 0)
    this.force = new this.viewport.ammoModel.btVector3(0, 0, 0)
    this.reverseForce = new this.viewport.ammoModel.btVector3(0, 0, 0)
    if (direction == 0 || direction == 2) {
      this.force.setValue(accelForce, 0, 0)
      this.reverseForce.setValue(-accelForce, 0, 0)
      this.keepSpeed.setValue(speed, 0, 0)
    } else if (direction == 1 || direction == 3) {
      this.force.setValue(0, 0, accelForce)
      this.reverseForce.setValue(0, 0, -accelForce)
      this.keepSpeed.setValue(0, 0, speed)
    }

    if (this.viewport.registerPhysicsUpdateCallBack.has(this.item.id)) {
      return null
    }

    this.travelAnimation = 'asd'

    this.viewport.registerPhysicsUpdateCallBack.set(this.item.id, () => {

      this.viewport.physicsWorld.stepSimulation(1 / 60, 10)

      // 同步Three.js对象位置
      const transform = new this.viewport.ammoModel.btTransform()
      this.boxBody.getMotionState().getWorldTransform(transform)
      const position = transform.getOrigin()
      this.position.set(position.x(), position.y(), position.z())
      // this.boxBody.getActivationState()
      // 获取当前速度
      const velocity = this.boxBody.getLinearVelocity()
      const distance = this.position.distanceTo(this.__toPos)
      const cSpeed = velocity.length()
      const stopTime = cSpeed / a
      const stopDistance = a * stopTime * stopTime / 2
      if (distance <= 0.01) {
        const stopVel = new this.viewport.ammoModel.btVector3(0, 0, 0)
        this.boxBody.setLinearVelocity(stopVel)
        this.viewport.ammoModel.destroy(stopVel)
        this.viewport.registerPhysicsUpdateCallBack.delete(this.item.id)
        this.viewport.ammoModel.destroy(this.force)
        this.viewport.ammoModel.destroy(this.keepSpeed)
        this.position.set(this.__toPos.x, this.__toPos.y, this.__toPos.z)
        this.travelAnimation = null
        this.onActionCompleted()
        return
      }
      if (cSpeed == 0) {
        this.boxBody.activate()
      }
      if (distance <= stopDistance + 0.2) {
        if (currentPhase != 'decelerate') {
          currentPhase = 'decelerate'
          const sp = new this.viewport.ammoModel.btVector3(velocity.x(), velocity.y(), velocity.z())
          this.boxBody.setLinearVelocity(sp)
          this.viewport.ammoModel.destroy(sp)
        }
      } else if (currentPhase == 'decelerate') {
        currentPhase = 'accelerate'
      }

      // 运动阶段控制
      switch (currentPhase) {
        case 'accelerate':
          // 施加X轴正向力
          this.boxBody.applyCentralForce(this.force)

          // 达到最大速度进入匀速阶段
          if (cSpeed >= Math.abs(speed) || distance <= stopDistance) {
            currentPhase = 'uniform'
          }
          this.boxBody.setDamping(0, 0);
          break
        case 'uniform':
          currentPhase = 'uniform'
          this.boxBody.setLinearVelocity(this.keepSpeed)
          this.boxBody.setDamping(0, 0);
          break
        case 'decelerate':
          // 检测停止
          if (cSpeed > 0) {
            this.boxBody.setDamping(0.6, 0);
          }
          this.boxBody.activate()
          break
      }

      for (let i = 0; i < this.runningStepTaskList.length; i++) {
        const task = this.runningStepTaskList[i]
        if (task.isCompleted == false) {
          if (this.position.distanceTo(task.position) < 0.1) {
            task.isCompleted = true
            this.runningStepTaskList.splice(0, i + 1)
            const content: AmrMsg20020 = new AmrMsg20020(this.vehicleId)
            content.CurLogicX = task.X
            content.CurLogicY = task.Y
            content.CurX = task.X
            content.CurY = task.Y
            content.CurOrientation = task.EndDirection * 90
            content.CurDirection = task.EndDirection
            this.send20020(content)
            break
          }
        }
      }

    })
    return null
  }

  // 取货
  addLoad(height: number, goodsId: string): void {
    this.actionAnimation = 'wq'
    console.log('取货')
    this.TaskMode = 2
    this.PickMode = 1
    this.OperationType = 4
    this.animationUpFork(height).then(
      () => this.animationShowFork(1.35).then(
        () => {
          // 将物品拾取到机械臂上
          const mesh = this.pickupItem(goodsId)
          mesh.position.set(0, 0, -0.15)
          mesh.rotation.set(0, THREE.MathUtils.degToRad(90), 0)
          this.getArmObject().add(mesh)

          this.animationUpFork(height + 0.2).then(
            () => this.animationHideFork().then(
              () => this.animationDownFork().then(() => {
                this.actionAnimation = null
                this.onActionCompleted()
              })
            )
          )
        }
      )
    )
  }

  // 卸货
  addUnload(height: number, goodsId: string): void {
    this.actionAnimation = 'wq'
    console.log('卸货')
    this.TaskMode = 2
    this.PickMode = 2
    this.OperationType = 4
    this.animationUpFork(height + 0.2).then(
      () => this.animationShowFork(1.35).then(
        ()=>this.animationUpFork(height).then(
          () => {
            const a = this.agvStatusVo.unloadBasLocationVo
            // 将物品从机械臂上卸下
            this.dropItem(goodsId, a.rack, a.bay, a.level, a.cell)

            this.animationHideFork().then(
              () => this.animationDownFork().then(() => {
                this.actionAnimation = null
                this.onActionCompleted()
              })
            )
          }
        )
      )
    )
  }

  override animationShowFork(z: number): Promise<void> {
    return null
  };

  override animationHideFork(): Promise<void> {
    return null
  }

  override animationUpFork(y: number, time?: number = 3): Promise<void> {
    return null
  }

  override animationDownFork(): Promise<void> {
    return null
  }

  override getArmObject(): THREE.Object3D | undefined {
    return null
  }

  //获取ptr的角度朝向
  getAmrOrientation(radY: number) {
    while (radY < 0) {
      radY += Math.PI * 2
    }
    while (radY > Math.PI * 2) {
      radY -= Math.PI * 2
    }
    radY = (Math.PI * 2 - radY) % (Math.PI * 2)
    return THREE.MathUtils.radToDeg(radY)
  }

  /**
   * 拾取物品
   */
  pickupItem(id: string): THREE.Object3D {
    // 找到物品所在的地方, 删除他
    const item = this.viewport.entityManager.findItemById(id)
    const wrap = this.viewport.entityManager.findObjectById(id) as MeshWrap

    if (wrap.type !== 'MeshWrap') {
      throw new Error(`无法拾取物品 ${id}，它不是一个有效的 MeshWrap`)
    }
    item.dt.storeAt = {
      item: this.id + ""
    }
    const mesh = wrap.manager.wrapToObject3D(wrap)
    this.viewport.entityManager.replaceObject(id, mesh)
    return mesh
  }

  /**
   * 卸下物品
   */
  dropItem(itemId: string, storeItemId: string, bay?: number, level?: number, cell?: number): void {

    const item = this.viewport.entityManager.findItemById(itemId)

    item.dt.storeAt = {
      item: storeItemId,
      bay: bay,
      level: level,
      cell: cell
    }

    const itemRenderer = getRenderer(item.t)
    if (!itemRenderer) {
      throw new Error(`Renderer for type ${item.t} not found`)
    }

    this.getArmObject().clear()

    itemRenderer.tempViewport = this.viewport
    itemRenderer.createOrUpdatePointForRuntime(item)
    itemRenderer.tempViewport = null
  }

}