bool
TraCIServerAPI_Simulation::commandDistanceRequest(traci::TraCIServer& server, tcpip::Storage& inputStorage,
        tcpip::Storage& outputStorage, int commandId) {
    Position pos1;
    Position pos2;
    std::pair<const MSLane*, SUMOReal> roadPos1;
    std::pair<const MSLane*, SUMOReal> roadPos2;

    // read position 1
    int posType = inputStorage.readUnsignedByte();
    switch (posType) {
        case POSITION_ROADMAP:
            try {
                std::string roadID = inputStorage.readString();
                roadPos1.second = inputStorage.readDouble();
                roadPos1.first = getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos1.second);
                pos1 = roadPos1.first->getShape().positionAtLengthPosition(roadPos1.second);
            } catch (TraCIException& e) {
                server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
                return false;
            }
            break;
        case POSITION_2D:
        case POSITION_3D: {
            SUMOReal p1x = inputStorage.readDouble();
            SUMOReal p1y = inputStorage.readDouble();
            pos1.set(p1x, p1y);
        }
        if (posType == POSITION_3D) {
            inputStorage.readDouble();		// z value is ignored
        }
        roadPos1 = convertCartesianToRoadMap(pos1);
        break;
        default:
            server.writeStatusCmd(commandId, RTYPE_ERR, "Unknown position format used for distance request");
            return false;
    }

    // read position 2
    posType = inputStorage.readUnsignedByte();
    switch (posType) {
        case POSITION_ROADMAP:
            try {
                std::string roadID = inputStorage.readString();
                roadPos2.second = inputStorage.readDouble();
                roadPos2.first = getLaneChecking(roadID, inputStorage.readUnsignedByte(), roadPos2.second);
                pos2 = roadPos2.first->getShape().positionAtLengthPosition(roadPos2.second);
            } catch (TraCIException& e) {
                server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
                return false;
            }
            break;
        case POSITION_2D:
        case POSITION_3D: {
            SUMOReal p2x = inputStorage.readDouble();
            SUMOReal p2y = inputStorage.readDouble();
            pos2.set(p2x, p2y);
        }
        if (posType == POSITION_3D) {
            inputStorage.readDouble();		// z value is ignored
        }
        roadPos2 = convertCartesianToRoadMap(pos2);
        break;
        default:
            server.writeStatusCmd(commandId, RTYPE_ERR, "Unknown position format used for distance request");
            return false;
    }

    // read distance type
    int distType = inputStorage.readUnsignedByte();

    SUMOReal distance = 0.0;
    if (distType == REQUEST_DRIVINGDIST) {
        // compute driving distance
        if ((roadPos1.first == roadPos2.first) && (roadPos1.second <= roadPos2.second)) {
            // same edge
            distance = roadPos2.second - roadPos1.second;
        } else {
            MSEdgeVector newRoute;
            MSNet::getInstance()->getRouterTT().compute(
                &roadPos1.first->getEdge(), &roadPos2.first->getEdge(), 0, MSNet::getInstance()->getCurrentTimeStep(), newRoute);
            MSRoute route("", newRoute, false, 0, std::vector<SUMOVehicleParameter::Stop>());
            distance = route.getDistanceBetween(roadPos1.second, roadPos2.second, &roadPos1.first->getEdge(), &roadPos2.first->getEdge());
        }
    } else {
        // compute air distance (default)
        distance = pos1.distanceTo(pos2);
    }
    // write response command
    outputStorage.writeUnsignedByte(TYPE_DOUBLE);
    outputStorage.writeDouble(distance);
    return true;
}
bool
TraCIServerAPI_VehicleType::setVariable(const int cmd, const int variable, const int valueDataType,
                                        MSVehicleType& v, traci::TraCIServer& server,
                                        tcpip::Storage& inputStorage, tcpip::Storage& outputStorage) {
    switch (variable) {
        case VAR_LENGTH: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting length requires a double.", outputStorage);
                return false;
            }
            double val = inputStorage.readDouble();
            if (val == 0.0 || fabs(val) == std::numeric_limits<double>::infinity()) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Invalid length.", outputStorage);
                return false;
            }
            v.setLength(val);
        }
        break;
        case VAR_MAXSPEED: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting maximum speed requires a double.", outputStorage);
                return false;
            }
            double val = inputStorage.readDouble();
            if (val == 0.0 || fabs(val) == std::numeric_limits<double>::infinity()) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Invalid maximum speed.", outputStorage);
                return false;
            }
            v.setMaxSpeed(val);
        }
        break;
        case VAR_VEHICLECLASS: {
            if (valueDataType != TYPE_STRING) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting vehicle class requires a string.", outputStorage);
                return false;
            }
            v.setVClass(getVehicleClassID(inputStorage.readString()));
        }
        break;
        case VAR_SPEED_FACTOR: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting speed factor requires a double.", outputStorage);
                return false;
            }
            v.setSpeedFactor(inputStorage.readDouble());
        }
        break;
        case VAR_SPEED_DEVIATION: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting speed deviation requires a double.", outputStorage);
                return false;
            }
            v.setSpeedDeviation(inputStorage.readDouble());
        }
        break;
        case VAR_EMISSIONCLASS: {
            if (valueDataType != TYPE_STRING) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting emission class requires a string.", outputStorage);
                return false;
            }
            v.setEmissionClass(getVehicleEmissionTypeID(inputStorage.readString()));
        }
        break;
        case VAR_WIDTH: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting width requires a double.", outputStorage);
                return false;
            }
            v.setWidth(inputStorage.readDouble());
        }
        break;
        case VAR_MINGAP: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting minimum gap requires a double.", outputStorage);
                return false;
            }
            v.setMinGap(inputStorage.readDouble());
        }
        break;
        case VAR_SHAPECLASS: {
            if (valueDataType != TYPE_STRING) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting vehicle shape requires a string.", outputStorage);
                return false;
            }
            v.setShape(getVehicleShapeID(inputStorage.readString()));
        }
        break;
        case VAR_ACCEL: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting acceleration requires a double.", outputStorage);
                return false;
            }
            v.getCarFollowModel().setMaxAccel(inputStorage.readDouble());
        }
        break;
        case VAR_DECEL: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting deceleration requires a double.", outputStorage);
                return false;
            }
            v.getCarFollowModel().setMaxDecel(inputStorage.readDouble());
        }
        break;
        case VAR_IMPERFECTION: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting driver imperfection requires a double.", outputStorage);
                return false;
            }
            v.getCarFollowModel().setImperfection(inputStorage.readDouble());
        }
        break;
        case VAR_TAU: {
            if (valueDataType != TYPE_DOUBLE) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "Setting headway time requires a double.", outputStorage);
                return false;
            }
            v.getCarFollowModel().setHeadwayTime(inputStorage.readDouble());
        }
        break;
        case VAR_COLOR: {
            if (valueDataType != TYPE_COLOR) {
                server.writeStatusCmd(cmd, RTYPE_ERR, "The color must be given using the according type.", outputStorage);
                return false;
            }
            SUMOReal r = (SUMOReal) inputStorage.readUnsignedByte() / 255.;
            SUMOReal g = (SUMOReal) inputStorage.readUnsignedByte() / 255.;
            SUMOReal b = (SUMOReal) inputStorage.readUnsignedByte() / 255.;
            inputStorage.readUnsignedByte(); // skip alpha level
            RGBColor col(r, g, b);
            v.setColor(col);
        }
        break;
        default:
            break;
    }
    return true;
}
bool
TraCIServerAPI_Simulation::commandPositionConversion(traci::TraCIServer& server, tcpip::Storage& inputStorage,
        tcpip::Storage& outputStorage, int commandId) {
    std::pair<MSLane*, SUMOReal> roadPos;
    Position cartesianPos;
    Position geoPos;
    SUMOReal z = 0;

    // actual position type that will be converted
    int srcPosType = inputStorage.readUnsignedByte();

    switch (srcPosType) {
        case POSITION_2D:
        case POSITION_3D:
        case POSITION_LAT_LON:
        case POSITION_LAT_LON_ALT: {
            SUMOReal x = inputStorage.readDouble();
            SUMOReal y = inputStorage.readDouble();
            if (srcPosType != POSITION_2D && srcPosType != POSITION_LAT_LON) {
                z = inputStorage.readDouble();
            }
            geoPos.set(x, y);
            cartesianPos.set(x, y);
            if (srcPosType == POSITION_LAT_LON || srcPosType == POSITION_LAT_LON_ALT) {
                GeoConvHelper::getFinal().x2cartesian_const(cartesianPos);
            } else {
                GeoConvHelper::getFinal().cartesian2geo(geoPos);
            }
        }
        break;
        case POSITION_ROADMAP: {
            std::string roadID = inputStorage.readString();
            SUMOReal pos = inputStorage.readDouble();
            int laneIdx = inputStorage.readUnsignedByte();
            try {
                cartesianPos = geoPos = getLaneChecking(roadID, laneIdx, pos)->getShape().positionAtLengthPosition(pos);
                GeoConvHelper::getFinal().cartesian2geo(geoPos);
            } catch (TraCIException& e) {
                server.writeStatusCmd(commandId, RTYPE_ERR, e.what());
                return false;
            }
        }
        break;
        default:
            server.writeStatusCmd(commandId, RTYPE_ERR, "Source position type not supported");
            return false;
    }

    int type = inputStorage.readUnsignedByte();
    if (type != TYPE_UBYTE) {
        server.writeStatusCmd(commandId, RTYPE_ERR, "Destination position type must be of type ubyte.");
        return false;
    }
    int destPosType = inputStorage.readUnsignedByte();

    switch (destPosType) {
        case POSITION_ROADMAP: {
            // convert road map to 3D position
            roadPos = convertCartesianToRoadMap(cartesianPos);
            // write result that is added to response msg
            outputStorage.writeUnsignedByte(POSITION_ROADMAP);
            outputStorage.writeString(roadPos.first->getEdge().getID());
            outputStorage.writeDouble(roadPos.second);
            const std::vector<MSLane*> lanes = roadPos.first->getEdge().getLanes();
            outputStorage.writeUnsignedByte((int)distance(lanes.begin(), find(lanes.begin(), lanes.end(), roadPos.first)));
        }
        break;
        case POSITION_2D:
        case POSITION_3D:
        case POSITION_LAT_LON:
        case POSITION_LAT_LON_ALT:
            outputStorage.writeUnsignedByte(destPosType);
            if (destPosType == POSITION_LAT_LON || destPosType == POSITION_LAT_LON_ALT) {
                outputStorage.writeDouble(geoPos.x());
                outputStorage.writeDouble(geoPos.y());
            } else {
                outputStorage.writeDouble(cartesianPos.x());
                outputStorage.writeDouble(cartesianPos.y());
            }
            if (destPosType != POSITION_2D && destPosType != POSITION_LAT_LON) {
                outputStorage.writeDouble(z);
            }
            break;
        default:
            server.writeStatusCmd(commandId, RTYPE_ERR, "Destination position type not supported");
            return false;
    }
    return true;
}