package electrosphere.collision;

import org.joml.Quaterniond;
import org.joml.Vector3d;
import org.ode4j.math.DQuaternion;
import org.ode4j.math.DQuaternionC;
import org.ode4j.ode.DBody;
import org.ode4j.ode.DMass;

import electrosphere.game.data.collidable.CollidableTemplate;

/**
 * Utilities for leveraging the collision system to perform physics
 */
public class PhysicsUtils {
    
    /**
     * Gets the current position of a rigid body as a joml vector
     * @param body The dbody
     * @return The position
     */
    public static Vector3d getRigidBodyPosition(DBody body){
        return odeVecToJomlVec(body.getPosition());
    }
    
    /**
     * Gets the rotation of an ode body as a joml quaternion
     * @param body
     * @return
     */
    public static Quaterniond getRigidBodyRotation(DBody body){
        return odeQuatToJomlQuat(body.getQuaternion());
    }
    
    /**
     * Converts an Ode vector to a joml vector
     * @param vector Ode vector
     * @return joml vector
     */
    public static Vector3d odeVecToJomlVec(org.ode4j.math.DVector3C vector){
        return new Vector3d(vector.get0(),vector.get1(),vector.get2());
    }

    /**
     * Converts a joml vector to an Ode vector
     * @param vector joml vector
     * @return Ode vector
     */
    public static org.ode4j.math.DVector3C jomlVecToOdeVec(Vector3d vector){
        return new org.ode4j.math.DVector3(vector.x,vector.y,vector.z);
    }
    
    /**
     * Converts an Ode quaternion to a joml quaternion
     * @param quaternion Ode quat
     * @return joml quat
     */
    public static Quaterniond odeQuatToJomlQuat(DQuaternionC quaternion){
        return new Quaterniond(quaternion.get1(), quaternion.get2(), quaternion.get3(), quaternion.get0());
    }

    /**
     * Converts a joml quat to an ode quat
     * @param quaternion The joml quat
     * @return The ode quata
     */
    public static DQuaternion jomlQuatToOdeQuat(Quaterniond quaternion){
        return new DQuaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z);
    }
    
    /**
     * Sets the position + rotation of a body
     * @param position The position
     * @param rotation The rotation
     * @param body The body
     */
    public static void setRigidBodyTransform(CollisionEngine collisionEngine, Vector3d position, Quaterniond rotation, DBody body){
        collisionEngine.setBodyTransform(body, position, rotation);
    }

    /**
     * Synchronizes the data on a body
     * @param body The body
     * @param position The position
     * @param rotation The rotation
     * @param linearVel The linear velocity
     * @param angularVel The angular velocity
     * @param linearForce The linear force
     * @param angularForce The angular force
     */
    public static void synchronizeData(CollisionEngine collisionEngine, DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce){
        collisionEngine.synchronizeData(body, position, rotation, linearVel, angularVel, linearForce, angularForce);
    }


    /**
     * Sets the position + rotation + scale of a body
     * @param position The position
     * @param rotation The rotation
     * @param scale The scale
     * @param body The body
     */
    public static void setRigidBodyTransform(CollisionEngine collisionEngine, CollidableTemplate template, Vector3d position, Quaterniond rotation, Vector3d scale, DBody body){
        collisionEngine.setBodyTransform(body, template, position, rotation, scale);
    }

    /**
     * Creates a mass in the physics space from the body and mass value input
     * @param collisionEngine The collision engine to create the mass in
     * @param body The DBody to give mass to
     * @param mass The amount of mass to give to the DBody
     * @return THe DMass wrapping the DBody with a given mass value
     */
    public static DMass createMassFromBody(CollisionEngine collisionEngine, DBody body, double mass){
        return collisionEngine.createDMass(mass, body);
    }

    
}
