package components.robot;
import java.io.EOFException;
import java.util.HashMap;
import org.eclipse.swt.SWT;
import org.eclipse.swt.graphics.Color;
import org.eclipse.swt.graphics.GC;
import org.eclipse.swt.widgets.Display;
import utils.Semaphore;
import utils.defines.Defines;
import utils.logging.Logger;
import utils.tracer.Trace;
import utils.tracer.TraceException;
import utils.tracer.TraceState;
import utils.tracer.Traceable;
import components.Component;
import components.commchannel.CommunicationChannel;
import components.commchannel.CommunicationChannelState;
import components.commchannel.serial.SerialCommunicationChannel;
import components.commchannel.virtual.VirtualCommunicationChannel;
import components.features.Feature;
import components.neuralnetwork.NeuralNetwork;
import components.neuralnetwork.NeuralNetworkState;
import core.simulation.Simulation;
/**
* <code>Robot</code> representa la clase base para cualquier tipo de robot
* (real o virtual) permitido en una simulaci�n. Esta clase no implementa
* ning�n tipo de l�gica de navegaci�n del robot (eso es responsabilidad de
* las clases hijas.)<br><br>
*
* Esta clase no puede ser instanciada (necesariamente, deber� extenderse).
*/
public abstract class Robot extends Component
implements Runnable, Playable, Drawable, Traceable
{
protected static final int SIMULATION_MODE = 0;
protected static final int REPRODUCTION_MODE = 1;
private static final Color DEFAULT_COLOR =
Display.getDefault().getSystemColor(
SWT.COLOR_GREEN);
// Atributos generales
protected CommunicationChannel commChannel;
protected NeuralNetwork neuralNet;
protected Feature[] features;
protected double[] initPosition;
protected int[] courseArrow;
protected Color color;
protected int status;
protected Thread thread;
protected HashMap statusTable;
protected boolean stopped, paused;
protected boolean visible;
protected boolean firstTime;
protected boolean firstUpdate;
protected boolean firstMovement;
protected boolean isPlaybackRobot;
protected int mode;
protected long sleepCounts;
private Semaphore commChannelSemaphore;
private Semaphore guiSemaphore;
private Semaphore robotStateSemaphore;
private Trace trace;
private String robotClassName;
private String channelClassName;
private String netClassName;
private RobotState robotState;
private CommunicationChannelState channelState;
private NeuralNetworkState netState;
private boolean disposed;
// EasyBot2
private double logical_course;
private boolean is_advancing;
/**
* Construye un nuevo robot, con nombre nulo, posici�n inicial (0.0,0.0), y
* direcci�n inicial 0.0 rad.
*/
public Robot()
{
super();
logical_course = 0;
is_advancing = false;
color = DEFAULT_COLOR;
visible = true;
initPosition = new double[2];
courseArrow = new int[6];
thread = null;
commChannelSemaphore = new Semaphore( 1 );
guiSemaphore = new Semaphore( 1 );
robotStateSemaphore = new Semaphore( 1 );
neuralNet = null;
disposed = false;
stopped = false;
paused = true;
firstTime = true;
firstUpdate = true;
firstMovement = true;
statusTable = new HashMap();
status = Defines.STATE_ROBOT_NOT_CALIBRATED;
sleepCounts = 0;
trace = new Trace();
isPlaybackRobot = Simulation.getCurrent().isPlaybackSimualtion();
statusTable.put(new Integer(Defines.STATE_ROBOT_NOT_CALIBRATED), "Robot sin calibrar");
if ( ! isPlaybackRobot )
{
try
{
// Este robot se registra como un "traceable"
trace.registerTraceable( this );
}
catch (TraceException e)
{
Logger.error( e.getMessage() );
}
Logger.info( "Robot creado para simulaci�n." );
}
else
{
robotClassName = getClass().getName();
Logger.info( "Robot creado para reproducci�n." );
}
}
//**********************************************************************//
// SETTERS y GETTERS //
//**********************************************************************//
/**
* Permite establecerle a este robot un canal de comunicaciones, a partir
* del cual se obtendr�n los datos para sensores y otros <i>features</i>.
* Es esperable que este m�todo no se utilice y, en cambio, la creaci�n
* del canal de comunicaciones se realice en el propio constructor del
* robot.
* @param commChannel canal de comunicaciones asociado al robot.
*/
public final void setCommChannel(CommunicationChannel commChannel)
{
this.commChannel = commChannel;
if ( ! isPlaybackRobot )
{
// El canal se registra como un "traceable"
// si es un robot de simulaci�n.
try
{
trace.registerTraceable( commChannel );
}
catch (TraceException e)
{
Logger.error( e.getMessage() );
}
}
else
{
channelClassName = commChannel.getClass().getName();
}
}
/**
* Retorna una referencia al canal de comunicaciones asociado al robot.
* @return canal de comunicaciones.
*/
public final CommunicationChannel getCommChannel()
{
return commChannel;
}
/**
* Permite agregarle al robot un <i>feature</i> (subcomponente), que
* podr�a ser, por ejemplo, un sensor de color, un sensor de proximidad,
* un motor, etc. El <i>feature</i> siendo agregado deber� poseer un nombre
* un�voco.
* @param feature subcomponente que se agregar� al robot.
* @throws RobotException si el subcomponente ya fue agregado
* anteriormente.
*/
public abstract void addFeature(Feature feature) throws RobotException;
/**
* Retorna la lista de <i>features</i> (subcomponentes) que conforman
* el robot.
* @return lista de <i>features</i>.
*/
public Feature[] getFeatures()
{
return features;
}
/**
* Permite establecer la red neuronal que controlar� las decisiones del
* robot.
* @param neuralNet red neuronal.
*/
public void setNeuralNetwork(NeuralNetwork neuralNet)
{
this.neuralNet = neuralNet;
if ( !isPlaybackRobot )
{
// La red se registra como un "traceable"
// si es un robot de simulaci�n.
try
{
trace.registerTraceable( neuralNet );
}
catch (TraceException e)
{
Logger.error( e.getMessage() );
}
}
else
{
netClassName = neuralNet.getClass().getName();
}
}
/**
* Retorna la red neuronal que controla las decisiones del robot, o
* <code>null</code> si no hay una red establecida.
* @return red neuronal.
*/
public NeuralNetwork getNeuralNetwork()
{
return neuralNet;
}
public final void updateState() throws RobotException
{
robotStateSemaphore.WAIT();
RobotState auxState = null;
try
{
auxState = Simulation.getCurrent().getGps().getRobotState( color.getRed() ,
color.getGreen(),
color.getBlue() );
}
catch(Exception e)
{
robotStateSemaphore.SIGNAL();
throw new RobotException( e.getMessage() );
}
// Se utiliza la variable auxiliar 'auxState' para
// no perder el estado anterior en caso de excepci�n.
robotState = auxState;
if ( firstUpdate )
{
// Se guarda la posici�n y direcci�n iniciales.
robotState.course = 0.0;
initPosition = new double[] { robotState.position[0] , robotState.position[1] };
firstUpdate = false;
}
robotStateSemaphore.SIGNAL();
}
public final void setInitPosition(double x, double y)
{
initPosition[0] = x;
initPosition[1] = y;
}
public final RobotState getState()
{
RobotState ret = null;
robotStateSemaphore.WAIT();
ret = (RobotState)robotState.clone();
robotStateSemaphore.SIGNAL();
return ret;
}
/**
* Establece el color que identifica un�vocamente al robot, dentro del
* sistema de posicionamiento.
* @param color color RGB que identifica al robot.
*/
public final void setColor(Color color)
{
this.color = color;
}
/**
* Establece el color que identifica un�vocamente al robot, dentro del
* sistema de posicionamiento.
* @param r componente <i>red</i>.
* @param g componente <i>green</i>.
* @param b componente <i>blue</i>.
*/
public final void setColor(int r, int g, int b)
{
this.color = new Color(null,r,g,b);
}
/**
* Retorna el color que identifica a este robot en el sistema de
* posicionamiento.
* @return color RGB que identifica al robot.
*/
public final Color getColor()
{
return color;
}
/**
* Muestra u oculta la representaci�n gr�fica del robot, dependiendo
* del par�metro <code>visible</code>.
* @param visible si es <code>true</code>, muestra la representaci�n
* gr�fica del robot; sino, lo oculta.
*/
public final void setVisible(boolean visible)
{
this.visible = visible;
}
/**
* Determina si la representaci�n gr�fica del robot es visible.
* @return <code>true</code> si es visible, o <code>false</code> en
* caso contrario.
*/
public final boolean isVisible()
{
return visible;
}
/**
* Establece el estado del robot, que debe pertencer a los definidos
* en {@link utils.defines.Defines utils.defines.Defines}.
* @param s estado.
*/
public final synchronized void setStatus(int s)
{
status = s;
if ( status == Defines.STATE_ROBOT_NOT_CALIBRATED )
firstUpdate = true;
}
/**
* Retorna el estado actual del robot.
* @return estado actual, definido en
* {@link utils.defines.Defines utils.defines.Defines}.
*/
public final synchronized int getStatus()
{
return status;
}
/**
* Este m�todo retorna una referencia al sem�foro que se utiliza
* en la sincronizaci�n del robot con su canal de comunicaciones.<br><br>
* Este m�todo no debe ser invocado m�s que por el canal de comunicaciones
* asociado al robot.
* @return sem�foro.
*/
public final Semaphore getCommChannelSemaphore()
{
return commChannelSemaphore;
}
/**
* Retorna una representaci�n textual del estado actual del robot.
* @return descripci�n del estado actual.
*/
public final String getStatusText()
{
return (String)statusTable.get( new Integer(this.status) );
}
//**********************************************************************//
// L�GICA DE CONTROL Y NAVEGACI�N //
//**********************************************************************//
/**
* Implementa la l�gica de navegaci�n del robot. Este m�todo debe
* sobreescribirse en las clases hijas, de acuerdo a c�mo el robot
* debe comportarse en el escenario.
*/
public abstract void move() throws RobotException;
public final void run()
{
Logger.info( "El robot comienza su ejecuci�n." );
while ( !stopped )
{
// Si est� pausado, espera...
while ( paused && !stopped)
try { Thread.sleep(10); } catch (InterruptedException e) {}
if ( isPlaybackRobot )
{
// *** ROBOT DE REPRODUCCI�N *** //
///////////////////// THREAD SAFE /////////////////////
guiSemaphore.WAIT();
robotStateSemaphore.WAIT();
try
{
// Obtiene el estado del robot.
if ( robotState == null )
robotState = new RobotState();
trace.readCurrentState( robotClassName , robotState );
// Obtiene el estado del canal de comunicaciones.
if ( channelState == null )
channelState = new CommunicationChannelState();
trace.readCurrentState( channelClassName , channelState );
if ( commChannel != null )
commChannel.setCommunicationChannelState( channelState );
else
throw new TraceException( "El robot no tiene un canal de comunicaciones establecido." );
// Obtiene el estado de la red neuronal.
if ( netState == null )
netState = new NeuralNetworkState();
trace.readCurrentState( netClassName , netState );
if ( neuralNet != null )
neuralNet.setNeuralNetworkState( netState );
else
throw new TraceException( "El robot no tiene una red neuronal establecida." );
}
catch (TraceException e)
{
Logger.error( e.getMessage() );
}
catch (EOFException e)
{
Logger.info( e.getMessage() );
trace.reset();
}
robotStateSemaphore.SIGNAL();
guiSemaphore.SIGNAL();
/////////////////////////////////////////////////////////
}
else
{
// *** ROBOT DE SIMULACI�N *** //
// Ordena que el estado actual de todos los componentes
// "traceables" sea escrito al archivo de traza.
try
{
robotStateSemaphore.WAIT();
robotState.course = getCourseForDrawing(robotState.course);
robotStateSemaphore.SIGNAL();
trace.writeCurrentState();
}
catch (TraceException e)
{
Logger.error( e.getMessage() );
}
///////////////////// THREAD SAFE /////////////////////
commChannelSemaphore.WAIT();
guiSemaphore.WAIT();
//*** Avanza a la siguiente posici�n ***//
try
{
move();
}
catch (RobotException e)
{
Logger.error( e.getMessage() );
}
commChannelSemaphore.SIGNAL();
guiSemaphore.SIGNAL();
/////////////////////////////////////////////////////////
}
// Notifica el cambio de estado a todos los observadores.
setChanged();
notifyObservers( this );
// Duerme un intervalo de tiempo
try
{
Simulation sim = Simulation.getCurrent();
Thread.sleep( sim != null ? sim.getStepTime() : 0 );
sleepCounts++;
}
catch (InterruptedException e)
{ }
}
dispose();
Logger.info( "El robot finaliza su ejecuci�n." );
}
protected void update_logical_course(double angulo,boolean advance_command)
{
if(is_advancing && !advance_command) {
// Si estaba abanzando y ahora no, el curso logico es el curso que traia
// (el que traia es el calculado mediante GPS, osea el "real")
robotStateSemaphore.WAIT();
logical_course = getCourseForDrawing(robotState.course);
robotStateSemaphore.SIGNAL();
}
else if(!is_advancing && advance_command) {
// Do nothing
}
logical_course+=angulo;
is_advancing=advance_command;
}
private double getCourseForDrawing(double course)
{
if(is_advancing || isPlaybackRobot) {
// Cuando esta avanzando utilizo el curso "real" (gps)
return course;
}
// Si esta girando utilizo el curso "logico" porque el real permanece fijo.
return logical_course;
}
public final void draw(GC gc)
{
double x, y, c;
if ( robotState == null )
return;
guiSemaphore.WAIT();
robotStateSemaphore.WAIT();
x = robotState.position[0];
y = robotState.position[1];
c = getCourseForDrawing(robotState.course);
//System.out.println("course="+robotState.course+",C="+c);
robotStateSemaphore.SIGNAL();
gc.setBackground( Display.getDefault().getSystemColor(SWT.COLOR_GRAY) );
gc.fillOval( (int)Math.round(x-(Defines.ROBOT_RADIO)),
(int)Math.round(y-(Defines.ROBOT_RADIO)),
(int)Math.round(2*Defines.ROBOT_RADIO) ,
(int)Math.round(2*Defines.ROBOT_RADIO) );
courseArrow[0] = (int)Math.round( x + Defines.ROBOT_RADIO * Math.cos(c + 3.0*Math.PI/2.0) );
courseArrow[1] = (int)Math.round( y + Defines.ROBOT_RADIO * Math.sin(c + 3.0*Math.PI/2.0) );
courseArrow[2] = (int)Math.round( x + Defines.ROBOT_RADIO * Math.cos(c + Math.PI/2.0) );
courseArrow[3] = (int)Math.round( y + Defines.ROBOT_RADIO * Math.sin(c + Math.PI/2.0) );
courseArrow[4] = (int)Math.round( x + Defines.ROBOT_RADIO * Math.cos(c) );
courseArrow[5] = (int)Math.round( y + Defines.ROBOT_RADIO * Math.sin(c) );
gc.setBackground( color );
gc.fillPolygon( courseArrow );
guiSemaphore.SIGNAL();
}
public final synchronized void play()
{
stopped = false;
paused = false;
//notify();
if ( firstTime )
{
if ( !isPlaybackRobot )
{
// Es la primera vez que se ejecuta "play" y,
// adem�s, no es un robot de reproducci�n.
// Entonces, se abre el canal de comunicaciones.
if ( commChannel == null )
{
Logger.error( "No se asoci� un canal de" +
"comunicaciones al robot." );
}
else
{
// TODO: ver c�mo hacer esto de una manera m�s elegante
if ( commChannel instanceof VirtualCommunicationChannel )
commChannel.open( name );
else if ( commChannel instanceof SerialCommunicationChannel )
commChannel.open( name + ", COM1" );
}
}
firstTime = false;
// Crea el thread y lo lanza
thread = new Thread( this );
thread.start();
}
}
public long getSleepCounts()
{
return sleepCounts;
}
public final synchronized void pause()
{
paused = true;
stopped = false;
}
public final synchronized void stop()
{
paused = false;
stopped = true;
firstTime = true;
//notify();
}
public void join()
{
try
{
if ( thread != null )
thread.join( 3000 );
}
catch (InterruptedException e)
{ }
}
public final synchronized boolean canPlay()
{
return true;
}
public final synchronized boolean isPlaying()
{
return thread.isAlive();
}
/**
* Reinicializa el robot a su estado original.
*/
public abstract void reset() throws RobotException;
public void dispose()
{
// Liberaci�n de recursos
if ( disposed )
return;
try
{
if ( commChannel != null && !commChannel.isClosed() )
{
commChannel.close();
commChannel.join();
}
if ( trace != null )
trace.close();
}
catch (Exception e)
{
Logger.error( e.getMessage() );
}
disposed = true;
}
public boolean isDisposed()
{
return disposed;
}
public TraceState getTraceableState()
{
return getState();
}
}