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

    var JOINT_FILTER_KEYWORDS = [
		{"keyword": "hair", "stiffness": 0.0, "gravity": {x: 0, y: -0.0096, z: 0} ,"damping": 0.85, "inertia": 0.25, "delta": 0.45}, 
		{"keyword": "skirt", "stiffness": 0.0, "gravity": {x: 0, y: -0.0096, z: 0} ,"damping": 0.85, "inertia": 0.25, "delta": 0.45}, 
		{"keyword": "Breast", "stiffness": 1, "gravity": {x: 0, y: -0.0096, z: 0} ,"damping": 0.65, "inertia": 0.8, "delta": 0.45}
	];
	
	var CONTRAINT_ITERATIONS = 1;
    var COLLISION_FRICTION = 0.0;
	var COLLISION_REPULSION = 1;
    
	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_SHAPES = {
        "Head": {type: "sphere", radius: 0.12, offset: {x: 0, y: 0.02, z: 0.00}},
		//"Spine2": {type: "cube", dimensions: {x: 0.35, y: 0.35, z: 0.08}, offset: {x: 0, y: -0.0, z: 0.01}},
		//"LeftUpLeg": {type: "cube", dimensions: {x: 0.12, y: 0.45, z: 0.12}, offset: {x: 0, y: 0.25, z: 0.0}},
		//"RightUpLeg": {type: "cube", dimensions: {x: 0.12, y: 0.45, z: 0.12}, offset: {x: 0, y: 0.25, z: 0.0}}
        "LeftShoulder": {type: "sphere",radius: 0.09, offset: {x: 0, y: 0.05, z: 0.00}},
        "RightShoulder": {type: "sphere",radius: 0.09, offset: {x: 0, y: 0.05, z: 0.00}},
		"Spine2": {type: "sphere",radius: 0.09, offset: {x: 0, y: 0.05, z: 0.00}}
        //"RightArm": {type: "sphere", radius: 0.07, offset: {x: 0, y: 0.00, z: 0.00}},
        //"LeftArm": {type: "sphere", radius: 0.07, offset: {x: 0, y: 0.00, z: 0.00}}
        //"Spine2": {radius: 0.09, offset: {x: 0, y: 0.06, z: 0.00}},
		//"Spine1": {radius: 0.09, offset: {x: 0, y: 0.02, z: 0.00}}
    };
	
	var nearbyAvatarHands = {};
	

	var FlowHandSystem = function() {
		var self = this;
		this.HANDS_COLLISION_JOINTS = ["RightHandMiddle1", "LeftHandMiddle1"];
		this.avatarIds = [];
		this.avatarHands = [];
		this.lastAvatarCount = 0;
		this.leftTriggerValue = 0;
		this.rightTriggerValue = 0;
		
		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();
				}
			});
			
			if (nearbyAvatars.length < self.lastAvatarCount) {
				var avatarsToUntrack = [];
				for (var i = 0; i < self.avatarIds.length; i++) {
					var index = nearbyAvatars.indexOf(self.avatarIds[i]);
					if (index == -1) {
						avatarsToUntrack.push(i);
					}
				}
				avatarsToUntrack.sort();
				for (var i = 0; i < avatarsToUntrack.length; i++) {
					self.avatarIds.splice(avatarsToUntrack[i]-i, 1);
					self.avatarHands.splice(avatarsToUntrack[i]-i, 1);
				}
			}
			self.lastAvatarCount = nearbyAvatars.length;

		}
		
		this.checkCollisions = function(point, index) {
			var collision = new FlowCollisionData();
			var avatarId, side;
            for (var i = 0; i < self.avatarHands.length; i++) {
				for (var j = 0; j < self.HANDS_COLLISION_JOINTS.length; j++) {
					side = self.HANDS_COLLISION_JOINTS[j];
					collision = self.avatarHands[i][side].checkCollision(point, index);
					if (collision.offset > 0) {
						break;
					}
				}
				if (collision.offset > 0) {
					avatarId = self.avatarIds[i];
					if (avatarId == MyAvatar.SELF_ID) {
						var grabbing_coeficient = (side == self.HANDS_COLLISION_JOINTS[1]) ? self.leftTriggerValue : self.rightTriggerValue;
						if (grabbing_coeficient > 0.1) {
							varsToDebug.grabbing_coeficient = grabbing_coeficient;
							collision.normal = addVec3s(collision.normal, scaleVec3(collision.normal, -2 * grabbing_coeficient));
							collision.radius = collision.radius - (collision.radius * collision.offset);
						}
					}
					break;
				}
            }
            return collision;
        }
		
		this.setRightTriggerValue = function(value) {
			self.rightTriggerValue = value;
		}
		
		this.setLeftTriggerValue = function(value) {
			self.leftTriggerValue = value;
		}
	}
	
	var handSystem = new FlowHandSystem();
	
    var FlowDebug = function() {
        var self = this;
        this.debugLines = {};
        this.debugSpheres = {};
		this.debugCubes = {};
        this.showDebugShapes = false;
		this.solidShapes = false;
        
		this.setDebugCube = function(cubeName, cubePosition, cubeRotation, cubeDimensions, shapeColor) {
            if (!self.showDebugShapes) return;
            var position = cubePosition ? cubePosition : {x: 0, y: 0, z: 0};
            var rotation = cubeRotation ? cubeRotation : {x: 0, y: 0, z: 0, w: 0};
			var dimensions = cubeDimensions ? cubeDimensions : {x: 1, y: 1, z: 1};
            var color = shapeColor ? shapeColor : { red: 0, green: 255, blue: 255 };
            if (self.debugCubes[cubeName] != undefined) {
                Overlays.editOverlay(self.debugCubes[cubeName], {
                    position: position,
					rotation: rotation,
					dimensions: dimensions,
					color: color,
					solid: self.solidShapes,
                    visible: self.showDebugShapes
                });
            } else if (self.showDebugShapes) {
                self.debugCubes[cubeName] = Overlays.addOverlay("cube", {
                    position: position,
					rotation: rotation,
					dimensions: dimensions,
					color: color,
					solid: self.solidShapes,
                    visible: self.showDebugShapes
                });
            }
        }
		
        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,
					solid: self.solidShapes,
                    visible: self.showDebugShapes
                });
            } else if (self.showDebugShapes){
                self.debugSpheres[sphereName] = Overlays.addOverlay("sphere", {
                    color: color,
                    position: pos,
                    scale: {x:scale, y:scale, z:scale},
					solid: self.solidShapes,
                    visible: self.showDebugShapes
                });
            }
        }
		
		this.deleteSphere = function(name) {
			Overlays.deleteOverlay(flowDebug.debugSpheres[name]);
		}
		
        this.deleteLine = function(name) {
			Overlays.deleteOverlay(flowDebug.debugLines[name]);
		}
		
		this.deleteCube = function(name) {
			Overlays.deleteOverlay(flowDebug.debugCubes[name]);
		}
		
        this.cleanup = function() {
            for (lineName in self.debugLines) {
                self.deleteLine(lineName);
            }
            for (sphereName in self.debugSpheres) {
                self.deleteSphere(sphereName);
            }
			for (cubeName in self.debugCubes) {
                self.deleteCube(cubeName);
            }
        }
        
        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.collisionCubes = [];
        this.addCollisionSphere = function(name, jointIndex, radius, offset) {
            self.collisionSpheres.push(new FlowCollisionSphere(name, jointIndex, radius, offset));
        }
		this.addCollisionCube = function(name, jointIndex, dimensions, offset) {
            self.collisionCubes.push(new FlowCollisionCube(name, jointIndex, dimensions, offset));
        }
        this.update = function() {
            for (var i = 0; i < self.collisionCubes.length; i++) {
                self.collisionCubes[i].update();
            }
			for (var i = 0; i < self.collisionSpheres.length; i++) {
                self.collisionSpheres[i].update();
            }
        }
		
		this.computeCollision = function(collisions) {
			var collisionData = new FlowCollisionData();
			if (collisions.length > 1) {
				for (var i = 0; i < collisions.length; i++) {
					collisionData.offset += collisions[i].offset 
					collisionData.normal = addVec3s(collisionData.normal, collisions[i].normal);
					collisionData.position = addVec3s(collisionData.position, collisions[i].position);
					collisionData.radius += collisions[i].radius 
				}
				collisionData.offset = collisionData.offset/collisions.length;
				collisionData.normal = normalizeVec3(scaleVec3(collisionData.normal, 1/collisions.length));
				collisionData.position = scaleVec3(collisionData.position, 1/collisions.length);
				collisionData.radius = dotVec3s(scaleVec3(collisions[0].normal, collisions[0].radius), collisionData.normal);
			} else if (collisions.length == 1){
				collisionData = collisions[0];
			}
			collisionData.collisionCount = collisions.length;
			return collisionData;
		}
        
		this.checkCollisions = function(point, index) {
			var collisions = [];
            for (var i = 0; i < self.collisionSpheres.length; i++) {
                var collision = self.collisionSpheres[i].checkCollision(point, index);
                if (collision.offset > 0) {
                    collisions.push(collision);
                }
            }
			for (var i = 0; i < self.collisionCubes.length; i++) {
                var collision = self.collisionCubes[i].checkCollision(point, index);
				console.log("checking");
                if (collision.offset > 0) {
					console.log("collision");
                    collisions.push(collision);
                }
            }
			return self.computeCollision(collisions);
        }
    }
	
	var FlowCollisionData = function(offset, position, radius, normal) {
		this.collisionCount = 0;
		this.offset = offset != undefined ? offset : 0;
		this.position = position != undefined ? position : {x: 0, y: 0, z: 0};
		this.radius = radius != undefined ? radius : 0;
		this.normal = normal != undefined ? normal : {x: 0, y: 0, z: 0};
	}
    
    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(point, index) {
            var centerToJoint = subtractVec3s(point, self.position);
            var distance = lengthVec3(centerToJoint);
            var offset =  self.radius - distance;
			var collisionData = new FlowCollisionData(offset, self.position, self.radius, normalizeVec3(centerToJoint));
            return collisionData;
        }
    }

	var FlowCollisionCube = function(name, jointIndex, dimensions, offset, avatarId) {
        var self = this;
		this.name = name;
        this.dimensions = dimensions;
        this.jointIndex = jointIndex;
        this.position = {x:0, y:0, z:0};
		this.rotation = {x:0, y:0, z:0, w: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);
				self.rotation = avatar.getJointRotation(self.jointIndex);
			} else {
				self.position = MyAvatar.jointToWorldPoint(offset, jointIndex);
				self.rotation = MyAvatar.jointToWorldRotation({x:0, y:0, z:0, w:0}, jointIndex);
			}				
			flowDebug.setDebugCube(self.name, self.position, self.rotation, self.dimensions, {red: 200, green: 10, blue: 50});
        }
        
        this.checkCollision = function(point, index) {
			var localPoint = MyAvatar.worldToJointPoint(point, self.jointIndex);
			var localPosition = MyAvatar.worldToJointPoint(self.position, self.jointIndex);
			var centerToJoint = subtractVec3s(localPoint, localPosition);
			var offsets = {	x: (self.dimensions.x/2) - Math.abs(centerToJoint.x), 
							y: (self.dimensions.y/2) - Math.abs(centerToJoint.y), 
							z: (self.dimensions.z/2) - Math.abs(centerToJoint.z)};
							
			var radius = 0;
			var normal = {x: 0, y: 0, z: 0};
			var position = {x: 0, y: 0, z: 0};
			var offset = 0;
			if (offsets.x > 0 && offsets.y > 0 && offsets.z > 0) {
				var offsetOrder = [{"coordinate": "x", "value": offsets.x}, {"coordinate": "y", "value": offsets.y}, {"coordinate": "z", "value": offsets.z}]; 
				offsetOrder.sort(function(a, b){
					return a.value - b.value;
				});
				switch (offsetOrder[0].coordinate) {
					case "x" :
						normal = {x: centerToJoint.x > 0 ? 1 : -1, y: 0, z: 0};
						radius = self.dimensions.x/2;
						position = addVec3s(self.offset, {x: 0, y: centerToJoint.y, z: centerToJoint.z});
						offset = offsets.x;						
					break;
					case "y" :
						normal = {x: 0, y: centerToJoint.y > 0 ? 1 : -1, z: 0};
						radius = self.dimensions.y/2;
						position = addVec3s(self.offset, {x: centerToJoint.x, y: 0, z: centerToJoint.z});
						offset = offsets.y;	
					break;
					case "z" :
						normal = {x: 0, y: 0, z: centerToJoint.z > 0 ? 1 : -1};
						radius = self.dimensions.z/2;
						position = addVec3s(self.offset, {x: centerToJoint.x, y: centerToJoint.y, z: 0});
						offset = offsets.z;	
					break;
				}
				normal = MyAvatar.jointToWorldDirection(normal, self.jointIndex);
				position = MyAvatar.jointToWorldPoint(position, self.jointIndex);
			} 			
            return new FlowCollisionData(offset, position, radius, normal);
        }
    }
    
    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, config){
        var self = this;
	
        this.index = index;
        this.name = name;
        this.parentIndex = parentIndex;
		this.childIndex = -1;
        this.stiffness = config.stiffness;
		this.gravity = config.gravity;
		this.damping = config.damping;
		this.inertia = config.inertia;
		this.delta = config.delta;
		
        this.initialPosition = MyAvatar.getJointPosition(index);
		this.initialRotation = MyAvatar.getJointRotation(index);
		this.initialTranslation = MyAvatar.getJointTranslation(index);
        
        this.previousPosition = this.initialPosition;
        this.currentPosition = this.initialPosition;
		this.translationDirection = normalizeVec3(this.initialTranslation);
		
        this.length = lengthVec3(subtractVec3s(this.initialPosition, MyAvatar.getJointPosition(self.parentIndex)));
        
        this.velocity = {x:0, y:0, z:0};
        this.acceleration = {x:0, y:0, z:0};
		this.recovery = {x:0, y:0, z:0};
        this.parents = [];
        this.ajusted = false;
        this.anchored = false;
        this.colliding = false;
        this.collision;
		
		this.computeLookAtRotation = function(jointIndex, parentIndex, point) {
			var jointPosition = MyAvatar.getJointPosition(jointIndex);
			var jointRotation = MyAvatar.getJointRotation(jointIndex);
			var localJoint = MyAvatar.worldToJointPoint(jointPosition, parentIndex);
			var localPoint = MyAvatar.worldToJointPoint(point, parentIndex);
			
			var localUp = MyAvatar.worldToJointPoint(Vec3.sum(MyAvatar.getJointPosition(parentIndex), {x: 0, y: 1, z: 0}), parentIndex);
			var distance = Vec3.distance(jointPosition, point);
			var lookAtRotation = Quat.lookAt(localPoint, localJoint, localUp);
			
			var corrector = Quat.fromVec3Degrees({x: 90, y: 0, z: 0});
			var jointRotation = Quat.multiply(lookAtRotation, corrector);
			return {rotation: jointRotation, distance: distance};
		}	
		
		this.getJointRotationToPoint2 = function(jointIndex, parentIndex, grandParentIndex, point) {
			var jointPosition = MyAvatar.getJointPosition(jointIndex);
			var parentPosition = MyAvatar.getJointPosition(parentIndex);
			var parentRotation = MyAvatar.getJointRotation(parentIndex);
			// local frame in grandparent since parent rotation is also grandparent frame
			var localJointPosition = MyAvatar.worldToJointPoint(jointPosition, grandParentIndex);
			var localParentPosition = MyAvatar.worldToJointPoint(parentPosition, grandParentIndex);
			var localPointPosition = MyAvatar.worldToJointPoint(point, grandParentIndex);
			var parentToJoint = Vec3.subtract(localJointPosition, localParentPosition);
			var parentToPoint = Vec3.subtract(localPointPosition, localParentPosition);
			var deltaRotation = Quat.rotationBetween(parentToJoint, parentToPoint);
			var jointRotation = Quat.multiply(deltaRotation, parentRotation);
			var distance = Vec3.distance(localParentPosition, localPointPosition);
			return {rotation: jointRotation, distance: distance};
		}	
		
		this.getJointRotationToPoint = function(jointIndex, parentIndex, grandParentIndex, point) {
			var parentRotation = MyAvatar.getJointRotation(parentIndex);
			var translation = Vec3.multiply(MyAvatar.worldToJointPoint(point, parentIndex), 100);
			return {rotation: parentRotation, translation: translation};
		}
		
		this.setJointPosition = function(jointIndex, parentIndex, grandParentIndex, position) {
			//var parentRotation = MyAvatar.getJointRotation(parentIndex);
			var translation = scaleVec3(MyAvatar.worldToJointPoint(position, parentIndex), 100);
			//MyAvatar.setJointRotation(parentIndex, parentRotation);
			MyAvatar.setJointTranslation(jointIndex, translation);
		}
        
        this.update = function () {
            self.acceleration = self.gravity;
            self.velocity = subtractVec3s(self.currentPosition, self.previousPosition);
			self.previousPosition = self.currentPosition;
            if (!self.anchored) {	
				var recoveryPosition = MyAvatar.jointToWorldPoint(scaleVec3(self.initialTranslation, 0.01), parentIndex);
				self.recovery = subtractVec3s(recoveryPosition, self.currentPosition);
				self.acceleration = addVec3s(self.acceleration, scaleVec3(self.velocity, self.inertia));	
				self.acceleration = addVec3s(self.acceleration, scaleVec3(self.recovery, self.stiffness));	
				self.currentPosition = addVec3s(addVec3s(self.currentPosition, scaleVec3(self.velocity, self.damping)), scaleVec3(self.acceleration, Math.pow(self.delta, 2)));
				                
            } else {
                self.acceleration = {x:0, y:0, z:0};
                self.velocity = {x:0, y:0, z:0};
				self.currentPosition = MyAvatar.getJointPosition(self.index);
            }
        }
        
        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(collision) {
			var parentPosition = flowJointData[self.parentIndex] ? flowJointData[self.parentIndex].currentPosition : MyAvatar.getJointPosition(self.parentIndex);
			var parentToJoint = subtractVec3s(self.currentPosition, parentPosition);
			var difference = self.length/lengthVec3(parentToJoint);
			self.currentPosition = difference < 1.0 ? addVec3s(parentPosition, scaleVec3(parentToJoint, difference)) : self.currentPosition;
			
			self.colliding = collision && (collision.offset > 0);
            self.collision = collision;
			
			if (self.colliding) {
				var normal = normalizeVec3(subtractVec3s(self.currentPosition, self.collision.position));
				var position = addVec3s(self.collision.position, scaleVec3(normal, self.collision.radius));
				self.currentPosition = addVec3s(scaleVec3(position, (1 - COLLISION_FRICTION)), scaleVec3(self.previousPosition, COLLISION_FRICTION));
			}
			
        }
        
        this.apply = function() {
			var parentJoint;
            if (!self.anchored) {
                parentJoint = flowJointData[self.parentIndex];
				if (!parentJoint) {
					return;
				}
				var grandpaJointIndex = parentJoint.parentIndex;				
				self.setJointPosition(self.index, parentJoint.index, grandpaJointIndex, self.currentPosition);
            }
            flowDebug.setDebugSphere(self.name, self.currentPosition, 0.02, {red: self.collision.collisionCount > 1 ? 0 : 255, green:self.colliding ? 0 : 255, blue:0});
            flowDebug.setDebugLine(self.name, self.currentPosition, !parentJoint ? MyAvatar.getJointPosition(self.parentIndex) : parentJoint.currentPosition, {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;
				var config;
                for (var i = 0; i < JOINT_FILTER_KEYWORDS.length; i++) {
					if (name.indexOf(JOINT_FILTER_KEYWORDS[i].keyword) > -1) {
						isFlowJoint = true;
						config = JOINT_FILTER_KEYWORDS[i];
						break;
					}
                }
                if (isFlowJoint){
                    if (flowJointData[jointInfo.index] === undefined) {
                        flowJointData[jointInfo.index] = new FlowJoint(jointInfo.index, jointInfo.parentIndex, name, config);
                    }
                } else if (COLLISION_SHAPES[name]){
					switch(COLLISION_SHAPES[name].type) {
						case "sphere":
							collisionSystem.addCollisionSphere(JOINT_COLLISION_PREFIX + jointInfo.index, jointInfo.index, COLLISION_SHAPES[name].radius, COLLISION_SHAPES[name].offset);
							break;
						case "cube":
							collisionSystem.addCollisionCube(JOINT_COLLISION_PREFIX + jointInfo.index, jointInfo.index, COLLISION_SHAPES[name].dimensions, COLLISION_SHAPES[name].offset);
							break;
					}
                }
                return isFlowJoint;
            }
        );  
        
		for (var i = 0; i < flowSkeleton.length; i++) {
            var index = flowSkeleton[i].index;
            var flowJoint = flowJointData[index];
            var parentFlowJoint = flowJointData[flowJoint.parentIndex];
			if (parentFlowJoint != undefined) {
				parentFlowJoint.childIndex = index;
			} else {
				flowJoint.anchored = true;
			}				
                
        }
		
        for (var i = 0; i < flowSkeleton.length; i++) {
            var index = flowSkeleton[i].index;
            var flowJoint = flowJointData[index];
			if (flowJoint.anchored && flowJoint.childIndex === -1) {
				flowJoint.anchored = false;
			}     
			updateOrder.push({"index": index, "upLevels": getUpHierarchyLevels(index)});
        }

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

    calculateConstraints();

    Script.update.connect(function(){
        if (!flowSkeleton) return;
        
        if (USE_COLLISIONS) {
			Script.beginProfileRange("JS-Update-Collisions");
            collisionSystem.update();
			handSystem.update();
			Script.endProfileRange("JS-Update-Collisions");
        }
		
        Script.beginProfileRange("JS-Update-JointsUpdate");
		
        for (var i = 0; i < updateOrder.length; i++) {
            var index = updateOrder[i].index;
            var joint = flowJointData[index];
            joint.update();
        }
		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;
				var joint = flowJointData[index];
				var jointPosition = MyAvatar.getJointPosition(joint.index);
				var bodyCollision = USE_COLLISIONS ? collisionSystem.checkCollisions(jointPosition, joint.index) : undefined;
				var handCollision = USE_COLLISIONS ? handSystem.checkCollisions(jointPosition, joint.index) : undefined;
				var collision = (handCollision && handCollision.offset > 0) ? handCollision : bodyCollision;
                flowJointData[index].solve(collision);
            }
        }
        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.setJointRotation(index, flowJointData[index].initialRotation);
            MyAvatar.setJointTranslation(index, flowJointData[index].initialTranslation);
        }
        flowDebug.cleanup();
    });
    
	var varsToDebug = {
		"toggleDebugShapes": function() {
			flowDebug.setVisible(!flowDebug.showDebugShapes);
		},
		"toggleCollisions": function() {
			USE_COLLISIONS = !USE_COLLISIONS;
		},
		"setFriction": function(friction) {
			COLLISION_FRICTION = friction;
		},
		"setRepulsion": function(repulsion) {
			COLLISION_REPULSION = repulsion;
		},
		"recovery": {x:0, y:0, z:0},
		"grabbing_coeficient": 0
	};
		
    // Register GlobalDebugger for API Debugger
    Script.registerValue("GlobalDebugger", varsToDebug);
	
	
	    // Capture the controller values
    
    var leftTriggerPress = function (val) {
		var value = (val <= 1) ? val : 0;
        grabPower.left = value;
		handSystem.setLeftTriggerValue(value);
    };

    var rightTriggerPress = function (val) {
		var value = (val <= 1) ? val : 0;
        grabPower.right = value;
		handSystem.setRightTriggerValue(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);
    
})()