MAC0318/HistogramFilter/src/MainProgram.java

import java.awt.BorderLayout;
import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.event.WindowEvent;
import java.awt.event.WindowListener;
import java.util.ArrayList;
import java.util.Iterator;

import javax.swing.JFrame;
import javax.swing.JOptionPane;
import javax.swing.JPanel;
import javax.swing.JSplitPane;
import javax.swing.SwingUtilities;

import config.Map;
import config.Models;
import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.Pose;
import robots.BluetoothRobot;
import robots.DataRead;
import robots.LCPRobot;
import robots.Robot;
import robots.RobotReturn;
import robots.VirtualRobot;

public class MainProgram extends JPanel implements KeyListener, WindowListener, RobotReturn {

	private MapImage imap;
	private Robot robot;
	private ScannerImage scanner;
	private Models model;
	private ProbabilityMatriz matriz;

	public static final byte FORWARD = 0;
	public static final byte STOP = 1;
	public static final byte EXIT = 2;
	public static final byte LEFT = 3;
	public static final byte RIGHT = 4;
	public static final byte BACKWARD = 5;

	public MainProgram(LineMap map, Robot robot) {
		this.robot = robot;
		JFrame frame = new JFrame("Mapa MAC0318");

		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		frame.setLayout(new BorderLayout());
		imap = new MapImage(map);
		scanner = new ScannerImage();
		model = new Models(map);
		// frame.add(this.map);
		frame.setSize(800, 800);
		frame.setVisible(true);

		frame.setFocusable(true);
		frame.requestFocusInWindow();
		frame.addKeyListener(this);
		frame.addWindowListener(this);

		JSplitPane splitPane = new JSplitPane(JSplitPane.HORIZONTAL_SPLIT, scanner, imap);
		splitPane.setOneTouchExpandable(true);
		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
		frame.add(splitPane);

		matriz = new ProbabilityMatriz(map.getBoundingRect(), 10, 9);
		
		crenca();
//		showHelp();
	}

	private void crenca() {
		for (DiscretePoint p: matriz) {
//			System.out.println(n(p.midAng()));
			if (p.z == 4 && p.x == 3 &&  p.y == 3)
				p.set(1);
			else
				p.set(0);
		}
		matriz.normalize();
		imap.showMatriz(matriz);
	}
	
	private double n (double x) {
		x = Math.toRadians(x);
		return Math.toDegrees(Math.atan2(Math.sin(x), Math.cos(x)));
	}

	private void showHelp() {
		String text = "1,2,3 - Change global map view\n";
		text += "l - Show global map trace.\n";
		text += "c - Clean maps.\n";
		text += "m - Enter robot movement.\n";
		text += "r - Enter robo rotation.\n";
		text += "a - Colect sonar data.\n";
		text += "z - Make sonar continuous scanner.\n";
		text += "i - Stop continuous scanner.\n";
		text += "g - Save global map image.\n";
		text += "f - Save scanner image.\n";
		text += "<arrows> - Move robot.\n";
		text += "h - help.\n";
		JOptionPane.showMessageDialog(null, text, "HELP", JOptionPane.PLAIN_MESSAGE);
	}

	public void addPoint(Pose p) {
		imap.addPoint(p);
	}

	@Override
	public void keyPressed(KeyEvent e) {

		char input = e.getKeyChar();
		switch (input) {
		case '1':
			imap.setVisual(0);
			break;
		case '2':
			imap.setVisual(1);
			break;
		case '3':
			imap.setVisual(2);
			break;
		case 'l':
			imap.showLine();
			break;
		case 'g':
			imap.save();
			break;
		case 'f':
			scanner.save();
			break;
		case 'c':
			imap.clean();
			scanner.clean();
			break;
		case 'm':
			moveRobot();
			break;
		case 'r':
			rotateRobot();
			break;
		case 'b':
			crenca();
			break;
		case 'a':
			colectSonar();
			break;
		case 'h':
			showHelp();
			break;
		case 's':
			// reseta a contagem
			System.out.println("resetou");
			matriz.setAll(1);
			matriz.normalize();
			imap.repaint();
			break;
		default:
			break;
		}

		if (robot == null)
			return;

		switch (e.getKeyCode()) {
		case KeyEvent.VK_UP:
			robot.moveForward();
			scanner.clean();
			break;
		case KeyEvent.VK_DOWN:
			robot.moveBackward();
			scanner.clean();
			break;
		case KeyEvent.VK_LEFT:
			robot.moveLeft();
			scanner.clean();
			break;
		case KeyEvent.VK_RIGHT:
			robot.moveRight();
			scanner.clean();
			break;
		}
	}

	private void colectSonar() {
		int interval;
		try {
			String rs = JOptionPane.showInputDialog("Interval (degress):");
			interval = Integer.parseInt(rs);
		} catch (Exception e) {
			return;
		}
		ArrayList<DataRead> data = robot.scann(-90, 90, interval);
		if (data == null) return;

		/* PROBABILIDADES DEPOIS LEITURA */
		for (DiscretePoint p: matriz) {
			Pose pose = p.pose();
			double prob = model.readProbability(pose, data);
			p.set(prob*p.get());
		}
		matriz.normalize();
		imap.setMatriz(matriz);
		
		for (DataRead d : data) {
			robotData(d);
		}
		System.out.println("... "+matriz.sum());
		imap.repaint();
	}

	private void rotateRobot() {
		try {
			String rs = JOptionPane.showInputDialog("Enter rotation (degress-clockwise):");
			double r = Double.parseDouble(rs);
			robot.rotate(r);
		
			rotateAndMoveRobot(0, r);
		} catch (Exception e) {
		}
	}

	private void moveRobot() {
		try {
			String rs = JOptionPane.showInputDialog("Enter distance (cm) - need to bigger than "+matriz.ssize+" cm:");
			double r = Double.parseDouble(rs);
			robot.move(r);
			if (r < matriz.ssize)
				moveRobot();
			else
				rotateAndMoveRobot(r, 0);
		} catch (Exception e) {
		}
	}
	
	private void rotateAndMoveRobot(double move, double ang) {
		/* PROBABILIDADES DEPOIS MOVIMENTO */
		ProbabilityMatriz newmatriz = new ProbabilityMatriz(matriz.bounds, matriz.ssize, matriz.alphad);
		System.out.print("Iniciando...");
		
		// Verifica o numero de cores
		int cores = Runtime.getRuntime().availableProcessors();
		
		// tudo isso
		ArrayList<Thread> threads = new ArrayList<Thread>();
		for (int i = 0; i < cores; i++) {
			Iterator<DiscretePoint> iteretor = newmatriz.iteratorSet((newmatriz.totalsize*i)/cores, newmatriz.totalsize*(i+1)/cores);
			Thread thread = new Thread () {
				public void run() {
					while(iteretor.hasNext()) {
						DiscretePoint newp = iteretor.next();
						double sum = 0;
						for (DiscretePoint p: matriz) {
							Pose pa = p.pose();
							Pose pb = newp.pose();
							sum += model.motionModelOdometry(pa, pb, move, ang)*p.get();
						}
						newp.set(sum);
					}
				}
			};
			thread.start();
			threads.add(thread);
		}
		
		for (Thread thread: threads) {
			try {
				thread.join();
			} catch (InterruptedException e) {
			}
		}
		
		// OU ISSO
//		for (DiscretePoint newp: newmatriz) {
//			double sum = 0;
//			for (DiscretePoint p: matriz) {
//				Pose pa = p.pose();
//				Pose pb = newp.pose();
//				sum += model.motionModelOdometry(pa, pb, move, ang)*p.get();
//			}
//			newp.set(sum);
//		}
			
		matriz = newmatriz;
		imap.setMatriz(matriz);
		matriz.normalize();

		System.out.println("Finalizado "+matriz.sum());

		scanner.clean();
		imap.repaint();
	}


	@Override
	public void keyReleased(KeyEvent arg0) {
		if (robot == null)
			return;
		robot.stop();
	}

	@Override
	public void keyTyped(KeyEvent arg0) {
	}

	@Override
	public void windowOpened(WindowEvent e) {

	}

	@Override
	public void windowClosing(WindowEvent e) {
		System.err.println("Fechando...");
		if (robot == null)
			return;
		robot.disconnect();

	}

	@Override
	public void windowClosed(WindowEvent e) {
	}

	@Override
	public void windowIconified(WindowEvent e) {

	}

	@Override
	public void windowDeiconified(WindowEvent e) {

	}

	@Override
	public void windowActivated(WindowEvent e) {

	}

	@Override
	public void windowDeactivated(WindowEvent e) {
	}

	@Override
	public void robotData(DataRead data) {
		// posicao do robo
//		System.out.print("   sang:"+data.getSensorAngle());
//		System.out.println("   distance:"+data.getDistance());
		scanner.addRead(data.getDistance(), data.getSensorAngle()-90);
	}

	public static void main(String[] args) {

		LineMap map = Map.makeMap();
		Robot robotv = new VirtualRobot(map);
		Robot robotbt = new BluetoothRobot("NXT");
		String s = "LCPRobot";

		Object[] possibleValues = { robotv, robotbt, s };
		Object robottmp = JOptionPane.showInputDialog(null, "Choose one", "Input", JOptionPane.PLAIN_MESSAGE, null,
				possibleValues, possibleValues[0]);
		Robot robot;
		
		
		boolean result = false;
		if (robottmp == s) {
			robot = new LCPRobot();
		} else {
			robot = (Robot) robottmp;
		}

		if (robot != null)
			result = ((Robot) robot).connect();

		if (result == false) {
			JOptionPane.showMessageDialog(null, "Não foi possível conectar ao robô");
			System.exit(ERROR);
		}

		SwingUtilities.invokeLater(new Runnable() {
			public void run() {
				new MainProgram(map, (Robot) robot);
			}
		});
	}
}