TraciSimulationController.java

package de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.traci;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import de.tud.kom.p2psim.impl.topology.movement.vehicular.information.LocationUtils;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.information.Position;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.information.Route;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.information.Street;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.SimulationSetupExtractor;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.VehicleController;
import de.tudresden.sumo.cmd.Junction;
import de.tudresden.sumo.cmd.Simulation;
import de.tudresden.sumo.cmd.Vehicle;
import de.tudresden.sumo.util.SumoCommand;
import de.tudresden.ws.container.SumoPosition2D;
import de.tudresden.ws.container.SumoStringList;
import it.polito.appeal.traci.SumoTraciConnection;

public class TraciSimulationController implements VehicleController, SimulationSetupExtractor {

    private static final Map<String, TraciSimulationController> CONTROLLER = new HashMap<>();

    private static final double CLUSTERING_DISTANCE = 50;

    private String _sumoExe;
    private String _configFile;

    private SumoTraciConnection _connection;

    private double _start = -1;
    private double _step = -1;

    private double _startX;
    private double _startY;
    private double _endX;
    private double _endY;

    private Map<Double, Map<String, Position>> _positonsByTimestamp = new HashMap<>();
    private int _futureInformation = 10;

    private boolean _initalized = false;

    private boolean _observedAreaSet;

    private Map<String, Double> _vehiclesOutOfRange = new HashMap<>();

    public static synchronized TraciSimulationController createSimulationController(String pSumoExe, String pConfigFile) {
        if (!CONTROLLER.containsKey(pConfigFile)) {
            CONTROLLER.put(pConfigFile, new TraciSimulationController(pSumoExe, pConfigFile));
        }
        return CONTROLLER.get(pConfigFile);
    }

    private TraciSimulationController(String pSumoExe, String pConfigFile) {
        _sumoExe = pSumoExe;
        _configFile = pConfigFile;
    }

    public static VehicleController getSimulationController() {
        return CONTROLLER.values().iterator().next();
    }

    @Override
    public synchronized void init() {
        if (!_initalized) {
            _connection = new SumoTraciConnection(_sumoExe, _configFile);

            try {
                _connection.runServer();

                for (int i = 0; i < _futureInformation; i++) {
                    nextStep();
                }
            } catch (RuntimeException e) {
                throw e;
            } catch (Exception e) {
                e.printStackTrace();
            }

            Runnable shutdownHook = new Runnable() {

                @Override
                public void run() {
                    _connection.close();
                }
            };

            Runtime.getRuntime().addShutdownHook(new Thread(shutdownHook));

            _initalized = true;
        }
    }

    @Override
    public Position getVehiclePosition(String pVehicleID) {
        return getVehiclePosition(_step, pVehicleID);
    }

    @Override
    public Position getVehiclePosition(double pStep, String pVehicleID) {
        Map<String, Position> map = _positonsByTimestamp.get(pStep);

        return map.get(pVehicleID);
    }

    @Override
    public List<Position> getAllIntersections(boolean pCluster) {
        List<Position> result = new ArrayList<>();

        SumoCommand intersectionCommand = Junction.getIDList();

        Object intersectionObject = requestObject(intersectionCommand);

        SumoStringList intersections = (SumoStringList) intersectionObject;

        for (String intersection : intersections) {
            SumoCommand positionCommand = Junction.getPosition(intersection);
            Object positionObject = requestObject(positionCommand);
            SumoPosition2D sumoPosition = (SumoPosition2D) positionObject;

            if (_observedAreaSet) {
                if (_startX <= sumoPosition.x && sumoPosition.x <= _endX && _startY <= sumoPosition.y && sumoPosition.y <= _endY) {
                    result.add(new Position(sumoPosition.x - _startX, sumoPosition.y - _startY, 0, 0));
                }
            } else {
                result.add(new Position(sumoPosition.x, sumoPosition.y, 0, 0));
            }
        }

        if (pCluster) {
            List<Position> tempResult = new ArrayList<>();

            outer:for (int i = 0; i < result.size(); i++) {
                Position position = result.get(i);

                for (int j = 0; j < tempResult.size(); j++) {
                    Position addedPosition = tempResult.get(j);
                    if (position != addedPosition && LocationUtils.calculateDistance(position, addedPosition) < CLUSTERING_DISTANCE / 1000.0) {
                        continue outer;
                    }
                }

                tempResult.add(position);
            }

            result = tempResult;
        }

        return result;
    }

    @Override
    public boolean nextStep() {
        try {
            _connection.do_timestep();

            try {
                int temp = (Integer) _connection.do_job_get(Simulation.getCurrentTime());

                _step = temp / 1000.0 - _futureInformation;

                if (_start == -1) {
                    _start = _step + _futureInformation;
                }

                double newStep = _step + _futureInformation;
                if (!_positonsByTimestamp.containsKey(newStep)) {
                    Map<String, Position> vehiclePositions = new HashMap<>();
                    _positonsByTimestamp.put(newStep, vehiclePositions);

                    List<String> allVehicles = requestAllVehicles();
                    for (String vehicle : allVehicles) {
                        Position vehiclePosition = requestVehiclePosition(vehicle);
                        if (vehiclePosition != null) {
                            vehiclePositions.put(vehicle, vehiclePosition);
                        }
                    }
                }

                if (_positonsByTimestamp.containsKey(_step - 1)) {
                    _positonsByTimestamp.remove(_step - 1);
                }
            } catch (Exception e) {
                e.printStackTrace();
            }

            return true;
        } catch (RuntimeException e) {
            throw e;
        } catch (Exception e) {
            e.printStackTrace();
        }
        return false;
    }

    private Position requestVehiclePosition(String pVehicleID) {
        if (_vehiclesOutOfRange.containsKey(pVehicleID)) {
            if (_vehiclesOutOfRange.get(pVehicleID) < _step) {
                return null;
            } else {
                _vehiclesOutOfRange.remove(pVehicleID);
            }
        }

        SumoCommand positionCommand = Vehicle.getPosition(pVehicleID);
        SumoCommand angleCommand = Vehicle.getAngle(pVehicleID);
        SumoCommand speedCommand = Vehicle.getSpeed(pVehicleID);

        Object positionObject = requestObject(positionCommand);
        Object angleObject = requestObject(angleCommand);
        Object speedObject = requestObject(speedCommand);

        SumoPosition2D sumoPosition = (SumoPosition2D) positionObject;
        double angle = (Double) angleObject;
        double speed = (Double) speedObject;

        if (_observedAreaSet) {
            if (_startX <= sumoPosition.x && sumoPosition.x <= _endX && _startY <= sumoPosition.y && sumoPosition.y <= _endY) {
                return new Position(sumoPosition.x - _startX, sumoPosition.y - _startY, 0, angle, speed);
            } else {
                double diffX = _startX - sumoPosition.x;
                if (diffX < 0 || sumoPosition.x - _endX < diffX) {
                    diffX = sumoPosition.x - _endX;
                }
                if (diffX < 0) {
                    diffX = 0;
                }
                double diffY = _startY - sumoPosition.y;
                if (diffY < 0 || sumoPosition.y - _endY < diffY) {
                    diffY = sumoPosition.y - _endY;
                }
                if (diffY < 0) {
                    diffY = 0;
                }
                double diff = Math.sqrt(Math.pow(diffX, 2) + Math.pow(diffY, 2));

                double timeTillNextJoin = diff / 50;

                //                _vehiclesOutOfRange.put(pVehicleID, _step + timeTillNextJoin);

                return null;
            }
        } else {
            return new Position(sumoPosition.x, sumoPosition.y, 0, angle);
        }
    }

    private List<String> requestAllVehicles() {
        SumoCommand idList = Vehicle.getIDList();
        Object object = requestObject(idList);
        SumoStringList list = (SumoStringList)object;

        List<String> result = new ArrayList<>();

        for (String vehicle : list) {
            result.add(vehicle);
        }

        return result;
    }

    @Override
    public List<String> getAllVehicles() {
        return getAllVehicles(_step);
    }

    @Override
    public List<String> getAllVehicles(double pStep) {
        Map<String, Position> map = _positonsByTimestamp.get(pStep);

        return new ArrayList<>(map.keySet());
    }

    public Route getRoute(String pVehicleID) {
        SumoCommand routeCommand = Vehicle.getRoute(pVehicleID);
        Object object = requestObject(routeCommand);
        SumoStringList streetList = (SumoStringList) object;

        Route route = new Route();
        List<Street> streets = route.getStreets();
        for (String street : streetList) {
            streets.add(new Street(street));
        }

        return route;
    }

    public Object requestObject(SumoCommand routeCommand) {
        Object object = null;
        try {
            object = _connection.do_job_get(routeCommand);
        } catch (RuntimeException e) {
            throw e;
        } catch (Exception e) {
            e.printStackTrace();
        }
        return object;
    }

    @Override
    public double getStep() {
        return _step;
    }

    @Override
    public double getStart() {
        return _start;
    }

    @Override
    public double getMaximumAvailablePrediction() {
        double max = Collections.max(_positonsByTimestamp.keySet());

        return max;
    }

    @Override
    public void setObservedArea(double pStartX, double pStartY, double pEndX, double pEndY) {
        _startX = pStartX;
        _startY = pStartY;
        _endX = pEndX;
        _endY = pEndY;

        _observedAreaSet = true;
    }

}