package components.robot.mazeresolver;
import java.awt.Color;
import java.util.Random;
import org.eclipse.swt.SWT;
import org.eclipse.swt.widgets.MessageBox;
import utils.defines.Defines;
import utils.images.RecognizedShape;
import utils.logging.Logger;
import components.commchannel.CommunicationChannel;
import components.commchannel.CommunicationChannelException;
import components.features.Feature;
import components.features.cam.Camera;
import components.features.engine.Engine;
import components.features.sensor.Sensor;
import components.gps.imagebased.ImageBasedGPS;
import components.imagecollector.virtual.VirtualImageCollector;
import components.robot.Robot;
import components.robot.RobotException;
import core.simulation.Simulation;
/**
* <code>MazeResolverRobot</code> representa la clase base para cualquier
* robot (real o virtual) capaz de resolver un laberinto. Esta clase implementa
* toda la l�gica de navegaci�n del robot en el laberinto, para lo cual toma
* datos de los <i>features</i> (por ejemplo, sensores) que lo conforman.
* No interesa si estos sensores son simulados por software (en el caso de un
* robot virtual), o si son componentes electr�nicos (en un robot real), como
* tampoco interesa la forma en que se le dan �rdenes al robot.
* Todos esos son detalles que resuelve el canal de comunicaciones asociado
* (solo es necesario conocer el protocolo para enviar y recibir datos a
* trav�s de �l).
*/
public abstract class MazeResolverRobot extends Robot
{
protected org.apache.log4j.Logger log = org.apache.log4j.Logger.getLogger(getClass());
private static final long serialVersionUID = 1L;
enum NAV_STATE{AVANZANDO,
BUSCANDO_PROXIMO_LUGAR,
OBTENIENDO_VISTAS_INICIANDO,
OBTENIENDO_VISTAS_EXPLORANDO,
CENTRANDO_PROXIMO_LUGAR_IZQUIERDA,
CENTRANDO_PROXIMO_LUGAR_DERECHA,
EVITANDO_PARED
};
// Atributos para control de movimiento
protected java.awt.Color viewedColor, lastViewColor;
protected Random randNums;
protected boolean move, leftRotating, rightRotating;
protected int leftRotations, rightRotations;
/* Variables nueva navegacion */
protected static final int UNRECOGNIZED_PLACE = -634;
/**
* Construye un nuevo robot resolvedor de laberintos, con nombre nulo,
* posici�n inicial (0.0,0.0), y direcci�n inicial 0.0 rad.
*/
public MazeResolverRobot()
{
super();
randNums = new Random();
move = true;
leftRotating = false;
rightRotating = false;
leftRotations = 0;
rightRotations = 0;
// statusTable.put(new Integer(Defines.STATE_ROBOT_NOT_CALIBRATED),"Sin posicionar");
// statusTable.put(new Integer(Defines.STATE_ROBOT_CALIBRATED), "Posicionado!");
}
// private final double computeDeltaNewCourse(int currentPlace)
// {
// RobotState state = getState();
//
// double newCourse = 0.0;
// int nextPlaceId;
//
// if ( currentPlace != neuralNet.getCurrentPlace() )
// {
// // La red neuronal "piensa" que estamos en un lugar, pero en
// // realidad, el robot est� en otro lugar diferente. Esto puede
// // suceder por las imperfecciones en la navegaci�n del robot.
// // TODO: ��� c�mo manejar esta situaci�n ???
// Logger.warn( "Lugar actual en la red neuronal: " +
// neuralNet.getCurrentPlace() +
// " Lugar actual real del robot: " +
// currentPlace );
// }
//
// neuralNet.move( Simulation.getCurrent().getPlaces()[neuralNet.getCurrentPlace()-1] , currentPlace );
// nextPlaceId = neuralNet.getCurrentPlace();
//
// Logger.info( "Actual: " + currentPlace + " Pr�ximo: " + nextPlaceId );
//
// // Busca el lugar pr�ximo en el arreglo de �conos coloreados.
// boolean encontrado = false;
// for (int i=0; !encontrado && i<Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons.length; i++)
// {
// RecognizedShape icon = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons[i];
// if ( icon.shapeId.equals(String.valueOf(nextPlaceId)) )
// {
// // Establece el nuevo curso
// newCourse = Math.atan2( icon.y-state.position[1] , icon.x-state.position[0] ) - state.course;
// encontrado = true;
// }
// }
//
// if ( !encontrado )
// {
// // TODO: qu� hacer si no se encuentra el �cono coloreado?
// // Por ahora, se mantiene el mismo curso.
// Logger.error( "No se encontr� el �cono coloreado con id=" + nextPlaceId );
// }
//
// return newCourse;
// }
public abstract void move() throws RobotException;
/**
* move original de EasyBot
*/
// @Deprecated
// private final void _move_original() throws RobotException
// {
//
// // Avanza
// if ( move )
// {
// try
// {
// advance();
// updateState();
// }
// catch (RobotException e)
// {
// Logger.error( e.getMessage() );
// }
// }
//
// move = false;
//
// if ( firstMovement )
// {
// turnRight( computeDeltaNewCourse( 1 ) );
// firstMovement = false;
// }
// else if ( leftRotating )
// {
// // Est� rotando a izquierda
//
// boolean aux = ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue();
//
// if ( !aux )
// {
// // Libr�
// leftRotating = false;
//
// if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
// {
// // Ahora empieza a girar a derecha
// rightRotating = true;
//
// turnRight( Defines.DELTA_PHI_ROTATIONS );
// rightRotations++;
// }
// else
// {
// // Simulaci�n real. Entonces empieza a avanzar.
// leftRotations = 0;
// move = true;
// }
// }
// else
// {
// turnLeft( Defines.DELTA_PHI_ROTATIONS );
// leftRotations++;
// }
// }
// else if ( rightRotating )
// {
// // Est� girando a derecha
//
// if ( ! ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() )
// {
// // Libr�
// leftRotating = false;
// rightRotating = false;
//
// rightRotations -= leftRotations;
//
// if ( leftRotations < rightRotations )
// {
// // Gira a izquierda
// for (int i=0; i<leftRotations+rightRotations/*+1*/; i++)
// turnLeft( Defines.DELTA_PHI_ROTATIONS );
// }
//
// leftRotations = 0;
// rightRotations = 0;
// }
// else
// {
// turnRight( Defines.DELTA_PHI_ROTATIONS );
// rightRotations++;
// }
// }
// else if ( (viewedColor=(java.awt.Color)((Sensor)commChannel.receive().get(CommunicationChannel.COLOR_SENSOR_NAME)).getValue()) != null &&
// ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() &&
// ( lastViewColor == null || !viewedColor.equals(lastViewColor)) )
// {
// // Est� percibiendo un color.
//
// int currentPlace = -1;
// RecognizedShape[] icons = Simulation.getCurrent().getGps().getMazeItems().recognizedColoredIcons;
// for (int i=0; currentPlace==-1 && i<icons.length; i++)
// if ( icons[i].color.equals(viewedColor) )
// currentPlace = Integer.valueOf(icons[i].shapeId).intValue();
//
// int[][] places = Simulation.getCurrent().getPlaces();
// if ( currentPlace != -1 && places != null )
// {
// // Calcula el nuevo curso
// turnRight( computeDeltaNewCourse( currentPlace ) );
//
// if ( currentPlace == Simulation.getCurrent().getTargetPlace() )
// // Se lleg� al objetivo
// reset();
// }
// else
// {
// // TODO: ver c�mo manejar la situaci�n en que no
// // se puede resolver el lugar
//
// // Por ahora, asume que si no puede resolver el lugar,
// // contin�a avanzando hacia una posici�n aleatoria
// turnRight( randNums.nextDouble() * 2.0*Math.PI );
// }
//
// }
// else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.FRONT_SENSOR_NAME)).getValue()).booleanValue() )
// {
// // Est� percibiendo una pared en el frente.
// // Comienza el proceso de rotaci�n.
//
// leftRotating = true;
// rightRotating = false;
//
// assert leftRotations == 0 : "leftRotations = "+leftRotations;
//
// turnLeft( Defines.DELTA_PHI_ROTATIONS );
// leftRotations++;
// }
// else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.LEFT_SENSOR_NAME)).getValue()).booleanValue() )
// {
// // Est� percibiendo una pared en la izquierda
//
// turnRight( randNums.nextDouble() * 0.5*Defines.DELTA_PHI_ROTATIONS );
//
// /*
// // SOLO AVANZAR EN CONDICIONES NORMALES
// if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
// move = true;
// */
// }
// else if ( ((Boolean)((Sensor)commChannel.receive().get(CommunicationChannel.RIGHT_SENSOR_NAME)).getValue()).booleanValue() )
// {
// // Est� percibiendo una pared en la derecha
//
// turnLeft( randNums.nextDouble() * 0.5*Defines.DELTA_PHI_ROTATIONS );
//
// /*
// // SOLO AVANZAR EN CONDICIONES NORMALES
// if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
// move = true;
// */
// }
// else
// {
// // En condiciones normales, avanza.
// move = true;
// }
//
// }
/**
* Reinicializa el robot a su estado original, y se invoca al m�todo
* {@link components.neuralnetwork.NeuralNetwork#reset()
* reset()} de la red neuronal.
* @throws RobotException
*/
public void reset() throws RobotException
{
// Se reinicializa la red neuronal.
if ( neuralNet != null )
neuralNet.reset();
if ( Simulation.getCurrent().getType() == Defines.VIRTUAL_SIMULATION )
{
// La simulaci�n es virtual (por lo tanto, el robot
// tambi�n lo es). Entonces, se lo vuelve a la posici�n
// original.
((VirtualImageCollector)((ImageBasedGPS) Simulation.getCurrent().getGps()).getImageCollector())
.drawRobot(
new Color(color.getRed(), color.getGreen(), color.getBlue()) ,
new double[] {initPosition[0], initPosition[1]} );
firstUpdate = true;
updateState();
Logger.info( "El robot virtual lleg� al objetivo." );
}
else
{
// TODO: qu� hacer si se lleg� al objetivo, y es
// un robot real ???
Logger.info( "El robot real lleg� al objetivo." );
Simulation.getCurrent().stop();
Simulation.getCurrent().infoMessage("El robot real lleg� al objetivo.");
}
firstMovement = true;
}
// TODO: ver este m�todo!!! (deber�a estar implementado de una manera
// m�s gen�rica en la clase ancestra Robot).
public void addFeature(Feature feature) throws RobotException
{
if ( features == null )
features = new Feature[0];
// Append
Feature[] temp = features;
features = new Feature[ temp.length + 1 ];
for (int i=0; i<temp.length; i++)
features[i] = temp[i];
features[ features.length - 1 ] = feature;
try
{
commChannel.registerFeature( feature );
}
catch (CommunicationChannelException e)
{
Logger.error( e.getMessage() );
throw new RobotException( e.getMessage() );
}
}
public String getXMLConfig(String ident)
{
String xmlConfig = new String();
String tabulator = new String();
tabulator = ident;
xmlConfig = xmlConfig.concat(tabulator + "<robot name=\""+ name + "\" description=\""+ description +"\">" + Defines.CR);
tabulator = ident + "\t";
xmlConfig = xmlConfig.concat(tabulator+ "<color>" + Defines.CR);
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator+ "<r>"+color.getRed()+"</r>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<g>"+color.getGreen()+"</g>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<b>"+color.getBlue()+"</b>" + Defines.CR);
tabulator = ident + "\t";
xmlConfig = xmlConfig.concat(tabulator+ "</color>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<position>"+Defines.CR);
tabulator= ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator + "<x>"+initPosition[0]+"</x>" + Defines.CR);
xmlConfig = xmlConfig.concat(tabulator + "<y>"+initPosition[1]+"</y>" + Defines.CR);
tabulator= ident + "\t";
xmlConfig = xmlConfig.concat(tabulator+ "</position>" + Defines.CR);
xmlConfig = xmlConfig.concat(commChannel.getXMLConfig(tabulator)+Defines.CR);
xmlConfig = xmlConfig.concat(neuralNet.getXMLConfig(tabulator)+Defines.CR);
xmlConfig = xmlConfig.concat(tabulator+ "<features>"+Defines.CR);
tabulator = ident + "\t\t";
//Bloque de Sensores
xmlConfig = xmlConfig.concat(tabulator+ "<sensors>"+Defines.CR);
tabulator = ident + "\t\t\t";
for (int i=0 ; i<features.length;i++)
{
if (features[i] instanceof Sensor)
xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator));
}
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator+ "</sensors>"+Defines.CR);
//Bloques para engines
xmlConfig = xmlConfig.concat(tabulator+ "<engines>"+Defines.CR);
tabulator = ident + "\t\t\t";
for (int i=0 ; i<features.length;i++)
{
if (features[i] instanceof Engine)
xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator)+Defines.CR);
}
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator+ "</engines>"+Defines.CR);
//Bloques para camaras
xmlConfig = xmlConfig.concat(tabulator+ "<cameras>"+Defines.CR);
tabulator = ident + "\t\t\t";
for (int i=0 ; i<features.length;i++)
{
if (features[i] instanceof Camera)
xmlConfig = xmlConfig.concat(features[i].getXMLConfig(tabulator)+Defines.CR);
}
tabulator = ident + "\t\t";
xmlConfig = xmlConfig.concat(tabulator + "</cameras>"+Defines.CR);
//TODO:Bloques para misc
tabulator = ident + "\t";
xmlConfig = xmlConfig.concat(tabulator + "</features>" + Defines.CR);
xmlConfig = xmlConfig.concat("</robot>");
return xmlConfig;
}
}