#include "PacmanBrown.h"

#include "core/CoreIncludes.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"

	
	namespace orxonox{

		RegisterClass(PacmanBrown);

		PacmanBrown::PacmanBrown(Context* context) : PacmanGhost(context){

        RegisterObject(PacmanBrown);
          
    }

    /**
    @brief
        Method for creating a ghost through XML.
    */
    void PacmanBrown::XMLPort(Element& xmlelement, XMLPort::Mode mode)
    {
        SUPER(PacmanBrown, XMLPort, xmlelement, mode);
    }



	void PacmanBrown::tick(float dt)
    {
        SUPER(PacmanGhost, tick, dt);

        this->actuelposition = this->getPosition();
        
        //Stop, if target arrived
        if((abs(this->actuelposition.x - this->target_x)<0.5) && (abs(this->actuelposition.z - this->target_z)<0.5)){
                 this->ismoving = false;
        }

        //Move, if ghost hasn't arrived yet
        if(this->ismoving){
            if(!(abs(this->actuelposition.z-target_z)<0.5)) {
                velocity = Vector3(0,0,-sgn(this->actuelposition.z-this->target_z));
                move(dt, actuelposition, velocity);
            }    
            if(!(abs(this->actuelposition.x-target_x)<0.5)){
                velocity = Vector3(-sgn(this->actuelposition.x-this->target_x),0,0);
                move(dt, actuelposition, velocity);
            }
        }
        
        //Check on which position the ghost has arrived and set new target
        else{

        	while(lockmove){};
            lockmove = true;

        	//do brown behavior
        	//put everything needed here

        	if((this->fleeing==true)&&(this->actuelposition!=(-215,10,-195))){
            //if fleeing, then go to corner map if not already there
        		fleeMode();
        	}
        	else{ // else go next to pacman

        		if(absoluteDistance(this->pos, player.pos)<10){//no idea if 10 ok
        	//if near player, flee away
        	fleemode();
        			}
        		else{ //go to neighboor of player nearest to brown pacman

        		//first find nearest neighboor point of player to brown pacman
        		//we can maybe call getShortestPath recursively to do so

player.pos.nearestneighboor=findPlayerNeighboorNearestToPacman(player.neighboors);
//btw the array player.neighboor can be defined with a big list as we
//already did several times for similar things

        	Vector3 nextMove = getShortestPath(this->actuelposition, player.pos.nearestneighboor);
        	setNewTargetBrown(nextMove);
        		}

        	}
   
        }
        
    }










	int graphDistance(Vector3 start, Vector3 goal){

        Vector3 differenceVector= Vector3(abs(goal.x-start.x), 0,abs(goal.z-start.z));

        return differenceVector.x+differenceVector.z;
    }

	void setNewTargetBrown(Vector3 goalToGo){

					this->target_x = goalToGo.x;
                    this->target_z = goalToGo.z; 
                    this->ismoving = true;
	}



	int absoluteDistance(Vector3 pos1, Vector3 pos2){


		Vector3 diffVector;
		diffVector.x=pos2-pos1;
		diffVector.y=pos2-pos1; //should always be 0
		diffVector.z=pos2-pos1;
		int result = sqrt(diffVector.x²+diffVector.z²);
		return result;
	}


	void fleeMode(){ //flees to corner of the map

		Vector3 cornerPos = Vector3(-215,10,-195); //let's take this. We can still change
		Vector3 nextMove = getShortestPath(this->actuelposition, cornerPos);
		setNewTargetBrown(nextMove);

		/*//while(this->actuelposition!=cornerPos){
	
		continue moving and each time find next point to go until
		we reach the corner
		}*/
	}


	Vector3 goAdjacentOfPlayer(Vector neighboorsOfPlayer[]){
		//find ,nearest to brown, player neighboor 

		int besTotDist=-1;
		Vector3 curPos=this->pos;
		Vector3  predPos;
		Vector3 chosenNeighbor;

		for(int i=0; i < 4; i++){
			int totDist=0;

			if(neighboorsOfPlayer[i]!=0){
				while(curPos!=neighboorsOfPlayer[i]){

					predPos=curPos;
					curPos = getShortestPath(curPos, neighboorsOfPlayer[i]);
					totDist+=graphDistance(curPos,predPos);

				}
			}

			if(besTotDist==-1){
				besTotDist=totDist;
				chosenNeighbor=neighboorsOfPlayer[i];
			}
			else if(besTotDist>totDist){
				besTotDist=totDist;
				chosenNeighbor=neighboorsOfPlayer[i];
			}
		}
		return chosenNeighbor;

	}


	Vector3 findPlayerNeighboorNearestToPacman(Vector3 neighboorArray[]){
		//uh, i think it does the same think as the function above

        Vector3 nextMove;
        int distCost=-1;
        for(int i=0; i < 4; i++){
        	int totDist=0;
        	Vector3 newCandidate=this->actuelposition;
        	Vector3 predPos;

            if(neighboorArray[i]!=NULL){
                while(nextMove!=neighboorArray[i]){
                	predPos=newCandidate;
                	newCandidate=getShortestPath(newCandidate, neighboorArray[i]);
                	totDist+=graphDistance(predPos, newCandidate);
                    }

                    if(distCost==-1){
                distCost=totDist;
                nextMove=gneighboorArray[i];
                    }
                    else if(totDist<distCost){
                distCost=totDist;
                nextMove=neighboorArray[i];
                    }
            }
        }
        return nextMove;

    }

}