/**
 *
 * @param id
 * @param name
 * @param physicsWorld
 * @constructor
 */
GameLib.D3.ComponentVehicleAIObjectAvoidance = function(
    id,
    name,
    physicsWorld
) {
    this.id = id|| GameLib.D3.Tools.RandomId();
    if (typeof name == 'undefined') {
        name = this.constructor.name;
    }
    this.name = name;

    this.parentEntity = null;
    GameLib.D3.Utils.Extend(GameLib.D3.ComponentVehicleAIObjectAvoidance, GameLib.D3.ComponentInterface);

    this.raycastVehicleComponent = null;
    this.physicsWorld = physicsWorld || null;
    this.sensors = [];

    // debug
    this.debugArrows = {};


    console.log("constructor for : ComponentVehicleAIObjectAvoidance");
};

//#ifdef RUNTIME__

/////////////////////////////////////////////////////////////////////////
///////////////////////// Methods to override ///////////////////////////
/////////////////////////////////////////////////////////////////////////
GameLib.D3.ComponentVehicleAIObjectAvoidance.prototype.onSetParentEntity = function(
    parentScene,
    parentEntity
) {
    this.parentEntity = parentEntity;
    this.raycastVehicleComponent = parentEntity.getComponent(GameLib.D3.RaycastVehicle);

    console.log("onSetParentEntity for : ComponentVehicleAIObjectAvoidance");


    if(!this.raycastVehicleComponent) {
        console.warn("NO RAYCAST VEHICLE FOUND!");
    }

    // create sensors

    var boundingBox = this.parentEntity.mesh.geometry.boundingBox;
    // this is taken from the main.js.
    // this should be configurable inside the editor
    var carBoxScaleModifier = { x : 3 / 4, y : 1 / 2, z : 0.90 };

    var sensorLength = 7.0;
    var sensorColor = new THREE.Color(0, 0, 1);

    // . . . . . . . . . . FRONT . . . . . . . . . .

    // right
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                1,
                0,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );


    // left
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                1,
                0,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                -boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );


    // center
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                1,
                0,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                0,
                0
            )
        }
    );


    // . . . . . .  DIAGONAL FRONT . . . . . . . .
    // right
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                0,
                1,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );


    // left
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                0,
                -1,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                -boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );

    // right
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                0.5,
                0.5,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );


    // left
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                0.5,
                -0.5,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                -boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );

    // right
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                0.75,
                0.25,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );


    // left
    this.sensors.push(
        {
            sensorLength : sensorLength,

            sensorColor : sensorColor,

            sensorDirection : new THREE.Vector3(
                0.75,
                -0.25,
                0
            ).normalize(),

            sensorPositionOffset : new THREE.Vector3(
                boundingBox.max.x * this.parentEntity.mesh.scale.x * carBoxScaleModifier.x,
                -boundingBox.max.z * this.parentEntity.mesh.scale.z * carBoxScaleModifier.z,
                0//boundingBox.max.y * this.parentEntity.mesh.scale.y * carBoxScaleModifier.y // this is still swapped with y, because cannon.
            )
        }
    );


    console.log("pushed sensors", this.sensors.length);

};

GameLib.D3.ComponentVehicleAIObjectAvoidance.prototype.onUpdate = function(
    deltaTime,
    parentEntity
) {
    if(this.raycastVehicleComponent && this.physicsWorld) {
        // debug
        this.debugArrows.avgDirection = this.debugArrows.avgDirection || {};
        this.debugArrows.sensors = this.debugArrows.sensors || [];

        var vehicleVelocity = this.parentEntity.getComponent(GameLib.D3.RigidBody).instance.velocity;
        var vehicleVelocityLength = 12;

        // shoot rays for each sensor & check collisions.
        var world = this.physicsWorld.instance;
        var result = new CANNON.RaycastResult();

        this.debugArrows.avgDirection.vector = new THREE.Vector3();

        for(var s = 0, l = this.sensors.length; s < l; ++s) {

            // debug
            if(!this.debugArrows.sensors[s]) {
                this.debugArrows.sensors[s] = {};
            }


            var sensor = this.sensors[s];

            var from = new THREE.Vector3(
                this.parentEntity.position.x,
                this.parentEntity.position.y,
                this.parentEntity.position.z
            ).add(new THREE.Vector3(
                sensor.sensorPositionOffset.x,
                sensor.sensorPositionOffset.y,
                sensor.sensorPositionOffset.z
            ).applyQuaternion(new THREE.Quaternion(
                this.parentEntity.quaternion.x,
                this.parentEntity.quaternion.y,
                this.parentEntity.quaternion.z,
                this.parentEntity.quaternion.w
            )));

            var fromC = new CANNON.Vec3(
                from.x,
                from.y,
                from.z
            );

            var to = new THREE.Vector3(
                sensor.sensorDirection.x,
                sensor.sensorDirection.y,
                sensor.sensorDirection.z
            ).applyQuaternion(new THREE.Quaternion(
                this.parentEntity.quaternion.x,
                this.parentEntity.quaternion.y,
                this.parentEntity.quaternion.z,
                this.parentEntity.quaternion.w
            )).normalize().multiplyScalar(sensor.sensorLength);


            var toC = new CANNON.Vec3(
                from.x + to.x,
                from.y + to.y,
                from.z + to.z
            );

            world.raycastClosest(
                fromC,

                toC,

                {
                    collisionFilterMask : 2 // check only group 2 (track)
                },

                result
            );

            if(result.hasHit) {
                sensor.sensorColor = new THREE.Color(1, 0, 0);

                var direction = new THREE.Vector3(
                    sensor.sensorDirection.x,
                    sensor.sensorDirection.y,
                    sensor.sensorDirection.z
                ).applyQuaternion(new THREE.Quaternion(
                    this.parentEntity.quaternion.x,
                    this.parentEntity.quaternion.y,
                    this.parentEntity.quaternion.z,
                    this.parentEntity.quaternion.w
                )).normalize();

                /*var reflection = direction.reflect(
                    new THREE.Vector3(
                        result.hitNormalWorld.x,
                        result.hitNormalWorld.y,
                        result.hitNormalWorld.z
                    )
                ).normalize();*/

                var reflection = direction.reflect(
                    new THREE.Vector3(
                        result.hitPointWorld.x,
                        result.hitPointWorld.y,
                        result.hitPointWorld.z
                    ).normalize().cross(direction)
                ).normalize().negate();

                var origin = new THREE.Vector3(
                    this.parentEntity.position.x,
                    this.parentEntity.position.y,
                    this.parentEntity.position.z
                ).add(new THREE.Vector3(
                    sensor.sensorPositionOffset.x,
                    sensor.sensorPositionOffset.y,
                    sensor.sensorPositionOffset.z
                ).applyQuaternion(new THREE.Quaternion(
                    this.parentEntity.quaternion.x,
                    this.parentEntity.quaternion.y,
                    this.parentEntity.quaternion.z,
                    this.parentEntity.quaternion.w
                )));

                var moveDirection = direction.add(reflection).setY(0).normalize();
                this.debugArrows.avgDirection.vector = this.debugArrows.avgDirection.vector.add(moveDirection);

                if(this.debugArrows.sensors[s].intersectionArrow && this.debugArrows.sensors[s].mesh) {
                    this.debugArrows.sensors[s].mesh.remove(this.debugArrows.sensors[s].intersectionArrow);
                }

                if(this.debugArrows.sensors[s].mesh){

                    var arrow = new THREE.ArrowHelper(

                        moveDirection,

                        origin,

                        22,

                        new THREE.Color(1, 1, 0)
                    );

                    this.debugArrows.sensors[s].intersectionArrow = arrow;
                    this.debugArrows.sensors[s].mesh.add(this.debugArrows.sensors[s].intersectionArrow);
                }

            } else {
                sensor.sensorColor = new THREE.Color(0, 0, 1);

                if(this.debugArrows.sensors[s].intersectionArrow && this.debugArrows.sensors[s].mesh) {
                    this.debugArrows.sensors[s].mesh.remove(this.debugArrows.sensors[s].intersectionArrow);
                }
            }
        }


        this.debugArrows.avgDirection.vector = this.debugArrows.avgDirection.vector.normalize();
        // draw the avg move direction

        /*if(!this.debugArrows.avgDirection.mesh) {
            this.debugArrows.avgDirection.mesh = new THREE.Mesh(
                new THREE.Geometry(),
                new THREE.MeshBasicMaterial(
                    {
                        color : 0x00ffff,
                        wireframe : true
                    }
                )
            );

            sys.game.scenes["MainScene"].instance.add(this.debugArrows.avgDirection.mesh);
        }

        if(this.debugArrows.avgDirection.arrow && this.debugArrows.avgDirection.mesh) {
            this.debugArrows.avgDirection.mesh.remove(this.debugArrows.avgDirection.arrow);
        }*/

        /*this.debugArrows.avgDirection.arrow = new THREE.ArrowHelper(
            this.debugArrows.avgDirection.vector,

            new THREE.Vector3(
                this.parentEntity.position.x,
                this.parentEntity.position.y,
                this.parentEntity.position.z
            ),

            12,

            this.debugArrows.avgDirection.mesh.material.color
        );*/

        //this.debugArrows.avgDirection.mesh.add(this.debugArrows.avgDirection.arrow);

        // draw sensors
        /*{
            for(var s = 0, l = this.sensors.length; s < l; ++s) {

                if(!this.debugArrows.sensors[s].mesh) {

                    var geometry = new THREE.Geometry();
                    var mesh = new THREE.Mesh(
                        geometry,
                        new THREE.MeshBasicMaterial(
                            {
                                color : 0x000000,
                                wireframe : true
                            }
                        )
                    );

                    this.debugArrows.sensors[s].mesh = mesh;
                    sys.game.scenes["MainScene"].instance.add(this.debugArrows.sensors[s].mesh);
                }

                // remove old arrow, if we have one
                if(this.debugArrows.sensors[s].arrow) {
                    this.debugArrows.sensors[s].mesh.remove(this.debugArrows.sensors[s].arrow);
                }


                var sensor = this.sensors[s];
                var sensorLength = sensor.sensorLength; // should get this from the sensor itself

                this.debugArrows.sensors[s].arrow = new THREE.ArrowHelper(
                    new THREE.Vector3(
                        sensor.sensorDirection.x,
                        sensor.sensorDirection.y,
                        sensor.sensorDirection.z
                    ).applyQuaternion(new THREE.Quaternion(
                        this.parentEntity.quaternion.x,
                        this.parentEntity.quaternion.y,
                        this.parentEntity.quaternion.z,
                        this.parentEntity.quaternion.w
                    )).normalize(),

                    new THREE.Vector3(
                        this.parentEntity.position.x,
                        this.parentEntity.position.y,
                        this.parentEntity.position.z
                    ).add(new THREE.Vector3(
                        sensor.sensorPositionOffset.x,
                        sensor.sensorPositionOffset.y,
                        sensor.sensorPositionOffset.z
                    ).applyQuaternion(new THREE.Quaternion(
                        this.parentEntity.quaternion.x,
                        this.parentEntity.quaternion.y,
                        this.parentEntity.quaternion.z,
                        this.parentEntity.quaternion.w
                    ))),

                    sensorLength,

                    sensor.sensorColor
                );

                this.debugArrows.sensors[s].mesh.add(this.debugArrows.sensors[s].arrow);
            }
        }*/


        // . . . . . . . . . . . . correct the path . . . . . . . . . . . . . .
        if( this.debugArrows.avgDirection.vector.x != 0
            || this.debugArrows.avgDirection.vector.y != 0
            || this.debugArrows.avgDirection.vector.z != 0
        ) {

            // get forward direction OR better the velocity angle of the car

            var avgMoveVector = this.debugArrows.avgDirection.vector;
            var forward = new THREE.Vector3(
                1,
                0,
                0
            ).applyQuaternion(new THREE.Quaternion(
                this.parentEntity.quaternion.x,
                this.parentEntity.quaternion.y,
                this.parentEntity.quaternion.z,
                this.parentEntity.quaternion.w
            )).setY(0).normalize();

            // get angle and steer.
            var cos = avgMoveVector.dot(forward);

            var angleRadians = Math.acos(cos);
            var steerAngleModifier = GameLib.D3.Vector3.AngleDirection(
                forward,
                avgMoveVector,
                new GameLib.D3.Vector3(
                    0,
                    1,
                    0
                )
            );

            var steerAngle = Math.min(angleRadians, 0.5) * steerAngleModifier;

            this.raycastVehicleComponent.instance.setSteeringValue(steerAngle, 0);
            this.raycastVehicleComponent.instance.setSteeringValue(steerAngle, 1);
        }

    }
};

//#endif