import * as THREE from 'three'
import {
  BoxEntity,
  ConveyorEntity,
  Cr20Entity,
  FollowMotionHelper,
  JointMotionHelper,
  RobotCursorHelper,
  PalletEntity,
  PartitionEntity,
  System,
  TransitionPointHelper,
  UserAxesHelper,
  poseToMatrix,
  SuckerEntity,
  WeldingGunEntity,
  AssembleHelper,
  decompose,
  Entity,
  numberToSix,
  toAng,
  Cr10Entity
} from '../../src'
import vcControl from './vcControl'

// 定义一个示例系统类，继承自System基类
export class ExampleSystem extends System {
  box: THREE.Mesh // 定义一个属性来存储一个THREE.Mesh对象

  joints = [0, 0, 0, 0, 0, 0]

  realCursor = new THREE.Object3D()

  jointMatrix: Array<THREE.Matrix4> = []

  fixObject!: THREE.Group

  async logModelInformation(entity: Entity, names: Array<string>) {
    // entity.group.updateMatrixWorld(true)
    // entity.group.traverse((mesh: any) => {
    //   if (names.includes(mesh.name)) {
    //     console.log(mesh.name)
    //     const pose = decompose(mesh.matrixWorld)

    //     if (mesh) console.log(pose.position, new THREE.Euler().setFromQuaternion(pose.quaternion))
    //   }
    // })

    await this.getCal(entity)
  }

  // 当系统被添加到应用中时调用此方法
  async onAdded() {
    // 创建一个机器人臂实体
    const cr20Entity = new Cr10Entity({
      name: 'cr20', // 实体名称
      position: new THREE.Vector3(0, 0, 0), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      lift: 2, // 升降参数
      paths: {
        // robotArm: '/static/model/pako/cr20.pako',
        // column: './md-lz.pako',
        // liftingColumn: './md-sj.pako',
        // pako: true
        robotArm: './CR5.glb'
      }
    })

    // 将机器人臂实体添加到系统中
    await this.addEntity(cr20Entity)

    console.log(cr20Entity.group)

    const names = ['base', 'J1', 'J2', 'J3', 'J4', 'J5', 'J6', 'TOOL']
    await this.logModelInformation(cr20Entity, names)

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

    await this.addHelper('user-axes-helper', userAxesHelper)

    // 将用户轴辅助对象附加到指定位置
    userAxesHelper.attach('basic-user', {
      matrixWorld: cr20Entity.UserMatrix0
    })

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

    robotCursorHelper.attach('cr20', {
      toolObject: cr20Entity.getObjectByName('TOOL') as any,
      basePose: [0, 0, 0, -180, 90, 0],
      toolPose: [0, 0, 0, 0, 0, 0]
    })
    await this.addHelper('robotCursorHelper', robotCursorHelper)

    robotCursorHelper.attach('realCursor', {
      toolObject: this.realCursor,
      basePose: [0, 0, 0, 0, 0, 0],
      toolPose: [0, 0, 0, 0, 0, 0],
      color: 'red'
    })
    await this.addHelper('robotCursorHelper', robotCursorHelper)

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

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

    jointMotionHelper.attach(cr20Entity.Name, {
      entity: cr20Entity,
      jointDescriptions: cr20Entity.JointDescriptions,
      initialJoints: [0, 0, 0, 0, 0, 0]
    })

    // 创建一个关节轴辅助对象
    const jointAxesHelper = new UserAxesHelper({
      axisRadius: 3, // 轴半径
      axisLength: 100 // 轴长度
    })

    await this.addHelper('joint-axes-helper', jointAxesHelper)

    this.showJointAxes()

    const { EventManager } = this._app
    EventManager.subscribe(
      'rotate',
      (index: number) => {
        this.joints[index] += 90
        jointMotionHelper.updateJointValues(cr20Entity.Name, this.joints, -1)
        this.logModelInformation(cr20Entity, names)

        this.showJointAxes()
      },
      'example-system'
    )

    EventManager.subscribe(
      'reset',
      (index: number) => {
        this.joints[index] = 0
        jointMotionHelper.updateJointValues(cr20Entity.Name, this.joints, -1)
        this.logModelInformation(cr20Entity, names)

        this.showJointAxes()
      },
      'example-system'
    )

    EventManager.subscribe(
      'fix',
      (data: any) => {
        if (this.fixObject) this._app.scene.remove(this.fixObject)

        this.fixObject = new THREE.Group()
        this.fixObject.position.set(1000, 0, 0)
        this._app.scene.add(this.fixObject)

        const baseObject = new THREE.Group()
        baseObject.name = 'base'
        this.fixObject.add(baseObject)

        const j1Object = new THREE.Group()
        j1Object.name = 'joint1'
        baseObject.add(j1Object)

        const j2Object = new THREE.Group()
        j2Object.name = 'joint2'
        j1Object.add(j2Object)
        const j3Object = new THREE.Group()
        j3Object.name = 'joint3'
        j2Object.add(j3Object)
        const j4Object = new THREE.Group()
        j4Object.name = 'joint4'
        j3Object.add(j4Object)
        const j5Object = new THREE.Group()
        j5Object.name = 'joint5'
        j4Object.add(j5Object)
        const j6Object = new THREE.Group()
        j6Object.name = 'joint6'
        j5Object.add(j6Object)

        const toolObject = new THREE.Group()
        toolObject.name = 'tool'
        j6Object.add(toolObject)

        cr20Entity.group.updateMatrixWorld(true)

        for (let name of names) {
          const object: any = cr20Entity.getObjectByName(name)
          if (!object) continue

          if (name == 'base') {
            const pose = decompose(cr20Entity.UserMatrix0)
            baseObject.position.copy(pose.position)
            baseObject.quaternion.copy(pose.quaternion)

            let index = 0
            if (object.isMesh) {
              const cmesh = object.clone()
              cmesh.children.length && cmesh.remove(cmesh.children[0])
              cmesh.name = `base_${index++}`
              const matrix = cmesh.matrixWorld.multiply(cr20Entity.UserMatrix0.clone().invert())
              const pose = decompose(matrix)
              cmesh.position.copy(pose.position)
              cmesh.quaternion.copy(pose.quaternion)
              baseObject.add(cmesh)
            } else {
              object.children.forEach((child: any) => {
                if (!child.name.toLowerCase().includes('base')) return
                child.traverse((mesh) => {
                  if (mesh.isMesh) {
                    const cmesh = mesh.clone()
                    cmesh.name = `base_${index++}`
                    const martix = mesh.matrixWorld.multiply(
                      cr20Entity.UserMatrix0.clone().invert()
                    )
                    const pose = decompose(martix)
                    cmesh.position.copy(pose.position)
                    cmesh.quaternion.copy(pose.quaternion)
                    baseObject.add(cmesh)
                  }
                })
              })
            }
          }

          if (name == 'J1') {
            const matrix = cr20Entity.UserMatrix0.clone().invert().multiply(this.jointMatrix[0])
            const pose = decompose(matrix)
            j1Object.position.copy(pose.position)
            j1Object.quaternion.copy(pose.quaternion)

            let index = 0

            object.children.forEach((child: any) => {
              if (!child.name.includes('J1')) return
              child.traverse((mesh) => {
                if (mesh.isMesh) {
                  const cmesh = mesh.clone()
                  cmesh.name = `joint1_${index++}`
                  const matrix = this.jointMatrix[0].clone().invert().multiply(mesh.matrixWorld)
                  const pose = decompose(matrix)
                  cmesh.position.copy(pose.position)
                  cmesh.quaternion.copy(pose.quaternion)
                  // cmesh.matrixWorld.copy(matrix);
                  // cmesh.matrixWorldNeedsUpdate = true;
                  j1Object.add(cmesh)
                }
              })
            })
          }

          if (name == 'J2') {
            const matrix = this.jointMatrix[0].clone().invert().multiply(this.jointMatrix[1])
            const pose = decompose(matrix)
            j2Object.position.copy(pose.position)
            j2Object.quaternion.copy(pose.quaternion)

            let index = 0
            object.children.forEach((child: any) => {
              if (!child.name.includes('J2')) return
              child.traverse((mesh) => {
                if (mesh.isMesh) {
                  const cmesh = mesh.clone()
                  cmesh.name = `joint2_${index++}`
                  const matrix = this.jointMatrix[1].clone().invert().multiply(mesh.matrixWorld)
                  const pose = decompose(matrix)
                  cmesh.position.copy(pose.position)
                  cmesh.quaternion.copy(pose.quaternion)
                  j2Object.add(cmesh)
                }
              })
            })
          }

          if (name == 'J3') {
            const matrix = this.jointMatrix[1].clone().invert().multiply(this.jointMatrix[2])
            const pose = decompose(matrix)
            j3Object.position.copy(pose.position)
            j3Object.quaternion.copy(pose.quaternion)

            let index = 0
            object.children.forEach((child: any) => {
              if (!child.name.includes('J3')) return
              child.traverse((mesh) => {
                if (mesh.isMesh) {
                  const cmesh = mesh.clone()
                  cmesh.name = `joint3_${index++}`
                  const matrix = this.jointMatrix[2].clone().invert().multiply(mesh.matrixWorld)
                  const pose = decompose(matrix)
                  cmesh.position.copy(pose.position)
                  cmesh.quaternion.copy(pose.quaternion)
                  j3Object.add(cmesh)
                }
              })
            })
          }

          if (name == 'J4') {
            const matrix = this.jointMatrix[2].clone().invert().multiply(this.jointMatrix[3])
            const pose = decompose(matrix)
            j4Object.position.copy(pose.position)
            j4Object.quaternion.copy(pose.quaternion)

            let index = 0
            object.children.forEach((child: any) => {
              if (!child.name.includes('J4')) return
              child.traverse((mesh) => {
                if (mesh.isMesh) {
                  const cmesh = mesh.clone()
                  cmesh.name = `joint4_${index++}`
                  const matrix = this.jointMatrix[3].clone().invert().multiply(mesh.matrixWorld)
                  const pose = decompose(matrix)
                  cmesh.position.copy(pose.position)
                  cmesh.quaternion.copy(pose.quaternion)
                  j4Object.add(cmesh)
                }
              })
            })
          }

          if (name == 'J5') {
            const matrix = this.jointMatrix[3].clone().invert().multiply(this.jointMatrix[4])
            const pose = decompose(matrix)
            j5Object.position.copy(pose.position)
            j5Object.quaternion.copy(pose.quaternion)

            let index = 0
            console.log(object)
            object.children.forEach((child: any) => {
              if (!child.name.includes('J5')) return
              child.traverse((mesh) => {
                if (mesh.isMesh) {
                  const cmesh = mesh.clone()
                  cmesh.name = `joint5_${index++}`
                  const matrix = this.jointMatrix[4].clone().invert().multiply(mesh.matrixWorld)
                  const pose = decompose(matrix)
                  cmesh.position.copy(pose.position)
                  cmesh.quaternion.copy(pose.quaternion)
                  cmesh.scale.copy(pose.scale)
                  j5Object.add(cmesh)
                }
              })
            })
          }

          if (name == 'J6') {
            const matrix = this.jointMatrix[4].clone().invert().multiply(this.jointMatrix[5])
            const pose = decompose(matrix)
            j6Object.position.copy(pose.position)
            j6Object.quaternion.copy(pose.quaternion)

            let index = 0

            if (object.isMesh) {
              const cmesh = object.clone()
              cmesh.name = `joint6_${index++}`
              const matrix = this.jointMatrix[5].clone().invert().multiply(object.matrixWorld)
              const pose = decompose(matrix)
              cmesh.position.copy(pose.position)
              cmesh.quaternion.copy(pose.quaternion)
              j6Object.add(cmesh)
            } else {
              object.children.forEach((child: any) => {
                if (!child.name.includes('J6')) return
                child.traverse((mesh) => {
                  if (mesh.isMesh) {
                    const cmesh = mesh.clone()
                    cmesh.name = `joint6_${index++}`
                    const matrix = this.jointMatrix[5].clone().invert().multiply(mesh.matrixWorld)
                    const pose = decompose(matrix)
                    cmesh.position.copy(pose.position)
                    cmesh.quaternion.copy(pose.quaternion)
                    j6Object.add(cmesh)
                  }
                })
              })
            }
          }

          // if (name == 'TOOL') {
          //   const matrix = this.jointMatrix[5].clone().invert().multiply(this.jointMatrix[6])
          //   const pose = decompose(matrix)
          //   toolObject.position.copy(pose.position)
          //   toolObject.quaternion.copy(pose.quaternion)
          // }

          // if (name == 'J2') {
          //   const matrix = this.jointMatrix[0].clone().invert().multiply(this.jointMatrix[1])
          //   const pose = decompose(matrix)
          //   object.position.copy(pose.position)
          //   object.quaternion.copy(pose.quaternion)
          // }

          // if (name == 'J3') {
          //   const matrix = this.jointMatrix[1].clone().invert().multiply(this.jointMatrix[2])
          //   const pose = decompose(matrix)
          //   object.position.copy(pose.position)
          //   object.quaternion.copy(pose.quaternion)
          // }

          // if (name == 'J4') {
          //   const matrix = this.jointMatrix[2].clone().invert().multiply(this.jointMatrix[3])
          //   const pose = decompose(matrix)
          //   object.position.copy(pose.position)
          //   object.quaternion.copy(pose.quaternion)
          // }

          // if (name == 'J5') {
          //   const matrix = this.jointMatrix[3].clone().invert().multiply(this.jointMatrix[4])
          //   const pose = decompose(matrix)
          //   object.position.copy(pose.position)
          //   object.quaternion.copy(pose.quaternion)
          // }

          // if (name == 'J6') {
          //   const matrix = this.jointMatrix[4].clone().invert().multiply(this.jointMatrix[5])
          //   const pose = decompose(matrix)
          //   object.position.copy(pose.position)
          //   object.quaternion.copy(pose.quaternion)
          // }
        }

        this.fixObject.updateMatrixWorld(true)
        jointAxesHelper.attach('fBase', { matrixWorld: baseObject.matrixWorld })
        jointAxesHelper.attach('fJ1', { matrixWorld: j1Object.matrixWorld })
        jointAxesHelper.attach('fJ2', { matrixWorld: j2Object.matrixWorld })
        jointAxesHelper.attach('fJ3', { matrixWorld: j3Object.matrixWorld })
        jointAxesHelper.attach('fJ4', { matrixWorld: j4Object.matrixWorld })
        jointAxesHelper.attach('fJ5', { matrixWorld: j5Object.matrixWorld })
        jointAxesHelper.attach('fJ6', { matrixWorld: j6Object.matrixWorld })

        // j3Object.rotateZ(Math.PI / 2)

        // setTimeout(() => {
        //   j3Object.rotateZ(-Math.PI / 2)
        // }, 1000)

        this.logModelInformation(cr20Entity, names)
      },
      'example-system'
    )
  }

  showJointAxes() {
    const jointAxesHelper = this.getHelper('joint-axes-helper') as UserAxesHelper
    const cr20Entity = this.getEntity('cr20') as Cr20Entity
    jointAxesHelper.clearHelperObjects()

    let j1Matrix = poseToMatrix([0, 0, 147, 0, 0, 0], cr20Entity.UserMatrix0)
    j1Matrix = poseToMatrix([0, 0, 0, 0, 0, this.joints[0]], j1Matrix)
    jointAxesHelper.attach('J1', {
      matrixWorld: j1Matrix
    })

    let j2Matrix = poseToMatrix([0, 0, 0, 90, 90, 0], j1Matrix)
    j2Matrix = poseToMatrix([0, 0, 0, 0, 0, this.joints[1]], j2Matrix)
    jointAxesHelper.attach('J2', {
      matrixWorld: j2Matrix
    })

    let j3Matrix = poseToMatrix([-377, 0, 0, 0, 0, 0], j2Matrix)
    j3Matrix = poseToMatrix([0, 0, 0, 0, 0, this.joints[2]], j3Matrix)
    jointAxesHelper.attach('J3', {
      matrixWorld: j3Matrix
    })

    let j4Matrix = poseToMatrix([-307, 0, 141, 0, 0, -90], j3Matrix)
    j4Matrix = poseToMatrix([0, 0, 0, 0, 0, this.joints[3]], j4Matrix)
    jointAxesHelper.attach('J4', {
      matrixWorld: j4Matrix
    })

    let j5Matrix = poseToMatrix([0, -116, 0, 90, 0, 0], j4Matrix)
    j5Matrix = poseToMatrix([0, 0, 0, 0, 0, this.joints[4]], j5Matrix)
    jointAxesHelper.attach('J5', {
      matrixWorld: j5Matrix
    })

    let j6Matrix = poseToMatrix([0, 105, 0, -90, 0, 0], j5Matrix)
    j6Matrix = poseToMatrix([0, 0, 0, 0, 0, this.joints[5]], j6Matrix)
    jointAxesHelper.attach('J6', {
      matrixWorld: j6Matrix
    })

    this.jointMatrix.push(j1Matrix, j2Matrix, j3Matrix, j4Matrix, j5Matrix, j6Matrix)
    this.jointMatrix.forEach((item) => {
      const pose = decompose(item)
      console.log(pose.position)
    })

    console.log(this.realCursor.position)
  }

  async getCal(entity: Cr20Entity) {
    const data: any = await vcControl.forwardCal({
      joint: this.joints,
      user: 0,
      tool: 0
    })

    const matrix = poseToMatrix(data.coordinate, entity.UserMatrix0)

    const pose = decompose(matrix)

    // console.log('end', pose.position, new THREE.Euler().setFromQuaternion(pose.quaternion))

    this.realCursor.position.copy(pose.position)
    this.realCursor.quaternion.copy(pose.quaternion)
    this.realCursor.updateMatrixWorld(true)
  }

  onRemoved(): void {
    // 当系统从应用中移除时调用此方法
  }
  onUpdating(deltaTime: number): void {
    // 每帧更新时调用此方法，可以在这里添加自定义的更新逻辑
  }
}