Algorithm: added history of commands and rollback() method to undo last action. Fixed condition for swapping x/y in linear regresison (now only slope of resulting line taken into account)

server/src/main/java/Logger.kt

@@ -7,9 +7,21 @@ object Logger {
     val file = File("debug.log")
     val fileWriter = FileWriter(file.absoluteFile)
     val bufferedWriter = BufferedWriter(fileWriter)
+    var padding = ""
+    var paddingStep = "  "
 
+    fun indent() {
+        padding += paddingStep
+    }
+
+    fun outdent() {
+        if (padding.length == 0) {
+            throw IllegalArgumentException("Called outdent without corresponsing indent")
+        }
+        padding = padding.removeSuffix(paddingStep)
+    }
     fun log(msg: String) {
-        bufferedWriter.write(msg + "\n")
+        bufferedWriter.write(padding + msg + "\n")
         bufferedWriter.flush()
     }
 }

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

@@ -11,6 +11,7 @@ import io.netty.handler.codec.http.*
 import objects.Car
 import setRouteMetricUrl
 import sonarUrl
+import sun.rmi.runtime.Log
 import java.util.*
 import java.util.concurrent.Exchanger
 import java.util.concurrent.TimeUnit
@@ -28,19 +29,21 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
     protected val LEFT = 2
     protected val RIGHT = 3
 
-    private var prevState: CarState? = null
+    private val historySize = 10
+    private val history = Stack<RouteMetricRequest>()
 
+    private var prevState: CarState? = null
     private var prevSonarDistances = mapOf<Angle, AngleData>()
     private val defaultAngles = arrayOf(Angle(0), Angle(70), Angle(75), Angle(80), Angle(85), Angle(90), Angle(95), Angle(100), Angle(105), Angle(110), 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 {
         WALL,
         INNER,
         OUTER
-    }
 
+    }
     private var iterationCounter = 0
 
     protected fun getData(angles: Array<Angle>): IntArray {
@@ -95,6 +98,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
 
     fun iterate() {
         Logger.log("============= STARTING ITERATION ${iterationCounter} ============")
+        Logger.indent()
         iterationCounter++
         if (RoomModel.finished) {
             return
@@ -118,10 +122,14 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
             return
         }
 
+        addToHistory(command)
+
         afterGetCommand(command)
         Logger.log("Sending command:")
-        Logger.log("    Directions = ${Arrays.toString(command.directions)}")
+        Logger.indent()
-        Logger.log("    Distanced = ${Arrays.toString(command.distances)}")
+        Logger.log("Directions = ${Arrays.toString(command.directions)}")
+        Logger.log("Distanced = ${Arrays.toString(command.distances)}")
+        Logger.outdent()
         println(Arrays.toString(command.directions))
         println(Arrays.toString(command.distances))
 
@@ -129,6 +137,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         this.prevState = state
 
         moveCar(command)
+        Logger.outdent()
         Logger.log("============= FINISHING ITERATION ${iterationCounter} ============")
         Logger.log("")
     }
@@ -146,6 +155,58 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         return requiredAngles
     }
 
+    private fun addToHistory(command: RouteMetricRequest) {
+        history.push(command)
+        while(history.size > historySize) {
+            history.removeAt(0)
+        }
+    }
+
+    private fun popFromHistory(): RouteMetricRequest {
+        return history.pop()
+    }
+
+    private fun inverseCommand(command: RouteMetricRequest): RouteMetricRequest {
+        val res = RouteMetricRequest.BuilderRouteMetricRequest(command.distances, command.directions)
+        res.distances.reverse()
+        res.directions.reverse()
+
+        for ((index, dir) in res.directions.withIndex()) {
+            res.directions[index] = when (dir) {
+                FORWARD -> BACKWARD
+                BACKWARD -> FORWARD
+                LEFT -> RIGHT
+                RIGHT -> LEFT
+                else -> throw IllegalArgumentException("Unexpected direction = ${dir} found during command inversion")
+            }
+        }
+        return res.build()
+    }
+
+    protected fun rollback() {
+        val lastCommand = popFromHistory()
+        val invertedCommand = inverseCommand(lastCommand)
+        Logger.log ("Rollback:")
+        Logger.indent()
+        Logger.log ("Last command: ${lastCommand.toString()}")
+        Logger.log ("Inverted cmd: ${invertedCommand.toString()}")
+        Logger.outdent()
+        moveCar(invertedCommand)
+    }
+
+    protected fun rollback(steps: Int) {
+        Logger.log("=== Starting rollback for ${steps} steps ===")
+        Logger.indent()
+        var stepsRemaining = steps
+        while(stepsRemaining > 0 && history.size > 0) {
+            Logger.log("Step: ${steps - stepsRemaining + 1}")
+            rollback()
+            stepsRemaining--
+        }
+        Logger.outdent()
+        Logger.log("=== Finished rollback ===")
+    }
+
     protected abstract fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState?
     protected abstract fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest?
     protected abstract fun afterGetCommand(route: RouteMetricRequest)

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

@@ -0,0 +1,25 @@
+package algorithm
+
+import RouteMetricRequest
+
+fun fromIntToDirection(value: Int): String {
+    return when (value) {
+        0 -> "FORWARD"
+        1 -> "BACKWARD"
+        2 -> "LEFT"
+        3 -> "RIGHT"
+        else -> throw IllegalArgumentException("Error parsing Direction from Int: got value = $value")
+    }
+}
+
+fun RouteMetricRequest.toString(): String {
+    var res = emptyArray<String>()
+
+    for (i in distances.indices) {
+        val dist = distances[i].toString()
+        val dir = fromIntToDirection(directions[i])
+        res += "(" + dist + ", " + dir + ")"
+    }
+
+    return res.joinToString("; ")
+}

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

@@ -4,13 +4,11 @@ import Logger.log
 import RouteMetricRequest
 import SonarRequest
 import algorithm.geometry.*
+import algorithm.geometry.Vector
 import objects.Car
 import java.util.concurrent.Exchanger
 
 class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
-
-
-    // TODO: set to appropriate values
     override val ATTEMPTS: Int = 5
     override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
     override val WINDOW_SIZE = 3
@@ -40,19 +38,14 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
         return CarState.WALL
     }
 
-    private fun noParallelWallFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
-        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        resultBuilder.setDirections(getIntArray(FORWARD))
-        resultBuilder.setDistances(getIntArray(10))
-        return resultBuilder.build()
-    }
-
     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))
         addWall(-wallAngle)
+        Logger.outdent()
         carAngle = RoomModel.walls.last().wallAngleOX.degs()
         calibrateAfterRotate = true
         return resultBuilder.build()
@@ -122,11 +115,11 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
 
     private fun addWall(angleWithPrevWall: Angle) {
         log("Adding wall")
+        Logger.indent()
         RoomModel.updateWalls()
-
+        Logger.outdent()
         val firstWall = RoomModel.walls.first()
         val lastWall = RoomModel.walls.last()
-//        if (firstWall == lastWall && RoomModel.walls.size > 1) {
         if (circleFound) {
             log("Found circle, finishing algorithm")
             isCompleted = true
@@ -155,7 +148,8 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                     && it.value.angle.degs() <= 120
                     && it.value.distance == -1
         }.size > anglesDistances.size / 2) {
-            log("Found to many -1 in angle distances, passing")
+            log("Found to many -1 in angle distances, falling back")
+            rollback()
             //todo Теоретически такая ситуация может быть валидной, если сразу после внутреннего угла идёт внешний
             return null
         }
@@ -166,9 +160,12 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                 .sumByDouble { it.distance.toDouble() }) / anglesDistances.values.size
         log("Estimated average = $average")
 
+        log("1. Checking if we just rotated and should re-calibrate")
         if (calibrateAfterRotate) {
             log("Calibrating after rotate")
+            Logger.indent()
             val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average)
+            Logger.outdent()
             if (maybeAlignment != null) {
                 log("Realigning")
                 return maybeAlignment
@@ -176,39 +173,35 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
             log("No need to realign")
             calibrateAfterRotate = false
         }
+        log("")
+
 
         // Check most basic measurements: 60/90/120
+        log("2. Check if we have measurement on 90")
         if (dist90.distance == -1 || dist90.distance > OUTER_CORNER_DISTANCE_THRESHOLD) {
             log("No orthogonal measurement found")
             return noOrthogonalMeasurementFound(anglesDistances, state)
         }
+        log("")
 
         // Add point to room map
         val point = Point(
                 x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
                 y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
         )
-        log("Adding ${point.toString()} to last wall")
+        log("Adding middle point ${point.toString()} to wall ${RoomModel.walls.last().id}")
         RoomModel.walls.last().pushBackPoint(point)
+        log("")
 
         // Check if corner reached
+        log("3. Check if we reached corner, dist0 = ${dist0.distance}")
         if (dist0.distance != -1 && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
             log("Wall ahead found")
             return wallAheadFound(anglesDistances, state)
         }
+        log("")
 
-        // Big fail, try to do something safe
+        log("Adding other points")
-        if (dist110.distance == -1 && dist70.distance == -1) {
-            log("No parallel wall found")
-            return noParallelWallFound(anglesDistances, state)
-        }
-
-        // Big fail too
-        if (dist110.distance == -1) {
-            log("No back measurement found, wtf")
-            return moveForward(dist0.distance)
-        }
-
         for (i in 70..110 step 10) {
             if (i == 90) {
                 continue    // point on 90 was already added
@@ -216,44 +209,57 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
 
             val curDist = anglesDistances[Angle(i)]!!.distance
             if (curDist > (average * 1.5) || curDist < (average / 2)) {
+                log("For point on ${i} dist = ${curDist}, while window is from ${average * 1.5} till ${average * 2}. Dropping it as outlier")
                 continue
             }
             val point = Point(
                     x = carX + curDist * Math.cos(degreesToRadian(carAngle - i)),
                     y = carY + curDist * Math.sin(degreesToRadian(carAngle - i))
             )
+            log("Adding ${point.toString()} to wall ${RoomModel.walls.last().id}")
             RoomModel.walls.last().pushBackPoint(point)
         }
+        log("")
 
         // Try align parallel to wall
+        log("4. Check if we have to align parallel to the wall")
+        Logger.indent()
         val maybeAlignment = tryAlignParallelToWall(anglesDistances, state, average)
+        Logger.outdent()
         if (maybeAlignment != null) {
             log("Realigning")
             return maybeAlignment
         }
+        log("")
 
         // Check if wall is too close or too far
+        log ("5. Check if we have to move closer or farther to the wall")
         if (dist90.distance > DISTANCE_TO_WALL_UPPER_BOUND || dist90.distance < DISTANCE_TO_WALL_LOWER_BOUND) {
             log("Flaw in distance to the parallel wall found, correcting")
             return correctDistanceToParallelWall(anglesDistances, state)
         }
+        log("")
 
         // Approaching inner corner and getting spurious reflection from 2 walls on 60;
         // Just move forward to get closer to the corner;
+        log ("6. Check if we are detecting echo reflections approaching inner corner")
         if (dist70.distance - dist0.distance > ECHO_REFLECTION_DIFF) {
-            log("Spurious reflection detected, moving forward")
+            log("Echo reflection detected, moving forward")
             return moveForward(dist0.distance)
         }
+        log ("")
 
         // Approaching outer corner (parallel wall is ending soon, but not yet);
         // Just move forward to get to the end of the wall
+        log ("7. Check if we are detecting far wall approaching outer corner")
         if (dist80.distance == -1 || Math.abs(dist100.distance - dist80.distance) > ISOSCALENESS_MAX_DIFF) {
-            log("Aprroaching outer corner, moving forward")
+            log("Approaching outer corner, moving forward")
             return moveForward(dist0.distance)
         }
+        log ("")
 
         // default case: everything is ok, just move forward
-        log("Default case: moving forward")
+        log("8. Default case: moving forward")
         return moveForward(dist0.distance)
     }
 
@@ -283,16 +289,15 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                 }
                 LEFT -> {
                     route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
-//                    carAngle += route.distances[idx]
                 }
                 RIGHT -> {
                     route.distances[idx] = (CHARGE_CORRECTION * route.distances[idx]).toInt()
-//                    carAngle -= route.distances[idx]
                 }
             }
         }
         if (Math.round(Math.cos(Angle(carAngle).rads())).toInt() == 1 && carAngle != 0
                 && Vector(carX.toDouble(), carY.toDouble()).length() < RANGE_FROM_ZERO_POINT_TO_FINISH_ALG) {
+            log("Found circle!")
             circleFound = true
         }
 

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

@@ -18,16 +18,22 @@ data class Wall(val wallAngleOX: Angle,
     }
 
     var isFinished = false
-    val MAX_REGRESSION_ERROR = 5.0
+    val MAX_SLOPE = 0.2
 
     fun pushBackPoint(point: Point) {
+        Logger.log("Adding ${point.toString()}")
         rawPoints.add(point)
+        Logger.log("Line before approximation ${line.toString()}")
         line = approximatePointsByLine()
+        Logger.log("Line after approximation ${line.toString()}")
     }
 
     fun pushFrontPoint(point: Point) {
+        Logger.log("Adding ${point.toString()}")
         rawPoints.add(0, point)
+        Logger.log("Line before approximation ${line.toString()}")
         line = approximatePointsByLine()
+        Logger.log("Line after approximation ${line.toString()}")
     }
 
     var points: ArrayList<Point> = arrayListOf()
@@ -86,20 +92,7 @@ data class Wall(val wallAngleOX: Angle,
         var beta1 = xybar / xxbar
         var beta0 = ybar - beta1 * xbar
 
-        // analyze results
+        if (Math.abs(beta1) > MAX_SLOPE) {
-        val df = n - 2
-        var rss = 0.0      // residual sum of squares
-        var ssr = 0.0      // regression sum of squares
-        for ((x, y) in rawPoints) {
-            val fit = beta1 * x + beta0
-            rss += (fit - y) * (fit - 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)