import {Mesh4D} from "../objects/Mesh4D.js"
import {Vector4} from "../math/Vector4.js"
import {RayPlane4D} from "../math/RayPlane4D.js"
import {G} from "./Constants.js"
import { Face4 } from "../core/Face4.js";
import { Vector2 } from '../math/Vector2.js';
import { Basis2 } from "../math/Basis2.js";

var _vec = new Vector4();

var _v1 = new Vector4();
var _v2 = new Vector4();

var _2v1 = new Vector2();
var _2v2 = new Vector2();
var _2v3 = new Vector2();
var _2v4 = new Vector2();

var _nudge = new Vector4();

function PhysicsMesh4D( geometry, material ) {
    if ( geometry.isGeometry !== true) {
        throw "PhysicsMesh4D: geometry must be Geometry4D (not BufferGeometry4D)";
    }

    Mesh4D.call( this, geometry, material );

    this.type = 'PhysicsMesh4D';
    this.velocity = new Vector4();

    this.isAffectedByGravity = true;
    this.canCollide = true;
    this.mass = 1;
    this.staticFriction = 0.96;
    this.dynamicFriction = 0.96;
    this.bounciness = 0.1;
    this.active = true;
}

PhysicsMesh4D.prototype = Object.assign( Object.create( Mesh4D.prototype ), {
    constructor: PhysicsMesh4D,
    isPhysicsObject: true,

    preUpdate() {
        this.geometry.computeBoundingBox();
        this.geometry.boundingBox.applyMatrix5(this.matrixWorld);
        //this.geometry.computeBoundingGlome();

        for (var child of this.children) {
            if (child.isPhysicsObject === true) {
                child.preUpdate();
            }
        }
    },
    
    update: function(delta, scene, GM) {

        //console.log(this.name + " bounding Box: <" + this.geometry.boundingBox.min.toArray() + ">, <" + this.geometry.boundingBox.max.toArray() + ">");
        if (this.isAffectedByGravity === true) {
            this.velocity.add(_vec.copy(G).multiplyScalar(delta));
        } else {
            //return;
        }
        var movement = _vec.copy(this.velocity).multiplyScalar(delta);

        var colliding = null;
        for (var child of scene.children) {
            if (child.isPhysicsObject === true) {
                colliding = this.getCollision(child);
                if (colliding !== null) {
                    break;
                }
            }
        }
        if (colliding === null) {
            this.position.add(movement);
        } else {
            var theirFaces = this.getFacesCollidingWith(colliding);
            if (theirFaces.length > 0) {
                var face = theirFaces[0];
                var normalComponent = face.parallelBasis.getOrthogonalComponent(this.velocity);
                normalComponent.multiplyScalar(-1 - this.bounciness);
                this.velocity.add(normalComponent);
                this.velocity.multiplyScalar((colliding.dynamicFriction + this.dynamicFriction) * 0.5);
                var movement = _vec.copy(this.velocity).multiplyScalar(delta);

                //_nudge.subVectors(this.position, colliding.position).normalize().multiplyScalar(5);
                //movement.add(_nudge);
            }
            this.position.add(movement);
        }
        for (var child of this.children) {
            if (child.isPhysicsObject === true) {
                child.update(delta, scene);
            }
        }
    },

    /**
     * Check for a collision between {this} and {object}.
     * 
     * If there is a collision, return the colliding object (may be a child of the given object)
     * @param {Object4D} object The object to check for collisions against
     * @returns {Object4D, null}
     */
    getCollision: function(object) {
        if (object === this || this.canCollide === false) return null;

        var colliding = null;
        if (object.geometry !== undefined) {
            colliding = this.geometry.boundingBox.intersectsBox(object.geometry.boundingBox) ? object : null;
        }

        if (colliding !== null) {
            //console.log(this.name + " colliding with " + object.name);
            return colliding;
        }
        for (var child of object.children) {
            if (child.isPhysicsObject === true) {
                colliding = this.getCollision(child);
            }
            if (colliding !== null)  {
                //console.log(this.name + " colliding with " + object.name);
                return colliding;
            }
        }

        return null;
    },

    /**
     * Given that {this} and {object} are colliding, find the exact face on {object} that is being
     * touched by {this}
     * 
     * Returns [] if none is found.
     * 
     * @param {Object4D} object 
     * @returns Face4
     */
    getFacesCollidingWith: function(object) {

        var faces = [];

        for (var objectFace of object.geometry.faces) {
            for (var thisFace of this.geometry.faces) {
                var a = this.geometry.vertices[thisFace.a].clone();
                var b = this.geometry.vertices[thisFace.b].clone();
                var c = this.geometry.vertices[thisFace.c].clone();
    
                a.applyMatrix5(this.matrixWorld);
                b.applyMatrix5(this.matrixWorld);
                c.applyMatrix5(this.matrixWorld);

                var d = object.geometry.vertices[objectFace.a].clone();
                var e = object.geometry.vertices[objectFace.b].clone();
                var f = object.geometry.vertices[objectFace.c].clone();

                d.applyMatrix5(object.matrixWorld);
                e.applyMatrix5(object.matrixWorld);
                f.applyMatrix5(object.matrixWorld);
        
                if (!thisFace.parallelBasis) {
                    _v1.subVectors(b, a);
                    _v2.subVectors(c, a);
                    thisFace.parallelBasis = new Basis2().gramSchmidt(_v1, _v2);
                }
        
                if (!objectFace.parallelBasis) {
                    _v1.subVectors(e, d);
                    _v2.subVectors(f, d);
                    objectFace.parallelBasis = new Basis2().gramSchmidt(_v1, _v2);
                }

                if (this.intersectsTriangle(a, b, c, d, e, f, thisFace.parallelBasis, objectFace.parallelBasis)) {
                    faces.push(objectFace);
                    break
                }
                if (this.intersectsTriangle(d, e, f, a, b, c, objectFace.parallelBasis, thisFace.parallelBasis)) {
                    faces.push(objectFace);
                    break
                }
            }
        }
        return faces;
    },

	intersectsTriangle(a, b, c, d, e, f, parallelBasisABC, parallelBasisDEF) {

        // all vectors in terms of A
        b.sub(a);
        c.sub(a);
        d.sub(a);
        e.sub(a);
        f.sub(a);
        a.set(0,0,0,0);

        // Find orthogonal components of second triangle
		var dortho = parallelBasisABC.getOrthogonalComponent(d);
		var dolen = dortho.length();
		var eortho = parallelBasisABC.getOrthogonalComponent(e);
		var eolen = eortho.length();
		var fortho = parallelBasisABC.getOrthogonalComponent(f);
		var folen = fortho.length();

        // First triangle in terms of A on R^2
        _2v1.set(0,0);
        _2v2.set(b.dot(parallelBasisABC.x), b.dot(parallelBasisABC.y));
        _2v3.set(c.dot(parallelBasisABC.x), c.dot(parallelBasisABC.y));


		// line segment DE intersects plane
		if (_v1.addVectors(dortho, eortho).length() <= Math.max(dolen, eolen)) {
			// Find the point on the plane
			var proportion = dolen / (dolen + eolen);
			_v1.subVectors(d, e).multiplyScalar(proportion).add(d);
			_2v4.set(_v1.dot(parallelBasisABC.x), _v1.dot(parallelBasisABC.y));

			var dX = _2v4.x-_2v3.x;
			var dY = _2v4.y-_2v3.y;
			var dX21 = _2v3.x-_2v2.x;
			var dY12 = _2v2.y-_2v3.y;
			var D = dY12*(_2v1.x-_2v3.x) + dX21*(_2v1.y-_2v3.y);
			var s = dY12*dX + dX21*dY;
			var t = (_2v3.y-_2v1.y)*dX + (_2v1.x-_2v3.x)*dY;

			var intersects;
			if (D<0) {
				intersects = s<=0 && t<=0 && s+t>=D;
			} else {
				intersects = s>=0 && t>=0 && s+t<=D;
			}

			if (intersects === true) return true;
		}
		// line segment DF intersects plane
		if (_v1.addVectors(dortho, fortho).length() <= Math.max(dolen, folen)) {
			// Find the point on the plane
			var proportion = dolen / (dolen + folen);
			_v1.subVectors(d, f).multiplyScalar(proportion).add(d);
			_2v4.set(_v1.dot(parallelBasisABC.x), _v1.dot(parallelBasisABC.y));

			var dX = _2v4.x-_2v3.x;
			var dY = _2v4.y-_2v3.y;
			var dX21 = _2v3.x-_2v2.x;
			var dY12 = _2v2.y-_2v3.y;
			var D = dY12*(_2v1.x-_2v3.x) + dX21*(_2v1.y-_2v3.y);
			var s = dY12*dX + dX21*dY;
			var t = (_2v3.y-_2v1.y)*dX + (_2v1.x-_2v3.x)*dY;

			var intersects;
			if (D<0) {
				intersects = s<=0 && t<=0 && s+t>=D;
			} else {
				intersects = s>=0 && t>=0 && s+t<=D;
			}

			if (intersects === true) return true;
		}
		// line segment EF intersects plane
		if (_v1.addVectors(eortho, fortho).length() <= Math.max(eolen, folen)) {
			// Find the point on the plane
			var proportion = eolen / (eolen + folen);
			_v1.subVectors(e, f).multiplyScalar(proportion).add(e);
			_2v4.set(_v1.dot(parallelBasisABC.x), _v1.dot(parallelBasisABC.y));

			var dX = _2v4.x-_2v3.x;
			var dY = _2v4.y-_2v3.y;
			var dX21 = _2v3.x-_2v2.x;
			var dY12 = _2v2.y-_2v3.y;
			var D = dY12*(_2v1.x-_2v3.x) + dX21*(_2v1.y-_2v3.y);
			var s = dY12*dX + dX21*dY;
			var t = (_2v3.y-_2v1.y)*dX + (_2v1.x-_2v3.x)*dY;

			var intersects;
			if (D<0) {
				intersects = s<=0 && t<=0 && s+t>=D;
			} else {
				intersects = s>=0 && t>=0 && s+t<=D;
			}

			if (intersects === true) return true;
		}

		return false;
	},

    setActive: function( active ) {
        this.active = active;
        this.visible = active;
        this.noCollision = !active;

        for (var child of this.children) {
            child.setActive(active);
        }
    },
} );

export { PhysicsMesh4D }