copied emulator logic to main server, add parsing cl args, add JSAP lib

server/build.gradle

@@ -46,12 +46,15 @@ jar {
                 "Main-Class": "ServerMainKt")
     }
 
+    from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } }
+
 }
 
 dependencies {
     compile "org.jetbrains.kotlin:kotlin-stdlib:$kotlin_version"
     compile "org.jetbrains.kotlin:kotlin-reflect:$kotlin_version"
     compile group: 'org.asynchttpclient', name: 'async-http-client', version: '2.0.0-RC9'
+    compile group: "com.martiansoftware", name: "jsap", version: "2.1"
 
     compile files ("../proto/compiler/build/protokot-runtime.jar")
 }

server/room_config_example.xml

@@ -0,0 +1,9 @@
+<room>
+<walls>
+<wall startX="200" startY="300" endX="-150" endY="300"/>
+<wall startX="-150" startY="300" endX="-150" endY="-45"/>
+<wall startX="-150" startY="-45" endX="200" endY="-45"/>
+<wall startX="200" startY="-45" endX="200" endY="300"/>
+</walls>
+</room>
+

server/src/main/java/ClUtils.kt

@@ -0,0 +1,33 @@
+import com.martiansoftware.jsap.FlaggedOption
+import com.martiansoftware.jsap.JSAP
+import com.martiansoftware.jsap.StringParser
+import com.martiansoftware.jsap.Switch
+
+fun setClOptions(clParser: JSAP) {
+    val optEmulator = Switch("emulator", 'e', "emulator")
+    val optUseRandom = Switch("random", 'r', "random")
+    val optSeedForRandom = getOption("seed", JSAP.LONG_PARSER, "1111", 's', "seed")
+    val optTestRoom = getOption("test room", JSAP.STRING_PARSER, "", null, "room")
+    val optHelp = getOption("help", JSAP.BOOLEAN_PARSER, "false", null, "help")
+
+    clParser.registerParameter(optEmulator)
+    clParser.registerParameter(optUseRandom)
+    clParser.registerParameter(optSeedForRandom)
+    clParser.registerParameter(optTestRoom)
+    clParser.registerParameter(optHelp)
+}
+
+fun getOption(name: String, parser: StringParser, default: String, shortFlag: Char?, longFlag: String, required: Boolean = false): FlaggedOption {
+    val result = FlaggedOption(name)
+            .setStringParser(parser)
+            .setDefault(default)
+            .setRequired(required)
+    if (shortFlag != null) {
+        result.setShortFlag(shortFlag)
+    }
+    if (!longFlag.equals("")) {
+        result.setLongFlag(longFlag)
+    }
+
+    return result
+}

server/src/main/java/ServerMain.kt

@@ -1,11 +1,36 @@
-import roomScanner.CarController
-import roomScanner.RoomScanner
 import clInterface.DebugClInterface
+import com.martiansoftware.jsap.JSAP
 import net.car.Dropper
 import net.car.client.Client
+import objects.CarReal
 import objects.Environment
+import objects.emulator.CarEmulator
+import objects.emulator.EmulatedRoom
+import objects.emulator.Rng
+import roomScanner.CarController
+import roomScanner.RoomScanner
 
 fun main(args: Array<String>) {
+
+    val clParser = JSAP()
+    setClOptions(clParser)
+    val argsConfig = clParser.parse(args)
+    if (!argsConfig.success() || argsConfig.getBoolean("help")) {
+        println(clParser.getHelp())
+        return
+    }
+    if (argsConfig.getBoolean("emulator")) {
+        val pathToRoomConfig = argsConfig.getString("test room")
+        val randomSeed = argsConfig.getLong("seed")
+        val useRandom = argsConfig.getBoolean("random")
+        val carUid = 1
+        val emulatedRoom = EmulatedRoom.EmulatedRoomFromFile(pathToRoomConfig)
+        if (emulatedRoom == null) {
+            println("error parsin room from file $pathToRoomConfig")
+            return
+        }
+        Environment.map.put(carUid, CarEmulator(carUid, emulatedRoom, useRandom, Rng(randomSeed)))
+    }
     var roomScanner: RoomScanner? = null
     val carServer = net.car.server.Server.createCarServerThread()
     val webServer = net.web.server.Server.createWebServerThread()
@@ -16,8 +41,10 @@ fun main(args: Array<String>) {
 
     if (args.contains("--scan")) {
         Environment.onCarConnect { car ->
-            roomScanner = RoomScanner(CarController(car))
+            if (car is CarReal) {
-            roomScanner!!.start()
+                roomScanner = RoomScanner(CarController(car))
+                roomScanner!!.start()
+            }
         }
     }
 
@@ -29,4 +56,5 @@ fun main(args: Array<String>) {
     Client.shutDownClient()
 
     roomScanner?.interrupt()
+
 }

server/src/main/java/algorithm/AbstractAlgorithm.kt

@@ -7,6 +7,7 @@ import SonarResponse
 import algorithm.geometry.Angle
 import algorithm.geometry.AngleData
 import objects.Car
+import objects.CarReal
 import roomScanner.CarController.Direction.*
 import java.util.*
 import java.util.concurrent.TimeUnit

server/src/main/java/clInterface/executor/DebugInformation.kt

@@ -6,6 +6,7 @@ import DebugResponseMemoryStats
 import DebugResponseSonarStats
 import roomScanner.serialize
 import net.car.client.Client
+import objects.CarReal
 import objects.Environment
 import java.rmi.UnexpectedException
 
@@ -14,6 +15,9 @@ class DebugInformation : CommandExecutor {
     override fun execute(command: String) {
         val params = command.split(" ")
         val car = Environment.map[params[1].toInt()]!!
+        if (!(car is CarReal)) {
+            return
+        }
         val type = DebugRequest.Type.fromIntToType(params[2].toInt())
 
         val request = DebugRequest.BuilderDebugRequest(type).build()

server/src/main/java/clInterface/executor/Explore.kt

@@ -5,6 +5,7 @@ import roomScanner.serialize
 import SonarExploreAngleRequest
 import SonarExploreAngleResponse
 import net.car.client.Client
+import objects.CarReal
 import objects.Environment
 
 class Explore : CommandExecutor {
@@ -12,6 +13,9 @@ class Explore : CommandExecutor {
     override fun execute(command: String) {
         val params = command.split(" ")
         val car = Environment.map[params[1].toInt()]!!
+        if (!(car is CarReal)) {
+            return
+        }
         val angle = params[2].toInt()
         val window = params[3].toInt()
 

server/src/main/java/clInterface/executor/RouteMetric.kt

@@ -4,6 +4,7 @@ import CodedOutputStream
 import RouteMetricRequest
 import net.car.client.Client
 import objects.Car
+import objects.CarReal
 import objects.Environment
 import java.net.ConnectException
 
@@ -39,7 +40,11 @@ class RouteMetric : CommandExecutor {
         val requestBytes = ByteArray(routeMessage.getSizeNoTag())
         routeMessage.writeTo(CodedOutputStream(requestBytes))
         try {
-            car.carConnection.sendRequest(Client.Request.ROUTE_METRIC, requestBytes)
+            routeMessage.distances.forEachIndexed { idx, distance ->
+                val direction = roomScanner.CarController.Direction.values()
+                        .filter { it.id == routeMessage.directions[idx] }.first()
+                car.moveCar(distance, direction).get()
+            }
         } catch (e: ConnectException) {
             synchronized(Environment, {
                 Environment.map.remove(id)

server/src/main/java/clInterface/executor/Sonar.kt

@@ -2,7 +2,6 @@ package clInterface.executor
 
 import CodedOutputStream
 import SonarRequest
-import net.car.client.Client
 import objects.Car
 import objects.Environment
 import java.net.ConnectException
@@ -35,7 +34,7 @@ class Sonar : CommandExecutor {
         val requestBytes = ByteArray(requestMessage.getSizeNoTag())
         requestMessage.writeTo(CodedOutputStream(requestBytes))
         try {
-            car.carConnection.sendRequest(Client.Request.SONAR, requestBytes)
+            car.scan(requestMessage.angles, requestMessage.attempts.first(), requestMessage.windowSize, requestMessage.smoothing)
         } catch (e: ConnectException) {
             synchronized(Environment, {
                 Environment.map.remove(id)

server/src/main/java/net/car/Dropper.kt

@@ -1,6 +1,7 @@
 package net.car
 
 import net.car.client.Client
+import objects.CarReal
 import objects.Environment
 import java.net.ConnectException
 import kotlin.concurrent.thread
@@ -34,6 +35,9 @@ object Dropper {
         for (key in environment.map.keys) {
             try {
                 val carValue = environment.map[key] ?: continue
+                if (!(carValue is CarReal)) {
+                    continue
+                }
                 carValue.carConnection.sendRequest(Client.Request.PING, ByteArray(0))
             } catch (e: ConnectException) {
                 environment.map.remove(key)

server/src/main/java/objects/Car.kt

@@ -1,44 +1,17 @@
 package objects
 
-import CodedOutputStream
-import RouteMetricRequest
 import SonarRequest
-import net.car.client.Client
 import org.asynchttpclient.ListenableFuture
 import org.asynchttpclient.Response
-import roomScanner.CarController.Direction
+import roomScanner.CarController
-import roomScanner.serialize
 
-class Car constructor(val uid: Int, host: String, port: Int) {
+abstract class Car(val uid: Int) {
-
-    private val CHARGE_CORRECTION = 1.0//on full charge ok is 0.83 - 0.86
 
     var x = 0.0
     var y = 0.0
     var angle = 0.0
-    val carConnection = CarConnection(host, port)
 
-    fun moveCar(distance: Int, direction: Direction): ListenableFuture<Response> {
+    abstract fun moveCar(distance: Int, direction: CarController.Direction): ListenableFuture<Response>
 
-        val route = RouteMetricRequest.BuilderRouteMetricRequest(
+    abstract fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): ListenableFuture<Response>
-                IntArray(1, { (distance * CHARGE_CORRECTION).toInt() }), IntArray(1, { direction.id }))
-        val bytesRoute = ByteArray(route.getSizeNoTag())
-        route.writeTo(CodedOutputStream(bytesRoute))
-        return carConnection.sendRequest(Client.Request.ROUTE_METRIC, bytesRoute)
-    }
-
-    fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): ListenableFuture<Response> {
-        val message = SonarRequest.BuilderSonarRequest(
-                angles = angles,
-                attempts = IntArray(angles.size, { attempts }),
-                smoothing = smoothing,
-                windowSize = windowSize)
-                .build()
-        val data = serialize(message.getSizeNoTag(), { message.writeTo(it) })
-        return carConnection.sendRequest(Client.Request.SONAR, data)
-    }
-
-    override fun toString(): String {
-        return "$uid ; x:$x; y:$y; target:$angle"
-    }
 }

server/src/main/java/objects/CarReal.kt

@@ -0,0 +1,40 @@
+package objects
+
+import CodedOutputStream
+import RouteMetricRequest
+import SonarRequest
+import net.car.client.Client
+import org.asynchttpclient.ListenableFuture
+import org.asynchttpclient.Response
+import roomScanner.CarController.Direction
+import roomScanner.serialize
+
+class CarReal(uid: Int, host: String, port: Int) : Car(uid) {
+
+    private val CHARGE_CORRECTION = 1.0//on full charge ok is 0.83 - 0.86
+    val carConnection = CarConnection(host, port)
+
+    override fun moveCar(distance: Int, direction: Direction): ListenableFuture<Response> {
+
+        val route = RouteMetricRequest.BuilderRouteMetricRequest(
+                IntArray(1, { (distance * CHARGE_CORRECTION).toInt() }), IntArray(1, { direction.id }))
+        val bytesRoute = ByteArray(route.getSizeNoTag())
+        route.writeTo(CodedOutputStream(bytesRoute))
+        return carConnection.sendRequest(Client.Request.ROUTE_METRIC, bytesRoute)
+    }
+
+    override fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): ListenableFuture<Response> {
+        val message = SonarRequest.BuilderSonarRequest(
+                angles = angles,
+                attempts = IntArray(angles.size, { attempts }),
+                smoothing = smoothing,
+                windowSize = windowSize)
+                .build()
+        val data = serialize(message.getSizeNoTag(), { message.writeTo(it) })
+        return carConnection.sendRequest(Client.Request.SONAR, data)
+    }
+
+    override fun toString(): String {
+        return "$uid ; x:$x; y:$y; target:$angle"
+    }
+}

server/src/main/java/objects/Environment.kt

@@ -8,7 +8,7 @@ object Environment {
     @Synchronized
     fun connectCar(host: String, port: Int): Int {
         uid++
-        val car = Car(uid, host, port)
+        val car = CarReal(uid, host, port)
         onConnect.forEach { it(car) }
         map.put(uid, car)
         return uid

server/src/main/java/objects/emulator/CarEmulator.kt

@@ -0,0 +1,130 @@
+package objects.emulator
+
+import SonarRequest
+import SonarResponse
+import algorithm.geometry.Angle
+import algorithm.geometry.Line
+import algorithm.geometry.Vector
+import objects.Car
+import org.asynchttpclient.ListenableFuture
+import org.asynchttpclient.Response
+import roomScanner.CarController
+import roomScanner.CarController.Direction.*
+import roomScanner.serialize
+
+class CarEmulator(uid: Int, val testRoom: EmulatedRoom, val useRandom: Boolean, val randomGenerator: Rng) : Car(uid) {
+
+    private var xReal = 0
+    private var yReal = 0
+    private var angleReal = Angle(0)
+
+    override fun moveCar(distance: Int, direction: CarController.Direction): ListenableFuture<Response> {
+        val coef = if (useRandom) randomGenerator.nextInt(900, 1100).toDouble() / 1000.0 else 1.0
+        val distanceWithRandom = (coef * distance).toInt()
+        when (direction) {
+            FORWARD -> {
+                xReal += (Math.cos(angleReal.rads()) * distanceWithRandom).toInt()
+                yReal += (Math.sin(angleReal.rads()) * distanceWithRandom).toInt()
+            }
+            BACKWARD -> {
+                xReal -= (Math.cos(angleReal.rads()) * distanceWithRandom).toInt()
+                yReal -= (Math.sin(angleReal.rads()) * distanceWithRandom).toInt()
+            }
+            LEFT -> angleReal += Angle(distanceWithRandom)
+            RIGHT -> angleReal -= Angle(distanceWithRandom)
+        }
+        println("x=$xReal y=$yReal angle=${angleReal.degs()}")
+        return ListenableFutureImpl(ByteArray(0))
+    }
+
+    override fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): ListenableFuture<Response> {
+
+
+        val xSensor0 = xReal.toInt()
+        val ySensor0 = yReal.toInt()
+        val carAngle = angleReal
+
+        val distances = arrayListOf<Int>()
+        angles.forEach { angle ->
+            val angleFinal = getSensorAngle(angle, carAngle.degs())
+            val xSensor1: Int
+            val ySensor1: Double
+            //tg can be equal to inf if angle = 90 or 270. it vertical line. x1 = x0, y1 = y0+-[any number. eg 1]
+            when (angleFinal) {
+                90 -> {
+                    xSensor1 = xSensor0
+                    ySensor1 = (ySensor0 + 1).toDouble()
+                }
+                270 -> {
+                    xSensor1 = xSensor0
+                    ySensor1 = (ySensor0 - 1).toDouble()
+                }
+                in (90..270) -> {
+                    xSensor1 = xSensor0 - 1
+                    ySensor1 = ySensor0 + (xSensor1 - xSensor0) * Math.tan(angleFinal * Math.PI / 180)
+                }
+                else -> {
+                    xSensor1 = xSensor0 + 1
+                    ySensor1 = ySensor0 + (xSensor1 - xSensor0) * Math.tan(angleFinal * Math.PI / 180)
+                }
+            }
+            val sensorLine = Line(ySensor0 - ySensor1, xSensor1.toDouble() - xSensor0,
+                    xSensor0 * ySensor1 - ySensor0 * xSensor1)
+
+            val sensorVector = Vector(xSensor0.toDouble(), ySensor0.toDouble(), xSensor1.toDouble(), ySensor1)
+            val distance = getDistance(xSensor0, ySensor0, sensorLine, sensorVector)
+            if (distance == -1) {
+                distances.add(distance)
+            } else {
+                val delta = if (useRandom) randomGenerator.nextInt(-2, 2) else 0//return one of -2 -1 0 1 or 2
+                distances.add(distance + delta)
+            }
+        }
+
+        val responseMessage = SonarResponse.BuilderSonarResponse(distances.toIntArray()).build()
+        val bytes = serialize(responseMessage.getSizeNoTag(), { responseMessage.writeTo(it) })
+
+        return ListenableFutureImpl(bytes)
+    }
+
+    private fun getDistance(xSensor0: Int, ySensor0: Int, sensorLine: Line, sensorVector: Vector): Int {
+        var result = Long.MAX_VALUE
+        for (wall in testRoom.emulatedWalls) {
+            val wallLine = wall.line
+            val slope = sensorLine.A * wallLine.B - sensorLine.B * wallLine.A
+            if (Math.abs(slope) < 0.01) {
+                //line is parallel.
+                continue
+            }
+            val xIntersection = (sensorLine.B * wallLine.C - wallLine.B * sensorLine.C) / slope
+            val yIntersection = (sensorLine.C * wallLine.A - wallLine.C * sensorLine.A) / slope
+
+            //filters by direction and intersection position
+            val intersectionVector = Vector(xSensor0.toDouble(), ySensor0.toDouble(), xIntersection, yIntersection)
+            if (intersectionVector.scalarProduct(sensorVector) < 0) {
+                continue
+            }
+            val wallVector1 = Vector(xIntersection, yIntersection, wall.xTo.toDouble(), wall.yTo.toDouble())
+            val wallVector2 = Vector(xIntersection, yIntersection, wall.xFrom.toDouble(), wall.yFrom.toDouble())
+            if (wallVector1.scalarProduct(wallVector2) > 0) {
+                continue
+            }
+            val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
+                    + Math.pow(yIntersection - ySensor0, 2.0)))
+            if (currentDistance < result) {
+                result = currentDistance
+            }
+        }
+        return result.toInt()
+    }
+
+
+    private fun getSensorAngle(requestAngle: Int, carAngle: Int): Int {
+        var angleTmp = carAngle - requestAngle
+        while (angleTmp < 0) {
+            angleTmp += 360
+        }
+        return angleTmp % 360
+    }
+}
+

server/src/main/java/objects/emulator/EmulatedRoom.kt

@@ -0,0 +1,44 @@
+package objects.emulator
+
+import org.w3c.dom.Node
+import java.io.File
+import javax.xml.parsers.DocumentBuilderFactory
+
+class EmulatedRoom() {
+    val emulatedWalls = arrayListOf<EmulatedWall>()
+
+    constructor(EmulatedWalls: Collection<EmulatedWall>) : this() {
+        this.emulatedWalls.addAll(EmulatedWalls)
+    }
+
+    companion object {
+        fun EmulatedRoomFromFile(pathToFile: String): EmulatedRoom? {
+
+            val factory = DocumentBuilderFactory.newInstance()
+            val builder = factory.newDocumentBuilder()
+            val doc = builder.parse(File(pathToFile))
+            val root = doc.getDocumentElement()
+            val rootChild = root.childNodes
+            var walls: Node? = null
+            for (i in 0..rootChild.length - 1) {
+                val node = rootChild.item(i)
+                if (node.nodeType == Node.ELEMENT_NODE) {
+                    walls = node
+                    break
+                }
+            }
+            walls ?: return null
+            val wallsChild = walls.childNodes
+            val result = EmulatedRoom()
+            for (i in 0..wallsChild.length - 1) {
+                val wall = wallsChild.item(i)
+                if (wall.nodeType != Node.ELEMENT_NODE) {
+                    continue
+                }
+                val EmulatedWall = EmulatedWall.wallFromXml(wall) ?: return null
+                result.emulatedWalls.add(EmulatedWall)
+            }
+            return result
+        }
+    }
+}

server/src/main/java/objects/emulator/EmulatedWall.kt

@@ -0,0 +1,27 @@
+package objects.emulator
+
+import algorithm.geometry.Line
+import org.w3c.dom.Node
+
+class EmulatedWall constructor(val line: Line, val xFrom: Int, val xTo: Int, val yFrom: Int, val yTo: Int) {
+
+    companion object {
+        fun wallFromXml(wall: Node): EmulatedWall? {
+            val points = wall.attributes
+            val startX: Int
+            val endX: Int
+            val endY: Int
+            val startY: Int
+            try {
+                startX = points.getNamedItem("startX").nodeValue.toInt()
+                startY = points.getNamedItem("startY").nodeValue.toInt()
+                endX = points.getNamedItem("endX").nodeValue.toInt()
+                endY = points.getNamedItem("endY").nodeValue.toInt()
+            } catch (e: NumberFormatException) {
+                return null
+            }
+            val line = Line((startY - endY).toDouble(), (endX - startX).toDouble(), (startX * endY - startY * endX).toDouble())
+            return EmulatedWall(line, startX, endX, startY, endY)
+        }
+    }
+}

server/src/main/java/objects/emulator/ListenableFutureImpl.kt

@@ -0,0 +1,54 @@
+package objects.emulator
+
+import io.netty.buffer.ByteBuf
+import io.netty.buffer.Unpooled
+import org.asynchttpclient.ListenableFuture
+import org.asynchttpclient.Response
+import org.asynchttpclient.netty.EagerResponseBodyPart
+import org.asynchttpclient.netty.NettyResponse
+import java.util.concurrent.CompletableFuture
+import java.util.concurrent.Executor
+import java.util.concurrent.TimeUnit
+
+class ListenableFutureImpl(bytes: ByteArray) : ListenableFuture<Response> {
+
+    val response = NettyResponse(null, null, listOf(EagerResponseBodyPart(Unpooled.copiedBuffer(bytes), true)))
+
+    override fun done() {
+
+    }
+
+    override fun cancel(p0: Boolean): Boolean {
+        return false
+    }
+
+    override fun touch() {
+    }
+
+    override fun get(): Response {
+        return response
+    }
+
+    override fun get(p0: Long, p1: TimeUnit): Response {
+        return response
+    }
+
+    override fun addListener(listener: Runnable?, exec: Executor?): ListenableFuture<Response> {
+        return this
+    }
+
+    override fun abort(t: Throwable?) {
+    }
+
+    override fun isDone(): Boolean {
+        return true
+    }
+
+    override fun toCompletableFuture(): CompletableFuture<Response> {
+        throw UnsupportedOperationException("not implemented")
+    }
+
+    override fun isCancelled(): Boolean {
+        return false
+    }
+}

server/src/main/java/objects/emulator/Rng.kt

@@ -0,0 +1,28 @@
+package objects.emulator
+
+class Rng(seed: Long) {
+    var curState = seed
+    val a = 1664525L
+    val c = 1013904223L
+    val mod = 2147483648L
+
+    fun abs(value: Long): Long {
+        if (value < 0)
+            return -value
+        return value
+    }
+
+    fun nextInt(): Int {
+        val res = curState
+        curState = abs(a * curState + c) % mod
+        return (res % mod).toInt()
+    }
+
+    fun nextInt(min_val: Int, max_val: Int): Int {
+        val rand = nextInt()
+        val size = max_val - min_val + 1
+        val randInRange = rand % size
+        val res = randInRange + min_val
+        return res
+    }
+}

server/src/main/java/roomScanner/CarController.kt

@@ -5,9 +5,9 @@ import RouteMetricRequest
 import SonarRequest
 import SonarResponse
 import net.car.client.Client
-import objects.Car
+import objects.CarReal
 
-class CarController(var car: Car) {
+class CarController(var car: CarReal) {
     enum class Direction(val id: Int) {
         FORWARD(0),
         BACKWARD(1),

server/src/main/java/roomScanner/RoomScanner.kt

@@ -6,7 +6,7 @@ class RoomScanner(val controller: CarController) : Thread() {
     private val GHOST_LEVEL = 50
 
     override fun run() {
-        println("Car connected ${controller.car.carConnection.host}")
+        println("CarReal connected ${controller.car.carConnection.host}")
         for (i in 1..100) {
             step()
             println(plot(points))