// src/core/RagdollPhysics.ts
import * as AmmoModule from 'ammojs3';
import { Vector3, Quaternion } from 'three';
import { PhysicsManager, PhysicsProperties, AmmoInstance } from './PhysicsManager';

interface BoneConfig {
  name: string;
  dimensions: Vector3;
  position: Vector3;
  rotation: Quaternion;
  mass: number;
  parentBone?: string;
  jointType: 'hinge' | 'cone' | 'point';
  jointLimits?: {
    minAngle?: Vector3;
    maxAngle?: Vector3;
    swingSpan1?: number;
    swingSpan2?: number;
    twistSpan?: number;
  };
}

interface RagdollBone {
    id: string;
    body: AmmoInstance['btRigidBody'];
    constraint?: AmmoInstance['btTypedConstraint'];
  }

export class RagdollPhysics {
  private physicsManager: PhysicsManager;
  private bones: Map<string, RagdollBone> = new Map();
  private isInitialized: boolean = false;

  constructor(physicsManager: PhysicsManager) {
    this.physicsManager = physicsManager;
  }

  /**
   * Creates a ragdoll from a configuration of bones
   */
  public createRagdoll(config: BoneConfig[]): void {
    if (!this.physicsManager) {
      throw new Error('PhysicsManager must be initialized first');
    }

    // Create rigid bodies for each bone
    config.forEach(bone => {
      this.createBone(bone);
    });

    // Create constraints between bones
    config.forEach(bone => {
      if (bone.parentBone) {
        this.createJointConstraint(bone, config.find(b => b.name === bone.parentBone)!);
      }
    });

    this.isInitialized = true;
  }

  /**
   * Creates a single bone rigid body
   */
  private createBone(config: BoneConfig): void {
    const transform = new Ammo.btTransform();
    transform.setIdentity();
    transform.setOrigin(new Ammo.btVector3(config.position.x, config.position.y, config.position.z));
    transform.setRotation(new Ammo.btQuaternion(config.rotation.x, config.rotation.y, config.rotation.z, config.rotation.w));

    const motionState = new Ammo.btDefaultMotionState(transform);
    const shape = new Ammo.btBoxShape(new Ammo.btVector3(
      config.dimensions.x * 0.5,
      config.dimensions.y * 0.5,
      config.dimensions.z * 0.5
    ));

    const localInertia = new Ammo.btVector3(0, 0, 0);
    shape.calculateLocalInertia(config.mass, localInertia);

    const rbInfo = new Ammo.btRigidBodyConstructionInfo(
      config.mass,
      motionState,
      shape,
      localInertia
    );

    const body = new Ammo.btRigidBody(rbInfo);

    // Set additional properties
    body.setDamping(0.05, 0.85);
    body.setFriction(0.8);
    body.setRestitution(0.1);

    this.bones.set(config.name, { id: config.name, body });
    // Add collision group and mask for better performance
    const collisionGroup = 2;  // Ragdoll group
    const collisionMask = -1;  // Collide with everything
    this.physicsManager['physicsWorld'].addRigidBody(body, collisionGroup, collisionMask);
  }

  /**
   * Creates a constraint between two bones
   */
  private createJointConstraint(childBone: BoneConfig, parentBone: BoneConfig): void {
    const childRagdollBone = this.bones.get(childBone.name);
    const parentRagdollBone = this.bones.get(parentBone.name);

    if (!childRagdollBone || !parentRagdollBone) {
      throw new Error(`Bones not found: ${childBone.name} or ${parentBone.name}`);
    }

    let constraint: AmmoInstance['btTypedConstraint'];

    const parentTransform = new Ammo.btTransform();
    const childTransform = new Ammo.btTransform();

    parentTransform.setIdentity();
    childTransform.setIdentity();

    // Add offset handling for joint positions
    const parentPivot = new Ammo.btVector3(
      childBone.position.x - parentBone.position.x,
      childBone.position.y - parentBone.position.y,
      childBone.position.z - parentBone.position.z
    );
    
    parentTransform.setOrigin(parentPivot);

    switch (childBone.jointType) {
      case 'hinge': {
        const hinge = new Ammo.btHingeConstraint(
          parentRagdollBone.body,
          childRagdollBone.body,
          parentTransform,
          childTransform,
          true
        );

        if (childBone.jointLimits) {
          const { minAngle, maxAngle } = childBone.jointLimits;
          if (minAngle && maxAngle) {
            hinge.setLimit(minAngle.y, maxAngle.y);
          }
        }

        constraint = hinge;
        break;
      }

      case 'cone': {
        const cone = new Ammo.btConeTwistConstraint(
          parentRagdollBone.body,
          childRagdollBone.body,
          parentTransform,
          childTransform
        );

        if (childBone.jointLimits) {
          const { swingSpan1, swingSpan2, twistSpan } = childBone.jointLimits;
          cone.setLimit(swingSpan1 || 0.5, swingSpan2 || 0.5, twistSpan || 0.5);
        }

        constraint = cone;
        break;
      }

      case 'point': {
        const point = new Ammo.btPoint2PointConstraint(
          parentRagdollBone.body,
          childRagdollBone.body,
          new Ammo.btVector3(0, 0, 0),
          new Ammo.btVector3(0, 0, 0)
        );

        constraint = point;
        break;
      }

      default:
        throw new Error(`Unsupported joint type: ${childBone.jointType}`);
    }

    this.physicsManager['physicsWorld'].addConstraint(constraint, true);
    childRagdollBone.constraint = constraint;
  }

  /**
   * Applies an impulse force to a specific bone
   */
  public applyImpulse(boneName: string, impulse: Vector3, worldPoint?: Vector3): void {
    const bone = this.bones.get(boneName);
    if (!bone) {
      throw new Error(`Bone ${boneName} not found`);
    }

    const btImpulse = new Ammo.btVector3(impulse.x, impulse.y, impulse.z);
    if (worldPoint) {
      const btWorldPoint = new Ammo.btVector3(worldPoint.x, worldPoint.y, worldPoint.z);
      bone.body.applyImpulse(btImpulse, btWorldPoint);
    } else {
      bone.body.applyCentralImpulse(btImpulse);
    }
  }

  /**
   * Gets the current position and rotation of a bone
   */
  public getBoneTransform(boneName: string): { position: Vector3; rotation: Quaternion } {
    const bone = this.bones.get(boneName);
    if (!bone) {
      throw new Error(`Bone ${boneName} not found`);
    }

    const transform = new Ammo.btTransform();
    bone.body.getMotionState().getWorldTransform(transform);

    const position = new Vector3(
      transform.getOrigin().x(),
      transform.getOrigin().y(),
      transform.getOrigin().z()
    );

    const rotation = new Quaternion(
      transform.getRotation().x(),
      transform.getRotation().y(),
      transform.getRotation().z(),
      transform.getRotation().w()
    );

    return { position, rotation };
  }

  /**
   * Cleans up physics resources for the ragdoll
   */
  public setKinematic(boneName: string, isKinematic: boolean): void {
    const bone = this.bones.get(boneName);
    if (!bone) {
      throw new Error(`Bone ${boneName} not found`);
    }

    if (isKinematic) {
      bone.body.setCollisionFlags(bone.body.getCollisionFlags() | 2);
      bone.body.setActivationState(4); // DISABLE_DEACTIVATION
    } else {
      bone.body.setCollisionFlags(bone.body.getCollisionFlags() & ~2);
      bone.body.setActivationState(1); // ACTIVE_TAG
    }
  }

  public cleanup(): void {
    this.bones.forEach(bone => {
      if (bone.constraint) {
        this.physicsManager['physicsWorld'].removeConstraint(bone.constraint);
        Ammo.destroy(bone.constraint);
      }
      this.physicsManager['physicsWorld'].removeRigidBody(bone.body);
      Ammo.destroy(bone.body);
    });

    this.bones.clear();
    this.isInitialized = false;
  }
}