import GetForwardVector from 'Assets/Library/GetForwardVector';
import * as RE from 'rogue-engine';
import * as THREE from 'three'
import RAPIER from '@dimforge/rapier3d-compat';
import RapierBody from '@RE/RogueEngine/rogue-rapier/Components/RapierBody.re';
import RapierConfig from '@RE/RogueEngine/rogue-rapier/Components/RapierConfig.re';
import RogueRapier from '@RE/RogueEngine/rogue-rapier/Lib/RogueRapier';

export default class RapierMovementController extends RE.Component {
  initialized = false
  characterController: RAPIER.KinematicCharacterController
  rapierConfig: RapierConfig

  @RE.props.num() speed: number = 1
  @RE.props.num() rotationSpeed: number = 1
  // @RE.props.num() jumpStrength: number = 10
  @RE.props.checkbox() useCameraForward: boolean = true
  @RE.props.checkbox() useObjectForward: boolean = false
  cameraVectorCalculator: GetForwardVector
  objectVectorCalculator: GetForwardVector
  bodyComponent: RapierBody
  velocity: THREE.Vector3 = new THREE.Vector3(0,0,0)

  @RE.props.vector3() drag: THREE.Vector3 = new THREE.Vector3(0.002, 0.002, 0.002)



  // private jumpVector: RAPIER.Vector3 = new RAPIER.Vector3(0,1,0)
  private scaledVelocity: THREE.Vector3 = new THREE.Vector3(0,0,0)
  private rapierScaledVelocity: RAPIER.Vector3 = new RAPIER.Vector3(0,0,0)

  elapsed = 0

  @RE.props.checkbox() thrust = false
  @RE.props.checkbox() brake = false
  @RE.props.checkbox() strafeLeft = false
  @RE.props.checkbox() strafeRight = false
  @RE.props.checkbox() rotateLeft = false
  @RE.props.checkbox() rotateRight = false

  awake() {
    this.bodyComponent = RE.getComponent(RapierBody, this.object3d) as RapierBody
  }

  start() {
    this.cameraVectorCalculator = new GetForwardVector(RE.Runtime.camera)
    this.objectVectorCalculator = new GetForwardVector(this.object3d)
  }

  beforeUpdate(): void {
    if (!RogueRapier.initialized) return;
    !this.initialized && this.init();
  }

  init() {
    this.characterController = RogueRapier.world.createCharacterController(0.1)
    // this.characterController.enableAutostep(this.autostepMaxHeight, this.autostepMinWidth, this.autostepIncludeDynamicBodies)
    // this.characterController.enableSnapToGround(this.snapToGroundDistance)
    this.characterController.setCharacterMass(100)
    this.characterController.setApplyImpulsesToDynamicBodies(true)
    // this.characterController.setSlideEnabled(this.slideEnabled)
  }

  update() {
    if(!this.bodyComponent.body) {
      return
    }
    this.elapsed += RE.Runtime.deltaTime

    // if(direction.x != 0) {
    // this.moveForward(direction.x * this.speed)
    // }
    // if(direction.y != 0) {
    // this.moveRight(direction.y * this.speed)
    // }

    // if(direction.z != 0) {
    //   const floorCheckComponent = RE.getComponent(FloorCheckComponent, this.object3d)
    //   if(floorCheckComponent) {
    //     if(floorCheckComponent.isOnFloor) {
    //       this.jumpVector.y = this.jumpStrength
    //       this.bodyComponent.body.applyImpulse(this.jumpVector, true)
    //     }
    //   }
    // }

    if(this.thrust) {
      this.moveForward(1 * this.speed)
    }
    if(this.brake) {
      this.moveForward(-1 * this.speed)
    }

    if(this.rotateLeft) {
      this.rotate(1 * this.rotationSpeed)
    }

    if(this.rotateRight) {
      this.rotate(-1 * this.rotationSpeed)
    }

    const fixedStep = RE.Runtime.deltaTime
    const nextPosition = this.bodyComponent.body.translation()
    const nextVelocity = this.convertRapierToThreeVec3(this.bodyComponent.body.linvel())

    nextVelocity.x *= (1 - this.drag.x)
    nextVelocity.y *= (1 - this.drag.y)
    nextVelocity.z *= (1 - this.drag.z)

    this.velocity.x *= (1 - this.drag.x)
    this.velocity.y *= (1 - this.drag.x)
    this.velocity.z *= (1 - this.drag.x)

    nextVelocity.x += this.velocity.x
    nextVelocity.y += this.velocity.y
    nextVelocity.z += this.velocity.z
    
   

    this.characterController.computeColliderMovement(
      this.bodyComponent.body.collider(0),
      nextVelocity.clone().multiplyScalar(fixedStep),
    )

    const characterMovement = this.characterController.computedMovement()

    nextPosition.x += characterMovement.x
    //nextPosition.y += characterMovement.y
    nextPosition.y = 0
    nextPosition.z += characterMovement.z

    this.bodyComponent.body.setNextKinematicTranslation(nextPosition)

    this.thrust = false
    this.brake = false
    this.strafeLeft = false
    this.strafeRight = false
    this.rotateLeft = false
    this.rotateRight = false
  }

  moveForward(distance) {

    let forwardVector;
    if(this.useCameraForward) {
      // forwardVector = this.cameraVectorCalculator.getForward()
      forwardVector = this.objectVectorCalculator.getForward()
      
      let euler = new THREE.Euler(0,0,0, 'XZY')
      euler.setFromQuaternion(RE.Runtime.camera.quaternion.normalize())
      euler.x = 0
      euler.z = 0
      let newRotation = new THREE.Quaternion()
      newRotation.setFromEuler(euler)
      const nextRotation = this.convertRapierToThreeQuat(this.bodyComponent.body.rotation())
      nextRotation.slerp(newRotation, RE.Runtime.deltaTime)
      this.bodyComponent.body.setNextKinematicRotation(nextRotation)
    } else {
      forwardVector = this.objectVectorCalculator.getForward()
    }
    
    this.scaledVelocity.set(0,0,0)
    this.scaledVelocity.addScaledVector(forwardVector, distance)

    // this.velocity.x += this.scaledVelocity.x
    // this.velocity.y += this.scaledVelocity.y
    // this.velocity.z += this.scaledVelocity.z    

    this.velocity.x += this.scaledVelocity.x
    this.velocity.y += this.scaledVelocity.y
    this.velocity.z += this.scaledVelocity.z    
  }

  moveRight(distance) {
    let rightVector;
    if(this.useCameraForward) {
      rightVector = this.cameraVectorCalculator.getRight()
    } else {
      rightVector = this.objectVectorCalculator.getRight()
    }
      this.scaledVelocity.set(0,0,0)
      this.scaledVelocity.addScaledVector(rightVector, distance)
      this.rapierScaledVelocity.x = this.scaledVelocity.x
      this.rapierScaledVelocity.y = this.scaledVelocity.y
      this.rapierScaledVelocity.z = this.scaledVelocity.z
      this.bodyComponent.body.applyImpulse(this.rapierScaledVelocity, true)
    
  }

  rotate(amount) {

      const nextRotation = this.convertRapierToThreeQuat(this.bodyComponent.body.rotation())
      const newRotation = this.quaternionFromAxisAngle(this.object3d.up, amount)
      nextRotation.multiply(newRotation)
      this.bodyComponent.body.setNextKinematicRotation(nextRotation)

  }

  
  convertRapierToThreeQuat(oldQuaternion: RAPIER.Quaternion) {
    return new THREE.Quaternion(oldQuaternion.x, oldQuaternion.y, oldQuaternion.z, oldQuaternion.w)
  }

  convertRapierToThreeVec3(oldVector: RAPIER.Vector3) {
    return new THREE.Vector3(oldVector.x, oldVector.y, oldVector.z)
  }

  quaternionFromAxisAngle(axis, rotationRadians) {
    let factor = Math.sin(rotationRadians / 2)
    let x = axis.x * factor
    let y = axis.y * factor
    let z = axis.z * factor
    let w = Math.cos(rotationRadians / 2)
    let normalized = new THREE.Quaternion(x, y, z, w).normalize()
    return new THREE.Quaternion(normalized.x, normalized.y, normalized.z, normalized.w)
  }
}



RE.registerComponent(RapierMovementController);