/*
 *   ORXONOX - the hottest 3D action shooter ever to exist
 *                    > www.orxonox.net <
 *
 *
 *   License notice:
 *
 *   This program is free software; you can redistribute it and/or
 *   modify it under the terms of the GNU General Public License
 *   as published by the Free Software Foundation; either version 2
 *   of the License, or (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 *
 *   Author:
 *      Marc Dreher
 *   Co-authors:
 *      ..
 *
 */

#include "PacmanGhost.h"

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

#include <vector>

namespace orxonox
{


	struct PacmanGhost::graphVertex {

		public:

			Vector3 position;
			graphVertex *adjacentVertices[4]; //neighbooring vertices

			int shortestDistanceToStart; //actual shortest distance to start point
			graphVertex* actuelPredecessor; //the predecessor giving the for now shortest 
			//path to start
			graphVertex* currentNearestNonVisitedNeighboor; 
			bool alreadyVisited;
			graphVertex(){ //default constructor
				position=0;
				shortestDistanceToStart= std::numeric_limits<int>::max();
				actuelPredecessor=nullptr;
				alreadyVisited=false;
				for(int kl =0; kl <4;kl++){
					adjacentVertices[kl]=nullptr;  //first put all position in array listing neighboors to 0
				}
			}
			graphVertex(Vector3 wantedPosition){  //normal constructor
				position=wantedPosition;
				shortestDistanceToStart= std::numeric_limits<int>::max(); //default distance is infinity
				actuelPredecessor=nullptr;
				alreadyVisited=false;
				for(int kl =0; kl <4;kl++){
					adjacentVertices[kl]=nullptr;  //first put all position in array listing neighboors to 0
				}
			}
			graphVertex& operator = (const graphVertex &rightSide){
				this->position=rightSide.position;
				this->shortestDistanceToStart=rightSide.shortestDistanceToStart;
				this->actuelPredecessor=rightSide.actuelPredecessor;
				this->currentNearestNonVisitedNeighboor=rightSide.currentNearestNonVisitedNeighboor;
				this->alreadyVisited=rightSide.alreadyVisited;

				return *this;
			}

	};


	static PacmanGhost::graphVertex listOfVertices[67];

	//Check if there is a collision
	bool findpos(Vector3 one, Vector3 other){
		if((abs(one.x - other.x)<0.5) && (abs(one.y - other.y)<0.5) && (abs(one.z - other.z)<0.5)) return true;
		return false;
	}

	//All positions in the map, see documentation
	Vector3 possibleposition[67] = {Vector3(20,10,245),Vector3(215,10,245),Vector3(215,10,195),Vector3(185,10,195),Vector3(135,10,195), //0-4
		Vector3(185,10,150),Vector3(135,10,150),Vector3(215,10,150),Vector3(215,10,105),Vector3(135,10,105), //5-9
		Vector3(135,10,15),Vector3(135,10,-85),Vector3(215,10,-85),Vector3(135,10,-135),Vector3(215,10,-135), //10-14
		Vector3(215,10,-195),Vector3(135,10,-195),Vector3(20,10,195),Vector3(-20,10,195),Vector3(-20,10,245), //15-19
		Vector3(-215,10,245),Vector3(-215,10,195),Vector3(-185,10,195),Vector3(-135,10,195),Vector3(-70,10,195), //20-24
		Vector3(70,10,195),Vector3(70,10,150),Vector3(20,10,150),Vector3(-20,10,150),Vector3(-70,10,150), //25-29
		Vector3(-135,10,150),Vector3(-185,10,150),Vector3(-215,10,150),Vector3(-215,10,105),Vector3(-135,10,105), //30-34
		Vector3(-70,10,105),Vector3(-20,10,105),Vector3(20,10,105),Vector3(70,10,105),Vector3(70,10,60), //35-39
		Vector3(0,10,60),Vector3(-70,10,60),Vector3(-135,10,15),Vector3(-70,10,60),Vector3(0,10,15), //40-44
		Vector3(70,10,15),Vector3(-70,10,-35),Vector3(-20,10,-35),Vector3(20,10,-35),Vector3(70,10,-35), //45-49
		Vector3(70,10,-85),Vector3(20,10,-85),Vector3(-20,10,-85),Vector3(-70,10,-85),Vector3(-135,10,-85), //50-54
		Vector3(-215,10,-85),Vector3(-215,10,-135),Vector3(-135,10,-135),Vector3(-70,10,-135),Vector3(-20,10,-135), //55-59
		Vector3(20,10,-135),Vector3(70,10,-135),Vector3(20,10,-195),Vector3(-20,10,-195),Vector3(-135,10,-195), //60-64
		Vector3(-215,10,-195),Vector3(0,10,-35)}; //65-66

	RegisterClass(PacmanGhost);

	/**
	  @brief
	  Constructor. Registers the object and initializes some default values.
	  @param creator
	  The creator of this object.
	  */
	PacmanGhost::PacmanGhost(Context* context) : ControllableEntity(context)
	{
		RegisterObject(PacmanGhost);

		this->velocity = Vector3(0, 0, 0);

		this->setCollisionType(CollisionType::Dynamic);

		this->actuelposition = this->getPosition();

		if(findpos(actuelposition, Vector3(0,-20,0)))
			dontmove = true;

		this->target_x = actuelposition.x;
		this->target_z = actuelposition.z; 

	}

	/**
	  @brief
	  Destructor. Destroys ghost, if present.
	  */
	PacmanGhost::~PacmanGhost()
	{
		// Deletes the controller if the object was initialized and the pointer to the controller is not nullptr.
	}

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

	//Change this with other ghost
	void PacmanGhost::changewith(PacmanGhost* otherghost){

		while(lockmove){};
		lockmove = true;    //Prevent change of target while ghost is changed

		otherghost->setPosition(this->getPosition());
		this->setPosition(0,-20,0);
		otherghost->target_x = this->target_x;   
		otherghost->target_z = this->target_z;
		otherghost->ismoving = this->ismoving;

		this->dontmove = true;
		otherghost->dontmove = false;

		lockmove = false;
	}

	//Move ghost with rotation
	void PacmanGhost::move(float dt, Vector3 actuelposition, Vector3 velocity){
		if(!dontmove){
			this->setPosition(Vector3(actuelposition.x+speed*velocity.x*dt,10,actuelposition.z+speed*velocity.z*dt));

			//Rotate ghost in the direction of movement 
			if((abs(abs(velocity.x)-1)<0.1) && (abs(velocity.z-0)<0.1)){
				if(velocity.x<0){
					this->setOrientation(Quaternion(Radian(-1.57), Vector3(0, 1, 0)));  
				}
				else{
					this->setOrientation(Quaternion(Radian(1.57), Vector3(0, 1, 0)));  
				}
			}
			if((abs(abs(velocity.z)-1)<0.1) && (abs(velocity.x-0)<0.1)){
				if(velocity.z<0){
					this->setOrientation(Quaternion(Radian(3.14), Vector3(0, 1, 0)));  
				}
				else{
					this->setOrientation(Quaternion(Radian(0), Vector3(0, 1, 0)));  
				}
			}

		}
	}

	//Change ability to move
	void PacmanGhost::changemovability(){
		if(dontmove){
			dontmove = false;}
		else{
			dontmove = true;   
		}
	}

	//ResetGhost
	void PacmanGhost::resetGhost(){

		this->setPosition(this->resetposition);
		this->ismoving = false;
		this->actuelposition = this->getPosition();

		this->target_x = actuelposition.x;
		this->target_z = actuelposition.z;

	}

	//Increase speed of ghosts
	void PacmanGhost::levelupvelo(){
		speed ++;
	}

	Vector3 PacmanGhost::getPlayerPos()
	{
		for (PacmanGelb* player : ObjectList<PacmanGelb>())
		{
			return player->getWorldPosition();
		}
		
		return Vector3(0,0,0); //default, should not be used

	}


	bool PacmanGhost::jeanfindpos(Vector3 one, Vector3 other){
		//This function is used to detect the last Vector3 position that the player crossed.

		if((abs(one.x - other.x)<15) && (abs(one.y - other.y)<15) && (abs(one.z - other.z)<15)) return true;
		return false;
	}

	void PacmanGhost::setNewTargetGhost(Vector3 goalToGo){
		//Ghosts will go to goalToGo

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


	///
	//// getShortestPath   /////////
	///



	Vector3 PacmanGhost::getShortestPath(Vector3 start, Vector3 goal, Vector3 pointToAvoidP1){
		//this function should then somehow produce the algorithm and call all other functions
		//and finally return the best neighboor of the actual position of the pacman

		//(optional parameter) pointToAvoidP1 is a point that cannot be considered


		graphVertex listOfVerticesM[67]; //our list of all possible graphs
		graphVertex* actualVertex; //we will walk through the array with a pointer


		if(start==goal){ // basic case
			return start; 
		}

		for(int an=0; an < 67; an++){
			listOfVerticesM[an]= graphVertex(possibleposition[an]); //same position order as in other file
            

			if(start==possibleposition[an]){
				actualVertex= &listOfVerticesM[an]; //our pointer points to the graph with position start in array
				
			}
		}

		actualVertex->alreadyVisited=true; //our start point is now visited
		actualVertex->shortestDistanceToStart=0; //At our start point, distance from start is 0
		findNeighboorVertices(actualVertex->position, actualVertex->adjacentVertices, listOfVerticesM); 
		// second parameter is an array ! //third is our global array

		while(actualVertex->position!=goal){
			for(int h=0;h < 4; h++){
				if(actualVertex->adjacentVertices[h]!=nullptr){ //check all neighboors of our current graphVertex

					
					updateShortestDistanceToStart(*actualVertex, *actualVertex->adjacentVertices[h]);
				} //we "update" the neighboors of our new visited vertex

			}

			actualVertex=findNextVertexToConsider(listOfVerticesM, pointToAvoidP1);
			actualVertex->alreadyVisited=true;
			
			if(actualVertex->position!=goal){
				findNeighboorVertices(actualVertex->position, actualVertex->adjacentVertices, listOfVerticesM); 
				//we find the neighboors of our new visited vertex
			}
		}

		//we should have reached our goal at this point

		while(actualVertex->actuelPredecessor->actuelPredecessor!=nullptr){ //the predecessor of our predecessor
			actualVertex=actualVertex->actuelPredecessor;
		}
		// the predecessor is our starting point, in other words we are now on an
		//adjacent vertex of the start

		return actualVertex->position; //we return the position of this - adjacent to start - vertex
	}

	//end of getShortestPath


	int PacmanGhost::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 PacmanGhost::updateShortestDistanceToStart(graphVertex &vertex, graphVertex &neighboor){
		//apply this method to all non visited neighboors of a vertex.
		// This method should always be run on a vertex after we marked it as visited.
		if(neighboor.alreadyVisited==false){ //we only consider non visited neighboors.
			if((vertex.shortestDistanceToStart!=std::numeric_limits<int>::max())&&
					(neighboor.shortestDistanceToStart > vertex.shortestDistanceToStart + 
					 graphDistance(vertex.position, neighboor.position))){ //need to consider overflow case !

				neighboor.shortestDistanceToStart= vertex.shortestDistanceToStart + 
					graphDistance(vertex.position, neighboor.position);
				neighboor.actuelPredecessor = &vertex;
			}
		}
	}

	void PacmanGhost::findNearestNonVisitedNeighboor (graphVertex &vertex, Vector3 pointToAvoidP3){ 
		//find nearest non visited neighboor of a given already visited vertex
		//(optional parameter) pointToAvoidP3 is a point that cannot be considered
		int shortestDistance = -1;
		graphVertex* nearestNonVisitedNeighboor=nullptr;//=graphVertex(); //by default there is not any.
		//Also, if all neighboors are already visited, we return NULL, i.e. there is no
		//nearest non visited neighboor.
		for(int i=0; i < 4; i++){
			if((vertex.adjacentVertices[i]!=nullptr)&&(vertex.adjacentVertices[i]->alreadyVisited==false)&&(vertex.adjacentVertices[i]->position!=pointToAvoidP3)){
				if(shortestDistance==-1){   //(concerns line above) we want a non visited neighboor //(optional) if the position of the neighboor is the one we want
					//to avoid, then we ignore it

					shortestDistance= graphDistance(vertex.position, vertex.adjacentVertices[i]->position);
					nearestNonVisitedNeighboor=vertex.adjacentVertices[i]; //warning, both sides are pointer adresses !
					
				}
				else if(graphDistance(vertex.position, vertex.adjacentVertices[i]->position)<shortestDistance){
					shortestDistance= graphDistance(vertex.position, vertex.adjacentVertices[i]->position);
					nearestNonVisitedNeighboor=vertex.adjacentVertices[i]; //warning, both sides are pointer adresses !
					
				}
			}
		}
		vertex.currentNearestNonVisitedNeighboor = nearestNonVisitedNeighboor; //warning, both sides are pointer adresses !
		
	}


	PacmanGhost::graphVertex* PacmanGhost::findNextVertexToConsider(graphVertex listOfVerticesP[], Vector3 pointToAvoidP2){ //find next, nearest from start, non visited vertex in our listOfVertices array
		//(optional parameter) pointToAvoidP2 is a point that cannot be considered

		int shortestDistance = -1;
		graphVertex* nextVertexToConsider;

		for(int i=0; i < 67; i++){ //we loop over all possible positions 

			if(listOfVerticesP[i].alreadyVisited==true){ //vertex should already be visited 

				findNearestNonVisitedNeighboor(listOfVerticesP[i], pointToAvoidP2); //we update nearest neighboor
				//of all visited vertices given that one of the nearest neighboor of a visited
				// vertex is now also visited because it was chosen as next optimal vertex

				if(listOfVerticesP[i].currentNearestNonVisitedNeighboor!=nullptr){ //we want a candidate!
					if(shortestDistance==-1){ //our first possible candidate

						shortestDistance=graphDistance(listOfVerticesP[i].position, 
								listOfVerticesP[i].currentNearestNonVisitedNeighboor->position) + 
							listOfVerticesP[i].shortestDistanceToStart;

						nextVertexToConsider=listOfVerticesP[i].currentNearestNonVisitedNeighboor;
						//adress of nextVertexToConsider is that of pointer currentNearestNonVisitedNeighboor

					}
					else if(shortestDistance > graphDistance(listOfVerticesP[i].position, 
								listOfVerticesP[i].currentNearestNonVisitedNeighboor->position) + 
							listOfVerticesP[i].shortestDistanceToStart){//if better candidate than our first candidate available

						shortestDistance=graphDistance(listOfVerticesP[i].position, 
								listOfVerticesP[i].currentNearestNonVisitedNeighboor->position) + 
							listOfVerticesP[i].shortestDistanceToStart;

						nextVertexToConsider=listOfVerticesP[i].currentNearestNonVisitedNeighboor;
						//we dont need the & because we are not giving the adress of the array element 
						//listOfVerticesP[i] but that of the pointer currentNearestNonVisitedNeighboor
					}
				}
			}
			//we want after all to return the nearest non visited neighboor
		}

		return nextVertexToConsider; //returns adress nextVertexToConsider is pointing to in array
	}

	//////////////////////////////////////////////////////////////////////////////////////////////

	void PacmanGhost::findNeighboorVertices(Vector3 actuelposition, graphVertex* adjacentVertices[], graphVertex listOfVerticesP2[]){      
		//Should put in adjacentVertices the neighboor points of actuelposition. listOfVertices is the
		//array containing all possible positions in the map.

		if(findpos(actuelposition,possibleposition[0])){
			
			adjacentVertices[0]=&listOfVerticesP2[1]; 
			adjacentVertices[1]=&listOfVerticesP2[17]; 
			adjacentVertices[2]=&listOfVerticesP2[19]; 
		}
		else if(findpos(actuelposition,possibleposition[1])){
			adjacentVertices[0]=&listOfVerticesP2[0]; 
			adjacentVertices[1]=&listOfVerticesP2[2]; 
		}
		else if(findpos(actuelposition,possibleposition[2])){
			adjacentVertices[0]=&listOfVerticesP2[1]; 
			adjacentVertices[1]=&listOfVerticesP2[3]; 
		}
		else if(findpos(actuelposition,possibleposition[3])){
			adjacentVertices[0]=&listOfVerticesP2[2]; 
			adjacentVertices[1]=&listOfVerticesP2[4]; 
			adjacentVertices[2]=&listOfVerticesP2[5]; 
		}
		else if(findpos(actuelposition,possibleposition[4])){
			adjacentVertices[0]=&listOfVerticesP2[3]; 
			adjacentVertices[1]=&listOfVerticesP2[6]; 
		}
		else if(findpos(actuelposition,possibleposition[5])){
			adjacentVertices[0]=&listOfVerticesP2[3]; 
			adjacentVertices[1]=&listOfVerticesP2[7]; 
		}
		else if(findpos(actuelposition,possibleposition[6])){
			adjacentVertices[0]=&listOfVerticesP2[4]; 
			adjacentVertices[1]=&listOfVerticesP2[9]; 
			adjacentVertices[2]=&listOfVerticesP2[26]; 
		}
		else if(findpos(actuelposition,possibleposition[7])){
			adjacentVertices[0]=&listOfVerticesP2[5]; 
			adjacentVertices[1]=&listOfVerticesP2[8]; 
		}
		else if(findpos(actuelposition,possibleposition[8])){
			adjacentVertices[0]=&listOfVerticesP2[7]; 
			adjacentVertices[1]=&listOfVerticesP2[9]; 
		}
		else if(findpos(actuelposition,possibleposition[9])){
			adjacentVertices[0]=&listOfVerticesP2[6]; 
			adjacentVertices[1]=&listOfVerticesP2[8]; 
			adjacentVertices[2]=&listOfVerticesP2[10]; 
			adjacentVertices[3]=&listOfVerticesP2[38]; 
		}
		else if(findpos(actuelposition,possibleposition[10])){
			adjacentVertices[0]=&listOfVerticesP2[9]; 
			adjacentVertices[1]=&listOfVerticesP2[11]; 
			adjacentVertices[2]=&listOfVerticesP2[45]; 
		}
		else if(findpos(actuelposition,possibleposition[11])){
			adjacentVertices[0]=&listOfVerticesP2[10]; 
			adjacentVertices[1]=&listOfVerticesP2[12]; 
			adjacentVertices[2]=&listOfVerticesP2[13]; 
		}
		else if(findpos(actuelposition,possibleposition[12])){
			adjacentVertices[0]=&listOfVerticesP2[11]; 
			adjacentVertices[1]=&listOfVerticesP2[14]; 
		}
		else if(findpos(actuelposition,possibleposition[13])){
			adjacentVertices[0]=&listOfVerticesP2[11]; 
			adjacentVertices[1]=&listOfVerticesP2[14]; 
			adjacentVertices[2]=&listOfVerticesP2[16]; 
			adjacentVertices[3]=&listOfVerticesP2[61]; 
		}
		else if(findpos(actuelposition,possibleposition[14])){
			adjacentVertices[0]=&listOfVerticesP2[12]; 
			adjacentVertices[1]=&listOfVerticesP2[13]; 
			adjacentVertices[2]=&listOfVerticesP2[15]; 
		}
		else if(findpos(actuelposition,possibleposition[15])){
			adjacentVertices[0]=&listOfVerticesP2[14]; 
			adjacentVertices[1]=&listOfVerticesP2[16]; 
		}
		else if(findpos(actuelposition,possibleposition[16])){
			adjacentVertices[0]=&listOfVerticesP2[13]; 
			adjacentVertices[1]=&listOfVerticesP2[15]; 
			adjacentVertices[2]=&listOfVerticesP2[62]; 
		}
		else if(findpos(actuelposition,possibleposition[17])){
			adjacentVertices[0]=&listOfVerticesP2[0]; 
			adjacentVertices[1]=&listOfVerticesP2[25]; 
		}
		else if(findpos(actuelposition,possibleposition[18])){
			adjacentVertices[0]=&listOfVerticesP2[19]; 
			adjacentVertices[1]=&listOfVerticesP2[24];                
		}
		else if(findpos(actuelposition,possibleposition[19])){
			adjacentVertices[0]=&listOfVerticesP2[0]; 
			adjacentVertices[1]=&listOfVerticesP2[18]; 
			adjacentVertices[2]=&listOfVerticesP2[20]; 
		}
		else if(findpos(actuelposition,possibleposition[20])){
			adjacentVertices[0]=&listOfVerticesP2[19]; 
			adjacentVertices[1]=&listOfVerticesP2[21]; 
		}
		else if(findpos(actuelposition,possibleposition[21])){
			adjacentVertices[0]=&listOfVerticesP2[20]; 
			adjacentVertices[1]=&listOfVerticesP2[22]; 
		}
		else if(findpos(actuelposition,possibleposition[22])){
			adjacentVertices[0]=&listOfVerticesP2[21]; 
			adjacentVertices[1]=&listOfVerticesP2[23]; 
			adjacentVertices[2]=&listOfVerticesP2[31]; 
		}
		else if(findpos(actuelposition,possibleposition[23])){
			adjacentVertices[0]=&listOfVerticesP2[22]; 
			adjacentVertices[1]=&listOfVerticesP2[30]; 
		}
		else if(findpos(actuelposition,possibleposition[24])){
			adjacentVertices[0]=&listOfVerticesP2[18]; 
			adjacentVertices[1]=&listOfVerticesP2[29]; 
		}
		else if(findpos(actuelposition,possibleposition[25])){
			adjacentVertices[0]=&listOfVerticesP2[17]; 
			adjacentVertices[1]=&listOfVerticesP2[26]; 
		}
		else if(findpos(actuelposition,possibleposition[26])){
			adjacentVertices[0]=&listOfVerticesP2[6]; 
			adjacentVertices[1]=&listOfVerticesP2[25]; 
			adjacentVertices[2]=&listOfVerticesP2[27]; 
		}
		else if(findpos(actuelposition,possibleposition[27])){
			adjacentVertices[0]=&listOfVerticesP2[26]; 
			adjacentVertices[1]=&listOfVerticesP2[28]; 
			adjacentVertices[2]=&listOfVerticesP2[37]; 
		}
		else if(findpos(actuelposition,possibleposition[28])){
			adjacentVertices[0]=&listOfVerticesP2[27]; 
			adjacentVertices[1]=&listOfVerticesP2[29]; 
			adjacentVertices[2]=&listOfVerticesP2[36];
		}
		else if(findpos(actuelposition,possibleposition[29])){
			adjacentVertices[0]=&listOfVerticesP2[24]; 
			adjacentVertices[1]=&listOfVerticesP2[28]; 
			adjacentVertices[2]=&listOfVerticesP2[30]; 
		}
		else if(findpos(actuelposition,possibleposition[30])){
			adjacentVertices[0]=&listOfVerticesP2[23]; 
			adjacentVertices[1]=&listOfVerticesP2[29]; 
			adjacentVertices[2]=&listOfVerticesP2[34]; 
		}
		else if(findpos(actuelposition,possibleposition[31])){
			adjacentVertices[0]=&listOfVerticesP2[22]; 
			adjacentVertices[1]=&listOfVerticesP2[32]; 
		}
		else if(findpos(actuelposition,possibleposition[32])){
			adjacentVertices[0]=&listOfVerticesP2[31]; 
			adjacentVertices[1]=&listOfVerticesP2[33];
		}
		else if(findpos(actuelposition,possibleposition[33])){
			adjacentVertices[0]=&listOfVerticesP2[32]; 
			adjacentVertices[1]=&listOfVerticesP2[34];
		}
		else if(findpos(actuelposition,possibleposition[34])){
			adjacentVertices[0]=&listOfVerticesP2[30]; 
			adjacentVertices[1]=&listOfVerticesP2[33]; 
			adjacentVertices[2]=&listOfVerticesP2[35]; 
			adjacentVertices[3]=&listOfVerticesP2[42]; 

		}
		else if(findpos(actuelposition,possibleposition[35])){
			adjacentVertices[0]=&listOfVerticesP2[34]; 
			adjacentVertices[1]=&listOfVerticesP2[36]; 
			adjacentVertices[2]=&listOfVerticesP2[41]; 
		}
		else if(findpos(actuelposition,possibleposition[36])){
			adjacentVertices[0]=&listOfVerticesP2[28]; 
			adjacentVertices[1]=&listOfVerticesP2[35]; 
		}
		else if(findpos(actuelposition,possibleposition[37])){
			adjacentVertices[0]=&listOfVerticesP2[27]; 
			adjacentVertices[1]=&listOfVerticesP2[38]; 
		}
		else if(findpos(actuelposition,possibleposition[38])){
			adjacentVertices[0]=&listOfVerticesP2[9]; 
			adjacentVertices[1]=&listOfVerticesP2[37]; 
			adjacentVertices[2]=&listOfVerticesP2[39]; 
		}
		else if(findpos(actuelposition,possibleposition[39])){
			adjacentVertices[0]=&listOfVerticesP2[38]; 
			adjacentVertices[1]=&listOfVerticesP2[40]; 
			adjacentVertices[2]=&listOfVerticesP2[45]; 
		}
		else if(findpos(actuelposition,possibleposition[40])){
			adjacentVertices[0]=&listOfVerticesP2[39]; 
			adjacentVertices[1]=&listOfVerticesP2[41]; 
		}
		else if(findpos(actuelposition,possibleposition[41])){
			adjacentVertices[0]=&listOfVerticesP2[35]; 
			adjacentVertices[1]=&listOfVerticesP2[43]; 
			adjacentVertices[2]=&listOfVerticesP2[40]; 
		}
		else if(findpos(actuelposition,possibleposition[42])){
			adjacentVertices[0]=&listOfVerticesP2[34]; 
			adjacentVertices[1]=&listOfVerticesP2[43]; 
			adjacentVertices[2]=&listOfVerticesP2[54]; 
		}
		else if(findpos(actuelposition,possibleposition[43])){
			adjacentVertices[0]=&listOfVerticesP2[41]; 
			adjacentVertices[1]=&listOfVerticesP2[46]; 
			adjacentVertices[2]=&listOfVerticesP2[42]; 
		}
		else if(findpos(actuelposition,possibleposition[44])){
			adjacentVertices[0]=&listOfVerticesP2[40]; 
			adjacentVertices[1]=&listOfVerticesP2[66]; 
		}
		else if(findpos(actuelposition,possibleposition[45])){
			adjacentVertices[0]=&listOfVerticesP2[10]; 
			adjacentVertices[1]=&listOfVerticesP2[39];
			adjacentVertices[2]=&listOfVerticesP2[49]; 
		}
		else if(findpos(actuelposition,possibleposition[46])){
			adjacentVertices[0]=&listOfVerticesP2[43]; 
			adjacentVertices[1]=&listOfVerticesP2[47]; 
		}
		else if(findpos(actuelposition,possibleposition[47])){
			adjacentVertices[0]=&listOfVerticesP2[46]; 
			adjacentVertices[1]=&listOfVerticesP2[52]; 
			adjacentVertices[2]=&listOfVerticesP2[66]; 
		}
		else if(findpos(actuelposition,possibleposition[48])){
			adjacentVertices[0]=&listOfVerticesP2[49]; 
			adjacentVertices[1]=&listOfVerticesP2[51]; 
			adjacentVertices[2]=&listOfVerticesP2[66]; 
		}
		else if(findpos(actuelposition,possibleposition[49])){
			adjacentVertices[0]=&listOfVerticesP2[45]; 
			adjacentVertices[1]=&listOfVerticesP2[48]; 
		}
		else if(findpos(actuelposition,possibleposition[50])){
			adjacentVertices[0]=&listOfVerticesP2[51]; 
			adjacentVertices[1]=&listOfVerticesP2[61]; 
		}
		else if(findpos(actuelposition,possibleposition[51])){
			adjacentVertices[0]=&listOfVerticesP2[48]; 
			adjacentVertices[1]=&listOfVerticesP2[50]; 
		}
		else if(findpos(actuelposition,possibleposition[52])){
			adjacentVertices[0]=&listOfVerticesP2[47]; 
			adjacentVertices[1]=&listOfVerticesP2[53]; 
		}
		else if(findpos(actuelposition,possibleposition[53])){
			adjacentVertices[0]=&listOfVerticesP2[52]; 
			adjacentVertices[1]=&listOfVerticesP2[58];
		}
		else if(findpos(actuelposition,possibleposition[54])){
			adjacentVertices[0]=&listOfVerticesP2[42]; 
			adjacentVertices[1]=&listOfVerticesP2[55]; 
			adjacentVertices[2]=&listOfVerticesP2[57]; 
		}
		else if(findpos(actuelposition,possibleposition[55])){
			adjacentVertices[0]=&listOfVerticesP2[54]; 
			adjacentVertices[1]=&listOfVerticesP2[56]; 
		}
		else if(findpos(actuelposition,possibleposition[56])){
			adjacentVertices[0]=&listOfVerticesP2[55]; 
			adjacentVertices[1]=&listOfVerticesP2[57]; 
			adjacentVertices[2]=&listOfVerticesP2[65]; 
		}
		else if(findpos(actuelposition,possibleposition[57])){
			adjacentVertices[0]=&listOfVerticesP2[54]; 
			adjacentVertices[1]=&listOfVerticesP2[56]; 
			adjacentVertices[2]=&listOfVerticesP2[58]; 
			adjacentVertices[3]=&listOfVerticesP2[64]; 

		}
		else if(findpos(actuelposition,possibleposition[58])){
			adjacentVertices[0]=&listOfVerticesP2[53]; 
			adjacentVertices[1]=&listOfVerticesP2[57]; 
			adjacentVertices[2]=&listOfVerticesP2[59];
		}
		else if(findpos(actuelposition,possibleposition[59])){
			adjacentVertices[0]=&listOfVerticesP2[58]; 
			adjacentVertices[1]=&listOfVerticesP2[60]; 
			adjacentVertices[2]=&listOfVerticesP2[63]; 
		}
		else if(findpos(actuelposition,possibleposition[60])){
			adjacentVertices[0]=&listOfVerticesP2[59]; 
			adjacentVertices[1]=&listOfVerticesP2[61]; 
			adjacentVertices[2]=&listOfVerticesP2[62]; 
		}
		else if(findpos(actuelposition,possibleposition[61])){
			adjacentVertices[0]=&listOfVerticesP2[13]; 
			adjacentVertices[1]=&listOfVerticesP2[50]; 
			adjacentVertices[2]=&listOfVerticesP2[60]; 
		}
		else if(findpos(actuelposition,possibleposition[62])){
			adjacentVertices[0]=&listOfVerticesP2[16]; 
			adjacentVertices[1]=&listOfVerticesP2[60]; 
		}
		else if(findpos(actuelposition,possibleposition[63])){
			adjacentVertices[0]=&listOfVerticesP2[59]; 
			adjacentVertices[1]=&listOfVerticesP2[64]; 
		}
		else if(findpos(actuelposition,possibleposition[64])){
			adjacentVertices[0]=&listOfVerticesP2[57]; 
			adjacentVertices[1]=&listOfVerticesP2[63]; 
			adjacentVertices[2]=&listOfVerticesP2[65]; 
		}
		else if(findpos(actuelposition,possibleposition[65])){
			adjacentVertices[0]=&listOfVerticesP2[56];
			adjacentVertices[1]=&listOfVerticesP2[64];
		}
		else if(findpos(actuelposition,possibleposition[66])){
			adjacentVertices[0]=&listOfVerticesP2[47];
			adjacentVertices[1]=&listOfVerticesP2[48]; 
		}
	}


	Vector3 PacmanGhost::frontPosition(){
		//Should return the Vector3 point in front of the player. 

		Vector3 neighborPos[4] = {Vector3(-1,-1,-1)};
		Vector3 frontPoint = Vector3(0,-1,0);


		findNeighboorPositions(this->lastPlayerPassedPoint, neighborPos, possibleposition);

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

			if((neighborPos[i]!=Vector3(-1,-1,-1))&&(neighborPos[i].y==10)){
				//y==10 to ignore many unwanted strange positions that pop up otherwise and create SIGSEV

				if(frontPoint==Vector3(0,-1,0)){
					frontPoint=neighborPos[i];

				}
				else if (graphDistance(this->getPlayerPos(), frontPoint)>graphDistance(this->getPlayerPos(), neighborPos[i])){
					frontPoint=neighborPos[i];
				}

			}
		}

		if(frontPoint==Vector3(0,-1,0)){
			//default
			return this->lastPlayerPassedPoint;
		}
		else{
			return frontPoint;
		}
	}






	void PacmanGhost::findNeighboorPositions(Vector3 actuelposition, Vector3 adjacentPositions[], Vector3 positionArray[]){      
		//this function should put in adjacentPositions[] the neighboors of actuelposition.
		// positionArray[] contains all possible positions of the map.

		if(findpos(actuelposition,possibleposition[0])){


			adjacentPositions[0]=positionArray[1];  
			adjacentPositions[1]=positionArray[17]; 
			adjacentPositions[2]=positionArray[19]; 
		}
		else if(findpos(actuelposition,possibleposition[1])){
			adjacentPositions[0]=positionArray[0]; 
			adjacentPositions[1]=positionArray[2]; 
		}
		else if(findpos(actuelposition,possibleposition[2])){
			adjacentPositions[0]=positionArray[1]; 
			adjacentPositions[1]=positionArray[3]; 
		}
		else if(findpos(actuelposition,possibleposition[3])){
			adjacentPositions[0]=positionArray[2]; 
			adjacentPositions[1]=positionArray[4]; 
			adjacentPositions[2]=positionArray[5]; 
		}
		else if(findpos(actuelposition,possibleposition[4])){
			adjacentPositions[0]=positionArray[3]; 
			adjacentPositions[1]=positionArray[6]; 
		}
		else if(findpos(actuelposition,possibleposition[5])){
			adjacentPositions[0]=positionArray[3]; 
			adjacentPositions[1]=positionArray[7]; 
		}
		else if(findpos(actuelposition,possibleposition[6])){
			adjacentPositions[0]=positionArray[4]; ;
			adjacentPositions[1]=positionArray[9]; 
			adjacentPositions[2]=positionArray[26]; 
		}
		else if(findpos(actuelposition,possibleposition[7])){
			adjacentPositions[0]=positionArray[5]; 
			adjacentPositions[1]=positionArray[8]; 
		}
		else if(findpos(actuelposition,possibleposition[8])){
			adjacentPositions[0]=positionArray[7]; 
			adjacentPositions[1]=positionArray[9]; 
		}
		else if(findpos(actuelposition,possibleposition[9])){
			adjacentPositions[0]=positionArray[6]; 
			adjacentPositions[1]=positionArray[8]; 
			adjacentPositions[2]=positionArray[10]; 
			adjacentPositions[3]=positionArray[38]; 
		}
		else if(findpos(actuelposition,possibleposition[10])){
			adjacentPositions[0]=positionArray[9]; 
			adjacentPositions[1]=positionArray[11]; 
			adjacentPositions[2]=positionArray[45]; 
		}
		else if(findpos(actuelposition,possibleposition[11])){
			adjacentPositions[0]=positionArray[10]; 
			adjacentPositions[1]=positionArray[12]; 
			adjacentPositions[2]=positionArray[13]; 
		}
		else if(findpos(actuelposition,possibleposition[12])){
			adjacentPositions[0]=positionArray[11]; 
			adjacentPositions[1]=positionArray[14]; 
		}
		else if(findpos(actuelposition,possibleposition[13])){
			adjacentPositions[0]=positionArray[11]; 
			adjacentPositions[1]=positionArray[14]; 
			adjacentPositions[2]=positionArray[16]; 
			adjacentPositions[3]=positionArray[61]; 
		}
		else if(findpos(actuelposition,possibleposition[14])){
			adjacentPositions[0]=positionArray[12]; 
			adjacentPositions[1]=positionArray[13]; 
			adjacentPositions[2]=positionArray[15]; 
		}
		else if(findpos(actuelposition,possibleposition[15])){
			adjacentPositions[0]=positionArray[14]; 
			adjacentPositions[1]=positionArray[16]; 
		}
		else if(findpos(actuelposition,possibleposition[16])){
			adjacentPositions[0]=positionArray[13]; 
			adjacentPositions[1]=positionArray[15]; 
			adjacentPositions[2]=positionArray[62]; 
		}
		else if(findpos(actuelposition,possibleposition[17])){
			adjacentPositions[0]=positionArray[0]; 
			adjacentPositions[1]=positionArray[25]; 
		}
		else if(findpos(actuelposition,possibleposition[18])){
			adjacentPositions[0]=positionArray[19]; 
			adjacentPositions[1]=positionArray[24];                 
		}
		else if(findpos(actuelposition,possibleposition[19])){
			adjacentPositions[0]=positionArray[0]; 
			adjacentPositions[1]=positionArray[18]; 
			adjacentPositions[2]=positionArray[20]; 
		}
		else if(findpos(actuelposition,possibleposition[20])){
			adjacentPositions[0]=positionArray[19]; 
			adjacentPositions[1]=positionArray[21]; 
		}
		else if(findpos(actuelposition,possibleposition[21])){
			adjacentPositions[0]=positionArray[20]; 
			adjacentPositions[1]=positionArray[22]; 
		}
		else if(findpos(actuelposition,possibleposition[22])){
			adjacentPositions[0]=positionArray[21]; 
			adjacentPositions[1]=positionArray[23]; 
			adjacentPositions[2]=positionArray[31]; 
		}
		else if(findpos(actuelposition,possibleposition[23])){
			adjacentPositions[0]=positionArray[22];
			adjacentPositions[1]=positionArray[30]; 
		}
		else if(findpos(actuelposition,possibleposition[24])){
			adjacentPositions[0]=positionArray[18]; 
			adjacentPositions[1]=positionArray[29]; 
		}
		else if(findpos(actuelposition,possibleposition[25])){
			adjacentPositions[0]=positionArray[17]; 
			adjacentPositions[1]=positionArray[26]; 
		}
		else if(findpos(actuelposition,possibleposition[26])){
			adjacentPositions[0]=positionArray[6]; 
			adjacentPositions[1]=positionArray[25]; 
			adjacentPositions[2]=positionArray[27]; 
		}
		else if(findpos(actuelposition,possibleposition[27])){
			adjacentPositions[0]=positionArray[26]; 
			adjacentPositions[1]=positionArray[28]; 
			adjacentPositions[2]=positionArray[37]; 
		}
		else if(findpos(actuelposition,possibleposition[28])){
			adjacentPositions[0]=positionArray[27]; 
			adjacentPositions[1]=positionArray[29]; 
			adjacentPositions[2]=positionArray[36]; 
		}
		else if(findpos(actuelposition,possibleposition[29])){
			adjacentPositions[0]=positionArray[24]; 
			adjacentPositions[1]=positionArray[28]; 
			adjacentPositions[2]=positionArray[30]; 
		}
		else if(findpos(actuelposition,possibleposition[30])){
			adjacentPositions[0]=positionArray[23]; 
			adjacentPositions[1]=positionArray[29]; 
			adjacentPositions[2]=positionArray[34]; 
		}
		else if(findpos(actuelposition,possibleposition[31])){
			adjacentPositions[0]=positionArray[22]; 
			adjacentPositions[1]=positionArray[32]; 
		}
		else if(findpos(actuelposition,possibleposition[32])){
			adjacentPositions[0]=positionArray[31]; 
			adjacentPositions[1]=positionArray[33]; 
		}
		else if(findpos(actuelposition,possibleposition[33])){
			adjacentPositions[0]=positionArray[32]; 
			adjacentPositions[1]=positionArray[34]; 
		}
		else if(findpos(actuelposition,possibleposition[34])){
			adjacentPositions[0]=positionArray[30]; 
			adjacentPositions[1]=positionArray[33]; 
			adjacentPositions[2]=positionArray[35]; 
			adjacentPositions[3]=positionArray[42]; 

		}
		else if(findpos(actuelposition,possibleposition[35])){
			adjacentPositions[0]=positionArray[34]; 
			adjacentPositions[1]=positionArray[36]; 
			adjacentPositions[2]=positionArray[41]; 
		}
		else if(findpos(actuelposition,possibleposition[36])){
			adjacentPositions[0]=positionArray[28]; 
			adjacentPositions[1]=positionArray[35]; 
		}
		else if(findpos(actuelposition,possibleposition[37])){
			adjacentPositions[0]=positionArray[27]; 
			adjacentPositions[1]=positionArray[38]; 
		}
		else if(findpos(actuelposition,possibleposition[38])){
			adjacentPositions[0]=positionArray[9]; 
			adjacentPositions[1]=positionArray[37]; 
			adjacentPositions[2]=positionArray[39]; 
		}
		else if(findpos(actuelposition,possibleposition[39])){
			adjacentPositions[0]=positionArray[38]; 
			adjacentPositions[1]=positionArray[40]; 
			adjacentPositions[2]=positionArray[45]; 
		}
		else if(findpos(actuelposition,possibleposition[40])){
			adjacentPositions[0]=positionArray[39]; 
			adjacentPositions[1]=positionArray[41]; 
		}
		else if(findpos(actuelposition,possibleposition[41])){
			adjacentPositions[0]=positionArray[35]; 
			adjacentPositions[1]=positionArray[43]; 
			adjacentPositions[2]=positionArray[40];  
		}
		else if(findpos(actuelposition,possibleposition[42])){
			adjacentPositions[0]=positionArray[34]; 
			adjacentPositions[1]=positionArray[43]; 
			adjacentPositions[2]=positionArray[54]; 
		}
		else if(findpos(actuelposition,possibleposition[43])){
			adjacentPositions[0]=positionArray[41]; 
			adjacentPositions[1]=positionArray[46]; 
			adjacentPositions[2]=positionArray[42]; 
		}
		else if(findpos(actuelposition,possibleposition[44])){
			adjacentPositions[0]=positionArray[40]; 
			adjacentPositions[1]=positionArray[66]; 
		}
		else if(findpos(actuelposition,possibleposition[45])){
			adjacentPositions[0]=positionArray[10]; 
			adjacentPositions[1]=positionArray[39]; 
			adjacentPositions[2]=positionArray[49]; 
		}
		else if(findpos(actuelposition,possibleposition[46])){
			adjacentPositions[0]=positionArray[43]; 
			adjacentPositions[1]=positionArray[47]; 
		}
		else if(findpos(actuelposition,possibleposition[47])){
			adjacentPositions[0]=positionArray[46]; 
			adjacentPositions[1]=positionArray[52]; 
			adjacentPositions[2]=positionArray[66]; 
		}
		else if(findpos(actuelposition,possibleposition[48])){
			adjacentPositions[0]=positionArray[49]; 
			adjacentPositions[1]=positionArray[51]; 
			adjacentPositions[2]=positionArray[66]; 
		}
		else if(findpos(actuelposition,possibleposition[49])){
			adjacentPositions[0]=positionArray[45]; 
			adjacentPositions[1]=positionArray[48]; 
		}
		else if(findpos(actuelposition,possibleposition[50])){
			adjacentPositions[0]=positionArray[51]; 
			adjacentPositions[1]=positionArray[61]; 
		}
		else if(findpos(actuelposition,possibleposition[51])){
			adjacentPositions[0]=positionArray[48]; 
			adjacentPositions[1]=positionArray[50]; 
		}
		else if(findpos(actuelposition,possibleposition[52])){
			adjacentPositions[0]=positionArray[47]; 
			adjacentPositions[1]=positionArray[53]; 
		}
		else if(findpos(actuelposition,possibleposition[53])){
			adjacentPositions[0]=positionArray[52]; 
			adjacentPositions[1]=positionArray[58]; 
		}
		else if(findpos(actuelposition,possibleposition[54])){
			adjacentPositions[0]=positionArray[42]; 
			adjacentPositions[1]=positionArray[55]; 
			adjacentPositions[2]=positionArray[57]; 
		}
		else if(findpos(actuelposition,possibleposition[55])){
			adjacentPositions[0]=positionArray[54]; 
			adjacentPositions[1]=positionArray[56]; 
		}
		else if(findpos(actuelposition,possibleposition[56])){
			adjacentPositions[0]=positionArray[55]; 
			adjacentPositions[1]=positionArray[57]; 
			adjacentPositions[2]=positionArray[65]; 
		}
		else if(findpos(actuelposition,possibleposition[57])){
			adjacentPositions[0]=positionArray[54]; 
			adjacentPositions[1]=positionArray[56]; 
			adjacentPositions[2]=positionArray[58]; 
			adjacentPositions[3]=positionArray[64]; 

		}
		else if(findpos(actuelposition,possibleposition[58])){
			adjacentPositions[0]=positionArray[53]; 
			adjacentPositions[1]=positionArray[57]; 
			adjacentPositions[2]=positionArray[59]; 
		}
		else if(findpos(actuelposition,possibleposition[59])){
			adjacentPositions[0]=positionArray[58]; 
			adjacentPositions[1]=positionArray[60]; 
			adjacentPositions[2]=positionArray[63]; 
		}
		else if(findpos(actuelposition,possibleposition[60])){
			adjacentPositions[0]=positionArray[59]; 
			adjacentPositions[1]=positionArray[61]; 
			adjacentPositions[2]=positionArray[62]; 
		}
		else if(findpos(actuelposition,possibleposition[61])){
			adjacentPositions[0]=positionArray[13]; 
			adjacentPositions[1]=positionArray[50]; 
			adjacentPositions[2]=positionArray[60]; 
		}
		else if(findpos(actuelposition,possibleposition[62])){
			adjacentPositions[0]=positionArray[16]; 
			adjacentPositions[1]=positionArray[60]; 
		}
		else if(findpos(actuelposition,possibleposition[63])){
			adjacentPositions[0]=positionArray[59]; 
			adjacentPositions[1]=positionArray[64]; 
		}
		else if(findpos(actuelposition,possibleposition[64])){
			adjacentPositions[0]=positionArray[57]; 
			adjacentPositions[1]=positionArray[63]; 
			adjacentPositions[2]=positionArray[65]; 
		}
		else if(findpos(actuelposition,possibleposition[65])){
			adjacentPositions[0]=positionArray[56]; 
			adjacentPositions[1]=positionArray[64]; 
		}
		else if(findpos(actuelposition,possibleposition[66])){
			adjacentPositions[0]=positionArray[47]; 
			adjacentPositions[1]=positionArray[48]; 
		}
	}


}