Script.include("/~/system/libraries/Xform.js");

(function(){
	
	var SHOW_AVATAR = true;
	var SHOW_DEBUG_SHAPES = false;
	var SHOW_SOLID_SHAPES = false;
	var USE_COLLISIONS = false;
	
	
    var JOINT_COLLISION_PREFIX = "joint_";
	var HAND_COLLISION_PREFIX = "hand_";
	var HAND_COLLISION_RADIUS = 0.03;
	var HAND_TOUCHING_DISTANCE = 2.0;
	var DUMMY_GROUP_NAME = "Extra"
	var DUMMY_JOINT_COUNT = 8;
	var DUMMY_JOINT_DISTANCE = 0.05;
		
	// 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);
	}

	// Joint groups by keyword
	
	var JOINT_FILTER_KEYWORDS = ["Hair", "Skirt", "Breast", DUMMY_GROUP_NAME];
	
	var JOINT_FILTER_DATA = {
		"Hair": {"stiffness": 0.0, "gravity": -0.0096 ,"damping": 0.85, "inertia": 0.8, "delta": 0.55, "friction": 0.0, "radius": 0.01}, 
		"Skirt": {"stiffness": 0.0, "gravity": -0.0096 ,"damping": 0.85, "inertia": 0.25, "delta": 0.45, "friction": 0.0, "radius": 0.01}, 
		"Breast": {"stiffness": 1, "gravity": -0.0096 ,"damping": 0.65, "inertia": 0.8, "delta": 0.45, "friction": 0.0, "radius": 0.01}
	};
	
	JOINT_FILTER_DATA[DUMMY_GROUP_NAME] = {"stiffness": 0.0, "gravity": -0.0096 ,"damping": 0.85, "inertia": 0.8, "delta": 0.55, "friction": 0.0, "radius": 0.01};
	
    var COLLISION_SHAPES = {
		// sphere params type, radius, offset
		// cube params type, dimensions, offset
		/*
        "Head": {type: "sphere", radius: 0.1, offset: {x: 0, y: 0.05, z: 0.03}},
		"Spine2": {type: "sphere",radius: 0.07, attenuation: 0.03, offset: {x: 0, y: 0.08, z: 0.00}}
		*/
		"Head": {type: "sphere", radius: 0.08, offset: {x: 0, y: 0.06, z: 0.05}},
        //"LeftShoulder": {type: "sphere", radius: 0.05, offset: {x: 0, y: 0.05, z: 0.00}},
        //"RightShoulder": {type: "sphere", radius: 0.05, offset: {x: 0, y: 0.05, z: 0.00}},
        "RightArm": {type: "sphere", radius: 0.05, offset: {x: 0.0, y: 0.02, z: 0.00}},
        "LeftArm": {type: "sphere", radius: 0.05, offset: {x: 0.0, y: 0.02, z: 0.00}},
        "Spine2": {type: "sphere", radius: 0.09, offset: {x: 0, y: 0.04, z: 0.00}},
		//"Spine1": {type: "sphere", radius: 0.09, offset: {x: 0, y: 0.02, z: 0.00}}
    };
	
	
	var FlowDebug = function() {
        var self = this;
        this.debugLines = {};
        this.debugSpheres = {};
		this.debugCubes = {};
        this.showDebugShapes = false;
		this.showSolidShapes = false;
        
		this.setDebugCube = function(cubeName, cubePosition, cubeRotation, cubeDimensions, shapeColor, forceRendering) {
			var doRender = self.showDebugShapes || forceRendering;
            if (!doRender) 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.showSolidShapes,
                    visible: true
                });
            } else {
                self.debugCubes[cubeName] = Overlays.addOverlay("cube", {
                    position: position,
					rotation: rotation,
					dimensions: dimensions,
					color: color,
					solid: self.showSolidShapes,
                    visible: true
                });
            }
        }
		
        this.setDebugLine = function(lineName, startPosition, endPosition, shapeColor, forceRendering) {
            var doRender = self.showDebugShapes || forceRendering;
			if (!doRender) 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: true
                });
            } else {
                self.debugLines[lineName] = Overlays.addOverlay("line3d", {
                    color: color,
                    start: start,
                    end: end,
                    visible: true
                });
            }
        }
        
        this.setDebugSphere = function(sphereName, pos, diameter, shapeColor, forceRendering) {
			var doRender = self.showDebugShapes || forceRendering;
            if (!doRender) 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,
					scale: {x:scale, y:scale, z:scale},
					solid: self.showSolidShapes,
                    visible: true
                });
            } else {
                self.debugSpheres[sphereName] = Overlays.addOverlay("sphere", {
                    color: color,
                    position: pos,
                    scale: {x:scale, y:scale, z:scale},
					solid: self.showSolidShapes,
                    visible: true
                });
            }
        }
		
		this.deleteSphere = function(name) {
			Overlays.deleteOverlay(self.debugSpheres[name]);
		}
		
        this.deleteLine = function(name) {
			Overlays.deleteOverlay(self.debugLines[name]);
		}
		
		this.deleteCube = function(name) {
			Overlays.deleteOverlay(self.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);
            }
			self.debugLines = {};
			self.debugSpheres = {};
			self.debugCubes = {};
        }
        
        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.setSolid = function(isSolid) {
            self.showSolidShapes = isSolid;
            for (var lineName in self.debugLines) {
                Overlays.editOverlay(self.debugLines[lineName], {
                    solid: isSolid
                });
            }
            for (var sphereName in self.debugSpheres) {
                Overlays.editOverlay(self.debugSpheres[sphereName], {
                    solid: isSolid
                });
            }
        }       

    }


	var FlowHandSystem = function() {
		var self = this;
		this.HANDS_COLLISION_JOINTS = ["RightHandMiddle1", "RightHandMiddle3", "LeftHandMiddle1", "LeftHandMiddle3","RightHandPinky3", "LeftHandPinky3"];
		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(HAND_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;
						var handCollisionSettings = {type: "sphere", radius: HAND_COLLISION_RADIUS, offset: {x: 0, y: 0, z: 0}, avatarId: avatarID};
						newHands[side] = new FlowCollisionSphere(name, jointId, handCollisionSettings);
					}
					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.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, scaleVec3(collisions[i].normal, collisions[i].distance));
					collisionData.position = addVec3s(collisionData.position, collisions[i].position);
					collisionData.radius += collisions[i].radius 
					collisionData.distance += collisions[i].distance 
				}
				collisionData.offset = collisionData.offset/collisions.length;
				collisionData.radius = lengthVec3(collisionData.normal)/2;
				collisionData.normal = normalizeVec3(collisionData.normal);
				collisionData.position = scaleVec3(collisionData.position, 1/collisions.length);
				collisionData.distance = collisionData.distance/collisions.length;
			} else if (collisions.length == 1){
				collisionData = collisions[0];
			}
			collisionData.collisionCount = collisions.length;
			return collisionData;
		}
		
		this.checkThreadCollisions = function(thread) {

			var threadCollisionData = Array(thread.joints.length);
			for (var i = 0; i < threadCollisionData.length; i++) {
				threadCollisionData[i] = [];
			}
			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];
					var rootCollision = self.avatarHands[i][side].checkCollision(thread.positions[0], thread.radius);
					var collisionData = [rootCollision];
					var tooFar = rootCollision.distance > (thread.length + rootCollision.radius);
					if (!tooFar) {
						for (var k = 1; k < thread.joints.length; k++) {
							var prevCollision = collisionData[k-1];
							var nextCollision = self.avatarHands[i][side].checkCollision(thread.positions[k], thread.radius);
							collisionData.push(nextCollision);
							if (prevCollision.offset > 0) {
								if (k == 1) {
									threadCollisionData[k-1].push(prevCollision);
								}
							} else if (nextCollision.offset > 0) {
								threadCollisionData[k].push(nextCollision);
							} else {
								var segmentCollision = self.avatarHands[i][side].checkSegmentCollision(thread.positions[k-1], thread.positions[k], prevCollision, nextCollision);
								if (segmentCollision.offset > 0) {
									threadCollisionData[k-1].push(segmentCollision);
									threadCollisionData[k].push(segmentCollision);
								}
							}						
						}
					}
				}
            }
			var collisionResult = [];
			for (var i = 0; i < thread.joints.length; i++) {
				collisionResult.push(self.computeCollision(threadCollisionData[i]));
			}
			return collisionResult;
        }

		this.setRightTriggerValue = function(value) {
			self.rightTriggerValue = value;
		}
		
		this.setLeftTriggerValue = function(value) {
			self.leftTriggerValue = value;
		}
	}

    var FlowCollisionSystem = function() {
        var self = this;
        this.collisionSpheres = [];
		this.collisionCubes = [];
		
        this.addCollisionSphere = function(name, jointIndex, settings) {
            self.collisionSpheres.push(new FlowCollisionSphere(name, jointIndex, settings));
        }
		
		this.addCollisionCube = function(name, jointIndex, settings) {
            self.collisionCubes.push(new FlowCollisionCube(name, jointIndex, settings));
        }
		
		this.addCollisionShape = function(name, jointIndex, settings) {
			switch(settings.type) {
				case "sphere":
					collisionSystem.addCollisionSphere(name, jointIndex, settings);
					break;
				case "cube":
					collisionSystem.addCollisionCube(name, jointIndex, settings);
					break;
			}
		}
		
        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, scaleVec3(collisions[i].normal, collisions[i].distance));
					collisionData.position = addVec3s(collisionData.position, collisions[i].position);
					collisionData.radius += collisions[i].radius 
					collisionData.distance += collisions[i].distance 
				}
				collisionData.offset = collisionData.offset/collisions.length;
				collisionData.radius = lengthVec3(collisionData.normal)/2;
				collisionData.normal = normalizeVec3(collisionData.normal);
				collisionData.position = scaleVec3(collisionData.position, 1/collisions.length);
				collisionData.distance = collisionData.distance/collisions.length;
			} else if (collisions.length == 1){
				collisionData = collisions[0];
			}
			collisionData.collisionCount = collisions.length;
			return collisionData;
		}

		this.checkThreadCollisions = function(thread, checkSegments) {
			var threadCollisionData = Array(thread.joints.length);
			for (var i = 0; i < threadCollisionData.length; i++) {
				threadCollisionData[i] = [];
			}
			for (var j = 0; j < self.collisionSpheres.length; j++) {
				
				var rootCollision = self.collisionSpheres[j].checkCollision(thread.positions[0], thread.radius);
				var collisionData = [rootCollision];
				var tooFar = rootCollision.distance > (thread.length + rootCollision.radius);
				
				if (!tooFar) {
					if (checkSegments) {
						for (var i = 1; i < thread.joints.length; i++) {
							var prevCollision = collisionData[i-1];
							var nextCollision = self.collisionSpheres[j].checkCollision(thread.positions[i], thread.radius);
							collisionData.push(nextCollision);
							if (prevCollision.offset > 0) {
								if (i == 1) {
									threadCollisionData[i-1].push(prevCollision);
								}
							} else if (nextCollision.offset > 0) {
								threadCollisionData[i].push(nextCollision);
							} else {
								var segmentCollision = self.collisionSpheres[j].checkSegmentCollision(thread.positions[i-1], thread.positions[i], prevCollision, nextCollision);
								if (segmentCollision.offset > 0) {
									threadCollisionData[i-1].push(segmentCollision);
									threadCollisionData[i].push(segmentCollision);
								}
							}						
						}
					} else {
						if (rootCollision.offset > 0) {
							threadCollisionData[0].push(rootCollision);
						}
						for (var i = 1; i < thread.joints.length; i++) {
							var nextCollision = self.collisionSpheres[j].checkCollision(thread.positions[i], thread.radius);
							if (nextCollision.offset > 0) {
								threadCollisionData[i].push(nextCollision);
							}
						}
					}
				}
			}
			var collisionResult = [];
			for (var i = 0; i < thread.joints.length; i++) {
				collisionResult.push(self.computeCollision(threadCollisionData[i]));
			}
			return collisionResult;
        }

    }
	
	var FlowCollisionData = function(offset, position, radius, normal, distance) {
		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};
		this.distance = distance != undefined ? distance : 0;
	}
    
    var FlowCollisionSphere = function(name, jointIndex, settings) {
        var self = this;
		this.name = name;
        this.jointIndex = jointIndex;
		this.radius = settings.radius;
        this.offset = settings.offset;
		this.attenuation = settings.attenuation;
		this.avatarId = settings.avatarId;
        
		this.position = {x:0, y:0, z:0};
		
        this.update = function() {
			if (self.avatarId != undefined){
				var avatar = AvatarList.getAvatar(self.avatarId);
				self.position = avatar.getJointPosition(self.jointIndex);
			} else {
				self.position = MyAvatar.jointToWorldPoint(self.offset, self.jointIndex);
			}				
            collisionDebug.setDebugSphere(self.name, self.position, 2*self.radius, {red: 200, green: 10, blue: 50});
			if (self.attenuation && self.attenuation > 0) {
				collisionDebug.setDebugSphere(self.name + "_att", self.position, 2*(self.radius + self.attenuation), {red: 120, green: 200, blue: 50});	
			}
        }
        
        this.checkCollision = function(point, radius) {
            var centerToJoint = subtractVec3s(point, self.position);
            var distance = lengthVec3(centerToJoint) - radius;
            var offset =  self.radius - distance;
			var collisionData = new FlowCollisionData(offset, self.position, self.radius, normalizeVec3(centerToJoint), distance);
            return collisionData;
        }
		
		this.checkSegmentCollision = function(point1, point2, pointCollision1, pointCollision2) {
			var collisionData = new FlowCollisionData();
			var segment = subtractVec3s(point2, point1);
			var segmentLength = lengthVec3(segment);
			var maxDistance = Math.sqrt(Math.pow(pointCollision1.radius,2) + Math.pow(segmentLength,2));
			if (pointCollision1.distance < maxDistance && pointCollision2.distance < maxDistance) {
				var segmentPercent = pointCollision1.distance/(pointCollision1.distance + pointCollision2.distance);
				var collisionPoint = addVec3s(point1, scaleVec3(segment, segmentPercent)) 
				var centerToSegment = subtractVec3s(collisionPoint, self.position);
				var distance = lengthVec3(centerToSegment);
				if (distance < self.radius) {
					var offset = self.radius - distance;
					collisionData = new FlowCollisionData(offset, self.position, self.radius, normalizeVec3(centerToSegment), distance);
				}
			}
			return collisionData;
        }
    }
	
	var FlowCollisionCube = function(name, jointIndex, settings) {
        var self = this;
		this.name = name;
        this.jointIndex = jointIndex;
		this.dimensions = settings.dimensions;
        this.offset = settings.offset;
		this.attenuation = settings.attenuation;
		this.avatarId = settings.avatarId;
		
        this.position = {x:0, y:0, z:0};
		this.rotation = {x:0, y:0, z:0, w:0};
        
        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);
			}				
			collisionDebug.setDebugCube(self.name, self.position, self.rotation, self.dimensions, {red: 200, green: 10, blue: 50});
        }
        
        this.checkCollision = function(point, radius) {
			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, offset);
        }
    }
	
	var FlowNode = function(initialPosition, settings) {
		var self = this;
		this.radius = settings.radius;
        this.gravity = settings.gravity;
		this.damping = settings.damping;
		this.inertia = settings.inertia;
		this.delta = settings.delta;
		this.friction = settings.friction;
		
        this.initialPosition = initialPosition;
        
        this.previousPosition = this.initialPosition;
        this.currentPosition = this.initialPosition;
		
		this.previousCollision = new FlowCollisionData();
        
        this.currentVelocity = {x:0, y:0, z:0};
		this.previousVelocity = {x:0, y:0, z:0};
        this.acceleration = {x:0, y:0, z:0};

        this.anchored = false;
        this.colliding = false;
        this.collision;
		
		this.update = function(accelerationOffset) {
			self.acceleration = {x: 0, y: self.gravity, z: 0};
            self.previousVelocity = self.currentVelocity;
			self.currentVelocity = subtractVec3s(self.currentPosition, self.previousPosition);
			self.previousPosition = self.currentPosition;
            if (!self.anchored) {	
				// Add inertia
				var centrifugeVector = normalizeVec3(subtractVec3s(self.previousVelocity, self.currentVelocity));
				self.acceleration = addVec3s(self.acceleration, scaleVec3(centrifugeVector, self.inertia * lengthVec3(self.currentVelocity)));
				
				// Add offset
				self.acceleration = addVec3s(self.acceleration, accelerationOffset);
				
				// Calculate new position
				self.currentPosition = addVec3s(addVec3s(self.currentPosition, scaleVec3(self.currentVelocity, self.damping)), scaleVec3(self.acceleration, Math.pow(self.delta, 2)));
				                
            } else {
                self.acceleration = {x:0, y:0, z:0};
                self.currentVelocity = {x:0, y:0, z:0};
            }
		}
		
        this.solve = function(constrainPoint, maxDistance, collision) {
			var constrainVector = subtractVec3s(self.currentPosition, constrainPoint);
			var difference = maxDistance/lengthVec3(constrainVector);
			self.currentPosition = difference < 1.0 ? addVec3s(constrainPoint, scaleVec3(constrainVector, difference)) : self.currentPosition;
			
			self.colliding = collision && (collision.offset > 0);
            self.collision = collision;
			
			if (self.colliding) {
				var position = addVec3s(self.currentPosition, scaleVec3(collision.normal, collision.offset));
				if (self.previousCollision) {
					var frictionPosition = addVec3s(self.currentPosition, scaleVec3(self.previousCollision.normal, collision.offset));
					self.currentPosition = addVec3s(scaleVec3(position, (1 - self.friction)), scaleVec3(frictionPosition, self.friction));
				} else {
					self.currentPosition = position;
				}
				self.previousCollision = collision;
			} else {
				self.previousCollision = undefined;
			}
        }
		
		this.apply = function(name, forceRendering) {
            jointDebug.setDebugSphere(name, self.currentPosition, 2*self.radius, {red: self.collision && self.collision.collisionCount > 1 ? 0 : 255, green:self.colliding ? 0 : 255, blue:0}, forceRendering);
		}
	}
	

    var FlowJoint = function(index, parentIndex, name, group, settings){
        var self = this;
		
        this.index = index;
        this.name = name;
		this.group = group;
        this.parentIndex = parentIndex;
		this.childIndex = -1;
		
		this.isDummy = false;
		
		this.initialPosition = MyAvatar.getJointPosition(index);
		this.initialXform = new Xform(MyAvatar.getJointRotation(index), MyAvatar.getJointTranslation(index));
		
		this.currentRotation;
		this.recoveryPosition;
		
		this.node = new FlowNode(self.initialPosition, settings);
		
        this.stiffness = settings.stiffness;

		this.translationDirection = normalizeVec3(this.initialXform.pos);
		
        this.length = lengthVec3(subtractVec3s(this.initialPosition, MyAvatar.getJointPosition(self.parentIndex)));
		
        this.update = function () {
			var accelerationOffset = {x: 0, y: 0, z: 0};
			if (!self.isDummy && self.recoveryPosition) {
				var recoveryVector = subtractVec3s(self.recoveryPosition, self.node.currentPosition);	
				var accelerationOffset = scaleVec3(recoveryVector, Math.pow(self.stiffness, 3));
			}
			self.node.update(accelerationOffset);
			if (self.node.anchored) {
				if (!self.isDummy) {
					self.node.currentPosition = MyAvatar.getJointPosition(self.index);
				} else {
					self.node.currentPosition = MyAvatar.getJointPosition(self.parentIndex);
				}
			}
        }
                
        this.solve = function(collision) {
			var parentPosition = flowJointData[self.parentIndex] ? flowJointData[self.parentIndex].node.currentPosition : MyAvatar.getJointPosition(self.parentIndex);
			self.node.solve(parentPosition, self.length, collision);			
        }
        
        this.apply = function() {
			
			var parentJoint;
			if (self.currentRotation) {
				MyAvatar.setJointRotation(self.index, self.currentRotation);
				//MyAvatar.setJointTranslation(self.index, scaleVec3(normalizeVec3(self.initialXform.pos, self.currentLength)));
			}
			jointDebug.setDebugSphere(self.name + "_deb", self.recoveryPosition, 0.02, {red: 255, green:100, blue:100}, false);
			self.node.apply(self.name, self.isDummy);
        }
    }
	
	var FlowJointDummy = function(initialPosition, index, parentIndex, childIndex, settings) {
		var group = DUMMY_GROUP_NAME;
		var name = DUMMY_GROUP_NAME + "_" + index;
		FlowJoint.call(this, index, parentIndex, name, group, settings);
		this.isDummy = true;
		this.childIndex = childIndex;
		this.initialPosition = initialPosition;
		this.node = new FlowNode(initialPosition, settings);
		this.length = DUMMY_JOINT_DISTANCE;
	}		
	
	var FlowThread = function(root) {
		var self = this;
		this.joints = [];
		this.positions = [];
		this.radius = 0;
		this.length = 0;
		
		this.computeThread = function(rootIndex) {
			var parentIndex = rootIndex;
			var childIndex = flowJointData[parentIndex].childIndex;
			var indexes = [parentIndex];
			for (var i = 0; i < flowSkeleton.length; i++) {
				if (childIndex > -1) {
					indexes.push(childIndex);
					childIndex = flowJointData[childIndex].childIndex;
				} else {
					break;
				}
			}
			for (var i = 0; i < indexes.length; i++) {
				var index = indexes[i];
				self.joints.push(index);
				if (i > 0) {
					self.length += flowJointData[index].length;
				}
			}
		}
		
		this.computeRecovery = function() {
			var parentIndex = self.joints[0];
			var parentJoint = flowJointData[parentIndex];
			parentJoint.recoveryPosition = parentJoint.node.currentPosition;
			var parentRotation = Quat.multiply(MyAvatar.jointToWorldRotation(Quat.IDENTITY, parentJoint.parentIndex), parentJoint.initialXform.rot);
			for (var i = 1; i < self.joints.length; i++) {
				var joint = flowJointData[self.joints[i]];
				var rotation = i === 1 ? parentRotation : Quat.multiply(rotation, parentJoint.initialXform.rot);
				joint.recoveryPosition = addVec3s(parentJoint.recoveryPosition, Vec3.multiplyQbyV(rotation, scaleVec3(joint.initialXform.pos, 0.01)));
				parentJoint = joint;
			}
			
		}

		this.update = function() {
			self.positions = [];
			self.radius = flowJointData[self.joints[0]].node.radius;
			self.computeRecovery();
			for (var i = 0; i < self.joints.length; i++){
				var joint = flowJointData[self.joints[i]];
				joint.update();
				self.positions.push(joint.node.currentPosition);
			}
		}
		
		this.solve = function(useCollisions) {
			if (useCollisions) {
				var handCollisions = handSystem.checkThreadCollisions(self);
				var bodyCollisions = collisionSystem.checkThreadCollisions(self);
				
				for (var i = 0; i < self.joints.length; i++) {
					var index = self.joints[i];
					var collision = (bodyCollisions[i].offset > 0) ? bodyCollisions[i] : handCollisions[i];
					flowJointData[index].solve(collision);
				}
			} else {
				for (var i = 0; i < self.joints.length; i++) {
					var index = self.joints[i];
					flowJointData[index].solve(new FlowCollisionData());
				}
			}
		}
		
		this.computeJointRotations = function() {
			var rootIndex = flowJointData[self.joints[0]].parentIndex;
			var rootFramePositions = [];
			for (var i = 0; i < self.joints.length; i++){
				rootFramePositions.push(MyAvatar.worldToJointPoint(flowJointData[self.joints[i]].node.currentPosition, rootIndex));
			}
			var pos0 = rootFramePositions[0];
			var pos1 = rootFramePositions[1];
			
			var joint0 = flowJointData[self.joints[0]];
			var joint1 = flowJointData[self.joints[1]];
			
			var initial_pos1 = addVec3s(pos0, Vec3.multiplyQbyV(joint0.initialXform.rot, scaleVec3(joint1.initialXform.pos, 0.01)));
			
			var vec0 = subtractVec3s(initial_pos1, pos0);
			var vec1 = subtractVec3s(pos1, pos0);
			
			var delta = Quat.rotationBetween(vec0, vec1);
			
			joint0.currentRotation = Quat.multiply(delta, joint0.initialXform.rot);
			
			for (var i = 1; i < self.joints.length-1; i++){
				var nextJoint = flowJointData[self.joints[i+1]];
				for (var j = i; j < self.joints.length; j++){
					rootFramePositions[j] = Vec3.multiplyQbyV(Quat.inverse(joint0.currentRotation), subtractVec3s(rootFramePositions[j], scaleVec3(joint0.initialXform.pos, 0.01)));
				}
				pos0 = rootFramePositions[i];
				pos1 = rootFramePositions[i+1];
				initial_pos1 = addVec3s(pos0, Vec3.multiplyQbyV(joint1.initialXform.rot, scaleVec3(nextJoint.initialXform.pos, 0.01)));
				
				vec0 = subtractVec3s(initial_pos1, pos0);
				vec1 = subtractVec3s(pos1, pos0);
			
				delta = Quat.rotationBetween(vec0, vec1);
			
				joint1.currentRotation = Quat.multiply(delta, joint1.initialXform.rot);
				joint0 = joint1;
				joint1 = nextJoint;
			}
		}

		this.apply = function() {
			self.computeJointRotations();

			for (var i = 0; i < self.joints.length; i++){
				var joint = flowJointData[self.joints[i]];
				var parentJoint = flowJointData[joint.parentIndex];
				jointDebug.setDebugLine(joint.name, joint.node.currentPosition, !parentJoint ? MyAvatar.getJointPosition(joint.parentIndex) : parentJoint.node.currentPosition, {red: 255, green:(joint.colliding ? 0 : 255), blue:0}, joint.isDummy);
				joint.apply();
			}
		}
		self.computeThread(root);
	}
	
	var isActive, flowSkeleton, flowJointData, flowJointNameMap, flowThreads, handSystem, collisionSystem, collisionDebug, jointDebug;
    
	function initFlow() {
		stopFlow();
		flowSkeleton = undefined;
		flowJointData = [];
		flowJointNameMap = {};
		flowThreads = [];

		handSystem = new FlowHandSystem();
		collisionSystem = new FlowCollisionSystem();
		    
		collisionDebug = new FlowDebug();
		jointDebug = new FlowDebug();
		
		collisionDebug.setVisible(SHOW_DEBUG_SHAPES);
		collisionDebug.setSolid(SHOW_SOLID_SHAPES);
		
		MyAvatar.setEnableMeshVisible(SHOW_AVATAR);
		jointDebug.setVisible(SHOW_DEBUG_SHAPES);
		jointDebug.setSolid(SHOW_SOLID_SHAPES);
		
		calculateConstraints();
		isActive = true;
	}
	
	function stopFlow() {
		isActive = false;
		if (!flowSkeleton) {
			return;
		}
		for (var i = 0; i < flowSkeleton.length; i++) {
            var index = flowSkeleton[i].index;
			MyAvatar.setJointRotation(index, flowJointData[index].initialXform.rot);
            MyAvatar.setJointTranslation(index, flowJointData[index].initialXform.pos);
        }
        collisionDebug.cleanup();
		jointDebug.cleanup();
	}
    
    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() {
		
		var filterKeys = Object.keys(JOINT_FILTER_DATA);
        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 jointSettings = {};
				var group = "";
                for (var i = 0; i < filterKeys.length; i++) {
					var jointGroup = JOINT_FILTER_DATA[filterKeys[i]];
					var keyword = filterKeys[i].toUpperCase();
					if (name.toUpperCase().indexOf(keyword) > -1) {
						isFlowJoint = true;
						jointSettings = jointGroup;
						group = filterKeys[i];
						break;
					}
                }
                if (isFlowJoint){
                    if (flowJointData[jointInfo.index] === undefined) {
                        flowJointData[jointInfo.index] = new FlowJoint(jointInfo.index, jointInfo.parentIndex, name, group, jointSettings);
                    }
                } else if (COLLISION_SHAPES[name]){
					collisionSystem.addCollisionShape(JOINT_COLLISION_PREFIX + jointInfo.index, jointInfo.index, COLLISION_SHAPES[name]);
                }
                return isFlowJoint;
            }
        );  
        
		var roots = [];
		
		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.node.anchored = true;
				roots.push(index);
			}      
        }
		
		for (var i = 0; i < roots.length; i++) {
			var thread = new FlowThread(roots[i]);
			// add threads with at least 2 joints
			if (thread.joints.length > 1) {
				flowThreads.push(thread);
			}
		}

		var jointCount = flowJointData.length;
		var extraIndex = flowJointData.length;
		var rightHandIndex = MyAvatar.getJointIndex("RightHand");
		var rightHandPosition = MyAvatar.getJointPosition(rightHandIndex);
		var parentIndex = rightHandIndex;
		for (var i = 0; i < DUMMY_JOINT_COUNT; i++) {
			var childIndex = (i == (DUMMY_JOINT_COUNT - 1)) ? -1 : extraIndex + 1;
			flowJointData[extraIndex] = new FlowJointDummy(rightHandPosition, extraIndex, parentIndex, childIndex, JOINT_FILTER_DATA[DUMMY_GROUP_NAME]);
			parentIndex = extraIndex;
			extraIndex++;
		}
		
		flowJointData[jointCount].node.anchored = true;
		
		var extraThread = new FlowThread(jointCount);
		flowThreads.push(extraThread);

    }
	

    Script.update.connect(function(){
        if (!isActive || !flowSkeleton) return;
        
		varsToDebug.flowJoint = flowJointData[flowThreads[0].joints[flowThreads[0].joints.length-1]];
		
        if (USE_COLLISIONS) {
			Script.beginProfileRange("JS-Update-Collisions");
            collisionSystem.update();
			handSystem.update();
			Script.endProfileRange("JS-Update-Collisions");
        }
		
        Script.beginProfileRange("JS-Update-JointsUpdate");
		
		flowThreads.forEach(function(thread){
			thread.update();
		});

		Script.endProfileRange("JS-Update-JointsUpdate");
		Script.beginProfileRange("JS-Update-JointsSolve");

		flowThreads.forEach(function(thread){
			thread.solve(USE_COLLISIONS);
		});
		
        Script.endProfileRange("JS-Update-JointsSolve");
		Script.beginProfileRange("JS-Update-JointsApply");
		
		flowThreads.forEach(function(thread){
			thread.apply();
		});
		

		Script.endProfileRange("JS-Update-JointsApply");
    });
    
    Script.scriptEnding.connect(stopFlow);
    
	var varsToDebug = {
		"flowJoint" : new FlowJoint(1,0,"test","Extra",JOINT_FILTER_DATA[JOINT_FILTER_KEYWORDS[0]]),
		"init": function() {
			initFlow();
		},		
		"stop": function() {
			stopFlow();
		},
		"isActive": function() {
			return isActive;
		},
		"toggleAvatarVisible": function() {
			SHOW_AVATAR = !SHOW_AVATAR;
			MyAvatar.setEnableMeshVisible(SHOW_AVATAR);
		},
		"toggleDebugShapes": function() {
			SHOW_DEBUG_SHAPES = !SHOW_DEBUG_SHAPES;
			if (USE_COLLISIONS) {
				collisionDebug.setVisible(SHOW_DEBUG_SHAPES);
			}
			jointDebug.setVisible(SHOW_DEBUG_SHAPES);
		},
		"toggleSolidShapes": function() {
			SHOW_SOLID_SHAPES = !SHOW_SOLID_SHAPES;
			collisionDebug.setSolid(SHOW_SOLID_SHAPES);
			jointDebug.setSolid(SHOW_SOLID_SHAPES);
		},
		"toggleCollisions": function() {
			USE_COLLISIONS = !USE_COLLISIONS;
			if (USE_COLLISIONS && SHOW_DEBUG_SHAPES) {
				collisionDebug.setVisible(true);
			} else {
				collisionDebug.setVisible(false);
			}
		},
		"setFriction": function(friction) {
			COLLISION_FRICTION = friction;
		},
		"setValue": function(group, name, value) {
			if (group != "Extra Hand Nodes") {
				var keyword = group.toUpperCase();
				for (var i = 0; i < flowThreads.length; i++) {
					for (var j = 0; j < flowThreads[i].joints.length; j++){
						var index = flowThreads[i].joints[j];
						var joint = flowJointData[index];
						if (joint.name.toUpperCase().indexOf(keyword) > -1) {
							var floatVal = parseFloat(value);
							JOINT_FILTER_DATA[group][name] = floatVal;
							if (name === "stiffness") {
								joint.stiffness = floatVal;
							} else {
								joint.node[name] = floatVal;
							}
						}
					}
				}
			} else {
				
			}
		}, 
		"getGroupData": function() {
			var joint_filter_keys = JOINT_FILTER_DATA;
			if (isActive) {
				joint_filter_keys = {};
				for (var i = 0; i < JOINT_FILTER_KEYWORDS.length; i++) {
					joint_filter_keys[JOINT_FILTER_KEYWORDS[i]] = undefined;
				}
				for (var i = 0; i < flowThreads.length; i++) {
					for (var j = 0; j < flowThreads[i].joints.length; j++){
						var index = flowThreads[i].joints[j];
						var joint = flowJointData[index];
						if (!joint_filter_keys[joint.group]) {
							joint_filter_keys[joint.group] = {
								"stiffness": joint.stiffness, 
								"radius": joint.node.radius,
								"gravity": joint.node.gravity,
								"damping": joint.node.damping, 
								"inertia": joint.node.inertia, 
								"delta": joint.node.delta,
								"friction": joint.node.friction
							};
						}
					}
				}
			}
			return joint_filter_keys;
			
		},
		"getSettingsData": function() {
			return {"avatar": SHOW_AVATAR, "collisions": USE_COLLISIONS, "debug": SHOW_DEBUG_SHAPES, "solid": SHOW_SOLID_SHAPES};
		}
	};
		
    // Register GlobalDebugger for API Debugger
    Script.registerValue("GlobalDebugger", varsToDebug);

	// Capture the controller values
    
    var leftTriggerPress = function (val) {
		var value = (val <= 1) ? val : 0;
		handSystem.setLeftTriggerValue(value);
    };

    var rightTriggerPress = function (val) {
		var value = (val <= 1) ? val : 0;
		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);
	console.log("Hair Script loaded");
	if (typeof FLOWAPP === 'undefined') {
		initFlow();
	}
}());