import java.awt.Color;
import java.awt.Graphics;
import java.util.ArrayList;
import java.util.concurrent.Semaphore;

import javax.swing.JPanel;

import lejos.geom.Line;
import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.Pose;

public class ScannerImage extends JPanel {
	private Pose pose;
	private LineMap map;
	private ArrayList<SonarRead> lista_leituras;
	private Semaphore semaphore;
	
	class SonarRead {
		public double distance;
		public double ang;
		SonarRead (double distance, double ang) {
			this.ang = ang;
			this.distance = distance;
		}
	}
	
    @Override
    protected void paintComponent(Graphics g) {
    		int w = this.getWidth()/2;
    		int h = this.getHeight();
    		int distance = h*2/25;
    		g.setColor(new Color(0f, 0f, 0f, 0.4f));
    		for (int i = 1; i <= 18; i++) {
    			int r = distance * i;
        		g.drawArc(w-r, h-r, 2*r, 2*r, 0, 180);
        		g.drawString(new Integer(i*20).toString(), w-7, h-r);
    		}
    		g.setColor(new Color(0f, 0f, 0f, 0.5f));
    		for (int i = 1; i < 6; i++) {
    			int lw = (int)(Math.cos(Math.PI/6.0*i)*distance * 18);
    			int lh = (int)(Math.sin(Math.PI/6.0*i)*distance * 18);
    			g.drawLine(w, h, lw+w, h-lh);
    		}
    		g.setColor(new Color(0f, 0f, 0f, 0.1f));
    		for (int i = 1; i < 12; i++) {
    			int lw = (int)(Math.cos(Math.PI/12.0*i)*distance * 18);
    			int lh = (int)(Math.sin(Math.PI/12.0*i)*distance * 18);
    			g.drawLine(w, h, lw+w, h-lh);
    		}
    		

    		if (semaphore.tryAcquire()) {
    			double d = h*2.0/25.0/20.0;
    			
    			g.setColor(new Color(0f, 1f, 0f));
		    	if (map != null && pose!= null) {
		    		Line[] lines = map.getLines();
		    		for (int i = 0; i < lines.length; i++) {
		    			Line l = lines[i];
		    			double sin = Math.sin(Math.toRadians(pose.getHeading()-90));
		    			double cos = Math.cos(Math.toRadians(pose.getHeading()-90));
		            g.drawLine(
			                (int)(w+(l.x1/10.0-pose.getX())*d*sin),
			                (int)(h-(l.y1/10.0-pose.getY())*d*cos), 
			                (int)(w+(l.x2/10.0-pose.getX())*d*sin),
			                (int)(h-(l.y2/10.0-pose.getY())*d*cos)
			        );
		    		}
		    	}
    		
    			drawDots(g, w, h);
    	        semaphore.release();
    		}
    	}

	private void drawDots(Graphics g, int w, int h) {
		g.setColor(new Color(1f, 0f, 0f));
		int oval_size = 16;
		int distance = h*2/25;
		double d = distance/20.0;
		double a = -oval_size/4;
		for (SonarRead r: lista_leituras) {
			double x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
			double y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
			x = w - x;
			y = h - y;
		g.fillOval((int)(x-oval_size/2.0), (int)(y-oval_size/2.0), oval_size/2, oval_size/2);
		}
		if (map == null) return;
	}
   

	public Pose getPose() {
		return pose;
	}

	public void addRead(Pose p, double distance, double ang) {
		if (semaphore.tryAcquire()) {
			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
					&& p.getHeading() == pose.getHeading())) {
				pose = new Pose(p.getX(), p.getY(), p.getHeading());
				lista_leituras.clear();
			}
			lista_leituras.add(new SonarRead(distance, ang+90));
	        semaphore.release();
		}
		repaint();
	}
	
	public ScannerImage(LineMap map) {
		this.map = map;
		semaphore = new Semaphore(1);
		lista_leituras =  new ArrayList<SonarRead>();
	}
}