(function() {
    var soccerDebug = 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 !== undefined ? 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]);
            self.debugSpheres[name] = undefined;
        };
        
        this.deleteLine = function(name) {
            Overlays.deleteOverlay(self.debugLines[name]);
            self.debugLines[name] = undefined;
        };
        
        this.deleteCube = function(name) {
            Overlays.deleteOverlay(self.debugCubes[name]);
            self.debugCubes[name] = undefined;
        };
        
        this.cleanup = function() {
            for (var lineName in self.debugLines) {
                if (lineName !== undefined) {
                        self.deleteLine(lineName);
                }
            }
            for (var sphereName in self.debugSpheres) {
                if (sphereName !== undefined) {
                        self.deleteSphere(sphereName);
                }
            }
            for (var cubeName in self.debugCubes) {
                    if (cubeName!== undefined) {
                        self.deleteCube(cubeName);
                }
            }
            self.debugLines = {};
            self.debugSpheres = {};
            self.debugCubes = {};
        };
        
        this.setVisible = function(isVisible) {
            self.showDebugShapes = isVisible;
            for (var lineName in self.debugLines) {
                if (lineName !== undefined) {
                    Overlays.editOverlay(self.debugLines[lineName], {
                        visible: isVisible
                    });
                }
            }
            for (var sphereName in self.debugSpheres) {
                if (sphereName !== undefined) {
                    Overlays.editOverlay(self.debugSpheres[sphereName], {
                        visible: isVisible
                    });
                }
            }
        };
        
        this.setSolid = function(isSolid) {
            self.showSolidShapes = isSolid;
            for (var lineName in self.debugLines) {
                if (lineName !== undefined) {
                    Overlays.editOverlay(self.debugLines[lineName], {
                        solid: isSolid
                    });
                }
            }
            for (var sphereName in self.debugSpheres) {
                if (sphereName !== undefined) {
                    Overlays.editOverlay(self.debugSpheres[sphereName], {
                        solid: isSolid
                    });
                }
            }
        };       

    };
    
    var SOCCER_WALK_SPEED = 5.0;
    var initialWalkSpeed = MyAvatar.walkSpeed;
    MyAvatar.walkSpeed = SOCCER_WALK_SPEED;

    var PI = 3.14159;
    var defaultRotation = {x: 0, y: 0, z: 0, w: 1};
    var debugSphere1ID = "sphere1";
    var debugSphere2ID = "sphere2";
    var visualDebugger = new soccerDebug();
    visualDebugger.setVisible(true);
    //visualDebugger.setDebugSphere(debugSphere1ID, MyAvatar.position, 0.1);
    //visualDebugger.setDebugSphere(debugSphere2ID, MyAvatar.position, 0.1);
    var hipsOffset = 0.4;
    var radius = 0.15;
    
    var FEET_COLLIDER_PROPERTIES = {
        type: "Sphere",
        dimensions: { x: radius * 2, y: radius * 2, z: radius * 2},
        position: { x: 0, y: 0, z: 0 },
        gravity: { x: 0, y: 0, z: 0 },
        density: 10000,
        collidesWith: "dynamic",
        visible: false,
        collisionMask: 2,
        dynamic: true  
    }
    
    var FEET_FORWARD = 0.2;
    
    var SoccerController = function() {
        var _this = this;
        this.lastLeftAlpha = 0.0;
        this.lastRightAlpha = 0.0;
        this.avatarData = new (function(){    
            var self = this;
            
            this.advancedControls = MyAvatar.useAdvancedMovementControls
            
            this.position = MyAvatar.position;
            this.orientation = MyAvatar.orientation;
            
            this.forward = Quat.getForward(MyAvatar.orientation);
            this.right = Quat.getRight(MyAvatar.orientation);
            this.up = Quat.getUp(MyAvatar.orientation);
            
            this.leftFootIndex =  MyAvatar.getJointIndex("LeftFoot");            
            this.rightFootIndex =  MyAvatar.getJointIndex("RightFoot");
            this.leftLegIndex =  MyAvatar.getJointIndex("LeftUpLeg");
            this.rightLegIndex =  MyAvatar.getJointIndex("RightUpLeg");
            this.leftHandIndex =  MyAvatar.getJointIndex("LeftHand");
            this.rightHandIndex =  MyAvatar.getJointIndex("RightHand");
            
            this.update = function(position, orientation) {
                self.position = position;
                self.orientation = orientation;
                self.forward = Quat.getForward(self.orientation);
                self.right = Quat.getRight(self.orientation);
                self.up = Quat.getUp(self.orientation);
            }
        });
        
        this.colliders = new (function(){
            var self = this;
            this.DELTA_MULTIPLIER = 30.0;
            this.left = {
                index: _this.avatarData.leftFootIndex,
                entityID: Entities.addEntity(FEET_COLLIDER_PROPERTIES),
                lastVelocity: Vec3.ZERO,
                offset: 0
            }
            this.right = {
                index: _this.avatarData.rightFootIndex,
                entityID: Entities.addEntity(FEET_COLLIDER_PROPERTIES),
                lastVelocity: Vec3.ZERO,
                offset: 0
            }
            this.moveToPosition = function(side) {
                var newPosition = Vec3.sum(MyAvatar.getJointPosition(side.index), Vec3.sum({x:0, y:side.offset, z:0}, Vec3.multiply(_this.avatarData.forward, FEET_FORWARD)));
                var currentPosition = Entities.getEntityProperties(side.entityID, ["position"]).position;
                var velocity = Vec3.multiply(Vec3.subtract(newPosition, currentPosition), self.DELTA_MULTIPLIER);
                var acceleration = Vec3.multiply(Vec3.subtract(velocity, side.lastVelocity), self.DELTA_MULTIPLIER);
                side.lastVelocity = velocity;
                Entities.editEntity(side.entityID, { velocity: velocity, gravity: acceleration });
            }
            this.setOffset = function(handController, offset) {
                var side = (handController === Controller.Standard.RightHand) ? self.right : self.left;
                side.offset = offset;
            }
            this.update = function() {
                self.moveToPosition(self.left);
                self.moveToPosition(self.right);
            }
            this.cleanup = function() {
                Entities.deleteEntity(self.left.entityID);
                Entities.deleteEntity(self.right.entityID);
            }
        });

        this.getControllerWorldLocation = function (handController) {
            var orientation;
            var position;
            var pose = Controller.getPoseValue(handController);
            var valid = pose.valid;
            var controllerJointIndex;
            if (pose.valid) {
                if (handController === Controller.Standard.RightHand) {
                    controllerJointIndex = MyAvatar.getJointIndex("_CAMERA_RELATIVE_CONTROLLER_RIGHTHAND");
                } else {
                    controllerJointIndex = MyAvatar.getJointIndex("_CAMERA_RELATIVE_CONTROLLER_LEFTHAND");
                }
                orientation = Quat.multiply(MyAvatar.orientation, MyAvatar.getAbsoluteJointRotationInObjectFrame(controllerJointIndex));
                position = Vec3.sum(MyAvatar.position, Vec3.multiplyQbyV(MyAvatar.orientation, MyAvatar.getAbsoluteJointTranslationInObjectFrame(controllerJointIndex)));

            } else if (!HMD.isHandControllerAvailable()) {
                // NOTE: keep this offset in sync with scripts/system/controllers/handControllerPointer.js:493
                var VERTICAL_HEAD_LASER_OFFSET = 0.1;
                position = Vec3.sum(Camera.position, Vec3.multiplyQbyV(Camera.orientation, {x: 0, y: VERTICAL_HEAD_LASER_OFFSET, z: 0}));
                orientation = Quat.multiply(Camera.orientation, Quat.angleAxis(-90, { x: 1, y: 0, z: 0 }));
                valid = true;
            }

            return {position: position,
                    translation: position,
                    orientation: orientation,
                    rotation: orientation,
                    valid: valid};
        };
        
        this.getControllerOffset = function(handController) {
            var controllerPosition = handController === Controller.Standard.RightHand ? MyAvatar.getJointPosition(_this.avatarData.rightHandIndex) : MyAvatar.getJointPosition(_this.avatarData.leftHandIndex);// _this.getControllerWorldLocation(handController).position;
            var rightMultiplier = (handController == Controller.Standard.RightHand) ? 0.5 : -0.5;
            var referenceOffset = Vec3.sum(Vec3.multiply(_this.avatarData.forward, hipsOffset), Vec3.multiply(_this.avatarData.right, rightMultiplier * hipsOffset));
            var hipOffset = Vec3.sum(_this.avatarData.position, {x: 0, y: 0.2, x: 0});
            var referencePos = Vec3.sum(hipOffset, referenceOffset);
            var controllerOffset = Vec3.subtract(controllerPosition, referencePos);
            return controllerOffset;
        };
        
        this.updateAvatarData = function() {
            _this.avatarData.update(MyAvatar.position, MyAvatar.orientation);
        };
        
        this.updateLeg = function(handController, alpha) {
            if (alpha > 0) {
                var controllerOffset = _this.getControllerOffset(handController);
                var xAngle = Vec3.getAngle(controllerOffset, Vec3.sum(_this.avatarData.forward, Vec3.multiply(_this.avatarData.up, 0.5)));
                var zAngle = Vec3.getAngle(controllerOffset, _this.avatarData.right);
                var yAmount = Vec3.dot(Vec3.normalize(controllerOffset), _this.avatarData.up);
                var yOffset = yAmount < 0 ? 0.5 * yAmount : 0;
                _this.colliders.setOffset(handController, yOffset);
                var rotation = Quat.fromVec3Radians({x:-1.5*xAngle, y:PI + -0.5 + 0.3*zAngle, z:0});
                var jointIndex = (handController == Controller.Standard.RightHand) ? _this.avatarData.rightLegIndex : _this.avatarData.leftLegIndex;
                MyAvatar.setJointRotation(jointIndex, Quat.slerp(MyAvatar.getJointRotation(jointIndex), rotation, alpha));
            }
            if (handController == Controller.Standard.RightHand) {
                if (_this.lastRightAlpha > 0.0 && alpha == 0.0) {
                    MyAvatar.clearJointsData();
                }
                _this.lastRightAlpha = alpha;
            } else {
                if (_this.lastLeftAlpha > 0.0 && alpha == 0.0) {
                    MyAvatar.clearJointsData();
                }
                _this.lastLeftAlpha = alpha;
            }
            
        };
    }
    
    var soccerController = new SoccerController();
    
    // Disable teleport
    Messages.sendMessage("Hifi-Teleport-Disabler", "both");
    
    Script.update.connect(function(){

        soccerController.updateAvatarData();
        soccerController.updateLeg(Controller.Standard.LeftHand, Controller.getValue(Controller.Standard.LeftGrip));
        soccerController.updateLeg(Controller.Standard.RightHand, Controller.getValue(Controller.Standard.RightGrip));
        soccerController.colliders.update();

    });
    Script.scriptEnding.connect(function() {
        Messages.sendMessage("Hifi-Teleport-Disabler", "none");
        soccerController.colliders.cleanup();
        visualDebugger.cleanup();
    });
})();