package version7_3_simu.sensorySystem;

import java.awt.Color;
import java.util.ArrayList;

import version7_3_simu.platform.Agent;

public class ColorRecognition {

	//public ArrayList<Color> red;
	//public ArrayList<Color> green;
	//public ArrayList<Color> blue;
	//public ArrayList<Color> white;
	
	int color=0;
	
	public int[][] colorMap;
	public int[][] colorMap2;
	public float[] distances;
	
	public Agent agent;
	
	public static int dMin=100;
	
	public ColorRecognition(Agent a){
		//red=new ArrayList<Color>();
		//green=new ArrayList<Color>();
		//blue=new ArrayList<Color>();
		//white=new ArrayList<Color>();
		
		colorMap=new int[640][480];
		colorMap2=new int[640][480];
		distances=new float[500];
		
		agent=a;
	}
	
	public void setColor(int c){
		color=c;
		System.out.println("+++ "+color);
	}
	
	/*public void addColor(Color c){
		if (color==1) red.add(c);
		if (color==2) green.add(c);
		if (color==3) blue.add(c);
		if (color==4) white.add(c);
		
		System.out.println("colors : "+red.size()+", "+green.size()+", "+blue.size()+", "+white.size());
	}
	
	public void removeLast(){
		if (color==1 && red.size()>0) red.remove(red.size()-1);
		if (color==2 && green.size()>0) green.remove(green.size()-1);
		if (color==3 && blue.size()>0) blue.remove(blue.size()-1);
		if (color==4 && white.size()>0) white.remove(white.size()-1);
	}*/
	
	
	public void getColorMap(){
		for (int i=0;i<640;i++){
			for (int j=0;j<480;j++){
				colorMap[i][j]=getFilteredColor(i,j);
			}
		}
		improveImage();
		getDistanceMap();
	}
	
	
	private void getDistanceMap(){
		double d;
		double theta;
		int x,y;
		
		for (int i=0;i<500;i++){
			distances[i]=500;
		}
		
		
		// compute minimum distances
		for (int i=0;i<640;i++){
			for (int j=0;j<480;j++){
				
				if (colorMap[i][j]!=-1 && colorMap[i][j]!=3){
				
					x=i-320;
					y=j-405;
					d=Math.sqrt( x*x+y*y );
					
					if (d<dMin) colorMap2[i][j]=colorMap[i][j];
					else{
						// compute angle;
						theta=Math.atan2(y, x)+Math.PI;
						theta= theta*500/(2*Math.PI);
						
						if (d<distances[(int)theta]) distances[(int)theta]=(float) d;
					}
				}
			}
		}
		
		// remove hidden elements
		for (int i=0;i<640;i++){
			for (int j=0;j<480;j++){
				
				if (colorMap[i][j]!=-1 && colorMap[i][j]!=3){
				
					x=i-320;
					y=j-405;
					d=Math.sqrt( x*x+y*y );
					
					if (d<dMin) colorMap2[i][j]=colorMap[i][j];
					else{
						// compute angle;
						theta=Math.atan2(y, x)+Math.PI;
						theta= theta*500/(2*Math.PI);
						
						if (d < distances[(int)theta]+18) colorMap2[i][j]=colorMap[i][j];
						else colorMap2[i][j]=-1;
					}
				}
				else colorMap2[i][j]=-1;
			}
		}
	}
	
	private void improveImage(){
		
		int[][] improved=new int[640][480];
		for (int i=1;i<639;i++){
			for (int j=1;j<479;j++){
				
				int c=colorMap[i][j];
				int nb=0;
				for (int i2=-1;i2<=1;i2++){
					for (int j2=-1;j2<=1;j2++){
						if (colorMap[i+i2][j+j2]==c) nb++;
					}
				}
				if (nb==9) improved[i][j]=c;
				else improved[i][j]=-1;
			}
		}
		for (int i=2;i<639;i++){
			for (int j=2;j<479;j++){
				colorMap[i][j]=improved[i][j];
			}
		}
	}
	
	
	private int getFilteredColor(int x, int y){
		int ret=3;
		
		if (agent.webcam.image[x][y][0]>agent.webcam.image[x][y][1]+20
		&&  agent.webcam.image[x][y][0]>agent.webcam.image[x][y][2]+20){
			ret=0;
		}
		if (agent.webcam.image[x][y][1]>agent.webcam.image[x][y][0]+20
		&&  agent.webcam.image[x][y][1]>agent.webcam.image[x][y][2]+20){
			ret=1;
		}
		if (agent.webcam.image[x][y][2]>agent.webcam.image[x][y][0]+20
		&&  agent.webcam.image[x][y][2]>agent.webcam.image[x][y][1]+20){
			ret=2;
		}
		
		return ret;
	}
	
	
	/*private int getMinDistance(int x, int y){
		
		float min=4000;
		int ret=0;
		
		for (int i=0;i<red.size();i++){
			float d= (agent.webcam.image[x][y][0]-red.get(i).getRed())*(agent.webcam.image[x][y][0]-red.get(i).getRed())
			        +(agent.webcam.image[x][y][1]-red.get(i).getGreen())*(agent.webcam.image[x][y][1]-red.get(i).getGreen())
			        +(agent.webcam.image[x][y][2]-red.get(i).getBlue())*(agent.webcam.image[x][y][2]-red.get(i).getBlue());
			if (d<min){
				ret=1;
				min=d;
			}
		}
		for (int i=0;i<green.size();i++){
			float d= (agent.webcam.image[x][y][0]-green.get(i).getRed())*(agent.webcam.image[x][y][0]-green.get(i).getRed())
			        +(agent.webcam.image[x][y][1]-green.get(i).getGreen())*(agent.webcam.image[x][y][1]-green.get(i).getGreen())
			        +(agent.webcam.image[x][y][2]-green.get(i).getBlue())*(agent.webcam.image[x][y][2]-green.get(i).getBlue());
			if (d<min){
				ret=2;
				min=d;
			}
		}
		for (int i=0;i<blue.size();i++){
			float d= (agent.webcam.image[x][y][0]-blue.get(i).getRed())*(agent.webcam.image[x][y][0]-blue.get(i).getRed())
			        +(agent.webcam.image[x][y][1]-blue.get(i).getGreen())*(agent.webcam.image[x][y][1]-blue.get(i).getGreen())
			        +(agent.webcam.image[x][y][2]-blue.get(i).getBlue())*(agent.webcam.image[x][y][2]-blue.get(i).getBlue());
			if (d<min){
				ret=3;
				min=d;
			}
		}
		for (int i=0;i<white.size();i++){
			float d= (agent.webcam.image[x][y][0]-white.get(i).getRed())*(agent.webcam.image[x][y][0]-white.get(i).getRed())
			        +(agent.webcam.image[x][y][1]-white.get(i).getGreen())*(agent.webcam.image[x][y][1]-white.get(i).getGreen())
			        +(agent.webcam.image[x][y][2]-white.get(i).getBlue())*(agent.webcam.image[x][y][2]-white.get(i).getBlue());
			if (d<min){
				ret=4;
				min=d;
			}
		}
		
		if (ret==1) return 0;
		else if (ret==2) return 1;
		else if (ret==3) return 2;
		else if (ret==4) return 3;
		else return -1;
	}*/
	
}
