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

// 根据索引获取共享相邻顶点的索引
function findSharedVertices(index1: number, index2: number, indices: any, vertices: any) {
  const sharedVertices = []
  const point1 = new THREE.Vector3().fromArray(vertices, index1 * 3)
  const point2 = new THREE.Vector3().fromArray(vertices, index2 * 3)

  for (let i = 0; i < indices.length; i += 3) {
    const npoint1 = new THREE.Vector3().fromArray(vertices, indices[i] * 3)
    const npoint2 = new THREE.Vector3().fromArray(vertices, indices[i + 1] * 3)
    const npoint3 = new THREE.Vector3().fromArray(vertices, indices[i + 2] * 3)

    if (
      (npoint1.distanceTo(point1) === 0 && npoint2.distanceTo(point2) === 0) ||
      (npoint1.distanceTo(point2) === 0 && npoint2.distanceTo(point1) === 0) ||
      (npoint2.distanceTo(point1) === 0 && npoint3.distanceTo(point2) === 0) ||
      (npoint2.distanceTo(point2) === 0 && npoint3.distanceTo(point1) === 0) ||
      (npoint3.distanceTo(point1) === 0 && npoint1.distanceTo(point2) === 0) ||
      (npoint3.distanceTo(point2) === 0 && npoint1.distanceTo(point1) === 0)
    ) {
      sharedVertices.push(indices[i], indices[i + 1], indices[i + 2])
      break
    }
  }
  return sharedVertices
}

// 根据共享相邻顶点计算相邻三角形的法向量
function calculateFaceNormal(sharedVertices, vertices) {
  const v1 = new THREE.Vector3().fromArray(vertices, sharedVertices[0] * 3)
  const v2 = new THREE.Vector3().fromArray(vertices, sharedVertices[1] * 3)
  const v3 = new THREE.Vector3().fromArray(vertices, sharedVertices[2] * 3)

  return new THREE.Vector3().crossVectors(v2.clone().sub(v1), v3.clone().sub(v1)).normalize()
}

function extractAndShowBoundary(group, scene, threshold) {
  const boundaryVertices = []

  group.traverse((child) => {
    if (child instanceof THREE.Mesh) {
      child.updateMatrixWorld(true)
      const geometry = child.geometry
      if (!geometry.index) return
      const indices = geometry.index.array
      const vertices = geometry.attributes.position.array
      const normals = geometry.attributes.normal.array

      for (let i = 0; i < indices.length; i += 3) {
        const faceNormal = calculateFaceNormal(indices.slice(i, i + 3), vertices)
        const point1 = new THREE.Vector3(
          vertices[indices[i] * 3],
          vertices[indices[i] * 3 + 1],
          vertices[indices[i] * 3 + 2]
        )
        point1.applyMatrix4(child.matrixWorld)

        const point2 = new THREE.Vector3(
          vertices[indices[i + 1] * 3],
          vertices[indices[i + 1] * 3 + 1],
          vertices[indices[i + 1] * 3 + 2]
        )
        point2.applyMatrix4(child.matrixWorld)

        const point3 = new THREE.Vector3(
          vertices[indices[i + 2] * 3],
          vertices[indices[i + 2] * 3 + 1],
          vertices[indices[i + 2] * 3 + 2]
        )
        point3.applyMatrix4(child.matrixWorld)

        // boundaryVertices.push(point1.x, point1.y, point1.z)
        // boundaryVertices.push(point2.x, point2.y, point2.z)

        // boundaryVertices.push(point2.x, point2.y, point2.z)
        // boundaryVertices.push(point3.x, point3.y, point3.z)

        // boundaryVertices.push(point3.x, point3.y, point3.z)
        // boundaryVertices.push(point1.x, point1.y, point1.z)

        const sharedVertices1 = findSharedVertices(
          indices[i],
          indices[i + 1],
          indices.slice(i + 3),
          vertices
        )
        console.log(i, sharedVertices1)
        if (sharedVertices1.length) {
          const faceNormal1 = calculateFaceNormal(sharedVertices1, vertices)
          if (Math.abs(faceNormal.dot(faceNormal1)) <= threshold) {
            boundaryVertices.push(point1.x, point1.y, point1.z)
            boundaryVertices.push(point2.x, point2.y, point2.z)
          }
        }

        const sharedVertices2 = findSharedVertices(
          indices[i + 1],
          indices[i + 2],
          indices.slice(i + 3),
          vertices
        )
        if (sharedVertices2.length) {
          const faceNormal1 = calculateFaceNormal(sharedVertices2, vertices)
          if (Math.abs(faceNormal.dot(faceNormal1)) <= threshold) {
            boundaryVertices.push(point2.x, point2.y, point2.z)
            boundaryVertices.push(point3.x, point3.y, point3.z)
          }
        }

        const sharedVertices3 = findSharedVertices(
          indices[i + 2],
          indices[i],
          indices.slice(i + 3),
          vertices
        )
        if (sharedVertices3.length) {
          const faceNormal1 = calculateFaceNormal(sharedVertices3, vertices)
          if (Math.abs(faceNormal.dot(faceNormal1)) <= threshold) {
            boundaryVertices.push(point3.x, point3.y, point3.z)
            boundaryVertices.push(point1.x, point1.y, point1.z)
          }
        }
      }
    }
  })

  const boundaryGeometry = new THREE.BufferGeometry()
  boundaryGeometry.setAttribute(
    'position',
    new THREE.BufferAttribute(new Float32Array(boundaryVertices), 3)
  )

  const boundaryMaterial = new THREE.LineBasicMaterial({
    color: 0xff0000,
    depthTest: false,
    depthWrite: false
  })
  const boundaryLines = new THREE.LineSegments(boundaryGeometry, boundaryMaterial)

  scene.add(boundaryLines) // 将边界线添加到场景中
}

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

  // 当系统被添加到应用中时调用此方法
  async onAdded() {
    // 创建一个箱子实体
    const boxEntity = new BoxEntity({
      name: 'box1', // 实体名称
      position: new THREE.Vector3(0, 0, 0), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      size: { length: 480, width: 320, height: 200 }, // 尺寸
      weight: 0.5, // 重量
      barcode: {
        position: 'left'
      }
    })

    // 将箱子实体添加到系统中
    await this.addEntity(boxEntity)

    // 创建一个传送带实体
    const conveyorEntity = new ConveyorEntity({
      name: 'conveyor1', // 实体名称

      position: new THREE.Vector3(0, 0, 1000), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      size: { length: 5000, width: 800, height: 800 }, // 尺寸
      arrowDirection: 0,
      style: 'belt' // 传送带样式
    })

    // 将传送带实体添加到系统中
    await this.addEntity(conveyorEntity)

    // 创建一个机器人臂实体
    const cr20Entity = new Cr20Entity({
      name: 'cr20', // 实体名称
      position: new THREE.Vector3(0, 0, -1000), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      lift: 1, // 升降参数
      paths: {
        robotArm: '/static/model/sim/cr20.glb',
        column: '/static/model/sim/md-lz-v2.glb',
        liftingColumn: '/static/model/sim/md-sj-v2.glb'
        // pako: true
      }
    })

    // 将机器人臂实体添加到系统中
    await this.addEntity(cr20Entity)
    // cr20Entity.setEmissive('red', 0.5, 'J1')
    cr20Entity.updateLifting(900, 1000)

    // 创建一个吸盘臂实体
    const suckerEntity = new SuckerEntity({
      name: 'sucker', // 实体名称
      position: new THREE.Vector3(0, 0, 0), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      size: {
        length: 628,
        width: 350,
        height: 70
      },
      offset: {
        x: -204,
        y: 0
      }
    })

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

    // const mesh = new THREE.Mesh(new THREE.BoxGeometry(1000, 1000, 1000))
    // this._app.scene.add(mesh)

    // 创建一个焊枪臂实体
    const weldingGunEntity = new WeldingGunEntity({
      name: 'weldingGun', // 实体名称
      position: new THREE.Vector3(0, 0, 0), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      paths: {
        weldingGun: '/static/model/sim/weldinggun.glb',
        decoder: '/static/draco/gltf/'
      }
    })

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

    // 创建一个托盘实体
    const palletEntity = new PalletEntity({
      name: 'pallet1', // 实体名称
      position: new THREE.Vector3(2000, 0, -1000), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      size: { length: 1200, width: 1010, height: 140 } // 尺寸
    })

    // 将托盘实体添加到系统中
    await this.addEntity(palletEntity)

    // 创建一个隔板实体
    const partitionEntity = new PartitionEntity({
      name: 'partition1', // 实体名称
      position: new THREE.Vector3(-2000, 0, -1000), // 初始位置
      rotation: new THREE.Euler(0, 0, 0), // 初始旋转
      size: { length: 1200, width: 1010, height: 5 } // 尺寸
    })

    // 将隔板实体添加到系统中
    await this.addEntity(partitionEntity)

    extractAndShowBoundary(partitionEntity.group, this._app.scene, 0.5)

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

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

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

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

    const jointAxesHelper = new UserAxesHelper({
      axisRadius: 3, // 轴半径
      axisLength: 100 // 轴长度
    })

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

    // 将用户轴辅助对象附加到指定位置
    jointAxesHelper.attach('J1', {
      matrixWorld: poseToMatrix(
        [0, 0, 0, -90, 0, 90],
        cr20Entity.getObjectByName('J1')?.matrixWorld,
        'XYZ'
      )
    })

    // 将用户轴辅助对象附加到指定位置
    jointAxesHelper.attach('J2', {
      matrixWorld: poseToMatrix(
        [0, 0, 0, 0, 90, 0],
        cr20Entity.getObjectByName('J2')?.matrixWorld,
        'XYZ'
      )
    })

    // 将用户轴辅助对象附加到指定位置
    jointAxesHelper.attach('J3', {
      matrixWorld: poseToMatrix(
        [0, 0, 0, 0, 90, 0],
        cr20Entity.getObjectByName('J3')?.matrixWorld,
        'XYZ'
      )
    })

    // 将用户轴辅助对象附加到指定位置
    jointAxesHelper.attach('J4', {
      matrixWorld: poseToMatrix(
        [0, 0, 0, 0, 90, 0],
        cr20Entity.getObjectByName('J4')?.matrixWorld,
        'XYZ'
      )
    })

    // 将用户轴辅助对象附加到指定位置
    jointAxesHelper.attach('J5', {
      matrixWorld: poseToMatrix(
        [0, 0, 0, -90, 0, 0],
        cr20Entity.getObjectByName('J5')?.matrixWorld,
        'XYZ'
      )
    })

    // 将用户轴辅助对象附加到指定位置
    jointAxesHelper.attach('J6', {
      matrixWorld: poseToMatrix(
        [0, 0, 0, 0, 90, 0],
        cr20Entity.getObjectByName('J6')?.matrixWorld,
        'XYZ'
      )
    })

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

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

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

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

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

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

    await this.addHelper('transition-point-helper', transitionPointHelper)

    // 将过渡点辅助对象附加到指定位置
    transitionPointHelper.attach('point1', {
      color: 'red',
      matrixWorld: poseToMatrix([-1000, 1000, 2000, 0, 0, 0], new THREE.Matrix4()) // 使用位置和旋转矩
    })

    // 创建一个工具助手并添加到系统中
    const assembleHelper = new AssembleHelper()
    await this.addHelper('assembleHelper', assembleHelper)
    assembleHelper.attach('sucker', {
      child: weldingGunEntity,
      parent: cr20Entity,
      anchor: 'TOOL'
    })

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

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

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

    followMotionHelper.attach('follow1', {
      follower: boxEntity, // 要跟随的实体
      guide: cr20Entity,
      anchor: 'J6'
    })

    // 创建一个关节运动助手并添加到系统中
    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]
    })

    setTimeout(() => {
      jointMotionHelper.updateJointValues(cr20Entity.Name, [90, 0, 90, 0, -90, 0], 1000)
    }, 1000)
  }

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