import {
    Matrix,
    Ray,
    TargetCamera,
    Vector3,
    Camera,
    DeepImmutable,
    Scalar,
    Epsilon,
    BoundingInfo,
    Axis,
    Frustum
} from '../loader/babylonjs-import';
import { intersectsPlaneAtToRef, Vertex2 } from '../math';
import { getCanvasCenterCoordinatesToRef } from './Inputs/input-utils';
import { PivotPlane } from './PivotPlane';
import { CoordinateAxesRenderer } from './CoordinateAxesRenderer';

import {
    CanvasPosition,
    PredefinedCanvasPosition,
    PickOption,
    PickOptionCanvas,
    PickOptionType
} from '../loader/Selectables';

import {
    TwinfinityCameraStateSnapshot,
    TwinfinityDefaultCameraStateSnapshot,
    TwinfinityTargetCameraStateSnapshot
} from './TwinfinityCameraStateSnapshot';
import { FrustumPlanes } from '../math/Frustum';
import { PivotTargetCamera } from './PivotTargetCamera';
import { Extensions } from '../Extensions';
import { ShortFrustumProjectionMatrixState } from '../loader/ShortFrustumProjectionMatrixState';
import { PickResult, PickResultEmpty, PickResultType } from '../loader/PickResult';
import { TwinfinityViewer } from '../loader/twinfinity-viewer';

/**
 * Consists of different strings explaining a viewing direction for the camera.
 */
export type CameraDirection = 'top' | 'bottom' | 'front' | 'back' | 'left' | 'right' | 'default';

const cameraDirectionVector = {
    top: new Vector3(0, -1, 0),
    bottom: new Vector3(0, 1, 0),
    front: new Vector3(0, 0, -1),
    back: new Vector3(0, 0, 1),
    left: new Vector3(1, 0, 0),
    right: new Vector3(-1, 0, 0),
    default: new Vector3(1, -1, 1)
};

/**
 * Includes methods and properties used to manipulate a wrapped `TargetCamera` relative a pivot point {@link target}.
 * The point is a point in the world and the pivot plane is a plane defined by that
 * point and the negated `TargetCamera` Z axis normal (the normalized camera forward vector).
 */
export interface TwinfinityCameraPivot {
    /**
     * If `true` then the pivot point cannot be modified. Calling {@link update} will only reorient the pivot plane to face
     * the camera. {@link target} will not be updated.
     */
    isFrozen: boolean;

    /**
     * Normal of the pivot plane.
     */
    readonly normal: DeepImmutable<Vector3>;

    /**
     * Whether the pivot point is visualized or not.
     */
    isVisible: boolean;

    /**
     * Pivot point. It is always located on the pivot plane.
     */
    readonly target: DeepImmutable<Vector3>;

    /**
     * Whether the pivot plane is in front of the camera or not.
     */
    readonly isInFront: boolean;

    /**
     * Closest distance from camera to pivotplane. Will be negative if camera is behind pivot plane
     */
    readonly signedDistanceToCamera: number;

    /**
     * Updates the pivot point {@link target} and the corresponding pivot plane.
     * Also see {@link isFrozen}.
     * @param o Optional. If not specified, the current pivot plane is simply reoriented to face the camera. (plane can be behind camera plane).
     * If specified, it is either a screen space coordinate or a world space coordinate. If a screen space
     * coordinate is used, a ray is shot from the camera through the screen cordinate. If the
     * ray intersects a object, the intersection coordinate is calculated and used as {@link target}.
     * If no intersection is found, the previous pivot plane is reoriented so it faces the
     * camera and a intersection test between the ray and the plane is calculated. If an intersection
     * is found that coordinate is used as {@link target}. If no such intersection could be found (because
     * current pivot plane is behind camera), a new pivot plane is calculated exactly at {@link PivotPlane.DefaultDistanceToPivotPlane}
     * world units in front of the camera. A new {@link target} is then calculated by intersecting
     * the ray from the center of the camera viewport with the plane.
     */
    update(o?: { canvasCoordinate: Vertex2 } | { target: Vector3 }): void;

    /**
     * Zooms a camera in or out. Actual camera movement is performed over a series of frames. The camera is animated
     * so we get a smooth visualization.
     * @param delta Value to zoom with. A positive value means zoom in. A negative value means zoom out.
     * @param screenCoordinate Intersects pivot plane with a ray shot through this screen coordinate.
     * Camera is then zoomed towards this coordinate.
     */
    zoom(delta: number, screenCoordinate: Vertex2): void;

    /**
     * Pans camera relative {@link target}.
     * @param currentScreenCoordinate A ray is shot from the camera through this screen coordinate and
     * a intersection with the pivot plane is calculated.
     * The camera is then repositioned according to the following formula
     * this._camera.position.subtractInPlace(currentOnPivotPlane.subtractInPlace(this.target));
     * cameraPos = cameraPos - (intersection - target);
     * @returns `true` if a intersection could be found (camera could be repositioned). Otherwise `false` only
     * happens if current pivotplane is behind the camera.
     */
    pan(currentScreenCoordinate: Vertex2): boolean;

    /**
     * Rotates the camera around {@link target}. Actual rotation of the camera is performed
     * over a series of frames (camera is animated) in order to get a smooth visualization.
     * @param pitch Relative delta pitch in radians
     * @param yaw Relative delta yaw in radians
     */
    rotate(pitch: number, yaw: number): void;

    /**
     * Creates a ray from camera through canvas position specified by `o` and intersect it with the pivot plane
     * and store the resulting coordinate in `dst`.
     * @param dst Holds resulting intersection (if any). Not set if < 0 is returned.
     * @param o Determines which canvas position to shoot intersection ray through.
     * @returns distance to pivot plane along ray. If no intersection can be found then -1.
     */
    intersectWithScreenCoordinateToRef(dst: Vector3, o: CanvasPosition): number;

    /**
     * Zooms camera to the extent of the specified bounding box.
     * @param aabb {@link BoundingInfo}.
     * @param direction Optional {@link CameraDirection}. If specified camera is reoriented according to this. Otherwise camera orientation is kept as is
     */
    zoomToExtent(aabb: BoundingInfo, direction?: CameraDirection): void;

    /**
     * Sets camera rotation to look towards the specified direction and zooms to extent of visible objects.
     * @param direction The direction to look from (top = -Z, bottom = +Z, front = +Y, back = -Y, left = +X, right = -X).
     */
    lookFrom(direction: CameraDirection): void;
}

/**
 * Includes methods and properties for querying and manipulating a {@link Camera} instance.
 */
export interface TwinfinityCameraExtensions {
    /**
     * Gets the distance that the camera has to be positioned at (perpendicular to pivot plane) when in perspective
     * mode to get approximately the same view. The objects in the center
     * of the frustum will appear to have almost the same size.
     */
    readonly perspectiveDistanceFromOrtho: number;

    /**
     * `true` if the camera has somehow moved during this frame. Otherwise `false`. Will automatically be reset to `false` after each frame has rendered (if camera is used by {@link TwinfinityViewer}).
     */
    readonly hasMovedInCurrentFrame: boolean;

    /**
     * Gets a {@link TwinfinityCameraStateSnapshot} instance which can be used to check if a camera has changed since a previous check.
     */
    stateSnapshot(): TwinfinityCameraStateSnapshot;

    /**
     * Sets {@link hasMovedInCurrentFrame} to `false`.
     */
    clearHasMovedInCurrentFrame(): void;

    /**
     * Sets the orthographic frustum based on either `distance` or `height`
     * @param o Sets orthographic frustum based on either `distance` or `height`.
     * If `height` then {@link Camera.orthoBottom} = -`height` / 2 and {@link Camera.orthoTop} is set to `height` / 2. {@link Camera.orthoLeft} and {@link Camera.orthoRight} is then calculated using the aspect ratio.-
     * If `distance` then frustum is sized optimally so a object in center of camera at `distance` will get approx the same size as it would with the perspective camera.
     */
    setOrtho(o: { distance: number } | { height: number }): void;

    /**
     * Gets the size of the orthographic frustm (defined by {@link Camera.orthoBottom}, {@link Camera.orthoTop}, {@link Camera.orthoLeft}, {@link Camera.orthoRight}).
     * @param dst Size is written here,. `x` is width and `y`i is height.
     * @return `dst`.
     */
    getOrthoFrustumSizeToRef(dst: Vertex2): Vertex2;

    /**
     * Is used by orthographic camera when calculating zoom speed.
     * @param distance Distance to change the orthographic frustum size with.
     * @internal Only for internal use.
     * @returns speed.
     */
    computeOrthographicZoomSpeed(distance: number): number;

    /**
     * Creates a ray from the camera through the screen coordinate defined by `o`.
     * This ray is useful for pick operations.
     * @param dst Ray is written here.
     * @param o Optional. If not specified then the center of the canvas is used.
     */
    createPickingRayToRef(dst: Ray, o: CanvasPosition): Ray;

    /**
     * Performs intersection testing against objects in the scene from the cameras point of view.
     * @param o See {@link PickOption}.
     */
    pick(o: PickOption): PickResult;

    /**
     * Gets the camera frustum planes
     * @param dst Camera frustum planes are written
     * @returns Frustum planes (same reference as `dst`)
     */
    getFrustumToRef(dst: FrustumPlanes): FrustumPlanes;

    /**
     * Gets the camera short frustum state, which is used to render the depth buffer
     * @returns The short frustum state
     * @hidden
     */
    get shortFrustum(): ShortFrustumProjectionMatrixState;

    /**
     * Twinfinity viewer instance.
     */
    get viewer(): TwinfinityViewer | undefined;
}

export interface TwinfinityTargetCameraExtensions extends TwinfinityCameraExtensions {
    /** If `true` then camera acts like a free camera during rotation and rotation is
     *  relative to camera's own axes (first person camera).
     */
    isFreeLook: boolean;
    /**
     * Includes methods and properties for camera manipulation around a pivot plane and point.
     */
    readonly pivot: TwinfinityCameraPivot;
}

class TwinfinityCameraPivotImplementation implements TwinfinityCameraPivot {
    private static readonly _tmpPickingRay = Ray.Zero();
    private static readonly _tmpCurrentOnPivotPlane = Vector3.Zero();
    private static readonly _tmpMouseInWorld = Vector3.Zero();
    private static readonly _pivotPickOptions: PickOptionCanvas = {
        type: PickOptionType.Canvas,
        position: { x: 0, y: 0 },
        isAABBIntersectionEnabled: true,
        isCenterIntersectionFallbackEnabled: true,
        isGeometryIntersectionEnabled: true,
        isNearbyRenderedObjectsIntersectionFallbackEnabled: true
    };

    private readonly _pivotPlane = PivotPlane.Empty();

    private readonly _pivotPointRenderer: CoordinateAxesRenderer;

    public readonly target: DeepImmutable<Vector3> = Vector3.Zero();

    public isFrozen = false;

    public constructor(private readonly _camera: TargetCamera) {
        // TODO Need to send in correct pivot point
        this._pivotPointRenderer = new CoordinateAxesRenderer(_camera.getScene(), () => this.target);
        const camera = this._camera;
        // Keep pivotplane and pivotpoint up to date with changes to camera viewmatrix (rotation, translation etc is included in this)
        let isUpdating = false;
        const cameraViewMatrixObserver = camera.onViewMatrixChangedObservable.add((camera, eventData) => {
            if (!isUpdating) {
                isUpdating = true; // avoid infinite recursion
                this._pivotPlane.update(this._camera);
            }
        });
        this._camera.onDispose = () => camera.onViewMatrixChangedObservable.remove(cameraViewMatrixObserver);
    }

    public get signedDistanceToCamera(): number {
        return this._pivotPlane.signedDistanceTo(this._camera.globalPosition);
    }

    public lookFrom(direction: CameraDirection): void {
        // TODO We must go from ortho to perspective abd then back to make this work

        const directionVector = cameraDirectionVector[direction] ?? cameraDirectionVector.default;
        const currentPosition = this._camera.globalPosition;
        const currentTarget = this._camera.target;
        const distance = Vector3.Distance(currentPosition, currentTarget);
        const newTargetDist = directionVector.scale(distance);
        this._camera.target = currentPosition.add(newTargetDist);
        this._camera.getViewMatrix(true);
    }

    public zoomToExtent(aabb: BoundingInfo, direction?: CameraDirection): void {
        const delta = aabb.maximum.subtract(aabb.minimum);
        const isAtleast2D = [delta.x > 0, delta.y > 0, delta.z > 0].filter(Boolean).length > 1;
        if (delta.x < 0 || delta.y < 0 || delta.z < 0 || !isAtleast2D) {
            return; // Do nothing because aabb is invalid
        }

        const min = aabb.minimum;
        const max = aabb.maximum;
        const center = aabb.boundingSphere.centerWorld.clone();
        const k = 2; //(direction ?? 'default') === 'default' ? 3.3 : 2;
        const size = Vector3.Distance(min, max) / k;
        center.y -= size / 6;

        const currentMode = this._camera.mode;
        this._camera.mode = Camera.PERSPECTIVE_CAMERA;

        let cameraForwardVector: Vector3;
        if (direction === undefined) {
            cameraForwardVector = this._camera.getDirection(Axis.Z);
        } else {
            cameraForwardVector = cameraDirectionVector[direction ?? 'default'].negate().normalize();
        }

        // calculate half angle of field ov view
        const angle = this._camera.fov * 0.5; // (this._camera.fov * (180 / Math.PI)) / 2;
        const targetDistance = size / Math.tan(angle);
        const newCameraPosition = cameraForwardVector.scale(targetDistance);
        this._camera.position.copyFrom(center.add(newCameraPosition));
        this._camera.setTarget(center);
        this._camera.getViewMatrix(true); // Force recompute of word matrix etc
        this._camera.twinfinity.pivot.update({ target: center });

        this._camera.twinfinity.setOrtho({ distance: targetDistance });
        this._camera.getViewMatrix(true);
        this._camera.mode = currentMode;
        this._camera.getViewMatrix(true);
        // Pivotpoint is always initially at center

        // NOTE Is this needed anymore?
        //this._camera.getScene().render(true);
    }

    /** `true` if camera is currently in front of pivot plane */
    public get isInFront(): boolean {
        this._pivotPlane.update(this._camera);
        return this._pivotPlane.isInFrontOfCamera(this._camera);
    }

    public get normal(): DeepImmutable<Vector3> {
        return this._pivotPlane.normal;
    }

    public update(o?: { canvasCoordinate: Vertex2 } | { target: Vector3 }): void {
        // TODO Should we use screenCoordinate?
        // Create a zoom vector. Hmm this becomes a bit odd. We already have a zoom vector. We get that when we do the
        // pickoperation. (x,y coordinate gives us it). Its actually not the result of the pick operation that should
        // determine the zoom vector!!! We should only use the zoom vector to calculate the DISTANCE!!! (By intersecting it with the pivotplane)
        if (o === undefined || this.isFrozen) {
            this._pivotPlane.update(this._camera);
            return;
        }

        if ('target' in o) {
            this.target.copyFrom(o.target);
            this._pivotPlane.update(this._camera, this.target);
            // TODO How do we calculate the zoomray here? I guess the easiest way is
            // to intersect with the plane? Hmm no!!
            // What is the zoomray here. We need to calculate a ray that goes from
            // center where camera is looking
        } else {
            // indata was a screen coordinate. Use it to pick something in the camera's view.
            const scene = this._camera.getScene();

            const pO = TwinfinityCameraPivotImplementation._pivotPickOptions;
            pO.position = o.canvasCoordinate;
            const pickResult = scene.twinfinity.viewer?.selectables.pick(pO) ?? PickResultEmpty.instance;
            if (pickResult.type === PickResultType.Empty) {
                // We have not direct position to use. Ensure that pivotplane is
                // rotated towards us. If it still is behind use we create
                // a new plane 2 meters in front of us. In both cases we perfrom a intersection on it to
                // get the coordinates of the target
                this._pivotPlane.update(this._camera);
                if (!this._pivotPlane.isInFrontOfCamera(this._camera)) {
                    this._pivotPlane.putInFrontOfCamera(this._camera);
                }
                this.intersectWithScreenCoordinateToRef(this.target, o.canvasCoordinate);
                return;
            }

            const { hitInfo } = pickResult;
            if (hitInfo.length > 0) {
                this.target.copyFrom(hitInfo[0].position);
                this._pivotPlane.update(this._camera, this.target);
                // We cannot be sure that the hitinfo actually represents a world coordinate below
                // the specified screenCoordinate (because the hit may actually be for a nearby object
                // due to the options (pO) we specified. Hence we need to reacalculate the pivot target
                // by intersecting the new pivotplane.
                this.intersectWithScreenCoordinateToRef(this.target, pO.position);
                // And then we update the plane again to use (possibly) new target.
                this._pivotPlane.update(this._camera, this.target);
                return;
            }
        }
    }

    public zoom(deltaToUse: number, screenCoordinate: Vertex2): void {
        // Move camera along zoom vector. When come up and are behinde pivot plane we will simply move at default (slow speed)
        const direction = Math.sign(deltaToUse);
        deltaToUse = Math.abs(deltaToUse);
        if (deltaToUse < this._camera.speed * Epsilon) {
            return;
        }

        if (this._camera.mode === Camera.ORTHOGRAPHIC_CAMERA) {
            //this.camera.getViewMatrix(true);
            //this.camera.computeWorldMatrix();
            const halfSizeX = Math.abs(this._camera.orthoRight! - this._camera.orthoLeft!);
            //const sizeXMinusOne = halfSizeX - 1;

            const speed = this._camera.twinfinity.computeOrthographicZoomSpeed(halfSizeX);

            let distanceToMove = halfSizeX * deltaToUse * speed;
            const minDistance = 0.01;
            distanceToMove =
                direction * Scalar.Clamp(distanceToMove, minDistance, Math.max(minDistance, halfSizeX / 2));

            const mouseInWorld = TwinfinityCameraPivotImplementation._tmpMouseInWorld;
            this.unprojectCurrentMouseCoordToRefToRef(screenCoordinate, mouseInWorld);
            this.zoomOrtho(distanceToMove, mouseInWorld);
        } else {
            const zoomRay = this._camera.twinfinity.createPickingRayToRef(
                TwinfinityCameraPivotImplementation._tmpPickingRay,
                screenCoordinate
            );

            let distanceToZoomTarget = zoomRay.intersectsPlane(this._pivotPlane);

            const minDistance = this._camera.minZ / 10.0;
            if (!distanceToZoomTarget || distanceToZoomTarget <= 0) {
                distanceToZoomTarget = minDistance;
            }
            const distanceToPivotPointAndBeyond = distanceToZoomTarget + 1;

            const speed = this._camera.twinfinity.computeOrthographicZoomSpeed(distanceToZoomTarget); //.computeLocalSpeed(distanceToPivotPoint, 0.5);

            let distanceToMove = distanceToPivotPointAndBeyond * deltaToUse * speed;
            const maxDistance = Math.max(minDistance, distanceToPivotPointAndBeyond / 2);
            distanceToMove = Scalar.Clamp(distanceToMove, minDistance, maxDistance);
            const dir = zoomRay.direction.scaleInPlace(distanceToMove);
            if (direction === -1) {
                dir.negateInPlace();
            }
            this._camera.position.addInPlace(dir); // new pos. Calculate change along old camera.y and camera.x
        }
    }

    public pan(currentScreenCoordinate: Vertex2): boolean {
        // Update plane to ensure that it is correct before we pan
        this._pivotPlane.update(this._camera);

        // TODO ensure that this.target is still on the plane!!! Or project target on plane
        const currentOnPivotPlane = TwinfinityCameraPivotImplementation._tmpCurrentOnPivotPlane;

        if (this.intersectWithScreenCoordinateToRef(currentOnPivotPlane, currentScreenCoordinate) > -1) {
            this._camera.position.subtractInPlace(currentOnPivotPlane.subtractInPlace(this.target));
            return true;
        }
        return false;
    }

    public rotate(pitch: number, yaw: number): void {
        // Calculate plane and rotate and the update plane?
        this._camera.cameraRotation.x += pitch;
        this._camera.cameraRotation.y += yaw;
    }

    public get isVisible(): boolean {
        // Turn on pivotpoint visualization
        return this._pivotPointRenderer.isEnabled;
    }

    public set isVisible(visible: boolean) {
        // Turn off pivotpoint visualization
        if (visible !== this.isVisible) {
            this._pivotPointRenderer.isEnabled = visible;
        }
    }

    public intersectWithScreenCoordinateToRef(dst: Vector3, o: CanvasPosition): number {
        // Intersection pos in dst. Returns distance. If distance < 0 then there was no intersection
        // if no screenPos then ray is shot through center of canvas.
        this._camera.twinfinity.createPickingRayToRef(TwinfinityCameraPivotImplementation._tmpPickingRay, o);
        return intersectsPlaneAtToRef(TwinfinityCameraPivotImplementation._tmpPickingRay, this._pivotPlane, dst);
    }

    private unprojectCurrentMouseCoordToRefToRef(screenCoordinate: Vertex2, dst: Vector3): void {
        // const cameraViewport = this._camera.viewport;
        const engine = this._camera.getEngine();
        Vector3.UnprojectFloatsToRef(
            screenCoordinate.x / engine.getHardwareScalingLevel(),
            screenCoordinate.y / engine.getHardwareScalingLevel(),
            0,
            engine.getRenderWidth(),
            engine.getRenderHeight(),
            this._camera.getWorldMatrix(),
            this._camera.getViewMatrix(),
            this._camera.getProjectionMatrix(),
            dst
        );
    }

    private zoomOrtho(deltaInMeters: number, mouseInWorld: Vertex2): void {
        const camera = this._camera;

        if (camera) {
            const oldHorizontalDirection = Math.sign(camera.orthoLeft! - camera.orthoRight!);
            const oldVerticalDirection = Math.sign(camera.orthoTop! - camera.orthoBottom!);
            const sizeX = Math.abs((camera.orthoLeft ?? 0) - (camera.orthoRight ?? 0));
            const sizeY = Math.abs((camera.orthoTop ?? 0) - (camera.orthoBottom ?? 0));

            const aspectRatio = sizeY / sizeX;

            let fromCoord = camera.orthoLeft! - mouseInWorld.x;
            let ratio = fromCoord / sizeX;
            const newOrthoLeft = camera.orthoLeft! - ratio * deltaInMeters;

            fromCoord = camera.orthoRight! - mouseInWorld.x;
            ratio = fromCoord / sizeX;
            const newOrthoRight = camera.orthoRight! - ratio * deltaInMeters;

            fromCoord = camera.orthoTop! - mouseInWorld.y;
            ratio = fromCoord / sizeY;
            const newOrthoTop = camera.orthoTop! - ratio * aspectRatio * deltaInMeters;

            fromCoord = camera.orthoBottom! - mouseInWorld.y;
            ratio = fromCoord / sizeY;
            const newOrthoBottom = camera.orthoBottom! - ratio * aspectRatio * deltaInMeters;

            const newHorizontalDirection = Math.sign(newOrthoLeft! - newOrthoRight!);
            const newVerticalDirection = Math.sign(newOrthoTop! - newOrthoBottom!);
            if (newHorizontalDirection !== oldHorizontalDirection || newVerticalDirection !== oldVerticalDirection) {
                return;
            }

            camera.orthoBottom = newOrthoBottom;
            camera.orthoTop = newOrthoTop;
            camera.orthoLeft = newOrthoLeft;
            camera.orthoRight = newOrthoRight;
        }
    }
}

class TwinfinityCameraExtensionsImplementation implements TwinfinityCameraExtensions {
    private static readonly _tmpIdentityMatrix = Matrix.Identity();
    private static readonly _tmpCenterPos: Vertex2 = { x: 0, y: 0 };
    private static readonly _maxLocalCameraSpeed = 0.4;

    private _projectionMatrixChangeCount = 0;
    private _viewMatrixChangeCount = 0;
    private readonly _shortFrustumProjectionMatrixState: ShortFrustumProjectionMatrixState;

    public constructor(private readonly _camera: Camera) {
        _camera.onProjectionMatrixChangedObservable.add(() => this._projectionMatrixChangeCount++);
        _camera.onViewMatrixChangedObservable.add(() => this._viewMatrixChangeCount++);
        this._shortFrustumProjectionMatrixState = new ShortFrustumProjectionMatrixState(_camera);
    }

    public get viewer(): TwinfinityViewer | undefined {
        return this._camera.getScene().twinfinity.viewer;
    }

    public get shortFrustum(): ShortFrustumProjectionMatrixState {
        return this._shortFrustumProjectionMatrixState;
    }

    public getFrustumToRef(dst: FrustumPlanes): FrustumPlanes {
        Frustum.GetPlanesToRef(this._camera.getTransformationMatrix(), dst);
        return dst;
    }

    public stateSnapshot(): TwinfinityCameraStateSnapshot {
        let state: TwinfinityCameraStateSnapshot;
        if (this._camera instanceof TargetCamera) {
            state = new TwinfinityTargetCameraStateSnapshot(this._camera);
        } else if (this._camera instanceof Camera) {
            state = new TwinfinityDefaultCameraStateSnapshot(this._camera);
        } else {
            throw new Error(`No supported.`);
        }
        state.refresh();
        return state;
    }

    public pick(o: PickOption): PickResult {
        return this._camera.getScene().twinfinity.viewer?.selectables.pick(o) ?? PickResultEmpty.instance;
    }

    public setOrtho(o: { distance: number } | { height: number }): void {
        const engine = this._camera.getEngine();
        const aspectRatio = engine.getRenderWidth() / engine.getRenderHeight();
        if ('distance' in o) {
            const s = o.distance * Math.tan(this._camera.fov * 0.5);
            this._camera.orthoTop = s;
            this._camera.orthoBottom = -s;
            this._camera.orthoLeft = -s * aspectRatio;
            this._camera.orthoRight = s * aspectRatio;
        } else {
            const h = o.height * 0.5;
            this._camera.orthoTop = h;
            this._camera.orthoBottom = -h;
            this._camera.orthoLeft = -h * aspectRatio;
            this._camera.orthoRight = h * aspectRatio;
        }
    }

    public get hasMovedInCurrentFrame(): boolean {
        return this._projectionMatrixChangeCount > 0 || this._viewMatrixChangeCount > 0;
    }

    public clearHasMovedInCurrentFrame(): void {
        this._projectionMatrixChangeCount = 0;
        this._viewMatrixChangeCount = 0;
    }

    public get perspectiveDistanceFromOrtho(): number {
        return (Math.abs(this._camera.orthoTop! - this._camera.orthoBottom!) * 0.5) / Math.tan(this._camera.fov * 0.5);
    }

    public getOrthoFrustumSizeToRef(dst: Vertex2): Vertex2 {
        const halfWidth = this._camera.getEngine().getRenderWidth() / 2.0;
        const halfHeight = this._camera.getEngine().getRenderHeight() / 2.0;

        dst.x = Math.abs((this._camera.orthoRight ?? halfWidth) - (this._camera.orthoLeft ?? -halfWidth));
        dst.y = Math.abs((this._camera.orthoTop ?? halfHeight) - (this._camera.orthoBottom ?? -halfHeight));
        return dst;
    }

    public computeOrthographicZoomSpeed(distance: number): number {
        const engine = this._camera.getEngine();
        const cameraSpeed = this._camera instanceof TargetCamera ? this._camera.speed : 1;
        let speed = cameraSpeed;
        if (this._camera.mode === Camera.ORTHOGRAPHIC_CAMERA) {
            speed = cameraSpeed + Math.log10(Math.max(1, distance));
        }

        let ret = speed * Math.sqrt(engine.getDeltaTime() / (engine.getFps() * 100.0));
        ret = Math.min(TwinfinityCameraExtensionsImplementation._maxLocalCameraSpeed, ret);
        return ret;
    }

    public createPickingRayToRef(dst: Ray, canvasPosition: CanvasPosition): Ray {
        const scene = this._camera.getScene();
        let canvasPos: Vertex2;
        if (canvasPosition === PredefinedCanvasPosition.Center) {
            // Calculate middle screen coordinate if screenpos is missing.
            // That is where the camera is looking.
            canvasPos = getCanvasCenterCoordinatesToRef(scene, TwinfinityCameraExtensionsImplementation._tmpCenterPos);
        } else if (canvasPosition === PredefinedCanvasPosition.Mouse) {
            TwinfinityCameraExtensionsImplementation._tmpCenterPos.x = scene.pointerX;
            TwinfinityCameraExtensionsImplementation._tmpCenterPos.y = scene.pointerY;
            canvasPos = TwinfinityCameraExtensionsImplementation._tmpCenterPos;
        } else {
            canvasPos = canvasPosition;
        }
        scene.createPickingRayToRef(
            canvasPos.x,
            canvasPos.y,
            TwinfinityCameraExtensionsImplementation._tmpIdentityMatrix,
            dst,
            this._camera
        );
        return dst;
    }
}

class TwinfinityTargetCameraExtensionsImplementation
    extends TwinfinityCameraExtensionsImplementation
    implements TwinfinityTargetCameraExtensions
{
    public readonly pivot: TwinfinityCameraPivot;
    public isFreeLook = false;
    public constructor(camera: TargetCamera) {
        super(camera);
        this.pivot = new TwinfinityCameraPivotImplementation(camera);
    }
}

declare module '@babylonjs/core/Cameras/camera' {
    export interface Camera {
        readonly twinfinity: TwinfinityCameraExtensions;
    }
}

declare module '@babylonjs/core/Cameras/targetCamera' {
    export interface TargetCamera {
        readonly twinfinity: TwinfinityTargetCameraExtensions;
    }
}

Object.defineProperty(Camera.prototype, Extensions.TWINFINITY_PROPERTY, {
    enumerable: false,
    configurable: true,
    get: function (this: Camera): TwinfinityCameraExtensions {
        const ret = new TwinfinityCameraExtensionsImplementation(this);
        Object.defineProperty(this, Extensions.TWINFINITY_PROPERTY, {
            value: ret
        });
        return ret;
    }
});

Object.defineProperty(TargetCamera.prototype, Extensions.TWINFINITY_PROPERTY, {
    enumerable: false,
    configurable: true,
    get: function (this: TargetCamera): TwinfinityTargetCameraExtensions {
        const ret = new TwinfinityTargetCameraExtensionsImplementation(this);
        Object.defineProperty(this, Extensions.TWINFINITY_PROPERTY, {
            value: ret
        });
        return ret;
    }
});

Object.defineProperty(TargetCamera.prototype, 'mode', {
    enumerable: true,
    configurable: true,
    get: function (this: TargetCamera): number {
        const self = this as any;
        return self._twinfinityMode ?? Camera.PERSPECTIVE_CAMERA;
    },
    set: function (this: TargetCamera, m: number): void {
        const self = this as any;
        if (self._twinfinityMode === undefined && m === 0) {
            self._twinfiinityMode = 0; // This is during camera initialization (constructor).
            return;
        }
        if (self._twinfinityMode !== m) {
            this.twinfinity.pivot.update();

            if (m === Camera.PERSPECTIVE_CAMERA) {
                // this.minZ /= 10;
                // this.maxZ /= 10;

                const cameraForwardIntersectionWithPivotPlane = Vector3.Zero();
                this.twinfinity.pivot.intersectWithScreenCoordinateToRef(
                    cameraForwardIntersectionWithPivotPlane,
                    PredefinedCanvasPosition.Center
                );
                // When switching from ortho to perspective means that we have to calculate a new camera position
                // First calculate the distance in front of the pivot plane that the camera should be positioned at
                const perspectiveDistance = this.twinfinity.perspectiveDistanceFromOrtho;

                // position camera at perspectiveDistance from pivotplane along plane normal positioned at pivot point
                // intersection.
                cameraForwardIntersectionWithPivotPlane.addToRef(
                    this.twinfinity.pivot.normal.scale(perspectiveDistance),
                    this.position
                );
            } else {
                // Set size of orthographic frustum based on the distance to the pivotplane.
                this.twinfinity.setOrtho({ distance: this.twinfinity.pivot.signedDistanceToCamera });
            }

            // NEVER change order here. If we change _mode before then
            // get mode() will return the new mode in calculations above. Ie instead
            // of doing calculations in orthographic view when going from orthographic -> perspective
            // we would perform calculations in perspective view instead (which would be completely wrong).
            self._twinfinityMode = m;

            // After new _mode is sat, notify Observers that mode has change.
            // The get mode() will return the new mode!
            if (this instanceof PivotTargetCamera) {
                this.onMode.notifyObservers(this);
            }
        }
    }
});
