/**
    * Find shortest path in given polygon
    * with A* algorithm using funnel path portals
    * 
    * @param {float[]} fromPoint -  from 2D point
    * @param {float[]} toPoint - to 2D point
    * @returns {float[float[]]} array of 2D points
*/
MajaPolygon.prototype.getShortestPath = function(fromPoint, toPoint){
    if(this.triangulation === null){
        //triangulate if necessary
        this.calculateTriangulation();
    }
    
    //get from/to vertex data and ensure points are in polygon
    var closestStartValues = this.getClosestTriangleVertex(fromPoint);
    var startVertex = closestStartValues[0];
    fromPoint = closestStartValues[1];

    var closestEndValues = this.getClosestTriangleVertex(toPoint);
    var endVertex = closestEndValues[0];
    toPoint = closestEndValues[1];

    if(startVertex !== false && endVertex !== false){
        if(startVertex == endVertex){
            //from/to lie in same triangle, return direct path
            return [fromPoint, toPoint];
        }

        var closedSet = [];
        var openSet = [startVertex];

        var cameFrom = {};

        //use direct distance as score estimation
        var fScore = {};
        fScore[startVertex] = $g.func.getPointDistance(fromPoint, toPoint);

        while(openSet.length > 0){
            var currentNode = false;
            var currentNodeIndex = false;
            var currentNodeValue = false;

            //get next node with lowest estimated distance value
            for(var i=0;i<openSet.length;i++){
                var nodeValue = fScore[openSet[i]];
                if(
                    currentNodeValue === false ||
                    nodeValue < currentNodeValue
                ){
                    currentNode = openSet[i];
                    currentNodeIndex = i;
                    currentNodeValue = nodeValue;
                }
            }

            if(currentNode == endVertex){
                //shortest path determined
                return cameFrom[currentNode][1];
            }

            $g.func.removeFromArray(openSet, currentNodeIndex);
            closedSet.push(currentNode);

            for(var i=0;i<this.triangulationConnections.length;i+=2){
                //find neighbour node to given vertex
                var neighbourNode = false;
                if(this.triangulationConnections[i] == currentNode){
                    neighbourNode = this.triangulationConnections[i+1];

                } else if(this.triangulationConnections[i+1] == currentNode){
                    neighbourNode = this.triangulationConnections[i];
                }

                if(
                    neighbourNode !== false &&
                    closedSet.indexOf(neighbourNode) == -1
                ){
                    //calculate new funnel portal
                    var fromVertices = [];
                    fromVertices.push(this.triangulation[currentNode*3]);
                    fromVertices.push(this.triangulation[(currentNode*3)+1]);
                    fromVertices.push(this.triangulation[(currentNode*3)+2]);

                    //find shared portal vertices with neighbour triangle
                    var sharedVerticePoints = [];
                    var toVerticePoint;
                    for(var j=0;j<3;j++){
                        var neighbourVertice = this.triangulation[neighbourNode*3+j];

                        var fromVerticeIndex = fromVertices.indexOf(neighbourVertice);
                        if(fromVerticeIndex == -1){
                            //neighbour vertex is not in portal
                            toVerticePoint = [
                                this.flatArray[(neighbourVertice*2)],
                                this.flatArray[(neighbourVertice*2)+1]
                            ];

                        } else {
                            //neighbour vertex is part of portal
                            var sharedVertice = $g.func.removeFromArray(
                                fromVertices, fromVerticeIndex
                            );
                            sharedVerticePoints.push([
                                this.flatArray[(sharedVertice*2)],
                                this.flatArray[(sharedVertice*2)+1]
                            ]);
                        }
                    }
                    var leftVertice = fromVertices.shift();
                    var fromVerticePoint = [
                        this.flatArray[(leftVertice*2)],
                        this.flatArray[(leftVertice*2)+1]
                    ];

                    var cproduct_1 = $g.func.crossProduct(
                        fromVerticePoint, sharedVerticePoints[0], toVerticePoint
                    );
                    var cproduct_2 = $g.func.crossProduct(
                        fromVerticePoint, sharedVerticePoints[1], toVerticePoint
                    );

                    //create portal sorted left/right vertex
                    var newPortal;
                    if(cproduct_1 < cproduct_2){
                        newPortal = [sharedVerticePoints[0], sharedVerticePoints[1]];

                    } else {
                        newPortal = [sharedVerticePoints[1], sharedVerticePoints[0]];
                    }

                    var atEndVertice = (neighbourNode == endVertex);

                    //extend or start funnel path algorithm through given portal
                    var aStarFunnelPath, tentativePath;
                    if(currentNode in cameFrom){
                        aStarFunnelPath = cameFrom[currentNode][0].getCopy();
                        tentativePath = aStarFunnelPath.extend(newPortal, atEndVertice);

                    } else {
                        aStarFunnelPath = new $g.func.AStarFunnelPath(
                            fromPoint, toPoint, [newPortal]
                        );
                        tentativePath = aStarFunnelPath.calculate(atEndVertice);
                    }

                    //get funnel path from start to neighbour node
                    var tentativeGScore = $g.func.getPathLength(tentativePath);
                    
                    if(openSet.indexOf(neighbourNode) == -1){
                        openSet.push(neighbourNode);

                    } else if(tentativeGScore >= fScore[neighbourNode]){
                        continue;
                    }

                    //store funnel path length as fScore(=gScore)
                    cameFrom[neighbourNode] = [aStarFunnelPath, tentativePath];
                    fScore[neighbourNode] = tentativeGScore;
                }
            }
        }

    } else {
        $g.log_e(
            "getShortestPath",
            "could not calculate start or end "+startVertex+", "+endVertex
        );
    }

    return [];
}

void ABeePawn::Tick(float DeltaTime)
{
    Super::Tick(DeltaTime);

    bool IsInMenu = (GetWorld()->GetAuthGameMode<ATheHiveGameModeBase>())->IsInMenu;

    if (!IsInMenu)
    {
        //remove bee power based on day state
        if ((GetWorld()->GetGameState<ABeeGameStateBase>())->IsNight)
        {
            RemovePower(tickPowerCostsNight);
        }
        else
        {
            RemovePower(tickPowerCosts);
        }
    }

    if (!IsInMenu && RestrictMovementTime <= 0.f)
    {
        //update rotation
        FRotator NewRotation = GetActorRotation();
        NewRotation.Yaw += CameraInput.X;
        NewRotation.Pitch = FMath::Clamp(
            NewRotation.Pitch + CameraInput.Y, -60.f, 89.9f
        );
        SetActorRotation(NewRotation);
    }

    if (IsAlive)
    {
        if (RestrictMovementTime > 0.f)
        {
            //keep movement restricted
            RestrictMovementTime -= DeltaTime;
            MovementInput = FVector(0.f);
        }
        else
        {
            CurrentBeeStatus = BeeStatus::Normal;

            //check npc actions
            if (NpcControlled)
            {
                if (HasTarget)
                {
                    if (
                        (TargetPosition - GetActorLocation()).SizeSquared() <=
                        TargetMinSqrDist
                    )
                    {
                        //bee arrived at destination, update target
                        HasTarget = false;

                        if (NpcTarget == BeeNpcTarget::Flower)
                        {
                            ++NpcFlowerVisits;
                            NpcTarget = BeeNpcTarget::Nothing;
                        }
                        else if (NpcTarget == BeeNpcTarget::Hive)
                        {
                            NpcTarget = BeeNpcTarget::Nothing;
                        }
                    }
                }
                if (!HasTarget)
                {
                    //update target if bee has none
                    CheckNpcAction();
                }

                if (HasTarget)
                {
                    //move npc bee towards target
                    MovementInput =
                        (TargetPosition - GetActorLocation()).GetClampedToMaxSize(MaxSpeed);
                }
            }
        }
    }
    else {
        MovementInput = FVector(0.f, 0.f, -9.81f);
    }

    if (OurMovementComponent && (OurMovementComponent->UpdatedComponent == RootComponent))
    {
        //update movement components velocity
        float UseAccelerationDelta = (CurrentVelocity.SizeSquared() > MovementInput.SizeSquared()) ?
            DeaccelerationDelta : AccelerationDelta;

        CurrentVelocity = FMath::Lerp<FVector>(
            CurrentVelocity, MovementInput, UseAccelerationDelta * DeltaTime
        );
        OurMovementComponent->CurrentVelocity = CurrentVelocity;

        if (NpcControlled && HasTarget)
        {
            //update npcs bee rotation
            SetActorRotation(CurrentVelocity.Rotation());
        }
    }

    MovementInput = FVector(0.f);
}

void ABeePawn::CheckNpcAction()
{
    ABeeGameStateBase* BeeGameState = GetWorld()->GetGameState<ABeeGameStateBase>();

    if (NpcTarget == BeeNpcTarget::HiveEnter)
    {
        //move into hive
        TargetPosition = BeeGameState->HiveCenterPos +
            FMath::VRand() * FMath::RandRange(0.f, HiveCenterRadius);
        HasTarget = true;

        NpcTarget = BeeNpcTarget::Hive;
    }
    else if (IsInHive && Pollen > 0.f)
    {
        PassPollenToHive();
    }
    else if (IsInHive && Power < 0.8f && BeeGameState->HiveHoney > 0.f)
    {
        ConsumeHiveHoney();
    }
    else if (Pollen < 1.f && IsAtFlower != nullptr && IsAtFlower->PollenLeft > 0.f)
    {
        CollectFlowerPollen();
    }
    else if (Pollen > 0.75f || NpcFlowerVisits >= 3 ||
        BeeGameState->IsNight && !IsInHive
    )
    {
        //return to hive, move to hive entry first
        NpcFlowerVisits = 0;

        TargetPosition = BeeGameState->HiveEnterPos +
            FMath::VRand() * FMath::RandRange(0.f, HiveEnterRadius);
        HasTarget = true;

        NpcTarget = BeeNpcTarget::HiveEnter;
    }
    else if (KnowsAboutFlowers.Num() > 0 && !BeeGameState->IsNight)
    {
        //bee is in idle, check next action
        if (NearbyBeePawns.Num() > 0 && FMath::FRandRange(0.f, 1.f) > 0.8f)
        {
            //show nearby bees way to known flowers
            DoFlowerDance();
        }
        else
        {
            if (IsInHive)
            {
                //fly out of hive
                TargetPosition = BeeGameState->HiveEnterPos +
                    FMath::VRand() * FMath::RandRange(0.f, HiveEnterRadius);
                HasTarget = true;
            }
            else
            {
                //fly to random known flower
                AFlower* MoveToFlower = KnowsAboutFlowers[rand()% KnowsAboutFlowers.Num()];

                FBox ColliderBox = MoveToFlower->BoxCollider->Bounds.GetBox();
                TargetPosition = ColliderBox.GetCenter() +
                    FMath::VRand() * FMath::RandRange(0.f, FlowerRadius);
                HasTarget = true;

                NpcTarget = BeeNpcTarget::Flower;
            }
        }
    }
}

public IEnumerator StartGymAnimation(PlayerTimer activeTimer){
    System.Type timerType = (activeTimer != null) ?
        activeTimer.GetType() : null;

    //prepare animations
    if(timerType != null){
        if(timerType == typeof(PlayerTimerTrainingSkill)){
            if(CheckPlayerFighterStatus(
                FighterState.IDLE, GameController.staticStateNames["to_idle"].stateName
            )){
                yield return new WaitForSeconds(1f);
            }

            BattleFighter.UpdateFighterState(FighterState.TRAINING);

            TrainingSkill trainingSkill =
                ((PlayerTimerTrainingSkill)activeTimer).GetTrainingSkill();
            string prepareStateName =
                trainingSkill.GetPrepareAnimationStateName();
            if(prepareStateName != ""){
                BattleFighter.FadeToAnimationState(prepareStateName, 0.15f);
                yield return new WaitForSeconds(1f);
            }

        } else {
            if(CheckPlayerFighterStatus(
                FighterState.STANCE,
                GameController.staticStateNames["to_stance"].stateName
            )){
                yield return new WaitForSeconds(1f);
            }
        }

    } else {
        if(CheckPlayerFighterStatus(
            FighterState.IDLE,
            GameController.staticStateNames["to_idle"].stateName
        )){
            yield return new WaitForSeconds(1f);
        }
    }

    //start animations
    if(timerType != null){
        if(timerType == typeof(PlayerTimerDefenseSkill)){
            DefenseSkill defenseSkill =
                ((PlayerTimerDefenseSkill)activeTimer).GetDefenseSkill();
            if(defenseSkill != activePartnerTrainingDefenseSkill){
                activePartnerTrainingDefenseSkill = defenseSkill;

                List<KeyValuePair<SkillAnimationState, Attack>> defenseSkillAttacks =
                    new List<KeyValuePair<SkillAnimationState, Attack>>();
                foreach(
                    SkillAnimationState skillAnimationState in
                    defenseSkill.skillAnimationStates
                ){
                    foreach(Attack attack in GameModel.attacks){
                        if(
                            attack.GetAttackCategory() ==
                            defenseSkill.GetAnswerToAttackCategory() &&
                            attack.GetAttackPosition() ==
                            skillAnimationState.GetRestrictedToAttackPosition()
                        ){
                            defenseSkillAttacks.Add(
                                new KeyValuePair<SkillAnimationState, Attack>(
                                    skillAnimationState, attack
                                )
                            );
                        }
                    }
                }

                partnerTrainingCoroutine = Game.StartSync(
                    ShowPartnerTraining(defenseSkill, defenseSkillAttacks)
                );
            }

        } else {
            activePartnerTrainingDefenseSkill = null;

            List<AnimationScript> gymAnimationScripts =
                activeTimer.GetGymAnimationScripts();
            if(gymAnimationScripts.Count > 0){
                BattleFighter.StartAnimationScript(
                    gymAnimationScripts[UnityEngine.Random.Range(
                        0, gymAnimationScripts.Count
                    )].GetIdentifier(), true
                );

            } else {
                float minRepeatTime = 1.2f;
                float maxRepeatTime = 1.8f;

                List<SkillAnimationState> animationStateNames =
                    activeTimer.GetSkillAnimationStates(true, true);
                BattleFighter.RepeatTrainingAnimationState(
                    GameController.staticStateNames["stance"], animationStateNames,
                    minRepeatTime, maxRepeatTime
                );
            }
        }

    } else {
        activePartnerTrainingDefenseSkill = null;

        List<SkillAnimationState> randomIdleAnimationStates =
            new List<SkillAnimationState>(){
                GameController.staticStateNames["random_idle_1"],
                GameController.staticStateNames["random_idle_2"],
                GameController.staticStateNames["random_idle_3"]
        };
        BattleFighter.RepeatTrainingAnimationState(
            GameController.staticStateNames["idle"], randomIdleAnimationStates,
            5f, 8f
        );
    }
}

public void ProcessInput(){
    float driftInput = Input.GetAxis("Drift");
    float accelerateInput = Input.GetAxis("Accelerate");
    float verticalInput = Input.GetAxis("Vertical");
    float horizontalInput = Input.GetAxis("Horizontal");
    float boostInput = Input.GetAxis("Boost");
    float rollInput = Input.GetAxis("Roll");

    float driftStiffness = (driftInput != 0) ? 0.5f : 2f;

    WheelHit wheelHit;
    bool groundContact = backLeftCollider.GetGroundHit(out wheelHit);
    
    //edit wheel colliders
    WheelFrictionCurve frontLectFriction = frontLeftCollider.sidewaysFriction;
    frontLeftCollider.sidewaysFriction = frontLectFriction;

    WheelFrictionCurve frontRightFriction = frontRightCollider.sidewaysFriction;
    frontRightCollider.sidewaysFriction = frontRightFriction;
    
    WheelFrictionCurve backLectFriction = backLeftCollider.sidewaysFriction;
    backLectFriction.stiffness = driftStiffness;
    backLeftCollider.sidewaysFriction = backLectFriction;

    WheelFrictionCurve backRightFriction = backRightCollider.sidewaysFriction;
    backRightFriction.stiffness = driftStiffness;
    backRightCollider.sidewaysFriction = backRightFriction;

    float motorTorque = (groundContact) ? (motorForce*accelerateInput) : 0f;
    float steeringAngle = (maxSteerAngle*horizontalInput);

    backLeftCollider.motorTorque = motorTorque;
    backRightCollider.motorTorque = motorTorque;

    frontLeftCollider.steerAngle = steeringAngle;
    frontRightCollider.steerAngle = steeringAngle;

    waterElementController.SetFixedGravity((constantGravity || groundContact));

    //apply torque
    if(!groundContact){
        carRigidbody.AddTorque(
            transform.right*verticalInput*frontTorqueSpeed, ForceMode.Acceleration
        );

        if(rollInput != 0){
            carRigidbody.AddTorque(
                -transform.forward*horizontalInput*torqueSpeed, ForceMode.Acceleration
            );

        } else {
            carRigidbody.AddTorque(
                transform.up*horizontalInput*sideTorqueSpeed, ForceMode.Acceleration
            );
        }
    }

    //apply jump forces
    if(inputJump){
        if(groundContact){
            timeForDoubleJump = (Time.time+2f);

            AddJumpForce(transform.up);

        } else if(Time.time <= timeForDoubleJump){
            timeForDoubleJump = 0;

            carRigidbody.drag = 0f;
            carRigidbody.angularDrag = 0f;
            waterElementController.SetUnfixedGravity(true);

            Vector3 jumpDirection = transform.forward*verticalInput +
                transform.right*horizontalInput + transform.up*0.001f;
            AddJumpForce(jumpDirection);

            if(
                Mathf.Abs(verticalInput) > Mathf.Epsilon ||
                Mathf.Abs(horizontalInput) > Mathf.Epsilon
            ){
                Vector3 torqueDirection = transform.right*verticalInput -
                    transform.forward*horizontalInput;
                carRigidbody.AddTorque(
                    torqueDirection*doubleJumpForce, ForceMode.Impulse
                );
            }

            StartCoroutine(ResetJumpGravity());
        }
    }
    
    //apply boost
    ApplyBoost(boostInput);
    
    UpdateEngineAudio(verticalInput, boostInput);
}

protected virtual void Update(){
    if(carryingTreeController != null){
        //calculate trees distance
        Vector3 treeDiff = transform.position -
            carryingTreeController.trunkRope.transform.position;
        float treeDistance = treeDiff.magnitude;

        if(treeDistance > 6f){
            //apply force to pull tree near to person
            carryingTreeController.transform.position +=
                treeDiff.normalized * (treeDistance-6f);
            if(carryingTreeController.body != null){
                carryingTreeController.body.AddForceAtPosition(
                    treeDiff, carryingTreeController.trunkRope.transform.position,
                    ForceMode.Acceleration
                );
            }

            if(Random.value < 0.1f){
                carryingTreeController.PlayPullSound();
            }
        }

        //update rope constraint positions
        rope.startPoint = rightHandTransform.position;
        rope.endPoint = carryingTreeController.trunkRope.transform.position;
        rope.minYPos = Mathf.Min(
            transform.position.y, carryingTreeController.trunkRope.transform.position.y
        );
    }
}

public void SetCarryTree(TreeController treeController){
    if(treeController != null && carryingTreeController == null){
        //set tree to carry
        if(treeController.body == null){
            StartCoroutine(treeController.AddRigidbody());
        }
        carryingTreeController = treeController;
        
        //add rope and set constraint positions
        rope = Instantiate(ropePrefab).GetComponent<Rope>();
        rope.transform.SetParent(transform, false);
        
        rope.startPoint = rightHandTransform.position;
        rope.endPoint = carryingTreeController.trunkRope.transform.position;
        rope.minYPos = Mathf.Min(
            transform.position.y, carryingTreeController.trunkRope.transform.position.y
        );

        carryingTreeController.trunkRope.SetActive(true);

        animator.SetBool("pull_tree", true);

    } else {
        //unset carrying tree, remove rope
        Destroy(rope.gameObject);

        carryingTreeController.trunkRope.SetActive(false);

        carryingTreeController = null;
        rope = null;

        animator.SetBool("pull_tree", false);
    }
}