(function(){
	
	var SHOW_DEBUG_SHAPES = false;
	var USE_COLLISIONS = true;
    
	var GRAVITY = 0.096;

    var DELTA = 1.0;
    var DAMPING = 1;
    var JOINT_FILTER_KEYWORDS = ["hair", "skirt"];
    var CONTRAINT_ITERATIONS = 1;
    var COLLISION_FRICTION = 0.5;
	var COLLISION_REPULSION = 20;
    
	var TOUCHING_DISTANCE = 2.0;
	var JOINT_COLLISION_PREFIX = "joint_";
	var HAND_COLLISION_PREFIX = "hand_";
	var HAND_COLLISION_RADIUS = 0.08;
	
	// Vec3 Utils ////////////////////////////////////
	var VEC3_UTIL_EPSILON = 0.000001;
	var VEC3_UTIL_EPSILON_SQUARED = VEC3_UTIL_EPSILON * VEC3_UTIL_EPSILON;
	var VEC3_UTIL_PI = 3.14159265358979;
	var VEC3_UTIL_ALMOST_ONE= 1.0 - VEC3_UTIL_EPSILON;
	var VEC3_UTIL_PI_OVER_TWO = 1.57079632679490;
	function vec3(x,y,z) {return {x:x,y:y,z:z};}
	function lengthVec3(V) {return Math.sqrt(V.x*V.x+V.y*V.y+V.z*V.z);}
	function addVec3s(A,B) {return {x:A.x+B.x,y:A.y+B.y,z:A.z+B.z};}
	function subtractVec3s(A,B) {return {x:A.x-B.x,y:A.y-B.y,z:A.z-B.z};}
	function scaleVec3(V, scale) {return {x:scale*V.x,y:scale*V.y,z:scale*V.z};}
	function dotVec3s(A,B) {return A.x*B.x+A.y*B.y+A.z*B.z;}
	function crossVec3s(A,B) {return {x:A.y*B.z-A.z*B.y,y:A.z*B.x-A.x*B.z,z:A.x*B.y-A.y*B.x};}
	function distanceVec3s(A,B) {return Math.sqrt((A.x-B.x)*(A.x-B.x)+(A.y-B.y)*(A.y-B.y)+(A.z-B.z)*(A.z-B.z));}
	function normalizeVec3(V) {
		var L2=V.x*V.x+V.y*V.y+V.z*V.z;
		if(L2<VEC3_UTIL_EPSILON_SQUARED) {return {x:V.x,y:V.y,z:V.z};}
		var invL=1.0/Math.sqrt(L2);
		return {x:invL*V.x,y:invL*V.y,z:invL*V.z}; 
	}
	function angleBetweenVec3s(A,B) {
		var dot=dotVec3s(A,B);
		if(dot<VEC3_UTIL_EPSILON) {return VEC3_UTIL_PI_OVER_TWO;}
		var cosAngle=dot/(lengthVec3(A)*lengthVec3(B));
		if(cosAngle>=VEC3_UTIL_ALMOST_ONE) {return 0.0;}
		if(cosAngle<=-VEC3_UTIL_ALMOST_ONE) {return VEC3_UTIL_PI;}
		return Math.acos(cosAngle);
	}
	
	

	
	var grabPower = {
		"left": 0.0,
		"right": 0.0
	};
    
    var COLLISION_SPHERES = {
        /*
        "RightHandPinky4": 0.01,
        "LeftHandPinky4": 0.01,
        "RightHandThumb4": 0.01,
        "LeftHandThumb4": 0.01,
        "RightHandRing4": 0.01,
        "LeftHandRing4": 0.01,
        "RightHandMiddle4": 0.01,
        "LeftHandMiddle4": 0.01,
        "RightHandIndex4": 0.01,
        "LeftHandIndex4": 0.01,
        "Spine": 0.05,
        "Hips": 0.01,
        "Spine2": 0.05,
        "Head": 0.1,
        "LeftShoulder": 0.05,
        "LeftUpLeg": 0.05,
        "LeftArm": 0.05,
        "Spine1": 0.05,
        "RightLeg": 0.05,
        "Neck": 0.05,
        "RightShoulder": 0.05,
        "LeftLeg": 0.05,
        "RightArm": 0.05        
        */
        "Head": {radius: 0.1, offset: {x: 0, y: 0.06, z: 0.05}},
        "LeftShoulder": {radius: 0.07, offset: {x: 0, y: 0.00, z: 0.00}},
        "RightShoulder": {radius: 0.07, offset: {x: 0, y: 0.00, z: 0.00}},
        "RightArm": {radius: 0.07, offset: {x: 0, y: 0.00, z: 0.00}},
        "LeftArm": {radius: 0.07, offset: {x: 0, y: 0.00, z: 0.00}},
        "Spine2": {radius: 0.07, offset: {x: 0, y: 0, z: 0.00}}
		//"RightHand": {radius: 0.08, offset: {x: 0, y: 0.06, z: 0.00}},
		//"LeftHand": {radius: 0.08, offset: {x: 0, y: 0.06, z: 0.00}}//,
        //"Spine1": {radius: 0.07, offset: {x: 0, y: 0.00, z: 0.00}},
        //"Spine2": {radius: 0.06, offset: {x: 0, y: 0.00, z: 0.00}}
    };
	
	var nearbyAvatarHands = {};
	

	var FlowHandSystem = function() {
		var self = this;
		this.HANDS_COLLISION_JOINTS = ["RightHandMiddle1", "LeftHandMiddle1"];
		this.avatarIds = [];
		this.avatarHands = [];
		
		this.getNearbyAvatars = function(distance) {
			return AvatarList.getAvatarIdentifiers().filter(function(avatarID){
				if (!avatarID) {
					return false;
				}
				var avatar = AvatarList.getAvatar(avatarID);
				var avatarPosition = avatar && avatar.position;
				if (!avatarPosition) {
					return false;
				}
				return distanceVec3s(avatarPosition, MyAvatar.position) < distance;
			});
		}
		
		this.update = function() {
			var nearbyAvatars = self.getNearbyAvatars(TOUCHING_DISTANCE);
			nearbyAvatars.push(MyAvatar.SELF_ID);
			nearbyAvatars.forEach(function(avatarID) {

				var avatar = AvatarList.getAvatar(avatarID);
				var avatarIndex = self.avatarIds.indexOf(avatarID);
				if (avatarIndex === -1) {
					var newHands = {};
					for (var i = 0; i < self.HANDS_COLLISION_JOINTS.length; i++) {
						var side = self.HANDS_COLLISION_JOINTS[i];
						var jointId = avatar.getJointIndex(side);
						var position = avatar.getJointPosition(jointId);
						var name =  avatarID + "_" + HAND_COLLISION_PREFIX + side;
						newHands[side] = new FlowCollisionSphere(name, jointId, HAND_COLLISION_RADIUS, {x: 0, y: 0, z: 0}, avatarID);
					}
					self.avatarHands.push(newHands);
					avatarIndex = self.avatarIds.length;
					self.avatarIds.push(avatarID);
				}

				for (var i = 0; i < self.HANDS_COLLISION_JOINTS.length; i++) {
					var side = self.HANDS_COLLISION_JOINTS[i];
					self.avatarHands[avatarIndex][side].update();
				}
			});
		}
		
		this.checkCollisions = function(jointIndex) {
			var collision = {"offset": -1, "position": {x: 0, y: 0, z: 0}, "radius": -1, "normal": {x: 0, y: -1, z: 0}};
            for (var i = 0; i < self.avatarHands.length; i++) {
				for (var j = 0; j < self.HANDS_COLLISION_JOINTS.length; j++) {
					var side = self.HANDS_COLLISION_JOINTS[j];
					collision = self.avatarHands[i][side].checkCollision(jointIndex);
					if (collision.offset > 0) {
						break;
					}
				}
				if (collision.offset > 0) {
					varsToDebug.collision = collision;
					break;
				}
            }
            return collision;
        }
	}
	
	var handSystem = new FlowHandSystem();
	
    var FlowDebug = function() {
        var self = this;
        this.debugLines = {};
        this.debugSpheres = {};
        this.showDebugShapes = false;
        
        this.setDebugLine = function(lineName, startPosition, endPosition, shapeColor) {
            if (!self.showDebugShapes) return;
            var start = startPosition ? startPosition : {x: 0, y: 0, z: 0};
            var end = endPosition ? endPosition : {x: 0, y: 1, z: 0};
            var color = shapeColor ? shapeColor : { red: 0, green: 255, blue: 255 };
            if (self.debugLines[lineName] != undefined) {
                Overlays.editOverlay(self.debugLines[lineName], {
                    color: color,
                    start: start,
                    end: end,
                    visible: self.showDebugShapes
                });
            } else if (self.showDebugShapes) {
                self.debugLines[lineName] = Overlays.addOverlay("line3d", {
                    color: color,
                    start: start,
                    end: end,
                    visible: self.showDebugShapes
                });
            }
        }
        
        this.setDebugSphere = function(sphereName, pos, diameter, shapeColor) {
            if (!self.showDebugShapes) return;
            var scale = diameter ? diameter : 0.01;
            var color = shapeColor ? shapeColor : { red: 255, green: 0, blue: 255 };
            if (self.debugSpheres[sphereName] != undefined) {
                Overlays.editOverlay(self.debugSpheres[sphereName], {
                    color: color,
                    position: pos,
                    visible: self.showDebugShapes
                });
            } else if (self.showDebugShapes){
                self.debugSpheres[sphereName] = Overlays.addOverlay("sphere", {
                    color: color,
                    position: pos,
                    scale: {x:scale, y:scale, z:scale},
                    visible: self.showDebugShapes
                });
            }
        }
        
        this.cleanup = function() {
            for (lineName in self.debugLines) {
                Overlays.deleteOverlay(flowDebug.debugLines[lineName]);
            }
            for (sphereName in self.debugSpheres) {
                Overlays.deleteOverlay(flowDebug.debugSpheres[sphereName]);
            }
        }
        
        this.setVisible = function(isVisible) {
            self.showDebugShapes = isVisible;
            for (var lineName in self.debugLines) {
                Overlays.editOverlay(self.debugLines[lineName], {
                    visible: isVisible
                });
            }
            for (var sphereName in self.debugSpheres) {
                Overlays.editOverlay(self.debugSpheres[sphereName], {
                    visible: isVisible
                });
            }
        }
        
        this.toggleVisible = function() {
            self.showDebugShapes = !self.showDebugShapes;
        }
    }
    
    var flowDebug = new FlowDebug();
    flowDebug.setVisible(SHOW_DEBUG_SHAPES);
    
    var FlowCollisionSystem = function() {
        var self = this;
        this.collisionSpheres = [];
        this.addCollisionSphere = function(name, jointIndex, radius, offset) {
            self.collisionSpheres.push(new FlowCollisionSphere(name, jointIndex, radius, offset));
        }
        this.update = function() {
            for (var i = 0; i < self.collisionSpheres.length; i++) {
                self.collisionSpheres[i].update();
            }
        }
        this.checkCollisions = function(jointIndex) {
            var collision = {"offset": -1, "direction": {x: 0, y: -1, z: 0}};
            for (var i = 0; i < self.collisionSpheres.length; i++) {
                collision = self.collisionSpheres[i].checkCollision(jointIndex);
                if (collision.offset > 0) {
                    break;
                }
            }
            return collision;
        }
    }
    
    var FlowCollisionSphere = function(name, jointIndex, radius, offset, avatarId) {
        var self = this;
		this.name = name;
        this.radius = radius;
        this.jointIndex = jointIndex;
        this.position = {x:0, y:0, z:0};
        this.offset = offset;
		this.avatarId = avatarId;
        
        this.update = function() {
			if (self.avatarId != undefined){
				var avatar = AvatarList.getAvatar(self.avatarId);
				self.position = avatar.getJointPosition(self.jointIndex);
			} else {
				self.position = MyAvatar.jointToWorldPoint(offset, jointIndex);
			}				
            flowDebug.setDebugSphere(self.name, self.position, 2*self.radius, {red: 200, green: 10, blue: 50});
        }
        
        this.checkCollision = function(jointIndex) {
            var jointPosition = MyAvatar.getJointPosition(jointIndex);
            var centerToJoint = subtractVec3s(jointPosition, self.position);
            var distance = lengthVec3(centerToJoint);
            var offset =  self.radius - distance;
            return {"offset": offset, "position": self.position, "radius": self.radius, "normal": normalizeVec3(centerToJoint)};
        }
    }
    
    var FlowAvatar = function() {
        var self = this;
        this.lastPosition = MyAvatar.position;
        this.currentPosition = MyAvatar.position;
        this.lastVelocity = {x: 0, y: 0, z:0};
        this.currentVelocity = {x: 0, y: 0, z:0};
        this.acceleration = {x: 0, y: 0, z:0};
        this.update = function() {
            self.currentVelocity = subtractVec3s(self.currentPosition, self.lastPosition);
            self.acceleration = subtractVec3s(self.currentVelocity, self.lastVelocity);
            self.lastPosition = self.currentPosition;
            self.lastVelocity = self.currentVelocity;
        }
    }
    
    var FlowJoint = function(index, parentIndex, name){
        var self = this;
        this.index = index;
        this.name = name;
        this.parentIndex = parentIndex;
        
        this.initialTranslation = MyAvatar.getJointTranslation(index);
        
        this.previousTranslation = this.initialTranslation;
        this.currentTranslation = this.initialTranslation;
        this.initialTranslation = this.initialTranslation;
        this.length = lengthVec3(this.initialTranslation);
        
        this.velocity = {x:0, y:0, z:0};
        this.acceleration = {x:0, y:0, z:0};
        this.parents = [];
        this.ajusted = false;
        this.anchored = false;
        this.colliding = false;
        this.collision;
        
        this.update = function (delta, damping, acceleration, collision) {
			
            self.colliding = collision && (collision.offset > 0);
            self.collision = collision;
            self.acceleration = MyAvatar.worldToJointPoint(addVec3s(acceleration, MyAvatar.getJointPosition(self.parentIndex)), self.parentIndex);
            self.velocity = subtractVec3s(self.currentTranslation, self.previousTranslation);
            if (!self.anchored) {               
                if (self.colliding) {
                    var normal = MyAvatar.worldToJointPoint(addVec3s(collision.normal, MyAvatar.getJointPosition(self.parentIndex)), self.parentIndex);
                    var upVector = {x: 0, y: 1, z: 0};
                    var xFromNormal = normalizeVec3(crossVec3s(normal, upVector));
                    var yFromNormal = normalizeVec3(crossVec3s(normal, xFromNormal));

                    var acceleration_x = scaleVec3(xFromNormal, (1 - COLLISION_FRICTION) * dotVec3s(self.acceleration, xFromNormal));
                    var acceleration_y = scaleVec3(yFromNormal, (1 - COLLISION_FRICTION) * dotVec3s(self.acceleration, yFromNormal));
                    var acceleration_z = scaleVec3(normal, Math.abs(dotVec3s(self.acceleration, normal)));
                    var velocity_x = scaleVec3(xFromNormal, (1 - COLLISION_FRICTION) * dotVec3s(self.velocity, xFromNormal));
                    var velocity_y = scaleVec3(yFromNormal, (1 - COLLISION_FRICTION) * dotVec3s(self.velocity, yFromNormal));
                    var velocity_z = scaleVec3(normal, COLLISION_REPULSION *collision.offset);
                    self.acceleration = addVec3s(addVec3s(acceleration_x, acceleration_y), acceleration_z);
                    self.velocity = addVec3s(addVec3s(velocity_x, velocity_y), velocity_z);
                } 
                self.previousTranslation = self.currentTranslation;
                self.currentTranslation = addVec3s(addVec3s(self.currentTranslation, scaleVec3(self.velocity, damping)), scaleVec3(self.acceleration, Math.pow(delta, 2)));
            } else {
                self.acceleration = {x:0, y:0, z:0};
                self.velocity = {x:0, y:0, z:0};
                self.previousTranslation = self.currentTranslation;
            }
        }
        
        this.ajust = function() {
            var parentJoint = flowJointData[self.parentIndex];
            if (!parentJoint) {
                return;
            }
            var worldVelocity = subtractVec3s(MyAvatar.jointToWorldPoint(parentJoint.velocity, parentJoint.index), MyAvatar.getJointPosition(parentJoint.index));
            var worldVelocity = addVec3s(MyAvatar.getJointPosition(self.index), worldVelocity);
            var jointVelocity = MyAvatar.worldToJointPoint(worldVelocity, self.index);
            self.velocity = subtractVec3s(self.velocity, jointVelocity);
        }
        
        this.solve = function() {
            var difference = self.length/lengthVec3(self.currentTranslation);
            self.currentTranslation = difference < 1.0 ? scaleVec3(self.currentTranslation, difference) : self.currentTranslation;
            self.ajusted = false;
        }
        
        this.apply = function() {
            if (!self.anchored) {
                MyAvatar.setJointTranslation(self.index, self.currentTranslation);
            }
            flowDebug.setDebugSphere(self.name, MyAvatar.getJointPosition(self.index), 0.02, {red: 255, green:self.colliding ? 0 : 255, blue:0});
            flowDebug.setDebugLine(self.name, MyAvatar.getJointPosition(self.index), MyAvatar.getJointPosition(self.parentIndex), {red: 255, green:(self.colliding ? 0 : 255), blue:0});
        }
    }
    
    var flowSkeleton;
    var flowJointData = [];
    var flowJointNameMap = {};
    var flowThreads = [];
    var flowAvatar = new FlowAvatar();
    var updateOrder = [];
    

    
    var collisionSystem = new FlowCollisionSystem();
    
    function getUpHierarchyLevels(baseIndex) {
        var upLevels = 0;
        var currentIndex = baseIndex;
        var siblings = [];
        for (var i = 0; i < flowSkeleton.length; i++) {
            var parentIndex = flowJointData[currentIndex].parentIndex;
            if (flowJointData[parentIndex] != undefined) {
                upLevels++;
                currentIndex = parentIndex;
            } else {
                break;
            }
        }
        return upLevels;
    }
    
    var calculateConstraints = function() {

        flowJointData = [];
        flowSkeleton = MyAvatar.getSkeleton().filter(
            function(jointInfo){
                var name = jointInfo.name;
                // We need a MyAvatar.getJointName(jointIndex) function
                flowJointNameMap[name] = jointInfo.index;
                var isFlowJoint = false;
                for (var i = 0; i < JOINT_FILTER_KEYWORDS.length; i++) {
                    isFlowJoint = isFlowJoint || name.indexOf(JOINT_FILTER_KEYWORDS[i]) > -1;
                }
                if (isFlowJoint){
                    if (flowJointData[jointInfo.index] === undefined) {
                        flowJointData[jointInfo.index] = new FlowJoint(jointInfo.index, jointInfo.parentIndex, name);
                    }
                } else if (COLLISION_SPHERES[name]){
                    collisionSystem.addCollisionSphere(JOINT_COLLISION_PREFIX + jointInfo.index, jointInfo.index, COLLISION_SPHERES[name].radius, COLLISION_SPHERES[name].offset);
                }
                return isFlowJoint;
            }
        );  
        
        for (var i = 0; i < flowSkeleton.length; i++) {
            var index = flowSkeleton[i].index;
            var flowJoint = flowJointData[index];
            var parentFlowJoint = flowJointData[flowJoint.parentIndex];
            flowJoint.anchored = (parentFlowJoint === undefined);
        }
        
        for (var i = 0; i < flowSkeleton.length; i++) {
            var index = flowSkeleton[i].index;
            var flowJoint = flowJointData[index];
            var parentFlowJoint = flowJointData[flowJoint.parentIndex];
            flowJoint.anchored = (parentFlowJoint === undefined);
            updateOrder.push({"index": index, "upLevels": getUpHierarchyLevels(index)});            
        }

        updateOrder.sort(function(a, b) {
            return a.upLevels - b.upLevels;
        });
    }

    calculateConstraints();
    
    var collision_keys = Object.keys(COLLISION_SPHERES);

    Script.update.connect(function(){
        if (!flowSkeleton) return;
        
        if (USE_COLLISIONS) {
			Script.beginProfileRange("JS-Update-Collisions");
            collisionSystem.update();
			handSystem.update();
			varsToDebug.handSystem = handSystem;
			Script.endProfileRange("JS-Update-Collisions");
        }
        var acceleration = {x:0, y:-GRAVITY, z:0};
        Script.beginProfileRange("JS-Update-JointsUpdate");
        for (var i = 0; i < updateOrder.length; i++) {
            var index = updateOrder[i].index;
            var joint = flowJointData[index];
            var bodyCollision = USE_COLLISIONS ? collisionSystem.checkCollisions(joint.index) : undefined;
			var handCollision = USE_COLLISIONS ? handSystem.checkCollisions(joint.index) : undefined;
			var collision = (handCollision && handCollision.offset > 0) ? handCollision : bodyCollision;
            joint.update(DELTA, DAMPING, acceleration, collision);
        }
		Script.endProfileRange("JS-Update-JointsUpdate");
		Script.beginProfileRange("JS-Update-JointsSolve");
        for (var j = 0; j < CONTRAINT_ITERATIONS; j++) {
            for (var i = 0; i < updateOrder.length; i++) {
                var index = updateOrder[i].index;
                flowJointData[index].solve();
            }
        }
        Script.endProfileRange("JS-Update-JointsSolve");
		Script.beginProfileRange("JS-Update-JointsApply");
        for (var i = 0; i < updateOrder.length; i++) {
            var index = updateOrder[i].index;
            flowJointData[index].apply();
        }
		Script.endProfileRange("JS-Update-JointsApply");
    });
    
    Script.scriptEnding.connect(function () {
        for (var i = 0; i < flowSkeleton.length; i++) {
            var index = flowSkeleton[i].index;
            MyAvatar.setJointTranslation(index, flowJointData[index].initialTranslation);
        }
        flowDebug.cleanup();
    });
    
	var varsToDebug = {
		"toggleDebugShapes": function() {
			flowDebug.setVisible(!flowDebug.showDebugShapes);
		},
		"toggleCollisions": function() {
			USE_COLLISIONS = !USE_COLLISIONS;
		},
		"handSystem":  handSystem,
		"collision": {"offset": -1, "position": {x: 0, y: 0, z: 0}, "radius": -1, "normal": {x: 0, y: -1, z: 0}}
	};
		
    // Register GlobalDebugger for API Debugger
    Script.registerValue("GlobalDebugger", varsToDebug);
	
	
	    // Capture the controller values
    
    var leftTriggerPress = function (value) {
        grabPower.left = value;
    };

    var rightTriggerPress = function (value) {
        grabPower.right = value;
    };
    
    var MAPPING_NAME = "com.highfidelity.hairTouch";
    var mapping = Controller.newMapping(MAPPING_NAME);
    mapping.from([Controller.Standard.RT]).peek().to(rightTriggerPress);
    mapping.from([Controller.Standard.LT]).peek().to(leftTriggerPress);

    Controller.enableMapping(MAPPING_NAME);
    
})()