package electrosphere.entity.types.camera;

import electrosphere.engine.Globals;
import electrosphere.entity.Entity;
import electrosphere.entity.EntityCreationUtils;
import electrosphere.entity.EntityDataStrings;
import electrosphere.entity.EntityUtils;
import electrosphere.entity.btree.BehaviorTree;
import electrosphere.util.math.MathUtils;

import org.joml.Matrix4d;
import org.joml.Matrix4f;
import org.joml.Quaterniond;
import org.joml.Quaternionf;
import org.joml.Vector3d;
import org.joml.Vector3f;
import org.joml.Vector4d;

/**
 * Camera entity utility functions
 */
public class CameraEntityUtils {
    
    
    public static Entity spawnBasicCameraEntity(Vector3f center, Vector3f eye){
        Entity rVal = EntityCreationUtils.createClientSpatialEntity();
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_TYPE, EntityDataStrings.DATA_STRING_CAMERA_TYPE_BASIC);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_CENTER, center);
        rVal.putData(EntityDataStrings .DATA_STRING_CAMERA_EYE, eye);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_DISTANCE, 2.0f);
        rVal.putData(EntityDataStrings.CAMERA_ORBIT_RADIAL_OFFSET, new Vector3f(0,1,0));
        rVal.putData(EntityDataStrings.CAMERA_PITCH, 0.0f);
        rVal.putData(EntityDataStrings.CAMERA_YAW, 0.0f);
        return rVal;
    }
    
    public static Entity spawnEntityTrackingCameraEntity(Vector3f center, Vector3f eye, Entity toTrack){
        Entity rVal = EntityCreationUtils.createClientSpatialEntity();
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_TYPE, EntityDataStrings.DATA_STRING_CAMERA_TYPE_ORBIT);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_CENTER, center);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_EYE, eye);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_DISTANCE, 2.0f);
        rVal.putData(EntityDataStrings.CAMERA_ORBIT_RADIAL_OFFSET, new Vector3f(0,1,0));
        rVal.putData(EntityDataStrings.CAMERA_PITCH, 0.0f);
        rVal.putData(EntityDataStrings.CAMERA_YAW, 0.0f);
        BehaviorTree entityTrackingTree = new BehaviorTree() {
            @Override
            public void simulate(float deltaTime) {
                if(toTrack != null){
                    Vector3d entityPos = EntityUtils.getPosition(toTrack);
                    CameraEntityUtils.setCameraCenter(rVal, new Vector3f((float)entityPos.x,(float)entityPos.y,(float)entityPos.z).add(getOrbitalCameraRadialOffset(rVal)));
                }
            }
        };
        Globals.clientScene.registerBehaviorTree(entityTrackingTree);
        return rVal;
    }

    public static Entity spawnPlayerEntityTrackingCameraEntity(Vector3f center, Vector3f eye){
        Entity rVal = EntityCreationUtils.createClientSpatialEntity();
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_TYPE, EntityDataStrings.DATA_STRING_CAMERA_TYPE_ORBIT);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_CENTER, center);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_EYE, eye);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_DISTANCE, 2.0f);
        rVal.putData(EntityDataStrings.CAMERA_ORBIT_RADIAL_OFFSET, new Vector3f(0,1,0));
        rVal.putData(EntityDataStrings.CAMERA_PITCH, 0.0f);
        rVal.putData(EntityDataStrings.CAMERA_YAW, 0.0f);
        Globals.cameraHandler.setTrackPlayerEntity(true);
        return rVal;
    }

    /**
     * Spawns a first person camera that tracks the player
     * @param center the center of the camera
     * @param eye the eye of the camera
     * @return the camera
     */
    public static Entity spawnPlayerEntityTrackingCameraFirstPersonEntity(Vector3f center, Vector3f eye){
        Entity rVal = EntityCreationUtils.createClientSpatialEntity();
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_TYPE, EntityDataStrings.DATA_STRING_CAMERA_TYPE_ORBIT);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_CENTER, center);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_EYE, eye);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_DISTANCE, 0.01f);
        rVal.putData(EntityDataStrings.CAMERA_ORBIT_RADIAL_OFFSET, new Vector3f(0,1.3f,0));
        rVal.putData(EntityDataStrings.CAMERA_PITCH, 0.0f);
        rVal.putData(EntityDataStrings.CAMERA_YAW, 0.0f);
        Globals.cameraHandler.setTrackPlayerEntity(true);
        return rVal;
    }

    public static Entity spawnPlayerEntityAirplaneTrackingCameraEntity(Vector3f center, Vector3f eye){
        Entity rVal = EntityCreationUtils.createClientSpatialEntity();
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_TYPE, EntityDataStrings.DATA_STRING_CAMERA_TYPE_ORBIT);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_CENTER, center);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_EYE, eye);
        rVal.putData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_DISTANCE, 2.0f);
        rVal.putData(EntityDataStrings.CAMERA_ORBIT_RADIAL_OFFSET, new Vector3f(0,0.45f,0));
        rVal.putData(EntityDataStrings.CAMERA_PITCH, 0.0f);
        rVal.putData(EntityDataStrings.CAMERA_YAW, 0.0f);
        BehaviorTree entityTrackingTree = new BehaviorTree() {
            @Override
            public void simulate(float deltaTime) {
                if(Globals.playerEntity != null){
                    Vector3d entityPos = EntityUtils.getPosition(Globals.playerEntity);
                    CameraEntityUtils.setCameraCenter(rVal, new Vector3f((float)entityPos.x,(float)entityPos.y,(float)entityPos.z).add(getOrbitalCameraRadialOffset(rVal)));
                }
            }
        };
        Globals.clientScene.registerBehaviorTree(entityTrackingTree);
        return rVal;
    }

    /**
     * Initializes the camera
     */
    public static void initCamera(){
        //destroy if it already exists
        if(Globals.playerCamera != null){
            Globals.clientSceneWrapper.getScene().deregisterEntity(Globals.playerCamera);
        }
        //create
        if(Globals.controlHandler.cameraIsThirdPerson()){
            Globals.playerCamera = CameraEntityUtils.spawnPlayerEntityTrackingCameraEntity(new Vector3f(0,0,0), MathUtils.getOriginVectorf());
        } else {
            Globals.playerCamera = CameraEntityUtils.spawnPlayerEntityTrackingCameraFirstPersonEntity(new Vector3f(0,0,0), MathUtils.getOriginVectorf());
        }
    }
    
    public static Entity getOrbitalCameraTarget(Entity camera){
        return (Entity)camera.getData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_TARGET);
    }
    
    public static float getOrbitalCameraDistance(Entity camera){
        return (float)camera.getData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_DISTANCE);
    }

    public static void setOrbitalCameraDistance(Entity camera, float distance){
        camera.putData(EntityDataStrings.DATA_STRING_CAMERA_ORBIT_DISTANCE, distance);
    }

    public static Vector3f getOrbitalCameraRadialOffset(Entity camera){
        return (Vector3f)camera.getData(EntityDataStrings.CAMERA_ORBIT_RADIAL_OFFSET);
    }

    public static void setOrbitalCameraRadialOffset(Entity camera, Vector3f offset){
        camera.putData(EntityDataStrings.CAMERA_ORBIT_RADIAL_OFFSET, offset);
    }
    
    public static void setCameraCenter(Entity camera, Vector3f center){
        camera.putData(EntityDataStrings.DATA_STRING_CAMERA_CENTER, center);
    }
    
    public static Vector3f getCameraCenter(Entity camera){
        return (Vector3f)camera.getData(EntityDataStrings.DATA_STRING_CAMERA_CENTER);
    }
    
    public static void setCameraEye(Entity camera, Vector3f eye){
        camera.putData(EntityDataStrings.DATA_STRING_CAMERA_EYE, eye);
    }
    
    public static Vector3f getCameraEye(Entity camera){
        return (Vector3f)camera.getData(EntityDataStrings.DATA_STRING_CAMERA_EYE);
    }
    
    public static void setCameraPitch(Entity camera, float pitch){
        camera.putData(EntityDataStrings.CAMERA_PITCH, pitch);
    }
    
    public static float getCameraPitch(Entity camera){
        return (float)camera.getData(EntityDataStrings.CAMERA_PITCH);
    }
    
    public static void setCameraYaw(Entity camera, float yaw){
        camera.putData(EntityDataStrings.CAMERA_YAW, yaw);
    }
    
    public static float getCameraYaw(Entity camera){
        return (float)camera.getData(EntityDataStrings.CAMERA_YAW);
    }

    /**
     * Gets the quaternion containing the camera pitch
     * @param camera The pitch of the camera
     * @return The quaternion
     */
    public static Quaternionf getPitchQuat(double pitch){
        Quaternionf pitchQuat = new Quaternionf().fromAxisAngleDeg(MathUtils.getLeftVectorf(), -(float)pitch);
        return pitchQuat;
    }

    /**
     * Gets the quaternion containing the camera yaw
     * @param yaw The yaw of the camera
     * @return The quaternion
     */
    public static Quaternionf getYawQuat(double yaw){
        Quaternionf yawQuat = new Quaternionf().fromAxisAngleDeg(MathUtils.getUpVectorf(), -(float)yaw);
        return yawQuat;
    }
    
    public static void destroyCameraEntity(Entity e){
        if(e != null){
            Globals.clientScene.deregisterEntity(e);
        }
    }
    
    public static Matrix4f getCameraViewMatrix(Entity camera){
        Vector3f cameraCenter = new Vector3f(0,0,0);//getViewMatrixCenterOffset(camera);
        Vector3f cameraEye = new Vector3f(cameraCenter).add(getCameraEye(camera));
        Vector3f cameraUp = MathUtils.getUpVectorf();
        //!!before you make the same mistake I made, cameraEye is NOT NECESSARILY normalized/unit vector
        //the orbital distance and offset are included in this vector
        //TODO: refactor this to some other matrix of transforms or something??
        cameraEye = new Vector3f(getCameraEye(camera));
    //    System.out.println("eye: " + cameraEye);
    //    System.out.println("center: " + cameraCenter);
//        System.out.println("up: " + cameraUp);
        Matrix4f rVal = new Matrix4f().setLookAt(
                    cameraEye, //eye
                    cameraCenter, //center
                    cameraUp // up
            ).scale(1.0f, 1.0f, 1.0f);
        return rVal;
    }

    /**
     * Gets the rotation quaternion from a camera entity
     * @param entity The entity
     * @return The rotation quaternion
     */
    public static Quaterniond getRotationQuat(Entity entity){
        double yaw = CameraEntityUtils.getCameraYaw(entity);
        double pitch = CameraEntityUtils.getCameraPitch(entity);
        //quaternion is multiplied by pi because we want to point away from the eye of the camera, NOT towards it
        Quaternionf quatRaw = CameraEntityUtils.getYawQuat(yaw).mul(CameraEntityUtils.getPitchQuat(pitch)).mul(new Quaternionf().rotateY((float)Math.PI));
        Quaterniond quatd = new Quaterniond(quatRaw).normalize();
        return quatd;
    }

    /**
     * Gets the rotation quaternion from a yaw and pitch
     * @param yaw The yaw
     * @param pitch The pitch
     * @return The rotation quaternion
     */
    public static Quaterniond getRotationQuat(double yaw, double pitch){
        //quaternion is multiplied by pi because we want to point away from the eye of the camera, NOT towards it
        Quaternionf quatRaw = CameraEntityUtils.getYawQuat(yaw).mul(CameraEntityUtils.getPitchQuat(pitch)).mul(new Quaternionf().rotateY((float)Math.PI));
        Quaterniond quatd = new Quaterniond(quatRaw).normalize();
        return quatd;
    }

    /**
     * Gets the rotation matrix from a yaw and pitch
     * @param yaw The yaw
     * @param pitch The pitch
     * @return The rotation matrix
     */
    public static Matrix4d getRotationMat(double yaw, double pitch){
        //quaternion is multiplied by pi because we want to point away from the eye of the camera, NOT towards it
        Quaternionf quatRaw = CameraEntityUtils.getYawQuat(yaw).mul(CameraEntityUtils.getPitchQuat(pitch)).mul(new Quaternionf().rotateY((float)Math.PI));
        Quaterniond quatd = new Quaterniond(quatRaw).normalize();
        Matrix4d rotationMat = new Matrix4d().rotate(quatd);
        return rotationMat;
    }

    /**
     * Gets the facing vector from the camera angles
     * @param cameraEntity The camera entity
     * @return The facing vector
     */
    public static Vector3d getFacingVec(Entity cameraEntity){
        float yaw = CameraEntityUtils.getCameraYaw(cameraEntity);
        float pitch = CameraEntityUtils.getCameraPitch(cameraEntity);
        //quaternion is multiplied by pi because we want to point away from the eye of the camera, NOT towards it
        Quaternionf quatRaw = CameraEntityUtils.getYawQuat(yaw).mul(CameraEntityUtils.getPitchQuat(pitch)).mul(new Quaternionf().rotateY((float)Math.PI));
        Quaterniond quatd = new Quaterniond(quatRaw).normalize();
        Matrix4d rotationMat = new Matrix4d().rotate(quatd);
        Vector4d rotationVecRaw = MathUtils.getOriginVector4();
        rotationVecRaw = rotationMat.transform(rotationVecRaw);
        return new Vector3d(rotationVecRaw.x,0,rotationVecRaw.z);
    }

    /**
     * Gets the facing vector from the camera angles
     * @param yaw The yaw of the camera
     * @param pitch The pitch of the camera
     * @return The facing vector
     */
    public static Vector3d getFacingVec(double yaw, double pitch){
        //quaternion is multiplied by pi because we want to point away from the eye of the camera, NOT towards it
        Quaternionf quatRaw = CameraEntityUtils.getYawQuat(yaw).mul(CameraEntityUtils.getPitchQuat(pitch)).mul(new Quaternionf().rotateY((float)Math.PI));
        Quaterniond quatd = new Quaterniond(quatRaw).normalize();
        Matrix4d rotationMat = new Matrix4d().rotate(quatd);
        Vector4d rotationVecRaw = MathUtils.getOriginVector4();
        rotationVecRaw = rotationMat.transform(rotationVecRaw);
        return new Vector3d(rotationVecRaw.x,0,rotationVecRaw.z);
    }

    /**
     * Gets the rotation matrix from a yaw and pitch
     * @param yaw The yaw
     * @param pitch The pitch
     * @return The rotation matrix
     */
    public static Matrix4d getRotationMat(Entity entity){
        float yaw = CameraEntityUtils.getCameraYaw(entity);
        float pitch = CameraEntityUtils.getCameraPitch(entity);
        //quaternion is multiplied by pi because we want to point away from the eye of the camera, NOT towards it
        Quaternionf quatRaw = CameraEntityUtils.getYawQuat(yaw).mul(CameraEntityUtils.getPitchQuat(pitch)).mul(new Quaternionf().rotateY((float)Math.PI));
        Quaterniond quatd = new Quaterniond(quatRaw).normalize();
        Matrix4d rotationMat = new Matrix4d().rotate(quatd);
        return rotationMat;
    }
    
}
