package version7_2_8.environment;

import javax.vecmath.Vector3f;

/**
 * representation of a physical agent in its environment
 * @author simon gay
 */
public class Robot {

	private Environment env;

	private Vector3f mPosition;     // position (x,y,z) in grid cell unit
	private Vector3f mOrientation;  // orientation (rx,ry,rz) in radius
	
	//////////////////////////////////////////////////////////////////////
	// sensors and "leds"
	public int[] sensors;   // [0] bump , [1] turn , [2] touch , [3] left , [4] right , [5] reach
	public boolean[] display;
	//--------------------------------------------------------------------
	
	public Robot(){
		
		// initialization of position vectors
		mPosition=new Vector3f();
		mOrientation=new Vector3f();
		
		// initialization of sensors
		sensors=new int[6];
		for(int i=0;i<6;i++){
			sensors[i]=0;
		}
		
		display=new boolean[3];
		display[0]=false;
		display[1]=false;
		display[2]=false;
	}
	
	public void setEnvironment(Environment e){
		env=e;
	}
	
	// activate the robot
	public void move(double[] act){
		
		////////////////////////////////////////////////////////////////////
		// movements
		double orientation=mOrientation.z;
		
		mPosition.x+=  act[0]*Math.sin(orientation)+act[1]*Math.cos(orientation);
		mPosition.y+=  act[0]*Math.cos(orientation)+act[1]*Math.sin(orientation);
		
		mOrientation.z+=act[2];
		if (mOrientation.z<0) mOrientation.z+=2*Math.PI;
		if (mOrientation.z>=2*Math.PI) mOrientation.z-=2*Math.PI;
		
		for(int k=0;k<6;k++){
			sensors[k]=0;
		}
		if (act[0]!=0 || act[1]!=0) sensors[0]=1;
		
		////////////////////////////////////////////////////////////////////
		// collisions
		
		int i=Math.round(mPosition.x);
		int j=Math.round(mPosition.y);
		
		
		for (int w=-1;w<=1;w++){
			for (int h=-1;h<=1;h++){
				if (i!=0 && j!=0){
					
					if (i+w>=0 && i+w<env.m_w && j+h>=0 && j+h<env.m_h){
						if (!env.isWalkthroughable(i+w,j+h)){
							// case lateral block
							if (w==0){
								if ((j+h - mPosition.y)*h<1){
									mPosition.y-= h*(1 - h*(j+h - mPosition.y));
									sensors[0]=2;
								}
							}
							else{ 
								if (h==0){
									if ((i+w - mPosition.x)*w<1){
										mPosition.x-= w*(1 - w*(i+w - mPosition.x));
										sensors[0]=2;
									}
								}
								else{
									// case corner block
									float x2= (i+w)-w*0.5f;
									float y2= (j+h)-h*0.5f;
									
									double dist=Math.sqrt((mPosition.x-x2)*(mPosition.x-x2)+(mPosition.y-y2)*(mPosition.y-y2));

									if (dist<0.5){
										sensors[0]=2;
										mPosition.x+= (mPosition.x-x2) * (0.5-(dist-0.05)) /0.5;
										mPosition.y+= (mPosition.y-y2) * (0.5-(dist-0.05)) /0.5;
									}
								}
							}
						}
					}
					
				}
			}
		}	
		
		if (sensors[0]==2){
			try{Thread.currentThread();
				Thread.sleep(20);}
			catch(Exception ie){}
		}/**/
		
		////////////////////////////////////////////////////////////////////
		// feel neighbor blocks
		display[0]=false;
		display[1]=false;
		display[2]=false;
		if (act[3]==1){                    // feel forward
			if (mOrientation.z>=  Math.PI/4 && mOrientation.z<3*Math.PI/4){
				if (env.isWall(i,j+1)) sensors[2]=2;
			}
			if (mOrientation.z>=5*Math.PI/4 && mOrientation.z<7*Math.PI/4){
				if (env.isWall(i,j-1)) sensors[2]=2;
			}
			if (mOrientation.z>=7*Math.PI/4 || mOrientation.z<  Math.PI/4){
				if (env.isWall(i+1,j)) sensors[2]=2;
			}
			if (mOrientation.z>=3*Math.PI/4 && mOrientation.z<5*Math.PI/4){
				if (env.isWall(i-1,j)) sensors[2]=2;
			}
			if (sensors[2]==0) sensors[2]=1;
			display[0]=true;
		}
		if (act[3]==2){                    // feel left
			if (mOrientation.z>=  Math.PI/4 && mOrientation.z<3*Math.PI/4){
				if (env.isWall(i-1,j)) sensors[2]=2;
			}
			if (mOrientation.z>=5*Math.PI/4 && mOrientation.z<7*Math.PI/4){
				if (env.isWall(i+1,j)) sensors[2]=2;
			}
			if (mOrientation.z>=7*Math.PI/4 || mOrientation.z<  Math.PI/4){
				if (env.isWall(i,j+1)) sensors[2]=2;
			}
			if (mOrientation.z>=3*Math.PI/4 && mOrientation.z<5*Math.PI/4){
				if (env.isWall(i,j-1)) sensors[2]=2;
			}
			if (sensors[2]==0) sensors[2]=1;
			display[1]=true;
		}
		if (act[3]==3){                    // feel right
			if (mOrientation.z>=  Math.PI/4 && mOrientation.z<3*Math.PI/4){
				if (env.isWall(i+1,j)) sensors[2]=2;
			}
			if (mOrientation.z>=5*Math.PI/4 && mOrientation.z<7*Math.PI/4){
				if (env.isWall(i-1,j)) sensors[2]=2;
			}
			if (mOrientation.z>=7*Math.PI/4 || mOrientation.z<  Math.PI/4){
				if (env.isWall(i,j-1)) sensors[2]=2;
			}
			if (mOrientation.z>=3*Math.PI/4 && mOrientation.z<5*Math.PI/4){
				if (env.isWall(i,j+1)) sensors[2]=2;
			}
			if (sensors[2]==0) sensors[2]=1;
			display[2]=true;
		}
		
		// uncomment for delay
		
		if (act[3]>0){
			try{Thread.currentThread();
				Thread.sleep(10);}
			catch(Exception ie){}
		}/**/
	}
	
	
	// finalize the movement
	public void center(){
		
		// feel block under the agent
		for (int i=-1;i<=1;i++){
			for (int j=-1;j<=1;j++){
				
				if (env.isFood(Math.round(mPosition.x)+i,Math.round(mPosition.y)+j)){
					double d=Math.sqrt( (Math.round(mPosition.x)+i-mPosition.x )*(Math.round(mPosition.x)+i-mPosition.x )
							           +(Math.round(mPosition.y)+j-mPosition.y )*(Math.round(mPosition.y)+j-mPosition.y ));
					
					if (d<=0.8){
						
						sensors[5]=1;
						env.setBlock(Math.round(mPosition.x)+i,Math.round(mPosition.y)+j,Environment.empty);
						
						// place randomly a new agent
						/*int rx=(int) (Math.random()*env.m_w);
						int ry=(int) (Math.random()*env.m_h);
						
						while (!env.isEmpty(rx,ry)){
							rx=(int) (Math.random()*env.m_w);
							ry=(int) (Math.random()*env.m_h);
						}
						env.setBlock(rx,ry,Environment.fish);
						/**/
					}
				}
			}
		}
	}
	
	
	///////////////////////////////////////////////////////////////////////////////
	
	public Vector3f getPosition(){
		return mPosition;
	}
	public float getpx(){
		return mPosition.x;
	}
	public float getpy(){
		return mPosition.y;
	}
	
	public void setPosition(Vector3f p){
		mPosition=p;
	}
	
	public Vector3f getOrientation(){
		return mOrientation;
	}
	public void setOrientation(Vector3f o){
		mOrientation=o;
	}
	
	public void reset(){
		for(int k=0;k<6;k++){
			sensors[k]=0;
		}
	}
}
