new algorithm version. Normal work on emulator with large errors in data

server/src/main/java/DebugClInterface.kt

@@ -64,7 +64,7 @@ object DebugClInterface {
             "refloc" -> executeRefreshLocationCommand()
             "sonar" -> executeSonarCommand(readString)
             "dbinfo" -> executeDebugInfoCommand(readString)
-            "lines" -> println(algorithm.RoomModel.lines)
+            "lines" -> algorithm.RoomModel.walls.forEach { println(it.line) }
             "pos" -> {
                 val tmp = algorithmImpl
                 if (tmp is RoomBypassingAlgorithm) {
@@ -207,7 +207,7 @@ object DebugClInterface {
             when (command) {
                 "reset" -> return null
                 "done" -> {
-                    val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 1 }), 0, SonarRequest.Smoothing.NONE)
+                    val sonarBuilder = SonarRequest.BuilderSonarRequest(angles.toIntArray(), IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN)
                     return sonarBuilder.build()
                 }
                 else -> {

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

@@ -19,8 +19,8 @@ import java.util.concurrent.TimeoutException
 abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntArray>) {
 
     open val ATTEMPTS = 1
-    open val THRESHOLD = 0
     open val SMOOTHING = SonarRequest.Smoothing.NONE
+    open val WINDOW_SIZE = 0
 
     protected val FORWARD = 0
     protected val BACKWARD = 1
@@ -30,7 +30,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
     private var prevState: CarState? = null
 
     private var prevSonarDistances = mapOf<Angle, AngleData>()
-    private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(90), Angle(120), Angle(180))
+    private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(70), Angle(80), Angle(90), Angle(100), Angle(110), Angle(120), Angle(180))
     protected var requiredAngles = defaultAngles
 
     protected enum class CarState {
@@ -45,8 +45,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         val message = SonarRequest.BuilderSonarRequest(
                 angles = anglesIntArray,
                 attempts = IntArray(angles.size, { ATTEMPTS }),
-                threshold = THRESHOLD,
+                smoothing = SMOOTHING,
-                smoothing = SMOOTHING)
+                windowSize = WINDOW_SIZE)
                 .build()
         val requestBytes = ByteArray(message.getSizeNoTag())
         message.writeTo(CodedOutputStream(requestBytes))
@@ -103,9 +103,6 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         }
         val anglesDistances = mutableMapOf<Angle, AngleData>()
         for (i in 0..angles.size - 1) {
-            if (Math.abs(distances[i]) < 0.01) {
-                continue
-            }
             anglesDistances.put(angles[i], AngleData(angles[i], distances[i]))
         }
 

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

@@ -1,6 +1,7 @@
 package algorithm
 
 import RouteMetricRequest
+import SonarRequest
 import algorithm.geometry.Angle
 import algorithm.geometry.AngleData
 import algorithm.geometry.Point
@@ -11,14 +12,19 @@ import java.util.concurrent.Exchanger
 class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
 
     // TODO: set to appropriate values
-    override val THRESHOLD = 0
+    override val ATTEMPTS: Int = 5
-    override val ATTEMPTS: Int = 3
     override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
+    override val WINDOW_SIZE = 3
 
-    private val MAX_DISTANCE_TO_WALL_AHEAD = 60
+    private val MAX_DISTANCE_TO_WALL_AHEAD = 30         // reached the corner and should turn left
-    private val ISOSCALENESS_THRESHOLD = 10
+    private val OUTER_CORNER_DISTANCE_THRESHOLD = 60    // reached outer corner
-    private val DISTANCE_TO_WALL_UPPER_BOUND = 50
+    private val ISOSCALENESS_MIN_DIFF = 10              // have to correct alignment to the wall
-    private val DISTANCE_TO_WALL_LOWER_BOUND = 25
+    private val ISOSCALENESS_MAX_DIFF = 50              // almost reached outer corner but not yet, and 120 meas. founds far wall
+    private val DISTANCE_TO_WALL_UPPER_BOUND = 50       // have to move closer to the parallel wall
+    private val DISTANCE_TO_WALL_LOWER_BOUND = 25       // have to move farther to the parallel wall
+    private val SPURIOUS_REFLECTION_DIFF = 70           // we're approaching corner and get spurious reflection from two walls on 60 meas. ! Should be before outer corner case !
+
+    private var calibrateAfterRotate = false
 
     var carX = 0
     var carY = 0
@@ -37,16 +43,21 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
 
     private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
         val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        resultBuilder.setDirections(getIntArray(RIGHT))
+        resultBuilder.setDirections(getIntArray(FORWARD, RIGHT, FORWARD))
-        resultBuilder.setDistances(getIntArray(10))
+        val wallAngle = calculateAngle(anglesDistances, state)
+        resultBuilder.setDistances(getIntArray(15, wallAngle.degs(), 60))
+        addWall(-wallAngle)
+        calibrateAfterRotate = true
         return resultBuilder.build()
     }
 
     private fun wallAheadFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
         val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        resultBuilder.setDirections(getIntArray(LEFT))
+        resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
-        resultBuilder.setDistances(getIntArray(calculateAngle(anglesDistances, state).degs()))
+        val wallAngle = calculateAngle(anglesDistances, state)
-        addWall()
+        resultBuilder.setDistances(getIntArray(wallAngle.degs(), 15))
+        addWall(wallAngle)
+        calibrateAfterRotate = true
         return resultBuilder.build()
     }
 
@@ -71,31 +82,53 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
         return resultBuilder.build()
     }
 
-    private fun addWall() {
+    private fun moveForward(): RouteMetricRequest {
+        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
+        resultBuilder.setDirections(getIntArray(FORWARD))
+        resultBuilder.setDistances(getIntArray(20))
+        return resultBuilder.build()
+    }
+
+    private fun addWall(angleWithPrevWall: Angle) {
         RoomModel.updateWalls()
 
-        if (RoomModel.walls.last() == RoomModel.walls.first() && RoomModel.walls.size > 1) {
+        val firstWall = RoomModel.walls.first()
-            println("YEAH!")
+        val lastWall = RoomModel.walls.last()
+        if (firstWall == lastWall && RoomModel.walls.size > 1) {
             RoomModel.finished = true
-            RoomModel.walls.removeAt(RoomModel.walls.size - 1)
+            lastWall.pushBackPoint(firstWall.rawPoints.last())
+            lastWall.markAsFinished()
+            RoomModel.walls.removeAt(0)
+        } else {
+            RoomModel.walls.add(Wall(lastWall.wallAngleOX + angleWithPrevWall))
         }
-        RoomModel.walls.add(Wall())
     }
 
     override fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
-        val dist0 = anglesDistances[Angle(0)]
+
-        val dist60 = anglesDistances[Angle(60)]
+        val dist0 = anglesDistances[Angle(0)]!!
-        val dist90 = anglesDistances[Angle(90)]
+        val dist60 = anglesDistances[Angle(60)]!!
-        val dist120 = anglesDistances[Angle(120)]
+        val dist90 = anglesDistances[Angle(90)]!!
+        val dist120 = anglesDistances[Angle(120)]!!
         val sonarAngle = carAngle - 90
 
-        // Check most basic measurements: 60/90/120
+        if (calibrateAfterRotate) {
-        if (dist90 == null) {
+            if (Math.abs(dist60.distance - dist120.distance) > 4) {
-            return noOrthogonalMeasurementFound(anglesDistances, state)
+                println("Flaw in align found, correcting")
+                val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
+                val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT
+                resultBuilder.setDirections(getIntArray(rotationDirection))
+                resultBuilder.setDistances(getIntArray(5))
+                return resultBuilder.build()
+            } else {
+                calibrateAfterRotate = false
+                carAngle = RoomModel.walls.last().wallAngleOX.degs()
+            }
         }
-
+        // Check most basic measurements: 60/90/120
-        if (dist120 == null || dist60 == null) {
+        if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) {
-            return noParallelWallFound(anglesDistances, state)
+            println("No orthogonal measurement found")
+            return noOrthogonalMeasurementFound(anglesDistances, state)
         }
 
         // Add point to room map
@@ -103,31 +136,65 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                 x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
                 y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
         )
-        RoomModel.walls.last().addPoint(point)
+        RoomModel.walls.last().pushBackPoint(point)
 
         // Check if corner reached
-        if (dist0 != null && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
+        if (dist0.distance != -1 && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
+            println("Wall ahead found")
             return wallAheadFound(anglesDistances, state)
         }
 
-        // Checks for paralleleness
+        // Big fail, try to do something safe
+        if (dist120.distance == -1 && dist60.distance == -1) {
+            println("No parallel wall found")
+            return noParallelWallFound(anglesDistances, state)
+        }
 
-        // Check if wall is not parallel
+        // Big fail too
-        val diff = Math.abs(dist120.distance - dist60.distance)
+        if (dist120.distance == -1) {
-        if (diff > ISOSCALENESS_THRESHOLD) {
+            println("No back measurement found, wtf")
+            return moveForward()
+        }
+
+        // Approaching inner corner and getting spurious reflection from 2 walls on 60 - just move forward to get closer to corner
+        if (dist60.distance - dist0.distance > SPURIOUS_REFLECTION_DIFF) {
+            println("Spurious reflection detected, moving forward")
+            return moveForward()
+        }
+
+        // Approaching outer corner (parallel wall is ending soon, but not yet) - just move forward to get to the end of the wall
+        if (dist60.distance == -1 || Math.abs(dist120.distance - dist60.distance) > ISOSCALENESS_MAX_DIFF) {
+            println("Aprroaching outer corner, moving forward")
+            return moveForward()
+        }
+
+        RoomModel.walls.last().rawPoints.removeAt(RoomModel.walls.last().rawPoints.size - 1)
+        for (i in 60..120 step 10) {
+            val point = Point(
+                    x = carX + anglesDistances[Angle(i)]!!.distance * Math.cos(degreesToRadian(carAngle - i)),
+                    y = carY + anglesDistances[Angle(i)]!!.distance * Math.sin(degreesToRadian(carAngle - i))
+            )
+            RoomModel.walls.last().pushBackPoint(point)
+        }
+
+        // Check if realignment need
+        if (Math.abs(dist60.distance - dist120.distance) > ISOSCALENESS_MIN_DIFF) {
+            println("Flaw in align found, correcting")
             return alignParallelToWall(anglesDistances, state)
         }
 
+//        car stay parallel with wall
+//        carAngle = RoomModel.walls.last().wallAngleOX.degs()
+
         // Check if wall is too close or too far
-        if (dist90.distance > 50 || dist90.distance < 25) {
+        if (dist90.distance > DISTANCE_TO_WALL_UPPER_BOUND || dist90.distance < DISTANCE_TO_WALL_LOWER_BOUND) {
+            println("Flaw in distance to the parallel wall found, correcting")
             return correctDistanceToParallelWall(anglesDistances, state)
         }
 
         // default case: everything is ok, just move forward
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
+        println("Default case: moving forward")
-        resultBuilder.setDirections(getIntArray(FORWARD))
+        return moveForward()
-        resultBuilder.setDistances(getIntArray(20))
-        return resultBuilder.build()
     }
 
     private fun calculateAngle(anglesDistances: Map<Angle, AngleData>, state: CarState): Angle {

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

@@ -3,6 +3,7 @@ package algorithm
 import DebugClInterface
 import DebugResponse
 import Waypoints
+import algorithm.geometry.Angle
 import algorithm.geometry.Line
 import algorithm.geometry.Point
 import algorithm.geometry.Wall
@@ -10,9 +11,7 @@ import objects.Car
 import java.util.*
 
 object RoomModel {
-
+    val walls = arrayListOf<Wall>(Wall(Angle(0)))
-    val lines = arrayListOf<Line>()
-    val walls = arrayListOf<Wall>(Wall())
     var finished = false
 
     val linesModel = listOf(Line(0.0, 1.0, -300.0),
@@ -61,33 +60,49 @@ object RoomModel {
         val line2 = walls[walls.size - 2].line
 
         val intersection: Point = line1.intersect(line2)
-        walls[walls.size - 2].addPoint(intersection)
+
+
+        val lastWall = walls[walls.size - 1]
+        lastWall.pushFrontPoint(intersection)
+        if (lastWall.isHorizontal()) {
+            lastWall.line = Line(0.0, 1.0, -intersection.y)
+        } else if (lastWall.isVertical()) {
+            lastWall.line = Line(1.0, 0.0, -intersection.x)
+        }
+
+        walls[walls.size - 2].pushBackPoint(intersection)
+        walls[walls.size - 2].markAsFinished()
     }
 
     fun getDebugInfo(): DebugResponse {
 
-        val aRef = IntArray(linesModel.size)
+        val points = getWallsPoints()
-        val bRef = IntArray(linesModel.size)
-        val cRef = IntArray(linesModel.size)
-        val a = IntArray(lines.size)
-        val b = IntArray(lines.size)
-        val c = IntArray(lines.size)
 
+        val begin_x = IntArray(points.size)
+        val begin_y = IntArray(points.size)
+        val end_x = IntArray(points.size)
+        val end_y = IntArray(points.size)
+
+        val rawPoints = walls.flatMap({ it.rawPoints }).toTypedArray()
+        val pointsX = rawPoints.map { it.x.toInt() }.toIntArray()
+        val pointsY = rawPoints.map { it.y.toInt() }.toIntArray()
+
+        for (i in 0..points.size - 2) {
+            val curPoint = points[i]
+            val nextPoint = points[i + 1]
+            begin_x[i] = (curPoint.x + 0.5).toInt()
+            begin_y[i] = (curPoint.y + 0.5).toInt()
+
+            end_x[i] = (nextPoint.x + 0.5).toInt()
+            end_y[i] = (nextPoint.y + 0.5).toInt()
+        }
         val alg = DebugClInterface.algorithmImpl
-        val car = if (alg == null) Car(0, "", 0) else alg.thisCar
+        if (alg is RoomBypassingAlgorithm) {
-        val mult = 100
+            return DebugResponse.BuilderDebugResponse(begin_x, begin_y, end_x, end_y, alg.carX,
-        linesModel.forEachIndexed { idx, line ->
+                    alg.carY, alg.carAngle, pointsX, pointsY).build()
-            aRef[idx] = (line.A * mult).toInt()
-            bRef[idx] = (line.B * mult).toInt()
-            cRef[idx] = (line.C * mult).toInt()
-        }
-        lines.forEachIndexed { idx, line ->
-            a[idx] = (line.A * mult).toInt()
-            b[idx] = (line.B * mult).toInt()
-            c[idx] = (line.C * mult).toInt()
         }
+        return DebugResponse.BuilderDebugResponse(begin_x, begin_y, end_x, end_y, 0,
+                0, 0, pointsX, pointsY).build()
 
-        val resultBuilder = DebugResponse.BuilderDebugResponse(a, b, c, car.x, car.y, car.angle, aRef, bRef, cRef)
-        return resultBuilder.build()
     }
 }

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

@@ -16,6 +16,10 @@ data class Angle(private val degrees: Int) {
         return Angle(degrees - other.degrees)
     }
 
+    operator fun unaryMinus(): Angle {
+        return Angle(-degrees)
+    }
+
     operator fun plus(other: Angle): Angle {
         return Angle(degrees + other.degrees)
     }

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

@@ -1,7 +1,7 @@
 package algorithm.geometry
 
 class Line(var A: Double, var B: Double, var C: Double) {
-    val COMPARISON_THRESHOLD = 0.5
+    val COMPARISON_THRESHOLD = 20
 
     init {
         normalize()
@@ -55,4 +55,9 @@ class Line(var A: Double, var B: Double, var C: Double) {
 
         return metricDist(other) < COMPARISON_THRESHOLD
     }
+
+    fun getDirectionVector(): Vector {
+        return Vector (A, -B)
+    }
+
 }

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

@@ -1,6 +1,7 @@
 package algorithm.geometry
 
 data class Point(val x: Double, val y: Double) {
-
+    operator fun plus(other: Vector): Point {
-
+        return Point(x + other.x, y + other.y)
+    }
 }

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

@@ -2,5 +2,5 @@ package algorithm.geometry
 
 object Util {
 
-    val eps = 1e-2
+    val eps = 0.1
 }

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

@@ -1,9 +1,10 @@
 package algorithm.geometry
 
-class Vector constructor(val x: Double, val y: Double) {
+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)
 
     fun scalarProduct(vector: Vector): Double {
         return this.x * vector.x + this.y * vector.y
@@ -13,4 +14,21 @@ class Vector constructor(val x: Double, val y: Double) {
         return Math.sqrt(x * x + y * y)
     }
 
+    fun angleTo(other: Vector): Angle {
+        var sp = scalarProduct(other)
+        var cosinus = sp / length() / other.length()
+        return Angle(Math.acos(cosinus))
+    }
+
+    fun inverse() {
+        x *= -1
+        y *= -1
+    }
+
+    fun normalize() {
+        val div = Math.sqrt(x * x + y * y)
+
+        x /= div
+        y /= div
+    }
 }

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

@@ -1,34 +1,108 @@
 package algorithm.geometry
 
 import java.util.*
+import algorithm.geometry.Vector
 
-data class Wall(val points: ArrayList<Point> = arrayListOf<Point>(),
+data class Wall(val wallAngleOX: Angle,
+                val rawPoints: ArrayList<Point> = arrayListOf<Point>(),
                 var line: Line = Line(0.0, 0.0, 0.0)) {
 
+    var isFinished = false
+    val MAX_REGRESSION_ERROR = 10.0
 
-    fun addPoint(point: Point) {
+    fun pushBackPoint(point: Point) {
-        points.add(point)
+        rawPoints.add(point)
         line = approximatePointsByLine()
     }
 
+    fun pushFrontPoint(point: Point) {
+        rawPoints.add(0, point)
+        line = approximatePointsByLine()
+    }
+
+    var points: ArrayList<Point> = arrayListOf()
+
+    fun markAsFinished() {
+        isFinished = true
+        points = generatePoints()
+    }
+
+    private fun generatePoints(): ArrayList<Point> {
+        val direction = Vector(rawPoints.first(), rawPoints.last())
+        val res: ArrayList<Point> = arrayListOf()
+
+        val pointsCount = direction.length().toInt()
+
+        direction.normalize()
+
+        var curPoint = rawPoints.first()
+        res.add(curPoint)
+        val endPoint = rawPoints.last()
+        for (i in 0..pointsCount - 1) {
+            curPoint += direction
+            res.add(curPoint)
+        }
+        res.add(endPoint)
+
+        return res
+    }
+
     private fun approximatePointsByLine(): Line {
+        var n = 0
 
-        val sumX = points.sumByDouble { it.x }
+        var sumx = 0.0
-        val sumXQuad = points.sumByDouble { it.x * it.x }
+        var sumy = 0.0
-        val sumY = points.sumByDouble { it.y }
+        var sumx2 = 0.0
-        val sumXY = points.sumByDouble { it.y * it.x }
+        for (pt in rawPoints) {
+            sumx += pt.x
+            sumx2 += pt.x * pt.x
+            sumy += pt.y
+            n++
+        }
+        val xbar = sumx / n
+        val ybar = sumy / n
 
-        val pointsCount = points.size
+        // second pass: compute summary statistics
-        val den = pointsCount * sumXQuad - sumX * sumX
+        var xxbar = 0.0
-
+        var yybar = 0.0
-        if (Math.abs(den) < 0.001) {
+        var xybar = 0.0
-            return Line(1.0, 0.0, -points.first().x)
+        for (pt in rawPoints) {
+            xxbar += (pt.x - xbar) * (pt.x - xbar)
+            yybar += (pt.y - ybar) * (pt.y - ybar)
+            xybar += (pt.x - xbar) * (pt.y - ybar)
         }
 
-        val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
+        var beta1 = xybar / xxbar
-        val b = (sumY - sumX * k) / pointsCount
+        var beta0 = ybar - beta1 * xbar
 
-        return Line(-k, 1.0, -b)
+        // analyze results
+        val df = n - 2
+        var rss = 0.0      // residual sum of squares
+        var ssr = 0.0      // regression sum of squares
+        for (pt in rawPoints) {
+            val fit = beta1 * pt.x + beta0
+            rss += (fit - pt.y) * (fit - pt.y)
+            ssr += (fit - ybar) * (fit - ybar)
+        }
+
+        val svar = rss / df
+        val svar1 = svar / xxbar
+
+        if (svar1.isNaN() || svar1.isInfinite() || Math.sqrt(svar1).gt(MAX_REGRESSION_ERROR) || Math.abs(beta1) > 1) {
+            beta1 = xybar / yybar
+            beta0 = xbar - beta1 * ybar
+            return Line(1.0, -beta1, -beta0)
+        }
+
+        return Line(-beta1, 1.0, -beta0)
+    }
+
+    fun isVertical(): Boolean {
+        return Math.abs(wallAngleOX.degs()) % 180 == 90
+    }
+
+    fun isHorizontal(): Boolean {
+        return Math.abs(wallAngleOX.degs()) % 180 == 0
     }
 
     override fun equals(other: Any?): Boolean {