/**
* Copyright (C) BFH www.bfh.ch 2011
* Code written by: Patrick Dobler, Marc Folly
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
package ch.bfh.ti.kybernetik.engine.controller;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Iterator;
import java.util.List;
import java.util.Observable;
import java.util.concurrent.CopyOnWriteArrayList;
import javax.vecmath.Point2d;
import javax.vecmath.Vector2d;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.ArrayRealVector;
import org.apache.commons.math.linear.DecompositionSolver;
import org.apache.commons.math.linear.InvalidMatrixException;
import org.apache.commons.math.linear.LUDecompositionImpl;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import ch.bfh.ti.kybernetik.engine.controller.lightBulb.LightBulbController;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.SimulatorObserverCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightBulbModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightBulbModelObserverCommand.LightBulbModelCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightSensorModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.LightSensorModelObserverCommand.LightSensorCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterModelObserverCommand.RoboterModelCommandState;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterMoveListModelObserverCommand;
import ch.bfh.ti.kybernetik.engine.controller.observerCommands.model.RoboterMoveListModelObserverCommand.RoboterMoveModelObserverCommandState;
import ch.bfh.ti.kybernetik.engine.controller.roboter.RoboterConstruction;
import ch.bfh.ti.kybernetik.engine.controller.roboter.RoboterController;
import ch.bfh.ti.kybernetik.engine.model.LightBulb;
import ch.bfh.ti.kybernetik.engine.model.Roboter;
import ch.bfh.ti.kybernetik.engine.model.RoboterMove;
import ch.bfh.ti.kybernetik.lego.ki.RobKIFactr;
import ch.bfh.ti.kybernetik.lego.ki.RoboterKI;
/**
* The default {@link Simulator} implementation
*
*/
class DefaultSimulatorImpl extends Observable implements Simulator {
private static Logger logger = LoggerFactory.getLogger(DefaultSimulatorImpl.class);
private final List<LightBulbController> lightBulbControllers;
private final List<RoboterController> roboterControllers;
private static int height = 10;
private static int width = 10;
private volatile boolean simulating = false;
private volatile boolean running = true;
private volatile int simulatorSpeed = 10;
public DefaultSimulatorImpl(LightBulbController[] lightBulbController, RoboterController... roboterController) {
this.roboterControllers = new CopyOnWriteArrayList<RoboterController>(roboterController);
this.lightBulbControllers = new CopyOnWriteArrayList<LightBulbController>(lightBulbController);
}
@Override
public void run() {
this.notifyObservers(SimulatorObserverCommandState.STARTED_SIMULATION);
while (running) {
Thread.yield();
while (simulating) {
try {
Thread.sleep(simulatorSpeed);
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
this.notifyObservers(SimulatorObserverCommandState.START_ROBOTER_MOVING);
this.iterateRoboterControllers(new ThreadSafeIterationAction<RoboterController>() {
@Override
public void performAction(RoboterController roboterController) {
// System.out.println("Simulator: Iterate RoboterController");
double leftSensorIntensity = 0;
double rightSensorIntensity = 0;
Roboter roboter = roboterController.getRoboter();
RoboterConstruction roboterConstruction = roboterController.getRoboterConstruction();
for (LightBulbController lightBulbController : lightBulbControllers) {
// Left Light Sensor
Point2d leftLightSensorPoint = roboterConstruction.getRoboterElementPoint(roboter.getLeftLightSensor());
Vector2d leftLightSensorVector = roboterConstruction.getRoboterElementAngleVector(roboter.getLeftLightSensor());
leftSensorIntensity = leftSensorIntensity
+ lightBulbController.calculateIntensity(leftLightSensorPoint, roboter.getLeftLightSensor(),
leftLightSensorVector);
// Right Light Sensor
Point2d rightLightSensorPoint = roboterConstruction.getRoboterElementPoint(roboter.getRightLightSensor());
Vector2d rightLightSensorVector = roboterConstruction.getRoboterElementAngleVector(roboter
.getRightLightSensor());
rightSensorIntensity = rightSensorIntensity
+ lightBulbController.calculateIntensity(rightLightSensorPoint, roboter.getRightLightSensor(),
rightLightSensorVector);
}
// Changed Left Sensor Intensity
if (leftSensorIntensity != roboter.getLeftLightSensor().getSensorIntensity()) {
notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_CHANGED,
roboter.getLeftLightSensor()));
logger.debug("Changed Left Light Sensor Intensity to {}: " + leftSensorIntensity);
roboter.getLeftLightSensor().setSensorIntensity(leftSensorIntensity);
}
if (leftSensorIntensity == 0) {
notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_REMAINS,
roboter.getLeftLightSensor()));
}
// Changed Right Sensor Intensity
if (rightSensorIntensity != roboter.getRightLightSensor().getSensorIntensity()) {
notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_CHANGED,
roboter.getRightLightSensor()));
logger.debug("Changed Right Light Sensor Intensity to {}: " + rightSensorIntensity);
roboter.getRightLightSensor().setSensorIntensity(rightSensorIntensity);
}
if (rightSensorIntensity == 0) {
notifyObservers(new LightSensorModelObserverCommand(roboter, LightSensorCommandState.SENSOR_INTENSITY_REMAINS,
roboter.getRightLightSensor()));
}
RoboterMove rm = roboterController.calculateNewRoboterPosition(leftSensorIntensity, rightSensorIntensity);
calculateOutOfWindowAreaPosition(roboterController, rm);
notifyObservers(new RoboterMoveListModelObserverCommand(roboter,
RoboterMoveModelObserverCommandState.ROBOTER_MOVE_ADDED, roboterController.getRoboterMoveHistory()));
notifyObservers(new RoboterModelObserverCommand(roboter,
RoboterModelObserverCommand.RoboterModelCommandState.ROBOTER_MOVED));
}
});
this.notifyObservers(SimulatorObserverCommandState.MOVED_ALL_ROBOTOS);
}
this.notifyObservers(SimulatorObserverCommandState.PAUSED_SIMULATING);
}
this.notifyObservers(SimulatorObserverCommandState.STOPPED_SIMULATION);
}
private void calculateOutOfWindowAreaPosition(RoboterController roboterController, RoboterMove rm) {
Point2d newPosition = null;
// Fall 1: Unten
Point2d roboter1 = rm.getNewRoboterPoint();
if (roboter1.getY() > height) {
newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_LEFT, SideStraigtLine.LINE_RIGHT,
SideStraigtLine.LINE_UP);
roboterController.getRoboter().setX(newPosition.getX());
roboterController.getRoboter().setY(newPosition.getY());
// Fall 2: Oben
} else if (roboter1.getY() < 0) {
newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_LEFT, SideStraigtLine.LINE_RIGHT,
SideStraigtLine.LINE_DOWN);
roboterController.getRoboter().setX(newPosition.getX());
roboterController.getRoboter().setY(newPosition.getY());
// Fall 3: Rechts
} else if (roboter1.getX() > width) {
newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_LEFT, SideStraigtLine.LINE_DOWN,
SideStraigtLine.LINE_UP);
roboterController.getRoboter().setX(newPosition.getX());
roboterController.getRoboter().setY(newPosition.getY());
// Fall 4: Links
} else if (roboter1.getX() < 0) {
newPosition = getIntersectionPointForSides(rm, roboter1, SideStraigtLine.LINE_RIGHT, SideStraigtLine.LINE_DOWN,
SideStraigtLine.LINE_UP);
roboterController.getRoboter().setX(newPosition.getX());
roboterController.getRoboter().setY(newPosition.getY());
}
}
public boolean isPointWithinField(Point2d p1) {
int t = (int) p1.getX();
if ((int) p1.getX() >= 0 && (int) p1.getX() <= width) {
if ((int) p1.getY() >= 0 && (int) p1.getY() <= height) {
return true;
}
}
return false;
}
private Point2d getIntersectionPointForSides(RoboterMove rm, Point2d roboter1, SideStraigtLine... lines) {
for (SideStraigtLine line : lines) {
try {
Point2d p1 = getIntersectionPointForSide(rm, roboter1, line);
if (isPointWithinField(p1)) {
// System.out.println("Found Intersection on Side: " +
// line);
return p1;
} else {
// System.out.println("No Intersection Found on Side: "
// + line);
}
} catch (InvalidMatrixException ime) {
// System.out.println("Line " + line + " was parallel");
}
}
return null;
}
private Point2d getIntersectionPointForSide(RoboterMove rm, Point2d roboter1, SideStraigtLine line) throws InvalidMatrixException {
double sideX = line.sideX;
double sideY = line.sideY;
double sideVectX = line.sideVectX;
double sideVectY = line.sideVectY;
Vector2d direction = rm.getNewDirectionVector();
direction.normalize();
RealMatrix coefficients = new Array2DRowRealMatrix(new double[][] { { direction.getX(), -(sideVectX) },
{ direction.getY(), -(sideVectY) } }, false);
DecompositionSolver solver = new LUDecompositionImpl(coefficients).getSolver();
RealVector constants = new ArrayRealVector(new double[] { sideX - roboter1.getX(), sideY - roboter1.getY() }, false);
RealVector solution = solver.solve(constants);
double r = solution.getEntry(0);
// double s = solution.getEntry(1);
double res1 = roboter1.getX() + direction.getX() * r;
double res2 = roboter1.getY() + direction.getY() * r;
Point2d p = new Point2d(res1, res2);
return p;
}
/**
* We need to override this method to make sure both calls the setChanged()
* and the notifyObservers() are called immediately after each other!
* Otherwise another Thread might toggle the changed attribute of the
* Observer and the next notifyCommand won't be distributed
*/
@Override
public synchronized void notifyObservers(Object arg) {
super.setChanged();
super.notifyObservers(arg);
}
public boolean isSimulating() {
return simulating;
}
public void setSimulating(boolean simulating) {
this.simulating = simulating;
}
public boolean isRunning() {
return running;
}
public void setRunning(boolean running) {
this.running = running;
}
/*
* (non-Javadoc)
*
* @see ch.bfh.ti.kybernetik.simulator.controller.SimulatorInterface#
* getSimulatorSpeed()
*/
@Override
public int getSimulatorSpeed() {
return simulatorSpeed;
}
/*
* (non-Javadoc)
*
* @see ch.bfh.ti.kybernetik.simulator.controller.SimulatorInterface#
* setSimulatorSpeed(int)
*/
@Override
public void setSimulatorSpeed(int simulatorSpeed) {
if (simulatorSpeed >= 0)
this.simulatorSpeed = simulatorSpeed;
}
@Override
public void setWidth(int width) {
this.width = width;
}
@Override
public int getWidth() {
return width;
}
@Override
public void setHeight(int height) {
this.height = height;
}
@Override
public int getHeight() {
return height;
}
@Override
public boolean addRoboterController(RoboterController roboterController) {
boolean added = this.getRoboterControllers().add(roboterController);
if (added) {
this.notifyObservers(new RoboterModelObserverCommand(roboterController.getRoboter(), RoboterModelCommandState.ROBOTER_ADDED));
}
return added;
}
/*
* (non-Javadoc)
*
* @see
* ch.bfh.ti.kybernetik.engine.controller.Simulator#addLightBulbController
* (ch.bfh.ti.kybernetik.engine.controller.lightBulb.LightBulbController)
*/
@Override
public boolean addLightBulbController(LightBulbController lightBulbController) {
boolean added = this.getLightBulbControllers().add(lightBulbController);
if (added) {
this.notifyObservers(new LightBulbModelObserverCommand(lightBulbController.getLightBulb(),
LightBulbModelCommandState.LIGHTBULB_ADDED));
}
return added;
}
@Override
public void removeRoboter(Roboter roboter) {
RoboterController rcToRemove = this.getRoboterControllerForRoboter(roboter);
if (rcToRemove != null) {
this.getRoboterControllers().remove(rcToRemove);
this.notifyObservers(new RoboterModelObserverCommand(rcToRemove.getRoboter(), RoboterModelCommandState.ROBOTER_REMOVED));
}
}
@Override
public void removeLightBulb(LightBulb lightBulb) {
LightBulbController lbcToRemove = this.getLightBulbControllerForLightBulb(lightBulb);
if (lbcToRemove != null) {
this.getLightBulbControllers().remove(lbcToRemove);
this.notifyObservers(new LightBulbModelObserverCommand(lbcToRemove.getLightBulb(), LightBulbModelCommandState.LIGHTBULB_REMOVED));
}
}
@Override
public void iterateRoboterControllers(ThreadSafeIterationAction<RoboterController> action) {
Collection<RoboterController> list = getRoboterControllers();
final Iterator<RoboterController> i = list.iterator();
while (i.hasNext()) {
action.performAction(i.next());
}
}
@Override
public List<Roboter> getRoboterList() {
final List<Roboter> roboterList = new ArrayList<Roboter>();
this.iterateRoboterControllers(new ThreadSafeIterationAction<RoboterController>() {
@Override
public void performAction(RoboterController iterateObject) {
roboterList.add(iterateObject.getRoboter());
}
});
return roboterList;
}
@Override
public List<LightBulb> getLightBulbList() {
final List<LightBulb> lightBulbList = new ArrayList<LightBulb>();
this.iterateLightBulbControllers(new ThreadSafeIterationAction<LightBulbController>() {
@Override
public void performAction(LightBulbController iterateObject) {
lightBulbList.add(iterateObject.getLightBulb());
}
});
return lightBulbList;
}
@Override
public void iterateLightBulbControllers(ThreadSafeIterationAction<LightBulbController> action) {
Collection<LightBulbController> list = getLightBulbControllers();
final Iterator<LightBulbController> i = list.iterator();
while (i.hasNext()) {
action.performAction(i.next());
}
}
private List<RoboterController> getRoboterControllers() {
return roboterControllers;
}
@Override
public void setDeltaT(final double deltaT) {
this.iterateRoboterControllers(new ThreadSafeIterationAction<RoboterController>() {
@Override
public void performAction(RoboterController iterateObject) {
iterateObject.setDeltaT(deltaT);
}
});
}
/*
* (non-Javadoc)
*
* @see
* ch.bfh.ti.kybernetik.simulator.controller.SimulatorInterface#getDeltaT()
*/
@Override
public double getDeltaT() {
return this.getRoboterControllers().get(0).getDeltaT();
}
private List<LightBulbController> getLightBulbControllers() {
return lightBulbControllers;
}
@Override
public RoboterKI getRoboterKIForRoboter(Roboter roboter) {
RoboterController roboterController = this.getRoboterControllerForRoboter(roboter);
if (roboterController != null) {
return roboterController.getRoboterKI();
}
return null;
}
@Override
public RoboterConstruction getRoboterConstructionForRoboter(Roboter roboter) {
RoboterController roboterController = this.getRoboterControllerForRoboter(roboter);
if (roboterController != null) {
return roboterController.getRoboterConstruction();
}
return null;
}
private RoboterController getRoboterControllerForRoboter(Roboter roboter) {
Collection<RoboterController> list = getRoboterControllers();
final Iterator<RoboterController> i = list.iterator();
while (i.hasNext()) {
RoboterController roboterController = i.next();
if (roboterController.getRoboter().equals(roboter)) {
return roboterController;
}
}
return null;
}
private LightBulbController getLightBulbControllerForLightBulb(LightBulb lightBulb) {
Collection<LightBulbController> list = getLightBulbControllers();
final Iterator<LightBulbController> i = list.iterator();
while (i.hasNext()) {
LightBulbController lightBulbController = i.next();
if (lightBulbController.getLightBulb().equals(lightBulb)) {
return lightBulbController;
}
}
return null;
}
@Override
public void changeRoboterKIForRoboter(Roboter roboter, RoboterKI roboterKI) {
RoboterController roboterController = this.getRoboterControllerForRoboter(roboter);
roboterController.setRoboterKI(roboterKI);
}
private enum SideStraigtLine {
LINE_LEFT(0, 0, 0, 1), LINE_RIGHT(width, 0, 0, 1), LINE_DOWN(0, height, 1, 0), LINE_UP(0, 0, 1, 0);
double sideX = 0;
double sideY = 0;
double sideVectX = 0;
double sideVectY = 1;
SideStraigtLine(double sideX, double sideY, double sideVectX, double sideVectY) {
this.sideVectX = sideVectX;
this.sideVectY = sideVectY;
this.sideX = sideX;
this.sideY = sideY;
}
}
}