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;
		public double expected;
		SonarRead (double distance, double ang, double expected) {
			this.ang = ang;
			this.distance = distance;
			this.expected = expected;
		}
	}
	
    @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()-180));
		    			double cos = Math.cos(-Math.toRadians(pose.getHeading()-180));
		    			
		    			double x1 = (l.x1/10.0-pose.getX())*d;
		    			double y1 = (l.y1/10.0-pose.getY())*d;
		    			
		    			double x2 = (l.x2/10.0-pose.getX())*d;
		    			double y2 = (l.y2/10.0-pose.getY())*d;
		    			
		    			double xx1 = x1*cos-y1*sin;
		    			double yy1 = y1*cos+x1*sin;

		    			double xx2 = x2*cos-y2*sin;
		    			double yy2 = y2*cos+x2*sin;
		    			
		            g.drawLine(
			                (int)(w+xx1),
			                (int)(h-yy1), 
			                (int)(w+xx2),
			                (int)(h-yy2)
			        );
		    		}
		    	}
    		
    			drawDots(g, w, h);
    	        semaphore.release();
    		}
    	}
    


	private void drawDots(Graphics g, int w, int h) {
		int oval_size = 16;
		int distance = h*2/25;
		double d = distance/20.0;
		double a = -oval_size/4;
		double x, y;
		
		g.setColor(new Color(1f, 0f, 0f));
		for (SonarRead r: lista_leituras) {
			x = a+(d*r.distance)*Math.sin(Math.toRadians(r.ang));
			y = a+(d*r.distance)*Math.cos(Math.toRadians(r.ang));
			x = w - x;
			y = h - y;
			g.setColor(new Color(1f, 0f, 0f));
			g.fillOval((int)(x-oval_size/2.0), (int)(y-oval_size/2.0), oval_size/2, oval_size/2);
			
			x = a+(d*r.expected)*Math.sin(Math.toRadians(r.ang));
			y = a+(d*r.expected)*Math.cos(Math.toRadians(r.ang));
			x = w - x;
			y = h - y;
			g.setColor(new Color(0f, 0f, 1f));
			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, double expected) {
		if (semaphore.tryAcquire()) {
			if (pose == null || !(p.getX() == pose.getX() && p.getY() == pose.getY() 
					&& p.getHeading() == pose.getHeading()) || lista_leituras.size() >= 360) {
				pose = new Pose(p.getX(), p.getY(), p.getHeading());
				lista_leituras.clear();
			}
			lista_leituras.add(new SonarRead(distance, ang+90, expected));
	        semaphore.release();
		}
		repaint();
	}
	
	public ScannerImage(LineMap map) {
		this.map = map;
		semaphore = new Semaphore(1);
		lista_leituras =  new ArrayList<SonarRead>();
	}
}
