main server refactoring

2016-09-05 13:09:49 +03:00
parent 191a65bede
commit a589f7da9c
30 changed files with 524 additions and 497 deletions
@@ -1,285 +0,0 @@
-import RoomScanner.serialize
-import algorithm.AbstractAlgorithm
-import algorithm.AlgorithmThread
-import algorithm.RoomBypassingAlgorithm
-import algorithm.RoomModel
-import net.car.client.Client
-import objects.Car
-import objects.Environment
-import java.net.ConnectException
-import java.rmi.UnexpectedException
-
-object DebugClInterface {
-
-    //todo remove
-    var algorithmImpl: AbstractAlgorithm? = null
-    var algorithmThread: AlgorithmThread? = null
-
-    private val routeRegex = Regex("route [0-9]{1,10}")
-    private val sonarRegex = Regex("sonar [0-9]{1,10}")
-    private val helpString = "available handlers:\n" +
-            "cars - get list of connected cars\n" +
-            "route [car_id] - setting a route for net.car with net.car id.\n" +
-            "sonar [car_id] - get sonar data\n" +
-            "dbinfo [car_id] [type] - refresh all net.car locations\n" +
-            "lines - print lines, detected by net.car\n" +
-            "alg [count] - run algorithm. Make [count] iteration\n" +
-            "type is string name of value or int value. available values: MEMORYSTATS - 0\n" +
-            "stop - exit from this interface and stop all threads\n"
-
-    fun run() {
-
-        println(helpString)
-        while (true) {
-            val readString = readLine()
-            if (readString == null || readString.equals("stop")) {
-                break
-            }
-            if (readString.equals("")) {
-                continue
-            }
-
-            try {
-                executeCommand(readString)
-            } catch (exception: Exception) {
-                exception.printStackTrace()
-                println("Fail to execute command[$readString]: $exception")
-            }
-        }
-    }
-
-    private fun printNotSupportedCommand(command: String) {
-        println("not supported command: $command")
-        println(helpString)
-    }
-
-    private fun executeCommand(readString: String) {
-        val commandType = readString.split(" ")[0].toLowerCase()
-        when (commandType) {
-            "cars" -> {
-                synchronized(Environment, {
-                    println(Environment.map.values)
-                })
-            }
-            "route" -> executeRouteCommand(readString)
-            "sonar" -> executeSonarCommand(readString)
-            "dbinfo" -> executeDebugInfoCommand(readString)
-            "lines" -> algorithm.RoomModel.walls.forEach { println(it.line) }
-            "pos" -> {
-                val tmp = algorithmImpl
-                if (tmp is RoomBypassingAlgorithm) {
-                    println("walls: ${RoomModel.walls}")
-                    println("x: ${tmp.thisCar.x} y: ${tmp.thisCar.y} angle:${tmp.thisCar.angle}")
-                }
-            }
-            "alg" -> executeAlg(readString)
-            "explore" -> executeExplore(readString)
-            else -> printNotSupportedCommand(readString)
-        }
-    }
-
-    private fun executeExplore(readString: String) {
-        val params = readString.split(" ")
-        val car = Environment.map[params[1].toInt()]!!
-        val angle = params[2].toInt()
-        val window = params[3].toInt()
-
-        val request = SonarExploreAngleRequest.BuilderSonarExploreAngleRequest(angle, window).build()
-        val responseData = car.carConnection.sendRequest(
-                Client.Request.EXPLORE_ANGLE,
-                serialize(request.getSizeNoTag(), { request.writeTo(it) })
-        ).get().responseBodyAsBytes
-
-        val distances = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0)).parseFrom(CodedInputStream(responseData)).build().distances
-
-        println("Received distances: [${distances.joinToString()}]")
-    }
-
-    private fun executeAlg(readString: String) {
-        val params = readString.split(" ")
-        val count =
-                if (params.size == 2) try {
-                    params[1].toInt()
-                } catch (e: Exception) {
-                    1
-                } else 1
-
-        var alg = algorithmImpl
-        if (alg == null) {
-            alg = RoomBypassingAlgorithm(Environment.map.values.last())
-        }
-        var algThread = algorithmThread
-        if (algThread == null) {
-            algThread = AlgorithmThread(alg)
-            algThread.start()
-        }
-        algorithmImpl = alg
-        algThread.setCount(count)
-        algorithmThread = algThread
-    }
-
-    private fun executeDebugInfoCommand(readString: String) {
-        val params = readString.split(" ")
-        val car = Environment.map[params[1].toInt()]!!
-        val type = DebugRequest.Type.fromIntToType(params[2].toInt())
-
-        val request = DebugRequest.BuilderDebugRequest(type).build()
-        val requestType = when (type) {
-            DebugRequest.Type.MEMORY_STATS -> Client.Request.DEBUG_MEMORY
-            DebugRequest.Type.SONAR_STATS -> Client.Request.DEBUG_SONAR
-            else -> throw UnexpectedException(type.toString())
-        }
-
-        val responseData = car.carConnection.sendRequest(
-                requestType,
-                serialize(request.getSizeNoTag(), { request.writeTo(it) })
-        ).get().responseBodyAsBytes
-
-        when (type) {
-            DebugRequest.Type.MEMORY_STATS -> {
-                val data = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build()
-                println("Heap static tail: ${data.heapStaticTail}")
-                println("Heap dynamic tail: ${data.heapDynamicTail}")
-                println("Heap dynamic max size: ${data.heapDynamicMaxBytes}")
-                println("Heap dynamic total size: ${data.heapDynamicTotalBytes}")
-            }
-            DebugRequest.Type.SONAR_STATS -> {
-                val data = DebugResponseSonarStats.BuilderDebugResponseSonarStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build()
-                println("Sonar measurement total: ${data.measurementCount}")
-                println("Failed checksums: ${data.measurementFailedChecksum}")
-                println("Failed command: ${data.measurementFailedCommand}")
-                println("Failed distance: ${data.measurementFailedDistance}")
-            }
-            else -> throw UnexpectedException(type.toString())
-        }
-    }
-
-    private fun executeSonarCommand(readString: String) {
-        if (!sonarRegex.matches(readString)) {
-            println("incorrect args of command sonar.")
-            println(helpString)
-            return
-        }
-        val id: Int
-        try {
-            id = readString.split(" ")[1].toInt()
-        } catch (e: NumberFormatException) {
-            e.printStackTrace()
-            println("error in converting id to int type")
-            println(helpString)
-            return
-        }
-        val car: Car? = synchronized(Environment, {
-            Environment.map[id]
-        })
-        if (car == null) {
-            println("net.car with id=$id not found")
-            return
-        }
-        val requestMessage = getSonarRequest() ?: return
-        val requestBytes = ByteArray(requestMessage.getSizeNoTag())
-        requestMessage.writeTo(CodedOutputStream(requestBytes))
-        try {
-            car.carConnection.sendRequest(Client.Request.SONAR, requestBytes)
-        } catch (e: ConnectException) {
-            synchronized(Environment, {
-                Environment.map.remove(id)
-            })
-        }
-    }
-
-    private fun getSonarRequest(): SonarRequest? {
-        println("print angles, after printing all angles print done")
-        val angles = arrayListOf<Int>()
-        while (true) {
-            val command = readLine()!!.toLowerCase()
-            when (command) {
-                "reset" -> return null
-                "done" -> {
-                    val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN)
-                    return sonarBuilder.build()
-                }
-                else -> {
-                    try {
-                        val angle = command.toInt()
-                        if (angle < 0 || angle > 180) {
-                            println("incorrect angle $angle. angle must be in [0,180] and div on 4")
-                        } else {
-                            angles.add(angle)
-                        }
-                    } catch (e: NumberFormatException) {
-                        println("error in converting angle to int. try again")
-                    }
-                }
-            }
-        }
-    }
-
-    private fun executeRouteCommand(readString: String) {
-        if (!routeRegex.matches(readString)) {
-            println("incorrect args of command route.")
-            println(helpString)
-            return
-        }
-        val id: Int
-        try {
-            id = readString.split(" ")[1].toInt()
-        } catch (e: NumberFormatException) {
-            e.printStackTrace()
-            println("error in converting id to int type")
-            println(helpString)
-            return
-        }
-        val car: Car? =
-                synchronized(Environment, {
-                    Environment.map[id]
-                })
-        if (car == null) {
-            println("net.car with id=$id not found")
-            return
-        }
-        val routeMessage = getRouteMessage() ?: return
-        val requestBytes = ByteArray(routeMessage.getSizeNoTag())
-        routeMessage.writeTo(CodedOutputStream(requestBytes))
-        try {
-            car.carConnection.sendRequest(Client.Request.ROUTE_METRIC, requestBytes)
-        } catch (e: ConnectException) {
-            synchronized(Environment, {
-                Environment.map.remove(id)
-            })
-        }
-    }
-
-    private fun getRouteMessage(): RouteRequest? {
-        println("print way points in polar coordinate als [distance/degrees] [direction] in sm and degrees." +
-                "after enter all points print \"done\". for reset route print \"reset\". available directions:" +
-                "0 - FORWARD, 1 - BACKWARD, 2 - LEFT, 3 - RIGHT")
-        val distances = arrayListOf<Int>()
-        val angles = arrayListOf<Int>()
-        while (true) {
-            val readLine = readLine()!!.toLowerCase()
-            when (readLine) {
-                "reset" -> return null
-                "done" -> {
-                    val routeBuilder = RouteRequest.BuilderRouteRequest(distances.toIntArray(), angles.toIntArray())
-                    return routeBuilder.build()
-                }
-                else -> {
-                    val wayPointData = readLine.split(" ")
-                    val distance: Int
-                    val angle: Int
-                    try {
-                        distance = wayPointData[0].toInt()
-                        angle = wayPointData[1].toInt()
-                        distances.add(distance)
-                        angles.add(angle)
-                    } catch (e: NumberFormatException) {
-                        println("error in converting target or distance to int. try again")
-                    } catch (e: IndexOutOfBoundsException) {
-                        println("format error, u must print two number separated by spaces. Try again")
-                    }
-                }
-            }
-        }
-    }
-}
@@ -1,6 +1,5 @@
 import java.io.BufferedWriter
 import java.io.File
-import java.io.FileOutputStream
 import java.io.FileWriter

 object Logger {
@@ -14,12 +13,13 @@ object Logger {
        padding += paddingStep
    }

-    fun outdent() {
+    fun outDent() {
        if (padding.length == 0) {
-            throw IllegalArgumentException("Called outdent without corresponsing indent")
+            throw IllegalArgumentException("Called out dent without corresponding indent")
        }
        padding = padding.removeSuffix(paddingStep)
    }
+
    fun log(msg: String) {
        bufferedWriter.write(padding + msg + "\n")
        bufferedWriter.flush()
@@ -1,5 +1,6 @@
-import RoomScanner.CarController
-import RoomScanner.RoomScanner
+import roomScanner.CarController
+import roomScanner.RoomScanner
+import clInterface.DebugClInterface
 import net.car.Dropper
 import net.car.client.Client
 import objects.Environment
@@ -20,9 +21,6 @@ fun main(args: Array<String>) {
        }
    }

-    //todo запуск потока алгоритма нужно вынести сюда,
-    //todo а debug интерфейсутребуется исключительно возможность установки кол-ва итераций
-    //CL user interface
    DebugClInterface.run()

    carsDestroy.interrupt()
@@ -1,19 +1,13 @@
 package algorithm

-import CodedInputStream
-import CodedOutputStream
 import Logger
 import RouteMetricRequest
 import SonarRequest
-import SonarResponse
 import algorithm.geometry.Angle
 import algorithm.geometry.AngleData
-import net.car.client.Client
 import objects.Car
-import java.net.ConnectException
+import roomScanner.CarController.Direction.*
 import java.util.*
-import java.util.concurrent.TimeUnit
-import java.util.concurrent.TimeoutException

 abstract class AbstractAlgorithm(val thisCar: Car) {

@@ -21,11 +15,7 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
    open val SMOOTHING = SonarRequest.Smoothing.NONE
    open val WINDOW_SIZE = 0

-    protected val FORWARD = 0
-    protected val BACKWARD = 1
-    protected val LEFT = 2
-    protected val RIGHT = 3
-
+    val carController = CarController(thisCar.carConnection)
    private val historySize = 10
    private val history = Stack<RouteMetricRequest>()

@@ -44,50 +34,6 @@ abstract class AbstractAlgorithm(val thisCar: Car) {

    private var iterationCounter = 0

-    protected fun getData(angles: Array<Angle>): IntArray {
-
-        val anglesIntArray = (angles.map { it -> it.degs() }).toIntArray()
-        val message = SonarRequest.BuilderSonarRequest(
-                angles = anglesIntArray,
-                attempts = IntArray(angles.size, { ATTEMPTS }),
-                smoothing = SMOOTHING,
-                windowSize = WINDOW_SIZE)
-                .build()
-        val requestBytes = ByteArray(message.getSizeNoTag())
-        message.writeTo(CodedOutputStream(requestBytes))
-        try {
-            val futureListener = thisCar.carConnection.sendRequest(Client.Request.SONAR, requestBytes)
-            val bytes = futureListener.get(300, TimeUnit.SECONDS).responseBodyAsBytes
-            val responseMessage = SonarResponse.BuilderSonarResponse(IntArray(0)).build()
-            responseMessage.mergeFrom(CodedInputStream(bytes))
-            return responseMessage.distances
-        } catch (e: ConnectException) {
-            println("connection error!")
-        } catch (e: TimeoutException) {
-            println("don't have response from net.car. Timeout!")
-        }
-        return IntArray(0)
-    }
-
-    //todo методы управления машинкой должны быть в отдельном классе по аналогии с carController
-    protected fun moveCar(message: RouteMetricRequest) {
-        val requestBytes = ByteArray(message.getSizeNoTag())
-        message.writeTo(CodedOutputStream(requestBytes))
-        moveCar(requestBytes)
-    }
-
-
-    private fun moveCar(messageBytes: ByteArray) {
-        try {
-            thisCar.carConnection.sendRequest(Client.Request.ROUTE_METRIC, messageBytes).get(60, TimeUnit.SECONDS)
-        } catch (e: ConnectException) {
-            println("connection error!")
-        } catch (e: TimeoutException) {
-            println("don't have response from net.car. Timeout!")
-        }
-        return
-    }
-
    fun iterate() {
        Logger.log("============= STARTING ITERATION $iterationCounter ============")
        Logger.indent()
@@ -96,7 +42,7 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
            return
        }
        val angles = getAngles()
-        val distances = getData(angles)
+        val distances = carController.scan(angles.map { it.degs() }.toIntArray(), ATTEMPTS, WINDOW_SIZE, SMOOTHING)
        if (distances.size != angles.size) {
            throw RuntimeException("error! angles and distances have various sizes")
        }
@@ -126,22 +72,22 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
        Logger.indent()
        Logger.log("Directions = ${Arrays.toString(command.directions)}")
        Logger.log("Distanced = ${Arrays.toString(command.distances)}")
-        Logger.outdent()
+        Logger.outDent()
        println(Arrays.toString(command.directions))
        println(Arrays.toString(command.distances))

        this.prevSonarDistances = anglesDistances
        this.prevState = state

-        moveCar(command)
-        Logger.outdent()
+        carController.moveCar(command)
+        Logger.outDent()
        Logger.log("============= FINISHING ITERATION $iterationCounter ============")
        Logger.log("")
    }

    private fun addCancelIterationToLog() {
        Logger.log("iteration cancelled. need more data from sonar")
-        Logger.outdent()
+        Logger.outDent()
        Logger.log("============= FINISHING ITERATION $iterationCounter ============")
        Logger.log("")
    }
@@ -169,10 +115,10 @@ abstract class AbstractAlgorithm(val thisCar: Car) {

        for ((index, dir) in res.directions.withIndex()) {
            res.directions[index] = when (dir) {
-                FORWARD -> BACKWARD
-                BACKWARD -> FORWARD
-                LEFT -> RIGHT
-                RIGHT -> LEFT
+                FORWARD.id -> BACKWARD.id
+                BACKWARD.id -> FORWARD.id
+                LEFT.id -> RIGHT.id
+                RIGHT.id -> LEFT.id
                else -> throw IllegalArgumentException("Unexpected direction = $dir found during command inversion")
            }
        }
@@ -186,8 +132,8 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
        Logger.indent()
        Logger.log("Last command: ${lastCommand.toString()}")
        Logger.log("Inverted cmd: ${invertedCommand.toString()}")
-        Logger.outdent()
-        moveCar(invertedCommand)
+        Logger.outDent()
+        carController.moveCar(invertedCommand)
    }

    protected fun rollback(steps: Int) {
@@ -199,7 +145,7 @@ abstract class AbstractAlgorithm(val thisCar: Car) {
            rollback()
            stepsRemaining--
        }
-        Logger.outdent()
+        Logger.outDent()
        Logger.log("=== Finished rollback ===")
    }

@@ -1,30 +1,29 @@
 package algorithm

-class AlgorithmThread(val algorithmImpl: AbstractAlgorithm) : Thread() {
+class AlgorithmThread() : Thread() {

+    var algorithmImpl: AbstractAlgorithm? = null
    private var count = 0

    override fun run() {
-        while (!algorithmImpl.isCompleted()) {
+        val currentAlgorithm = algorithmImpl ?: return
+        while (!currentAlgorithm.isCompleted()) {
            try {
                if (count > 0) {
-                    algorithmImpl.iterate()
+                    currentAlgorithm.iterate()
                    count--
                } else {
                    Thread.sleep(1000)
                }
            } catch (e: InterruptedException) {
-
+                println("algorithm thread is interrupted")
+                break
            }
        }
-        println("algorithm is finished!")
+        println("algorithm is completed")
    }

    fun setCount(count: Int) {
        this.count = count
    }
-
-    fun cancel() {
-        count = 0
-    }
-}
+}
@@ -0,0 +1,60 @@
+package algorithm
+
+import CodedInputStream
+import roomScanner.serialize
+import RouteMetricRequest
+import SonarRequest
+import SonarResponse
+import net.car.client.Client
+import objects.CarConnection
+import java.net.ConnectException
+import java.util.concurrent.TimeUnit
+import java.util.concurrent.TimeoutException
+
+
+class CarController(val carConnection: CarConnection) {
+
+    fun scan(angles: IntArray, attempts: Int, windowSize: Int, smoothing: SonarRequest.Smoothing): IntArray {
+        val message = SonarRequest.BuilderSonarRequest(
+                angles = angles,
+                attempts = IntArray(angles.size, { attempts }),
+                smoothing = smoothing,
+                windowSize = windowSize)
+                .build()
+        val data = serialize(message.getSizeNoTag(), { message.writeTo(it) })
+        val response: ByteArray
+        try {
+            val future = carConnection.sendRequest(Client.Request.SONAR, data)
+            response = future.get(300, TimeUnit.SECONDS).responseBodyAsBytes
+
+        } catch (e: ConnectException) {
+            println("connection error!")
+            return IntArray(0)
+        } catch (e: TimeoutException) {
+            println("don't have response from net.car. Timeout!")
+            return IntArray(0)
+        }
+        return SonarResponse.BuilderSonarResponse(IntArray(0)).parseFrom(CodedInputStream(response)).build().distances
+    }
+
+    fun moveCar(message: RouteMetricRequest) {
+        val requestBytes = serialize(message.getSizeNoTag(), { message.writeTo(it) })
+        moveCar(requestBytes)
+    }
+
+    fun moveCar(distances: IntArray, directions: IntArray) {
+        val routeMetric = RouteMetricRequest.BuilderRouteMetricRequest(distances, directions).build()
+        moveCar(routeMetric)
+    }
+
+    private fun moveCar(messageBytes: ByteArray) {
+        try {
+            carConnection.sendRequest(Client.Request.ROUTE_METRIC, messageBytes).get(60, TimeUnit.SECONDS)
+        } catch (e: ConnectException) {
+            println("connection error!")
+        } catch (e: TimeoutException) {
+            println("don't have response from net.car. Timeout!")
+        }
+        return
+    }
+}
@@ -6,13 +6,10 @@ import RouteMetricRequest
 import SonarRequest
 import algorithm.geometry.*
 import objects.Car
+import roomScanner.CarController.Direction.*

 class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {

-
-    //todo refactoring all algorithm package
-
-
    override val ATTEMPTS: Int = 5
    override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
    override val WINDOW_SIZE = 3
@@ -39,27 +36,23 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
    }

    private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD))
        val wallAngle = calculateAngle(anglesDistances, state)
        Logger.indent()
-        resultBuilder.setDistances(getIntArray(20, wallAngle.degs(), 75))
        thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() - wallAngle.degs()).toDouble()
        addWall(-wallAngle)
-        Logger.outdent()
+        Logger.outDent()
        calibrateAfterRotate = true
-        return resultBuilder.build()
+        return buildRoute(
+                getIntArray(20, wallAngle.degs(), 75),
+                getIntArray(FORWARD.id, RIGHT.id, FORWARD.id))
    }

    private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
        val wallAngle = calculateAngle(anglesDistances, state)
-        resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15))
        thisCar.angle = (RoomModel.walls.last().wallAngleOX.degs() + wallAngle.degs()).toDouble()
        addWall(wallAngle)
        calibrateAfterRotate = true
-        return resultBuilder.build()
+        return buildRoute(getIntArray(wallAngle.degs(), 15), getIntArray(LEFT.id, FORWARD.id))
    }

    private fun tryAlignParallelToWall(anglesDistances: Map<Angle, AngleData>, state: CarState, average: Double): RouteMetricRequest? {
@@ -74,50 +67,41 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
                    distRightBackward.distance < (average / 2) || distRightForward.distance < (average / 2)) {
                continue
            }
-
            if (Math.abs(distRightForward.distance - distRightBackward.distance) <= ISOSCALENESS_MIN_DIFF) {
                return null
            }
-
            log("Flaw in align found, correcting")
-            val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
            val rotationDirection = if (distRightBackward.distance > distRightForward.distance) LEFT else RIGHT
-            resultBuilder.setDirections(getIntArray(rotationDirection))
-            resultBuilder.setDistances(getIntArray(Math.min(Math.abs(distRightBackward.distance - distRightForward.distance), 20)))
-            return resultBuilder.build()
+            return buildRoute(
+                    getIntArray(Math.min(Math.abs(distRightBackward.distance - distRightForward.distance), 20)),
+                    getIntArray(rotationDirection.id))
        }

        return null  // TODO: everything is broken, what to do?
    }

    private fun correctDistanceToParallelWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-
        val distToWall = anglesDistances[Angle(90)]!!.distance
        val rotationDirection = if (distToWall > DISTANCE_TO_WALL_UPPER_BOUND) RIGHT else LEFT
        val backRotationDirection = if (rotationDirection == RIGHT) LEFT else RIGHT
-        resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD, backRotationDirection))
        val rangeToCorridor = if (distToWall > DISTANCE_TO_WALL_UPPER_BOUND) {
            distToWall - DISTANCE_TO_WALL_UPPER_BOUND
        } else {
            DISTANCE_TO_WALL_LOWER_BOUND - distToWall
        }
-        resultBuilder.setDistances(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt()))
-        return resultBuilder.build()
+        return buildRoute(getIntArray(2 * rangeToCorridor + 5, 20, (1.5 * rangeToCorridor).toInt()),
+                getIntArray(rotationDirection.id, FORWARD.id, backRotationDirection.id))
    }

    private fun moveForward(distToWall: Int): RouteMetricRequest {
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        resultBuilder.setDirections(getIntArray(FORWARD))
-        resultBuilder.setDistances(getIntArray(Math.max(distToWall / 4, 20)))
-        return resultBuilder.build()
+        return buildRoute(getIntArray(Math.max(distToWall / 4, 20)), getIntArray(FORWARD.id))
    }

    private fun addWall(angleWithPrevWall: Angle) {
        log("Adding wall")
        Logger.indent()
        updateWalls()
-        Logger.outdent()
+        Logger.outDent()
        val firstWall = RoomModel.walls.first()
        val lastWall = RoomModel.walls.last()
        if (circleFound) {
@@ -134,7 +118,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
        }
    }

-    fun updateWalls() {
+    private fun updateWalls() {
        synchronized(RoomModel) {
            val walls = RoomModel.walls
            if (walls.size < 2) {
@@ -166,7 +150,6 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
        val dist80 = anglesDistances[Angle(80)]!!
        val dist90 = anglesDistances[Angle(90)]!!
        val dist100 = anglesDistances[Angle(100)]!!
-        val dist110 = anglesDistances[Angle(110)]!!
        val sonarAngle = (thisCar.angle - 90).toInt()

        if (anglesDistances.filter {
@@ -191,7 +174,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
            log("Calibrating after rotate")
            Logger.indent()
            val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average)
-            Logger.outdent()
+            Logger.outDent()
            if (maybeAlignment != null) {
                log("Realigning")
                return maybeAlignment
@@ -212,8 +195,8 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {

        // Add point to room map
        val point = Point(
-                x = thisCar.x + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
-                y = thisCar.y + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
+                x = thisCar.x + dist90.distance * Math.cos(Angle(sonarAngle).rads()),
+                y = thisCar.y + dist90.distance * Math.sin(Angle(sonarAngle).rads())
        )
        log("Adding middle point ${point.toString()} to wall ${RoomModel.walls.last().id}")
        RoomModel.walls.last().pushBackPoint(point)
@@ -239,8 +222,8 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
                continue
            }
            val point = Point(
-                    x = thisCar.x + curDist * Math.cos(degreesToRadian((thisCar.angle - i).toInt())),
-                    y = thisCar.y + curDist * Math.sin(degreesToRadian((thisCar.angle - i).toInt()))
+                    x = thisCar.x + curDist * Math.cos(Angle((thisCar.angle - i).toInt()).rads()),
+                    y = thisCar.y + curDist * Math.sin(Angle((thisCar.angle - i).toInt()).rads())
            )
            log("Adding ${point.toString()} to wall ${RoomModel.walls.last().id}")
            RoomModel.walls.last().pushBackPoint(point)
@@ -251,7 +234,7 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
        log("4. Check if we have to align parallel to the wall")
        Logger.indent()
        val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average)
-        Logger.outdent()
+        Logger.outDent()
        if (maybeAlignment != null) {
            log("Realigning")
            return maybeAlignment
@@ -298,39 +281,21 @@ class RoomBypassingAlgorithm(thisCar: Car) : AbstractAlgorithm(thisCar) {
        return args
    }

+    private fun buildRoute(distances: IntArray, directions: IntArray): RouteMetricRequest {
+        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(distances, directions)
+        return resultBuilder.build()
+    }
+
    override fun isCompleted(): Boolean {
        return isCompleted
    }

    override fun afterGetCommand(route: RouteMetricRequest) {
-        route.directions.forEachIndexed { idx, direction ->
-            when (direction) {
-                FORWARD -> {
-                    thisCar.x += (Math.cos(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt()
-                    thisCar.y += (Math.sin(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt()
-                }
-                BACKWARD -> {
-                    thisCar.x -= (Math.cos(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt()
-                    thisCar.y -= (Math.sin(degreesToRadian(thisCar.angle.toInt())) * route.distances[idx]).toInt()
-                }
-                LEFT -> {
-                    route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
-                }
-                RIGHT -> {
-                    route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
-                }
-            }
-        }
+        thisCar.refreshPositionAfterRoute(route)
        if (Math.round(Math.cos(Angle(thisCar.angle.toInt()).rads())).toInt() == 1 && thisCar.angle.toInt() != 0
                && Vector(thisCar.x.toDouble(), thisCar.y.toDouble()).length() < RANGE_FROM_ZERO_POINT_TO_FINISH_ALG) {
            log("Found circle!")
            circleFound = true
        }
-
    }
-
-    private fun degreesToRadian(angle: Int): Double {
-        return Math.PI * angle / 180
-    }
-
 }
@@ -3,7 +3,6 @@ package algorithm.geometry

 class AngleData(val angle: Angle, val distance: Int) {

-
    fun toPoint(carAngleOX: Angle): Point {

        //convert to global coordinate system
@@ -1,13 +1,15 @@
 package algorithm.geometry

+private val eps = 0.1
+
 fun Double.lt(other: Double): Boolean {
-    return this - other < -Util.eps
+    return this - other < -eps
 }

 fun Double.gt(other: Double): Boolean {
-    return this - other > Util.eps
+    return this - other > eps
 }

 fun Double.eq(other: Double): Boolean {
-    return Math.abs(this - other) < Util.eps
+    return Math.abs(this - other) < eps
 }
@@ -1,7 +1,6 @@
 package algorithm.geometry

 class Line(var A: Double, var B: Double, var C: Double) {
-    val COMPARISON_THRESHOLD = 0

    init {
        normalize()
@@ -33,10 +32,8 @@ class Line(var A: Double, var B: Double, var C: Double) {
                B *= -1
                C *= -1
            }
-        }
-        else
-        {
-            if (A.lt(0.0)){
+        } else {
+            if (A.lt(0.0)) {
                A *= -1
                B *= -1
                C *= -1
@@ -44,28 +41,8 @@ class Line(var A: Double, var B: Double, var C: Double) {
        }
    }

-    fun metricDist(other: Line): Double {
-        return Math.sqrt((A - other.A) * (A - other.A) + (B - other.B) * (B - other.B) + (C - other.C) * (C - other.C))
-    }
-
-    override fun equals(other: Any?): Boolean {
-        if (other == null) return false
-
-        if (other !is Line) return false
-
-        Logger.log("Comparing lines: ")
-        Logger.log("    this = ${this.toString()}")
-        Logger.log("    other = ${other.toString()}")
-
-        val dist = metricDist(other)
-
-        Logger.log("    Distance = ${dist}")
-        Logger.log("    COMPARISON_THRESHOLD = ${COMPARISON_THRESHOLD}")
-        return dist < COMPARISON_THRESHOLD
-    }
-
    fun getDirectionVector(): Vector {
-        return Vector (A, -B)
+        return Vector(A, -B)
    }

 }
@@ -1,6 +0,0 @@
-package algorithm.geometry
-
-object Util {
-
-    val eps = 0.1
-}
@@ -4,7 +4,7 @@ class Vector constructor(var x: Double, var y: Double) {

    constructor(x1: Double, y1: Double, x2: Double, y2: Double) : this(x2 - x1, y2 - y1)

-    constructor(begin: Point, end: Point) : this (begin.x, begin.y, end.x, end.y)
+    constructor(begin: Point, end: Point) : this(begin.x, begin.y, end.x, end.y)

    fun scalarProduct(vector: Vector): Double {
        return this.x * vector.x + this.y * vector.y
@@ -1,5 +1,6 @@
 package algorithm.geometry

+import Logger
 import java.util.*

 data class Wall(val wallAngleOX: Angle,
@@ -18,7 +19,7 @@ data class Wall(val wallAngleOX: Angle,
    }

    var isFinished = false
-    val MAX_SLOPE = 0.2
+    private val MAX_SLOPE = 0.2

    fun pushBackPoint(point: Point) {
        Logger.log("Adding ${point.toString()}")
@@ -67,34 +68,34 @@ data class Wall(val wallAngleOX: Angle,
    private fun approximatePointsByLine(): Line {
        var n = 0

-        var sumx = 0.0
-        var sumy = 0.0
-        var sumx2 = 0.0
+        var sumX = 0.0
+        var sumY = 0.0
+        var sumX2 = 0.0
        for ((x2, y2) in rawPoints) {
-            sumx += x2
-            sumx2 += x2 * x2
-            sumy += y2
+            sumX += x2
+            sumX2 += x2 * x2
+            sumY += y2
            n++
        }
-        val xbar = sumx / n
-        val ybar = sumy / n
+        val xBar = sumX / n
+        val yBar = sumY / n

        // second pass: compute summary statistics
        var xxbar = 0.0
        var yybar = 0.0
        var xybar = 0.0
        for ((x1, y1) in rawPoints) {
-            xxbar += (x1 - xbar) * (x1 - xbar)
-            yybar += (y1 - ybar) * (y1 - ybar)
-            xybar += (x1 - xbar) * (y1 - ybar)
+            xxbar += (x1 - xBar) * (x1 - xBar)
+            yybar += (y1 - yBar) * (y1 - yBar)
+            xybar += (x1 - xBar) * (y1 - yBar)
        }

        var beta1 = xybar / xxbar
-        var beta0 = ybar - beta1 * xbar
+        var beta0 = yBar - beta1 * xBar

        if (Math.abs(beta1) > MAX_SLOPE) {
            beta1 = xybar / yybar
-            beta0 = xbar - beta1 * ybar
+            beta0 = xBar - beta1 * yBar
            return Line(1.0, -beta1, -beta0)
        }

@@ -108,10 +109,4 @@ data class Wall(val wallAngleOX: Angle,
    fun isHorizontal(): Boolean {
        return Math.abs(wallAngleOX.degs()) % 180 == 0
    }
-
-    override fun equals(other: Any?): Boolean {
-        if (other == null) return false
-        if (other !is Wall) return false
-        return line.equals(other.line)
-    }
 }
@@ -0,0 +1,62 @@
+package clInterface
+
+import clInterface.executor.*
+
+object DebugClInterface {
+
+    private val HELP_STRING = "available handlers:\n" +
+            "cars - get list of connected cars\n" +
+            "route [car_id] - setting a route for net.car with net.car id.\n" +
+            "sonar [car_id] - get sonar data\n" +
+            "dbinfo [car_id] [type] - refresh all net.car locations\n" +
+            "type is int value from: MEMORY_STATS - 0, SONAR_STATS - 1\n" +
+            "alg [count] - run algorithm. Make [count] iteration\n" +
+            "stop - exit from this interface and stop all threads\n"
+
+    private val algorithmExecutor = Algorithm()
+    private val executors = mapOf(
+            Pair("cars", CarsInformation()),
+            Pair("route", RouteMetric()),
+            Pair("sonar", Sonar()),
+            Pair("dbinfo", DebugInformation()),
+            Pair("alg", algorithmExecutor),
+            Pair("explore", Explore())
+    )
+
+    fun run() {
+        println(HELP_STRING)
+        while (true) {
+            val readString = readLine()
+            if (readString == null || readString.equals("stop")) {
+                algorithmExecutor.interruptAlgorithmThread()
+                break
+            }
+            if (readString.equals("")) {
+                continue
+            }
+            try {
+                executeCommand(readString)
+            } catch (exception: Exception) {
+                exception.printStackTrace()
+                println("Fail to execute command[$readString]: $exception")
+                println(HELP_STRING)
+            }
+        }
+
+    }
+
+    private fun printNotSupportedCommand(command: String) {
+        println("Incorrect command: $command")
+        println(HELP_STRING)
+    }
+
+    private fun executeCommand(readString: String) {
+        val command = readString.split(" ")[0].toLowerCase()
+        val executor = executors[command]
+        if (executor == null) {
+            printNotSupportedCommand(command)
+            return
+        }
+        executor.execute(readString)
+    }
+}
@@ -0,0 +1,30 @@
+package clInterface.executor
+
+import algorithm.AlgorithmThread
+import algorithm.RoomBypassingAlgorithm
+import objects.Environment
+
+class Algorithm : CommandExecutor {
+
+    private val algorithmThread = AlgorithmThread()
+
+    override fun execute(command: String) {
+        val params = command.split(" ")
+        val count =
+                if (params.size == 2) try {
+                    params[1].toInt()
+                } catch (e: Exception) {
+                    1
+                } else 1
+        if (algorithmThread.algorithmImpl == null) {
+            algorithmThread.algorithmImpl = RoomBypassingAlgorithm(Environment.map.values.last())
+            algorithmThread.start()
+        }
+        algorithmThread.setCount(count)
+    }
+
+    fun interruptAlgorithmThread() {
+        algorithmThread.setCount(0)
+        algorithmThread.interrupt()
+    }
+}
@@ -0,0 +1,12 @@
+package clInterface.executor
+
+import objects.Environment
+
+class CarsInformation : CommandExecutor {
+
+    override fun execute(command: String) {
+        synchronized(Environment, {
+            println(Environment.map.values)
+        })
+    }
+}
@@ -0,0 +1,7 @@
+package clInterface.executor
+
+interface CommandExecutor {
+
+    fun execute(command:String)
+
+}
@@ -0,0 +1,49 @@
+package clInterface.executor
+
+import CodedInputStream
+import DebugRequest
+import DebugResponseMemoryStats
+import DebugResponseSonarStats
+import roomScanner.serialize
+import net.car.client.Client
+import objects.Environment
+import java.rmi.UnexpectedException
+
+class DebugInformation : CommandExecutor {
+
+    override fun execute(command: String) {
+        val params = command.split(" ")
+        val car = Environment.map[params[1].toInt()]!!
+        val type = DebugRequest.Type.fromIntToType(params[2].toInt())
+
+        val request = DebugRequest.BuilderDebugRequest(type).build()
+        val requestType = when (type) {
+            DebugRequest.Type.MEMORY_STATS -> Client.Request.DEBUG_MEMORY
+            DebugRequest.Type.SONAR_STATS -> Client.Request.DEBUG_SONAR
+            else -> throw UnexpectedException(type.toString())
+        }
+
+        val responseData = car.carConnection.sendRequest(
+                requestType,
+                serialize(request.getSizeNoTag(), { request.writeTo(it) })
+        ).get().responseBodyAsBytes
+
+        when (type) {
+            DebugRequest.Type.MEMORY_STATS -> {
+                val data = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build()
+                println("Heap static tail: ${data.heapStaticTail}")
+                println("Heap dynamic tail: ${data.heapDynamicTail}")
+                println("Heap dynamic max size: ${data.heapDynamicMaxBytes}")
+                println("Heap dynamic total size: ${data.heapDynamicTotalBytes}")
+            }
+            DebugRequest.Type.SONAR_STATS -> {
+                val data = DebugResponseSonarStats.BuilderDebugResponseSonarStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build()
+                println("Sonar measurement total: ${data.measurementCount}")
+                println("Failed check sums: ${data.measurementFailedChecksum}")
+                println("Failed command: ${data.measurementFailedCommand}")
+                println("Failed distance: ${data.measurementFailedDistance}")
+            }
+            else -> throw UnexpectedException(type.toString())
+        }
+    }
+}
@@ -0,0 +1,28 @@
+package clInterface.executor
+
+import CodedInputStream
+import roomScanner.serialize
+import SonarExploreAngleRequest
+import SonarExploreAngleResponse
+import net.car.client.Client
+import objects.Environment
+
+class Explore : CommandExecutor {
+
+    override fun execute(command: String) {
+        val params = command.split(" ")
+        val car = Environment.map[params[1].toInt()]!!
+        val angle = params[2].toInt()
+        val window = params[3].toInt()
+
+        val request = SonarExploreAngleRequest.BuilderSonarExploreAngleRequest(angle, window).build()
+        val responseData = car.carConnection.sendRequest(
+                Client.Request.EXPLORE_ANGLE,
+                serialize(request.getSizeNoTag(), { request.writeTo(it) })
+        ).get().responseBodyAsBytes
+
+        val distances = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0)).parseFrom(CodedInputStream(responseData)).build().distances
+
+        println("Received distances: [${distances.joinToString()}]")
+    }
+}
@@ -0,0 +1,86 @@
+package clInterface.executor
+
+import CodedOutputStream
+import RouteMetricRequest
+import net.car.client.Client
+import objects.Car
+import objects.Environment
+import java.net.ConnectException
+
+class RouteMetric : CommandExecutor {
+
+    private val ROUTE_REGEX = Regex("route [0-9]{1,10}")
+    private val HELP_STRING = "print way points als [distance/degrees] [direction] in sm and degrees." +
+            "after enter all points print \"done\". for reset route print \"reset\". available directions:" +
+            "0 - FORWARD, 1 - BACKWARD, 2 - LEFT, 3 - RIGHT"
+
+    override fun execute(command: String) {
+        if (!ROUTE_REGEX.matches(command)) {
+            println("incorrect args of route command")
+            return
+        }
+        val id: Int
+        try {
+            id = command.split(" ")[1].toInt()
+        } catch (e: NumberFormatException) {
+            e.printStackTrace()
+            println("error in converting id to int type")
+            return
+        }
+        val car: Car? =
+                synchronized(Environment, {
+                    Environment.map[id]
+                })
+        if (car == null) {
+            println("car with id=$id not found")
+            return
+        }
+        val routeMessage = getRouteMessage() ?: return
+        val requestBytes = ByteArray(routeMessage.getSizeNoTag())
+        routeMessage.writeTo(CodedOutputStream(requestBytes))
+        try {
+            car.carConnection.sendRequest(Client.Request.ROUTE_METRIC, requestBytes)
+        } catch (e: ConnectException) {
+            synchronized(Environment, {
+                Environment.map.remove(id)
+            })
+        }
+    }
+
+    private fun getRouteMessage(): RouteMetricRequest? {
+        println(HELP_STRING)
+        val distances = arrayListOf<Int>()
+        val directions = arrayListOf<Int>()
+        while (true) {
+            val readLine = readLine()!!.toLowerCase()
+            when (readLine) {
+                "reset" -> return null
+                "done" -> {
+                    val routeBuilder = RouteMetricRequest.BuilderRouteMetricRequest(distances.toIntArray(), directions.toIntArray())
+                    return routeBuilder.build()
+                }
+            }
+            val wayPointData = readLine.split(" ")
+            val distance: Int
+            val direction: Int
+            try {
+                distance = wayPointData[0].toInt()
+                direction = wayPointData[1].toInt()
+            } catch (e: NumberFormatException) {
+                println("error in converting distance or direction to int. try again")
+                continue
+            } catch (e: IndexOutOfBoundsException) {
+                println("format error, you must print two number separated by spaces. Try again")
+                continue
+            }
+            if (direction != 0 && direction != 1 && direction != 2 && direction != 3) {
+                println("direction $direction don't supported!")
+                println(HELP_STRING)
+            }
+            distances.add(distance)
+            directions.add(direction)
+        }
+    }
+
+
+}
@@ -0,0 +1,72 @@
+package clInterface.executor
+
+import CodedOutputStream
+import SonarRequest
+import net.car.client.Client
+import objects.Car
+import objects.Environment
+import java.net.ConnectException
+
+class Sonar : CommandExecutor {
+
+    private val SONAR_REGEX = Regex("sonar [0-9]{1,10}")
+
+    override fun execute(command: String) {
+        if (!SONAR_REGEX.matches(command)) {
+            println("incorrect args of command sonar.")
+            return
+        }
+        val id: Int
+        try {
+            id = command.split(" ")[1].toInt()
+        } catch (e: NumberFormatException) {
+            e.printStackTrace()
+            println("error in converting id to int type")
+            return
+        }
+        val car: Car? = synchronized(Environment, {
+            Environment.map[id]
+        })
+        if (car == null) {
+            println("car with id=$id not found")
+            return
+        }
+        val requestMessage = getSonarRequest() ?: return
+        val requestBytes = ByteArray(requestMessage.getSizeNoTag())
+        requestMessage.writeTo(CodedOutputStream(requestBytes))
+        try {
+            car.carConnection.sendRequest(Client.Request.SONAR, requestBytes)
+        } catch (e: ConnectException) {
+            synchronized(Environment, {
+                Environment.map.remove(id)
+            })
+        }
+    }
+
+    private fun getSonarRequest(): SonarRequest? {
+        println("print angles, after printing all angles print done")
+        val angles = arrayListOf<Int>()
+        while (true) {
+            val command = readLine()!!.toLowerCase()
+            when (command) {
+                "reset" -> return null
+                "done" -> {
+                    val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN)
+                    return sonarBuilder.build()
+                }
+                else -> {
+                    try {
+                        val angle = command.toInt()
+                        if (angle < 0 || angle > 180) {
+                            println("incorrect angle $angle. angle must be in [0,180] and div on 4")
+                        } else {
+                            angles.add(angle)
+                        }
+                    } catch (e: NumberFormatException) {
+                        println("error in converting angle to int. try again")
+                    }
+                }
+            }
+        }
+    }
+}
@@ -52,7 +52,6 @@ class ServerHandler(val handlers: Map<String, Handler>) : SimpleChannelInboundHa
    }

    override fun exceptionCaught(ctx: ChannelHandlerContext?, cause: Throwable?) {
-        println("exception")
        cause?.printStackTrace()
        ctx?.close()
    }
@@ -1,5 +1,6 @@
 package net.web.server

+import ModeChange
 import io.netty.bootstrap.ServerBootstrap
 import io.netty.channel.nio.NioEventLoopGroup
 import io.netty.channel.socket.nio.NioServerSocketChannel
@@ -1,11 +1,13 @@
 package net.web.server.handlers

-import net.web.server.Server
-import java.util.*
 import CodedInputStream
 import CodedOutputStream
 import GenericResponse
+import ModeChange
+import Result
 import net.Handler
+import net.web.server.Server
+import java.util.*

 class ChangeMode : Handler {

@@ -1,8 +1,10 @@
 package net.web.server.handlers

-import CodedOutputStream
 import CodedInputStream
+import CodedOutputStream
+import DirectionRequest
 import GenericResponse
+import Result
 import net.Handler
 import net.web.server.Server

@@ -1,12 +1,40 @@
 package objects

+import roomScanner.CarController.Direction
+import RouteMetricRequest
+import algorithm.geometry.Angle

 class Car constructor(val uid: Int, host: String, port: 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 refreshPositionAfterRoute(route:RouteMetricRequest) {
+        val carAngle = angle.toInt()
+        route.directions.forEachIndexed { idx, direction ->
+            when (direction) {
+                Direction.FORWARD.id -> {
+                    x += (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt()
+                    y += (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt()
+                }
+                Direction.BACKWARD.id -> {
+                    x -= (Math.cos(Angle(carAngle).rads()) * route.distances[idx]).toInt()
+                    y -= (Math.sin(Angle(carAngle).rads()) * route.distances[idx]).toInt()
+                }
+                Direction.LEFT.id -> {
+                    route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
+                }
+                Direction.RIGHT.id -> {
+                    route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
+                }
+            }
+        }
+    }
+
    override fun toString(): String {
        return "$uid ; x:$x; y:$y; target:$angle"
    }
@@ -4,7 +4,6 @@ import net.car.client.Client
 import org.asynchttpclient.ListenableFuture
 import org.asynchttpclient.Response

-
 class CarConnection(val host: String, val port: Int) {
    fun sendRequest(request: Client.Request, data: ByteArray): ListenableFuture<Response> {
        return Client.makeRequest("http://$host:$port/${request.url}", data)
@@ -1,4 +1,4 @@
-package RoomScanner
+package roomScanner

 import CodedInputStream
 import RouteMetricRequest
@@ -1,4 +1,4 @@
-package RoomScanner
+package roomScanner

 class RoomScanner(val controller: CarController) : Thread() {
    private val points = mutableListOf<Pair<Double, Double>>()
@@ -1,4 +1,4 @@
-package RoomScanner
+package roomScanner

 import CodedOutputStream