working algorithm on very simple room

car_srv/kotlinSrv/src/Test.kt

@@ -55,6 +55,18 @@ private fun testCarEmulator() {
         val dist90 = sonarResponse.distances[0]
         eq(dist90.toDouble(), 98.9949, "check angle 90", 2.5)
     })
+
+    println("--------------------")
+    CarState.instance.x = 150
+    CarState.instance.y = 50
+    CarState.instance.angle = 0
+    sonarRequest = SonarRequest.BuilderSonarRequest(IntArray(7, { it*5 })).build()
+    sonarResponse = SonarResponse.BuilderSonarResponse(IntArray(0)).build()
+    controller.executeRequestSensorData(sonarRequest, { bytes ->
+        sonarResponse.mergeFrom(CodedInputStream(bytes))
+        val dist30 = sonarResponse.distances[7]
+        eq(dist30.toDouble(), 98.9949, "check angle 30", 2.5)
+    })
 }
 
 private var testNum = 1

car_srv/kotlinSrv/src/control/emulator/ControllerEmulator.kt

@@ -11,10 +11,14 @@ import encodeProtoBuf
 import geometry.Line
 import geometry.Vector
 import room.Room
+import kotlin.Pair
 
 class ControllerEmulator : Controller {
-    private val MOVE_VELOCITY = 32.78//sm/s
+
-    private val ROTATION_VELOCITY = 12.3//degrees/s
+    private val MOVE_VELOCITY = 0.05//sm/ms
+    private val ROTATION_VELOCITY = 0.05//degrees/ms
+
+    private val ADD_RANDOM = false
 
     enum class MoveDirection {
         LEFT,
@@ -87,7 +91,7 @@ class ControllerEmulator : Controller {
             if (distance == -1) {
                 distances.add(distance)
             } else {
-                val delta = getRandomIntFrom(IntArray(5, { x -> x - 2 }))//return one of -2 -1 0 1 or 2
+                val delta = if (ADD_RANDOM) getRandomIntFrom(IntArray(5, { x -> x - 2 })) else 0//return one of -2 -1 0 1 or 2
                 distances.add(distance + delta)
             }
         }
@@ -119,14 +123,14 @@ class ControllerEmulator : Controller {
                 continue
             }
             val currentDistance = Math.round(Math.sqrt(Math.pow(xIntersection - xSensor0, 2.0)
-                    + Math.pow(yIntersection - ySensor0, 2.0)))
+                    + Math.pow(yIntersection - ySensor0, 2.0))*100000)
             if (currentDistance < result) {
                 result = currentDistance
             }
         }
-        if (result < 5 || result > 500) {
+//        if (result < 5 || result > 500) {
-            return -1
+//            return -1
-        }
+//        }
         return result
     }
 
@@ -158,13 +162,13 @@ class ControllerEmulator : Controller {
         //refresh car state
         val carInstance = CarState.instance
         val commandTime = currentCommand.second
-        val delta = Math.random() * 0.2 + 0.9// delta in [0.9, 1.1)
+        val delta = if (ADD_RANDOM) Math.random() * 0.2 + 0.9 else 1.0// delta in [0.9, 1.1)
         val commandTimeIncludeRandom = (commandTime * delta).toInt()
         when (currentCommand.first) {
-            MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000)
+            MoveDirection.FORWARD -> carInstance.moving((commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
-            MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt() / 1000)
+            MoveDirection.BACKWARD -> carInstance.moving(-(commandTimeIncludeRandom * MOVE_VELOCITY).toInt())
-            MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000)
+            MoveDirection.RIGHT -> carInstance.rotate(-(commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
-            MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt() / 1000)
+            MoveDirection.LEFT -> carInstance.rotate((commandTimeIncludeRandom * ROTATION_VELOCITY).toInt())
             else -> {
             }
         }

car_srv/kotlinSrv/src/room/Room.kt

@@ -32,45 +32,42 @@ object Room {
     |___|
 
     */
-    val upLine = Line(0.0, 1.0, -300.0)
+//    val upLine = Line(0.0, 1.0, -300.0)
-    val leftLine = Line(1.0, 0.0, 150.0)
+//    val leftLine = Line(1.0, 0.0, 150.0)
-    val bottomLine = Line(0.0, 1.0, 20.0)
+//    val bottomLine = Line(0.0, 1.0, 20.0)
-    val rightLine = Line(1.0, 0.0, -200.0)
+//    val rightLine = Line(1.0, 0.0, -200.0)
-
+//
-    val bottomLine2 = Line(0.0, 1.0, -100.0)
+//    val bottomLine2 = Line(0.0, 1.0, -100.0)
-    val rightLine2 = Line(1.0, 0.0, -300.0)
+//    val rightLine2 = Line(1.0, 0.0, -300.0)
-
+//
-    val walls = listOf<Wall>(Wall(upLine, 300, -150, 300, 300),
+//    val walls = listOf<Wall>(Wall(upLine, 300, -150, 300, 300),
-            Wall(leftLine, -150, -150, 300, -20),
+//            Wall(leftLine, -150, -150, 300, -20),
-            Wall(bottomLine, -150, 200, -20, -20),
+//            Wall(bottomLine, -150, 200, -20, -20),
-            Wall(rightLine, 200, 200, -20, 100),
+//            Wall(rightLine, 200, 200, -20, 100),
-            Wall(bottomLine2, 200, 300, 100, 100),
+//            Wall(bottomLine2, 200, 300, 100, 100),
-            Wall(rightLine2, 300, 300, 100, 300)
+//            Wall(rightLine2, 300, 300, 100, 300)
-    )
+//    )
 
     /*
 
-    ________
+    _______
-    |     __|
+    |     |
     |____/
 
 
      */
 
-//    val upLine = Line(0.0, 1.0, -300.0)
+    val upLine = Line(0.0, 1.0, -300.0)
-//    val leftLine = Line(1.0, 0.0, 150.0)
+    val leftLine = Line(1.0, 0.0, 150.0)
-//    val bottomLine = Line(0.0, 1.0, 20.0)
+    val bottomLine = Line(0.0, 1.0, 20.0)
-//    val rightLine = Line(-1.2, 1.0, 140.0)
+    val rightLine = Line(-1.2, 1.0, 140.0)
-//
+    val rightLine2 = Line(1.0, 1.0, -350.0)
-//    val bottomLine2 = Line(0.0, 1.0, -100.0)
+
-//    val rightLine2 = Line(1.0, 1.0, -350.0)
+    val walls = listOf<Wall>(Wall(upLine, 200, -150, 300, 300),
-//
+            Wall(leftLine, -150, -150, 300, -20),
-//    val walls = listOf<Wall>(Wall(upLine, 350, -150, 300, 300),
+            Wall(bottomLine, -150, 100, -20, -20),
-//            Wall(leftLine, -150, -150, 300, -20),
+            Wall(rightLine, 100, 200, -20, 100),
-//            Wall(bottomLine, -150, 100, -20, -20),
+            Wall(rightLine2, 200, 200, 100, 300)
-//            Wall(rightLine, 100, 200, -20, 100),
+    )
-//            Wall(bottomLine2, 200, 350, 100, 100),
-//            Wall(rightLine2, 350, 350, 100, 300)
-//    )
 
 }

server/src/main/java/DebugClInterface.kt

@@ -1,6 +1,7 @@
 import Exceptions.InactiveCarException
 import algorithm.AbstractAlgorithm
 import algorithm.RoomBypassingAlgorithm
+import algorithm.RoomModel
 import car.client.Client
 import io.netty.buffer.Unpooled
 import io.netty.handler.codec.http.*
@@ -10,7 +11,7 @@ import java.util.concurrent.Exchanger
 object DebugClInterface {
 
     val exchanger = Exchanger<IntArray>()
-    var algorithm: AbstractAlgorithm? = null
+    var algorithmImpl: AbstractAlgorithm? = null
 
     private val routeRegex = Regex("route [0-9]{1,10}")
     private val sonarRegex = Regex("sonar [0-9]{1,10}")
@@ -56,16 +57,36 @@ object DebugClInterface {
             "refloc" -> executeRefreshLocationCommand()
             "sonar" -> executeSonarCommand(readString)
             "dbinfo" -> executeDebugInfoCommand(readString)
-            "alg" -> {
+            "points" -> println(algorithm.RoomModel.lines)
-                if (algorithm == null) {
+            "pos" -> {
-                    algorithm = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
+                val tmp = algorithmImpl
+                if (tmp is RoomBypassingAlgorithm) {
+                    println("length: ${tmp.wallLength} angleOX: ${tmp.wallAngleWithOX}")
                 }
-                algorithm!!.iterate()
             }
+            "alg" -> executeAlg(readString)
             else -> printNotSupportedCommand(readString)
         }
     }
 
+    private fun executeAlg(readString: String) {
+        val params = readString.split(" ")
+        var count =
+                if (params.size == 2) try {
+                    params[1].toInt()
+                } catch (e: Exception) {
+                    1
+                } else 1
+
+        if (algorithmImpl == null) {
+            algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
+        }
+        while (count > 0) {
+            count--
+            algorithmImpl!!.iterate()
+        }
+    }
+
     private fun executeDebugInfoCommand(readString: String) {
         val params = readString.split(" ")
         if (params.size != 3) {

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

@@ -21,7 +21,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
     protected val LEFT = 2
     protected val RIGHT = 3
 
-    private var prevState = CarState.WALL
+    private var prevState:CarState? = null
 
     private var prevSonarDistances = mapOf<Int, Double>()
 
@@ -33,7 +33,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
 
     protected fun getData(angles: IntArray): DoubleArray {
 
-        val copyElemCount = 5
+        val copyElemCount = 1
         val anglesCoped = IntArray(angles.size * copyElemCount, { angles[it / copyElemCount] })
 
         val message = SonarRequest.BuilderSonarRequest(anglesCoped).build()
@@ -50,7 +50,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
             val distances = exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
             val result = DoubleArray(angles.size)
             for (i in 0..result.size - 1) {
-                //todo sonar can send -1!!!
                 val distancesOnCurrentAngle = distances.drop(i * copyElemCount).take(copyElemCount)
                 var sum = 0
                 for (distance in distancesOnCurrentAngle) {
@@ -59,7 +58,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
                     }
                 }
                 if (sum != 0) {
-                    result[i] = sum.toDouble() / copyElemCount
+                    result[i] = (sum.toDouble()) / (100000*copyElemCount)
                 }
             }
             return result
@@ -108,6 +107,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         val state = getCarState(anglesDistances)
 
         val command = getCommand(anglesDistances, state)
+        calculateCarPosition(command, state)
         println(Arrays.toString(command.directions))
         println(Arrays.toString(command.times))
 
@@ -117,7 +117,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         moveCar(command)
     }
 
-    protected fun getPrevState(): CarState {
+
+    protected fun getPrevState(): CarState? {
         return prevState
     }
 
@@ -125,6 +126,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         return prevSonarDistances
     }
 
+    protected abstract fun calculateCarPosition(route: RouteRequest, state: CarState)
     protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState
     protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
     protected abstract fun getAngles(): IntArray

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

@@ -1,36 +1,37 @@
 package algorithm
 
 import RouteRequest
+import algorithm.geometry.Line
+import algorithm.geometry.Vector
 import objects.Car
 import java.util.*
 import java.util.concurrent.Exchanger
 
 class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
 
-    private val MOVE_VELOCITY = 30.3//sm/s
+    private val MOVE_VELOCITY = 0.05//sm/ms
-    private val ROTATION_VELOCITY = 11.0//degrees/s
+    private val ROTATION_VELOCITY = 0.05//degrees/ms
 
+    public var wallAngleWithOX = 0.0//in radian
+    public var wallLength = 0.0//sm
+//    var xPos = 0.0
+//    var yPos = 0.0
+//    var carAngle = 0.0
 
-    private var wallLength = 0.0//in sm
+    private var prevPoint = Pair(0.0, 0.0)
-    private var wallAngle = 0.0//in radian
-    private var xPos = 0.0
-    private var yPos = 0.0
 
 
     override fun getCarState(anglesDistances: Map<Int, Double>): CarState {
-        val dist0 = anglesDistances[0]!!
+        val dist0 = anglesDistances[0]
-        val dist90 = anglesDistances[90]!!
+        val dist90 = anglesDistances[90]
-        val dist60 = anglesDistances[60]!!
+        if (dist90 == null || dist90 > 85) {
-        val oldDist90 = getPrevSonarDistances()[90]
+            //best analysis of outer angle is check minimum in values 60,70,80,90,100,...
-        println(dist90)
-        println(oldDist90)
-        if (oldDist90 != null && oldDist90 + 60 < dist90) {
             return CarState.OUTER
         }
         if (dist90 < 20) {
             return CarState.WALL
         }
-        if (dist0 > 60) {
+        if (dist0 == null || dist0 > 70) {
             return CarState.WALL
         }
         return CarState.INNER
@@ -41,125 +42,180 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
     }
 
     override fun getAngles(): IntArray {
-        return IntArray(19, { it * 10 })
+        return IntArray(37, { it * 5 })
+//        return IntArray(181, { it * 1 })
+    }
+
+    override fun calculateCarPosition(route: RouteRequest, state: CarState) {
+        //todo for best values make calculate carAngle from right wall
+
+//        for (i in 0..route.directions.size - 1) {
+//            when (route.directions[i]) {
+//                FORWARD -> {
+//                    xPos += route.times[i] * MOVE_VELOCITY * Math.cos(carAngle)
+//                    yPos += route.times[i] * MOVE_VELOCITY * Math.sin(carAngle)
+//                }
+//                LEFT -> {
+//                    carAngle += degreesToRadian((route.times[i] * ROTATION_VELOCITY).toInt())
+//                }
+//                RIGHT -> {
+//                    carAngle -= degreesToRadian((route.times[i] * ROTATION_VELOCITY).toInt())
+//                }
+//            }
+//        }
+//        if (state == CarState.INNER) {
+//            carAngle = wallAngleWithOX
+//        }
     }
 
     override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
-        val dist0 = anglesDistances[0]!!
+        val dist0 = anglesDistances[0]
-        val dist60 = anglesDistances[60]!!
+        val dist60 = anglesDistances[60]
-        val dist90 = anglesDistances[90]!!
+        val dist90 = anglesDistances[90]
-        val dist120 = anglesDistances[120]!!
+        val dist120 = anglesDistances[120]
+        val dist180 = anglesDistances[180]
         val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
+        if (dist120 == null || dist90 == null || dist60 == null) {
+            println("null distance!")
+            return resultBuilder.build()
+        }
+        if (getPrevState() == null) {
+            //its first run, save right wall
+            RoomModel.lines.add(Line(0.0, 1.0, dist90))
+            prevPoint = Pair(0.0, -dist90)
+            if (dist0 != null) {
+                wallLength = dist0
+            }
+        }
         when (state) {
             CarState.WALL -> {
+                if (wallLength.toInt() == 0 && (dist0 != null && dist180 != null)) {
+                    wallLength = dist0 + dist180
+                }
                 if (dist90 > 40 || dist90 < 20) {
                     val rotationDirection = if (dist90 > 40) RIGHT else LEFT
                     resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
-                    wallLength += 1 * MOVE_VELOCITY
+                    resultBuilder.setTimes(getIntArray((30.0 / ROTATION_VELOCITY).toInt(),
-                    resultBuilder.setTimes(getIntArray(2000, 1000))
+                            (25.0 / MOVE_VELOCITY).toInt()))
                     return resultBuilder.build()
                 }
 
                 if (Math.abs(dist120 - dist60) > 10) {
                     val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
                     resultBuilder.setDirections(getIntArray(rotationDirection))
-                    resultBuilder.setTimes(getIntArray(1000))
+                    resultBuilder.setTimes(getIntArray((15.0 / ROTATION_VELOCITY).toInt()))
                     return resultBuilder.build()
                 }
 
                 resultBuilder.setDirections(getIntArray(FORWARD))
-                wallLength += 1 * MOVE_VELOCITY
+                resultBuilder.setTimes(getIntArray((25.0 / MOVE_VELOCITY).toInt()))
-                resultBuilder.setTimes(getIntArray(1000))
                 return resultBuilder.build()
             }
             CarState.INNER -> {
 
-                if (getPrevState() != state) {
+                //в угле первое действие - встать максимально паралельно стене.
-                    val pointsNextLine = arrayListOf<Pair<Double, Double>>()
+                if (Math.abs(dist120 - dist60) > 10) {
-                    val pointsCurrentLine = arrayListOf<Pair<Double, Double>>()
+                    val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
-                    for (angleDist in anglesDistances) {
+                    resultBuilder.setDirections(getIntArray(rotationDirection))
-                        if (angleDist.key <= 40) {
+                    resultBuilder.setTimes(getIntArray((3.0 / ROTATION_VELOCITY).toInt()))
-                            pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair()))
+                    return resultBuilder.build()
-                        } else if (angleDist.key <= 120) {
-                            pointsCurrentLine.add(sonarAngleDistToPoint(angleDist.toPair()))
-                        }
-                    }
-                    val innerAnglePoint = getInnerAnglePoint(pointsNextLine, pointsCurrentLine)
-                    val vectorNextLine = Vector(innerAnglePoint.first, innerAnglePoint.second,
-                            pointsNextLine[0].first, pointsNextLine[0].second)
-
-
-                    val vectorCurrentLine = Vector(innerAnglePoint.first, innerAnglePoint.second,
-                            pointsCurrentLine.last().first, pointsCurrentLine.last().second)
-
-                    val scalarMult = vectorCurrentLine.scalarProduct(vectorNextLine)
-                    val angle = Math.acos(scalarMult / (vectorCurrentLine.length() * vectorNextLine.length()))
-                    wallAngle += angle
-                    //todo add length and save this point
                 }
 
+                //если стоим паралельно - можно мерить угол
+                val pointsNextLine = arrayListOf<Pair<Double, Double>>()
+                for (angleDist in anglesDistances) {
+                    if (angleDist.key <= 15) {
+                        pointsNextLine.add(sonarAngleDistToPoint(angleDist.toPair()))
+                    }
+                }
+                val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray())
+                val currentLine = RoomModel.lines.last()
+
+                val xIntersection = prevPoint.first + wallLength * Math.cos(wallAngleWithOX)
+                val yIntersection = prevPoint.second + wallLength * Math.sin(wallAngleWithOX)
+                nextLine.C = -nextLine.A * xIntersection - nextLine.B * yIntersection
+
+                var xOnNextLine = xIntersection + 1
+                var yOnNextLine = (-nextLine.C - nextLine.A * xOnNextLine) / nextLine.B
+
+                //точка должна находится слева от нашей правой стены
+                if (currentLine.A * xOnNextLine + currentLine.B * yOnNextLine + currentLine.C < 0) {
+                    xOnNextLine = xIntersection - 1
+                    yOnNextLine = (-nextLine.C - nextLine.A * xOnNextLine) / nextLine.B
+                }
+
+                val vectorNextLine = Vector(xIntersection, yIntersection, xOnNextLine, yOnNextLine)
+                val vectorCurrentLine = Vector(xIntersection, yIntersection, prevPoint.first, prevPoint.second)
+
+                val scalarProduct = vectorCurrentLine.scalarProduct(vectorNextLine)
+                val angle = Math.acos(scalarProduct / (vectorCurrentLine.length() * vectorNextLine.length()))
+                val wallsAngleInDegrees = radiansToDegrees(angle)
+                wallAngleWithOX += degreesToRadian(180 - wallsAngleInDegrees.toInt())
+                prevPoint = Pair(xIntersection, yIntersection)
+                wallLength = 0.0
+                RoomModel.lines.add(nextLine)
                 resultBuilder.setDirections(getIntArray(LEFT, FORWARD, LEFT))
-                resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt()))
+                resultBuilder.setTimes(getIntArray((radiansToDegrees(wallAngleWithOX) / (2 * ROTATION_VELOCITY)).toInt(),
+                        (40 / MOVE_VELOCITY).toInt(), (radiansToDegrees(wallAngleWithOX) / (2 * ROTATION_VELOCITY)).toInt()))
                 return resultBuilder.build()
             }
             CarState.OUTER -> {
+                //todo calculate angle
                 resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
-                resultBuilder.setTimes(getIntArray((45 * 1000 / ROTATION_VELOCITY).toInt(), 1000, (45 * 1000 / ROTATION_VELOCITY).toInt()))
+                resultBuilder.setTimes(getIntArray((45 / ROTATION_VELOCITY).toInt(),
+                        (40 / MOVE_VELOCITY).toInt(), (45 / ROTATION_VELOCITY).toInt()))
                 return resultBuilder.build()
             }
         }
     }
 
-    private fun getInnerAnglePoint(pointsNextLine: ArrayList<Pair<Double, Double>>,
-                                   pointsCurrentLine: ArrayList<Pair<Double, Double>>): Pair<Double, Double> {
-
-        val nextLine = approximatePointsByLine(pointsNextLine.toTypedArray())
-        val currentLine = approximatePointsByLine(pointsCurrentLine.toTypedArray())
-        val b1 = currentLine.second
-        val k1 = currentLine.first
-        val b2 = nextLine.second
-        val k2 = nextLine.first
-        val intersectionPoint = Pair((b1 - b2) / (k2 - k1), (k2 * b1 - b2 * k1) / (k2 - k1))
-        return intersectionPoint
-    }
-
     private fun degreesToRadian(angle: Int): Double {
         return Math.PI * angle / 180
     }
 
+    private fun radiansToDegrees(angle: Double): Double {
+        return angle * 180 / Math.PI
+    }
+
     private fun sonarAngleDistToPoint(angleDist: Pair<Int, Double>): Pair<Double, Double> {
 
-        val angle = wallAngle - degreesToRadian(angleDist.first)
+        val angle = wallAngleWithOX - degreesToRadian(angleDist.first)
         val dist = angleDist.second
 
-        val result = Pair(xPos + Math.cos(angle) * dist, yPos + Math.sin(angle) * dist)
+        val result = Pair(Math.cos(angle) * dist, Math.sin(angle) * dist)
         return result
     }
 
     //returns coef in y=kx+b (first is k, second is b)
-    private fun approximatePointsByLine(points: Array<Pair<Double, Double>>): Pair<Double, Double> {
+    private fun approximatePointsByLine(points: Array<Pair<Double, Double>>): Line {
 
-        val sumX = points.sumByDouble { it.first }
+        val p1 = points.first()
-        val sumXQuad = points.sumByDouble { it.first * it.first }
+        val p2 = points[1]
-        val sumY = points.sumByDouble { it.second }
-        val sumXY = points.sumByDouble { it.second * it.first }
 
-        val pointsCount = points.size
+        return Line(p2.second - p1.second, p1.first - p2.first, -p1.first * p2.second + p1.second * p2.first)
-        val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
-        val b = (sumY - sumX * k) / pointsCount
 
-        return Pair(k, b)
+//
+//        val sumX = points.sumByDouble { it.first }
+//        val sumXQuad = points.sumByDouble { it.first * it.first }
+//        val sumY = points.sumByDouble { it.second }
+//        val sumXY = points.sumByDouble { it.second * it.first }
+//
+//        val pointsCount = points.size
+//        val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
+//        val b = (sumY - sumX * k) / pointsCount
+//
+//        return Line(-k, 1.0, -b)
     }
 
 
     private fun calculateAngleWithWall(anglesDistances: MutableMap<Int, Double>): Double {
-        val dist60 = anglesDistances[60]!!
+        val dist60 = anglesDistances[60]
-        val dist120 = anglesDistances[120]!!
+        val dist120 = anglesDistances[120]
 
         //Math.cos(60) = 1/2
-        val wallLength = Math.sqrt(Math.pow(dist60, 2.0) + Math.pow(dist120, 2.0) - dist120 * dist60)//in triangle
+//        val wallLength = Math.sqrt(Math.pow(dist60, 2.0) + Math.pow(dist120, 2.0) - dist120 * dist60)//in triangle
-
+//
-        val hOnWall = getRangeToWall(wallLength, dist60, dist120)
+//        val hOnWall = getRangeToWall(wallLength, dist60, dist120)
         return 0.0
     }
 

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

@@ -1,11 +1,11 @@
 package algorithm
 import Waypoints
-/**
+import algorithm.geometry.Line
- * Created by user on 8/23/16.
+
- */
 object RoomModel {
 
-    val point = arrayListOf<Pair<Int, Int>>()
+    val point = arrayListOf<Pair<Double, Double>>()
+    val lines = arrayListOf<Line>()
 
 
 //  DEBUG BELOW

server/src/main/java/algorithm/geometry/Line.kt

@@ -0,0 +1,21 @@
+package algorithm.geometry
+
+class Line(val A: Double, val B: Double, var C: Double) {
+
+
+    fun getIntersectionPoint(lineTwo: Line): Pair<Double, Double> {
+        val slope = this.A * lineTwo.B - this.B * lineTwo.A
+        if (Math.abs(slope) < 0.001) {
+            throw ArithmeticException("lines is parallel")
+        }
+        val xIntersection = (this.B * lineTwo.C - lineTwo.B * this.C) / slope
+        val yIntersection = (this.C * lineTwo.A - lineTwo.C * this.A) / slope
+        return Pair(xIntersection, yIntersection)
+    }
+
+    override fun toString(): String{
+        return "Line(A=$A, B=$B, C=$C)"
+    }
+
+
+}

server/src/main/java/algorithm/geometry/Vector.kt

@@ -1,4 +1,4 @@
-package algorithm
+package algorithm.geometry
 
 class Vector constructor(val x: Double, val y: Double) {
 

server/src/main/java/car/client/ClientHandler.kt

@@ -14,6 +14,7 @@ import io.netty.util.AttributeKey
 import objects.Environment
 import setRouteUrl
 import sonarUrl
+import java.util.*
 import java.util.concurrent.TimeUnit
 import java.util.concurrent.TimeoutException
 
@@ -84,8 +85,8 @@ class ClientHandler : SimpleChannelInboundHandler<Any> {
     }
 
     private fun handlerSonarResponse(message: SonarResponse, angles: IntArray) {
-//        println("request angles: ${Arrays.toString(angles)}")
+        println("request angles: ${Arrays.toString(angles)}")
-//        println("distances from sonar: ${Arrays.toString(message.distances)}")
+        println("distances from sonar: ${Arrays.toString(message.distances)}")
         try {
             DebugClInterface.exchanger.exchange(message.distances, 20, TimeUnit.SECONDS)
         } catch (e: InterruptedException) {