import * as THREE from 'three'
import {
  System,
  Entity,
  Cr20Entity,
  PalletEntity,
  BoxEntity,
  ConveyorEntity,
  localToWorld,
  poseToMatrix,
  decompose,
  CollisionType,
  PartitionEntity,
  FollowMotionHelper,
  JointMotionHelper,
  UserAxesHelper,
  SuckerEntity,
  AssembleHelper,
  RobotCursorHelper,
  TransitionPointHelper
} from '../../src'
import { BoxPositionType, BoxStateType, type PalletParams } from './PalletizingTypes'

import {
  ConveyorBoxInterval,
  ConveyorLength,
  ConveyorWidth,
  defaultConfig,
  defaultUserCoordinatesWithLift
} from './DefaultPalletizingConfig'
export const delayExecute = async (handler: any, ms: number = 5000) => {
  await new Promise((resolve) => {
    setTimeout(() => {
      resolve(null)
    }, ms)
  })
  handler()
}
export interface PalletizingConfigureAction {
  type: 'Box' | 'Pallet' | 'Point' | 'Template' | 'Layer' | 'Motion' | 'RobotArm'
  params: PalletParams
}

interface BoxItem {
  id: number
  pallet: number //1: 左  2： 右
  layer: number
  expectPosition?: THREE.Vector3
  expectQuaternion?: THREE.Quaternion
  entity: BoxEntity
  positionDeviation: number
  directionDeviation: number
  onRobot: boolean
}

class BoxContainer {
  boxes: BoxItem[] = []

  constructor() {}

  add(id: number, palletNum: number, layerNum: number, entity: BoxEntity) {
    this.boxes.push({
      id,
      pallet: palletNum,
      layer: layerNum,
      entity,
      positionDeviation: 0,
      directionDeviation: 0,
      onRobot: true
    })
  }

  forEach(handler: any) {
    this.boxes.forEach((box: BoxItem, index: number) => {
      handler(box, index)
    })
  }

  remove(box: BoxItem) {
    const index = this.boxes.indexOf(box)
    if (index !== -1) {
      this.boxes.splice(index, 1)
    }
  }

  setNotOnRobot(entity: BoxEntity) {
    for (const item of this.boxes) {
      if (item.entity == entity) {
        item.onRobot = false
        return
      }
    }
  }

  getBoxByEntity(entity: BoxEntity): BoxItem | null {
    for (const item of this.boxes) {
      if (item.entity == entity) {
        return item
      }
    }
    return null
  }

  clearBoxOnPallet(pallet: number) {
    this.boxes = this.boxes.filter((item) => item.pallet != pallet)
  }

  clear() {
    this.boxes = []
  }
}

export class PalletizingSimulateSystem extends System {
  leftUserMatrix!: THREE.Matrix4 //左栈板用户坐标系
  rightUserMatrix!: THREE.Matrix4 //右栈板用户坐标系
  partitionUserMatrix!: THREE.Matrix4 //隔板用户坐标系

  palletParams!: PalletParams
  userCoordinates!: any[]
  toolCoordinates!: any[]
  initialJoint!: number[]

  entityMap: Map<string, Entity> = new Map<string, Entity>()

  boxContainer = new BoxContainer()

  partitionNum = 0
  nextPartitionNum = 0
  partitionOnRobot!: Entity | undefined

  totalBoxNum = 0

  onAdded(): void {
    const { EventManager, view } = this._app

    EventManager.subscribe(
      'palletizing:simulate:init',
      (data: any) => {
        this.boxContainer.clear()
        this.removeAllEntityAndHelpers()
        this.initHelpers()
        this.initConfigureScene(data)
      },
      'simulate-system'
    )

    EventManager.subscribe(
      'palletizing:simulate:process',
      (data: any) => {
        this.handleProcess(data)
      },
      'simulate-system'
    )

    EventManager.subscribe(
      'palletizing:simulate:robotAction',
      (data: any) => {
        this.handleRobotAction(data)
      },
      'simulate-system'
    )

    EventManager.subscribe(
      'palletizing:simulate:partitionAction',
      (data: any) => {
        this.handlePartitionAction(data)
      },
      'simulate-system'
    )

    EventManager.subscribe(
      'palletizing:simulate:liftAction',
      (data: any) => {
        this.handleLiftingAction(data)
      },
      'simulate-system'
    )

    EventManager.subscribe(
      'palletizing:simulate:clearBoxes',
      (data: any) => {
        if (data.includes(BoxPositionType.ALL)) {
          this.entityMap.forEach((entity, entityName) => {
            if (entity.Type == 'box') {
              this.removeEntity(entity)
            }
          })
          this.boxContainer.clear()
        } else {
          this.boxContainer.forEach((box: BoxItem) => {
            const entityName = `box${box.id}`
            const entity = this.entityMap.get(entityName)
            if (
              entity &&
              ((data.includes(BoxPositionType.LEFT_PALLET) && box.pallet == 1) ||
                (data.includes(BoxPositionType.RIGHT_PALLET) && box.pallet == 2))
            ) {
              this.removeEntity(entity)
            }
          })

          if (data.includes(BoxPositionType.LEFT_PALLET)) {
            this.boxContainer.clearBoxOnPallet(BoxPositionType.LEFT_PALLET)
          }

          if (data.includes(BoxPositionType.RIGHT_PALLET)) {
            this.boxContainer.clearBoxOnPallet(BoxPositionType.RIGHT_PALLET)
          }
        }

        //清空所有隔板同时复位状态
        this.entityMap.forEach((entity, entityName) => {
          if (entity.Type == 'partition') {
            this.removeEntity(entity)
          }
        })

        this.partitionNum = 0
        this.nextPartitionNum = 0
        this.partitionOnRobot = undefined

        this.configurePartitions()
      },
      'simulate-system'
    )

    EventManager.subscribe(
      'palletizing:simulate:status',
      (data: any) => {
        this.updatePose(data.data, data.interval || 200)
      },
      'simulate-system'
    )

    EventManager.subscribe(
      'collisions',
      (data: any) => {
        const { entityA, objectA, entityB, objectB, normalOnB, distance, ptA, ptB } = data
        const types = [entityA.Type, entityB.Type]
        if (entityA.Type == 'box' && entityB.Type == 'box' && distance < -10) {
          const boxA = this.boxContainer.getBoxByEntity(entityA)
          const boxB = this.boxContainer.getBoxByEntity(entityB)

          if (boxA?.onRobot || boxB?.onRobot) {
            EventManager.publish('collisions:boxCarrying', boxA?.onRobot ? boxB : boxA)
          }
        }
      },
      'simulate-system'
    )
  }

  onRemoved(): void {
    const { EventManager, view } = this._app
    EventManager.unsubscribeInScope('palletizing:simulate:init', 'simulate-system')
    EventManager.unsubscribeInScope('palletizing:simulate:process', 'simulate-system')
    EventManager.unsubscribeInScope('palletizing:simulate:robotAction', 'simulate-system')
    EventManager.unsubscribeInScope('palletizing:simulate:partitionAction', 'simulate-system')
    EventManager.unsubscribeInScope('palletizing:simulate:liftAction', 'simulate-system')
    EventManager.unsubscribeInScope('palletizing:simulate:clearPallets', 'simulate-system')
    EventManager.unsubscribeInScope('palletizing:simulate:status', 'simulate-system')
  }

  onUpdating(deltaTime: number): void {}

  //配置机械臂实体
  async configureRobotArm(initialJoints: []) {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams
    const entity = new Cr20Entity({
      name: 'cr20',
      position: { x: 0, y: 0, z: 0 },
      rotation: { x: 0, y: 0, z: 0 },
      lift: 0,
      paths: {
        robotArm: `${import.meta.env.VITE_CDN || ''}/static/model/sim/cr30.glb`,
        column: `${import.meta.env.VITE_CDN || ''}/static/model/sim/md-lz-v2.glb`,
        liftingColumn: `${import.meta.env.VITE_CDN || ''}/static/model/sim/md-sj.glb`,
        pako: false
      },
      phyShapes: [
        {
          size: { length: 1100, width: 400, height: 1050  },
          offset: new THREE.Vector3(-200, 1050 / 2, 0),
          rotation: new THREE.Quaternion()
        },
      ]
    })

    await this.addEntity(entity)

    this.PhysicEnv.addCollideCheckEntity(entity)

    if (box.tool.length && box.tool.width && box.tool.height) {
      const toolParams = [0, 2].includes(basic.pallet)
        ? this.toolCoordinates[basic.tool1].params
        : this.toolCoordinates[basic.tool2].params

      const suckerEntity = new SuckerEntity({
        name: 'sucker',
        position: { x: 0, y: 0, z: 0 },
        rotation: { x: 0, y: 0, z: 0 },
        size: box.tool,
        offset: {
          x: 0,
          y: 0
        }
      })

      await this.addEntity(suckerEntity)

      const assembleHelper = this.getHelper('assembleHelper') as AssembleHelper
      assembleHelper.attach(suckerEntity.Name, {
        child: suckerEntity,
        parent: entity,
        anchor: 'TOOL'
      })

      this.PhysicEnv.addCollideCheckEntity(suckerEntity)
    }

    const jointMotionHelper = this.getHelper('jointMotionHelper') as JointMotionHelper
    jointMotionHelper.attach(entity.Name, {
      entity,
      jointDescriptions: entity.JointDescriptions,
      initialJoints
    })

    const userAxesHelper = this.getHelper('userAxesHelper') as UserAxesHelper
    userAxesHelper.attach('basic-axes', {
      matrixWorld: entity.UserMatrix0
    })

    const baseObject: any = entity.getObjectByName('base')
    if (!baseObject) {
      console.error('base部件不存在,机械臂模型错误')
      return
    }

    //更新机械臂基坐标系、左栈板用户坐标系、右栈板用户坐标系
    const userMatrix0 = entity.UserMatrix0

    this.leftUserMatrix = poseToMatrix(this.userCoordinates[basic.userLeft].params, userMatrix0)
    this.rightUserMatrix = poseToMatrix(this.userCoordinates[basic.userRight].params, userMatrix0)
    if (basic.interlayer) {
      this.partitionUserMatrix = poseToMatrix(
        this.userCoordinates[basic.userInterlayer].params,
        userMatrix0
      )
    }
  }

  //配置栈板
  async configurePallets() {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams

    //添加左栈板实体
    if ([0, 2].includes(basic.pallet)) {
      const matrix = poseToMatrix(
        [pallet.size.length / 2, -pallet.size.width / 2, 0, 90, 0, 0],
        this.leftUserMatrix
      )
      const pose = decompose(matrix)

      const entity = new PalletEntity({
        name: 'left-pallet',
        type: 'pallet',
        position: pose.position,
        rotation: new THREE.Euler().setFromQuaternion(pose.quaternion),
        size: pallet.size
      })
      await this.addEntity(entity)
    }

    if ([1, 2].includes(basic.pallet)) {
      const matrix = poseToMatrix(
        [pallet.size.length / 2, pallet.size.width / 2, 0, 90, 0, 0],
        this.rightUserMatrix
      )
      const pose = decompose(matrix)
      //添加右栈板实体
      const entity = new PalletEntity({
        name: 'right-pallet',
        type: 'pallet',
        position: pose.position,
        rotation: new THREE.Euler().setFromQuaternion(pose.quaternion),
        size: pallet.size
      })

      await this.addEntity(entity)
    }
  }

  //配置栈板
  async configurePartitions() {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams

    if (basic.interlayer) {
      for (let i = 0; i < layer.left.partitionCount; i++) {
        const matrix = poseToMatrix(
          [
            pallet.size.length / 2,
            -pallet.size.width / 2,
            layer.left.partitionHeight * i,
            90,
            0,
            0
          ],
          this.partitionUserMatrix
        )
        const pose = decompose(matrix)

        const entity = new PartitionEntity({
          name: `partition${this.partitionNum}`,
          type: 'partition',
          position: pose.position,
          rotation: new THREE.Euler().setFromQuaternion(pose.quaternion),
          size: { ...pallet.size, height: layer.left.partitionHeight }
        })
        this.partitionNum += 1
        await this.addEntity(entity)
      }
    }
  }

  //配置传送带
  async configureConveyor() {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams

    //计算出左栈在传送带上抓取点的坐标系
    const leftPickMatrix = poseToMatrix(conveyer.position[0], this.leftUserMatrix)
    const conveyorHeight =
      Math.abs(
        leftPickMatrix.elements[13] /
          new THREE.Vector3(
            leftPickMatrix.elements[8],
            leftPickMatrix.elements[9],
            leftPickMatrix.elements[10]
          ).dot(new THREE.Vector3(0, 1, 0))
      ) - box.left.height

    let conveyorMatrix: THREE.Matrix4
    if (basic.suction == 0) {
      conveyorMatrix = poseToMatrix(
        [
          ConveyorLength / 2 - box.left.length / 2,
          ConveyorBoxInterval,
          conveyorHeight + box.left.height,
          -90,
          0,
          0
        ],
        leftPickMatrix
      )
    } else {
      conveyorMatrix = poseToMatrix(
        [
          ConveyorLength / 2 - box.left.length - ConveyorBoxInterval * 0.5,
          0,
          conveyorHeight + box.left.height,
          -90,
          0,
          0
        ],
        leftPickMatrix
      )
    }

    const cpose = decompose(conveyorMatrix)
    const cposition = cpose.position
    const croation = new THREE.Euler().setFromQuaternion(cpose.quaternion)

    const conveyerSize = {
      length: ConveyorLength,
      width: ConveyorWidth,
      height: conveyorHeight
    }

    //添加传送带实体
    const entity = new ConveyorEntity({
      name: 'conveyor',
      type: 'conveyor',
      position: cposition,
      rotation: croation,
      size: conveyerSize,
      style: 'belt' //滚轴、皮带
    })
    await this.addEntity(entity)
  }

  //配置盒子
  async configureBoxes() {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams
    const { typeA, typeB, typeC, typeD } = template
    const typeLeft = [typeA, typeB]
    const typeRight = [typeC, typeD]

    const sides = []
    if ([0, 2].includes(basic.pallet)) sides.push('left')
    if ([1, 2].includes(basic.pallet)) sides.push('right')

    for (const side of sides) {
      let partitionOffsetHeight = 0
      const userMatrix = side == 'left' ? this.leftUserMatrix : this.rightUserMatrix
      const layerSide = side == 'left' ? layer.left : layer.right
      const boxSide = side == 'left' ? box.left : box.right
      const types = side == 'left' ? typeLeft : typeRight
      const partition = layerSide.partition
      const partitionHeight = layerSide.partitionHeight

      for (let layerNum = 0; layerNum < layerSide.count; layerNum++) {
        const type = types[layerSide.order[layerNum]]
        const style = type?.style || []

        const height = layerNum * boxSide.height + pallet.size.height + partitionOffsetHeight

        for (const item of style) {
          const boxMatrixes = []

          const x = item.x
          const y = side == 'left' ? item.y : item.y

          if (item.connection == 1) {
            const matrix = poseToMatrix([x, y, height, 0, 0, 90 * item.rotate], userMatrix)
            if (boxSide.length > boxSide.width) {
              boxMatrixes.push(
                poseToMatrix([-boxSide.width / 2 - ConveyorBoxInterval / 2, 0, 0, 90, 0, 0], matrix)
              )
              boxMatrixes.push(
                poseToMatrix([boxSide.width / 2 + ConveyorBoxInterval / 2, 0, 0, 90, 0, 0], matrix)
              )
            } else {
              boxMatrixes.push(
                poseToMatrix(
                  [0, -boxSide.length / 2 - ConveyorBoxInterval / 2, 0, 90, 0, 0],
                  matrix
                )
              )
              boxMatrixes.push(
                poseToMatrix([0, boxSide.length / 2 + ConveyorBoxInterval / 2, 0, 90, 0, 0], matrix)
              )
            }
          } else if (item.connection == 2) {
            const matrix = poseToMatrix([x, y, height, 0, 0, 90 * item.rotate], userMatrix)
            if (boxSide.length > boxSide.width) {
              boxMatrixes.push(
                poseToMatrix(
                  [0, -boxSide.length / 2 - ConveyorBoxInterval / 2, 0, 90, 0, 0],
                  matrix
                )
              )
              boxMatrixes.push(
                poseToMatrix([0, boxSide.length / 2 + ConveyorBoxInterval / 2, 0, 90, 0, 0], matrix)
              )
            } else {
              boxMatrixes.push(
                poseToMatrix([-boxSide.width / 2 - ConveyorBoxInterval / 2, 0, 0, 90, 0, 0], matrix)
              )
              boxMatrixes.push(
                poseToMatrix([boxSide.width / 2 + ConveyorBoxInterval / 2, 0, 0, 90, 0, 0], matrix)
              )
            }
          } else {
            boxMatrixes.push(poseToMatrix([x, y, height, 90, 0, 90 * item.rotate], userMatrix))
          }

          boxMatrixes.forEach(async (boxMatrix, index) => {
            const { position, quaternion } = decompose(boxMatrix)
            const entity = new BoxEntity({
              name: `${side}-${layerNum}-${item.index}-${index}`,
              position,
              rotation: new THREE.Euler().setFromQuaternion(quaternion),
              size: {
                width: boxSide.width,
                length: boxSide.length,
                height: boxSide.height
              },
              weight: boxSide.weight,
              barcode: {
                position: 'left'
              }
            })
            await this.addEntity(entity)
          })
        }

        if (
          basic.interlayer == 1 &&
          ((layerNum > 0 && (layerNum + 1) % partition == 0) || partition == 1) &&
          layerNum < layerSide.count - 1
        ) {
          partitionOffsetHeight += partitionHeight
          const matrix = poseToMatrix(
            [
              pallet.size.length / 2,
              side == 'left' ? -pallet.size.width / 2 : pallet.size.width / 2,
              height + boxSide.height,
              90,
              0,
              0
            ],
            userMatrix
          )
          const pose = decompose(matrix)

          const entity = new PartitionEntity({
            name: `${side}_partition_${layerNum / partition}`,
            position: pose.position,
            rotation: new THREE.Euler().setFromQuaternion(pose.quaternion),
            size: { ...pallet.size, height: layer.left.partitionHeight }
          })
          await this.addEntity(entity)
        }
      }
    }
  }

  async initConfigureScene(data: any) {
    const { type, params, userCoordinates, toolCoordinates, initialJoint } = data
    this.palletParams = params
    this.userCoordinates = userCoordinates
    this.toolCoordinates = toolCoordinates
    this.initialJoint = initialJoint

    //配置机械臂
    await this.configureRobotArm(initialJoint)

    //配置栈板
    await this.configurePallets()

    //配置隔板
    await this.configurePartitions()

    //配置传送带
    await this.configureConveyor()

    await this.configureBoxes()
  }

  async initHelpers() {
    // 创建一个用户轴辅助对象
    const userAxesHelper = new UserAxesHelper({
      axisRadius: 2, // 轴半径
      axisLength: 250 // 轴长度
    })

    await this.addHelper('userAxesHelper', userAxesHelper)

    // 创建一个过渡点辅助对象
    const transitionPointHelper = new TransitionPointHelper({
      pointRadius: 20, //点半径
      axisRadius: 2, // 轴半径
      axisLength: 50 // 轴长度
    })

    await this.addHelper('transitionPointHelper', transitionPointHelper)

    // 创建一个跟随运动助手并添加到系统中
    const followMotionHelper = new FollowMotionHelper()
    await this.addHelper('followMotionHelper', followMotionHelper)

    // 创建一个关节运动助手并添加到系统中
    const jointMotionHelper = new JointMotionHelper()
    await this.addHelper('jointMotionHelper', jointMotionHelper)

    // 创建一个光标助手并添加到系统中
    const robotCursorHelper = new RobotCursorHelper({
      size: 100,
      width: 8,
      inverseCalHandler: async () => {}
    })
    await this.addHelper('robotCursorHelper', robotCursorHelper)

    // 创建一个装配辅助对象
    const assembleHelper = new AssembleHelper()

    await this.addHelper('assembleHelper', assembleHelper)
  }

  updatePose(data: any, deltaTime: number) {
    const { jointCoordinate } = data

    const cr20Entity = this.entityMap.get('cr20') as Cr20Entity
    const helper = this.getHelper('joint-motion') as JointMotionHelper
    helper.updateJointValues(cr20Entity.Name, jointCoordinate, deltaTime)
  }

  async handleConveyorData(boxList: any, stepTime: number) {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams
    const conveyorEntity = this.entityMap.get('conveyor') as ConveyorEntity

    //解析传送带上的盒子
    for (const item of boxList) {
      //item [id type loaction state]
      const id = item[0]
      const type = item[1]
      const location = item[2]
      const state = item[3]

      if (state == BoxStateType.ONROBOT) continue //已经被抓起了

      const pose = localToWorld(
        conveyorEntity.Position,
        conveyorEntity.Quaternion,
        new THREE.Vector3(
          conveyorEntity.UserData.size.length / 2 - location,
          conveyorEntity.UserData.size.height,
          0
        ),
        new THREE.Euler(0, Math.PI / 2, 0)
      )

      const entityName = `box${id}`

      const boxEntity = this.entityMap.get(entityName) as BoxEntity
      if (!boxEntity) {
        const entity = new BoxEntity({
          name: entityName,
          type: 'box',
          position: pose.position,
          rotation: new THREE.Euler().setFromQuaternion(pose.quaternion),
          size: {
            width: box.left.width,
            length: box.left.length,
            height: box.left.height
          },
          weight: box.left.weight
        })
        await this.addEntity(entity)
      } else {
        boxEntity.updatePose(pose.position, boxEntity.Quaternion, stepTime)
      }
    }
  }

  handleLiftingAction(data: any) {
    const cr20Entity = this.entityMap.get('cr20') as Cr20Entity
    cr20Entity?.updateLifting(data[0], data[1])
  }

  //设置跟随机械臂
  setFollowRobotArm(entity: Entity) {
    const cr20Entity = this.entityMap.get('cr20') as Cr20Entity

    const helper = this.getHelper('follow-motion') as FollowMotionHelper
    helper.attach(entity.Name, {
      entity,
      followObject: cr20Entity.getObjectByName('J6')
    })
  }

  async handleBoxOnRobot(boxList: any) {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams
    const conveyorEntity = this.entityMap.get('conveyor') as ConveyorEntity

    const helper = this.getHelper('follow-motion') as FollowMotionHelper

    boxList.forEach((data: any) => {
      const id = data[0]
      const layerNum = data[1]
      const palletNum = data[2]
      const state = data[3]

      const entityName = `box${id}`
      const boxEntity = this.entityMap.get(entityName) as BoxEntity

      if (boxEntity) {
        if (state == BoxStateType.ONROBOT && !helper.has(entityName)) {
          //机械臂抓取
          this.PhysicEnv.addCollideCheckEntity(boxEntity)
          this.setFollowRobotArm(boxEntity)
          this.boxContainer.add(id, palletNum, layerNum, boxEntity)
        } else if (state == BoxStateType.ONPALLET && helper.has(entityName)) {
          // 机械臂放下
          helper.detach(entityName)
          boxEntity.changePhyBodyCollisionType(CollisionType.DYNAMICS)
          this.PhysicEnv.removeCollideCheckEntity(boxEntity)
          this.handleBoxCount(this.totalBoxNum) //只在放下的时候统计

          this.boxContainer.setNotOnRobot(boxEntity)
          delayExecute(() => {
            boxEntity.setPhyBodyToStatic()
          }, 5000)
        }
      }
    })
  }

  handleBoxOnPallet(data: any) {}

  handleRobotAction(data: any) {
    const boxOnRobot = data[0]
    const jointCoordinate = data[1]
    this.updatePose({ jointCoordinate }, 0)
    this.handleBoxOnRobot(boxOnRobot)
  }

  handlePartitionAction(data: any) {
    const action = data[0]
    const jointCoordinate = data[1]

    const helper = this.getHelper('follow-motion') as FollowMotionHelper

    if (action) {
      if (!this.partitionOnRobot) {
        this.updatePose({ jointCoordinate }, 0)
        this.partitionOnRobot = this.entityMap.get(`partition${this.nextPartitionNum}`)
        this.nextPartitionNum += 1

        if (this.nextPartitionNum == this.partitionNum) {
          //生成新隔板
          setTimeout(() => {
            this.configurePartitions()
          }, 3000)
        }

        if (this.partitionOnRobot) {
          this.setFollowRobotArm(this.partitionOnRobot)
          this._app.PhysicEnv.addCollideCheckEntity(this.partitionOnRobot)
        }
      }
    } else {
      if (this.partitionOnRobot) {
        helper.detach(this.partitionOnRobot.Name)

        this.partitionOnRobot.changePhyBodyCollisionType(CollisionType.DYNAMICS)

        setTimeout(() => {
          this.partitionOnRobot?.setPhyBodyToStatic()
        }, 3000)

        this._app.PhysicEnv.removeCollideCheckEntity(this.partitionOnRobot)
        this.partitionOnRobot = undefined
      }
    }
  }

  getPoseOnPallet(boxInf: BoxItem, index: number) {
    const { basic, signal, box, pallet, conveyer, template, layer, motion } = this.palletParams
    const { typeA, typeB, typeC, typeD } = template
    const types = [typeA, typeB, typeC, typeD]

    const interval = 10
    const boxSide = boxInf.pallet == 1 ? box.left : box.right
    const layerSide = boxInf.pallet == 1 ? layer.left : layer.right

    let offset = 0
    for (let layerNum = 0; layerNum < layerSide.count; layerNum++) {
      const type = types[layerSide.order[layerNum]]
      const style = type?.style || []
      for (const item of style) {
        const num = item.connection > 0 ? 2 : 1
        offset += num

        if (index < offset) {
          let matrix
          if (boxInf.pallet == 1) {
            matrix = poseToMatrix(
              [
                item.x,
                -pallet.size.width - item.y,
                layerNum * box.left.height,
                90,
                0,
                90 * item.rotate
              ],
              this.leftUserMatrix
            )
          } else {
            matrix = poseToMatrix(
              [
                item.x,
                item.y + pallet.size.width,
                layerNum * box.right.height,
                90,
                0,
                90 * item.rotate
              ],
              this.rightUserMatrix
            )
          }

          if (item.connection == 1) {
            if (boxSide.length > boxSide.width) {
              matrix =
                index < offset - 1
                  ? poseToMatrix([0, -boxSide.width / 2 - interval, 0], matrix)
                  : poseToMatrix([0, boxSide.width / 2 + interval, 0], matrix)
            } else {
              matrix =
                index < offset - 1
                  ? poseToMatrix([-boxSide.length / 2 - interval, 0, 0], matrix)
                  : poseToMatrix([boxSide.length / 2 + interval, 0, 0], matrix)
            }
          } else if (item.connection == 2) {
            if (boxSide.length > boxSide.width) {
              matrix =
                index < offset - 1
                  ? poseToMatrix([-boxSide.length / 2 - interval, 0, 0], matrix)
                  : poseToMatrix([boxSide.length / 2 + interval, 0, 0], matrix)
            } else {
              matrix =
                index < offset - 1
                  ? poseToMatrix([0, -boxSide.width / 2 - interval, 0], matrix)
                  : poseToMatrix([0, boxSide.width / 2 + interval, 0], matrix)
            }
          }

          return decompose(matrix)
        }
      }
    }
    return null
  }

  handleBoxInBoxContainer() {}

  handleBoxCount(boxCount: any) {
    const { EventManager, view } = this._app
    EventManager.publish('simulate:statistic', boxCount)
  }

  handleProcess(data: any) {
    const stepTime = data[0]
    const boxListOnConveyor = data[1]
    const boxListOnLeft = data[2]
    const boxListOnRight = data[3]
    this.totalBoxNum = data[4]

    this.handleConveyorData(boxListOnConveyor, stepTime)
    this.handleBoxOnPallet(boxListOnLeft)
    this.handleBoxOnPallet(boxListOnRight)
    this.handleBoxInBoxContainer()
  }
}