working algorithm on inner angles

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

@@ -4,6 +4,7 @@ import CodedOutputStream
 import Exceptions.InactiveCarException
 import RouteMetricRequest
 import SonarRequest
+import algorithm.geometry.Angle
 import algorithm.geometry.AngleData
 import io.netty.buffer.Unpooled
 import io.netty.handler.codec.http.*
@@ -17,6 +18,10 @@ 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
+
     protected val FORWARD = 0
     protected val BACKWARD = 1
     protected val LEFT = 2
@@ -24,8 +29,8 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
 
     private var prevState: CarState? = null
 
-    private var prevSonarDistances = mapOf<Int, AngleData>()
+    private var prevSonarDistances = mapOf<Angle, AngleData>()
-    private val defaultAngles = listOf(0, 60, 90, 120, 180).toIntArray()
+    private val defaultAngles = arrayOf(Angle(0), Angle(60), Angle(90), Angle(120), Angle(180))
     protected var requiredAngles = defaultAngles
 
     protected enum class CarState {
@@ -34,23 +39,20 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         OUTER
     }
 
-    protected fun getData(angles: IntArray): IntArray {
+    protected fun getData(angles: Array<Angle>): IntArray {
-
-        val attempts = 1
-        val threshold = 0
-        val smoothing = SonarRequest.Smoothing.NONE
 
+        val anglesIntArray = (angles.map { it -> it.degs() }).toIntArray()
         val message = SonarRequest.BuilderSonarRequest(
-                angles = angles,
+                angles = anglesIntArray,
-                attempts = IntArray(angles.size, { attempts }),
+                attempts = IntArray(angles.size, { ATTEMPTS }),
-                windowSize = 0,
+                threshold = THRESHOLD,
-                smoothing = smoothing)
+                smoothing = SMOOTHING)
                 .build()
         val requestBytes = ByteArray(message.getSizeNoTag())
         message.writeTo(CodedOutputStream(requestBytes))
         val request = getDefaultHttpRequest(thisCar.host, sonarUrl, requestBytes)
         try {
-            car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", angles)))
+            car.client.Client.sendRequest(request, thisCar.host, thisCar.port, mapOf(Pair("angles", anglesIntArray)))
         } catch (e: InactiveCarException) {
             println("connection error!")
         }
@@ -91,12 +93,15 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
     }
 
     fun iterate() {
+        if (RoomModel.finished) {
+            return
+        }
         val angles = getAngles()
         val distances = getData(angles)
         if (distances.size != angles.size) {
             throw RuntimeException("error! angles and distances have various sizes")
         }
-        val anglesDistances = mutableMapOf<Int, AngleData>()
+        val anglesDistances = mutableMapOf<Angle, AngleData>()
         for (i in 0..angles.size - 1) {
             if (Math.abs(distances[i]) < 0.01) {
                 continue
@@ -106,7 +111,10 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
 
         this.requiredAngles = defaultAngles
 
-        val state = getCarState(anglesDistances) ?: return
+        val state = getCarState(anglesDistances)
+        if (state == null) {
+            return
+        }
         val command = getCommand(anglesDistances, state)
         afterGetCommand(command)
         println(Arrays.toString(command.directions))
@@ -119,12 +127,20 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
     }
 
 
-    private fun getAngles(): IntArray {
+    protected fun getPrevState(): CarState? {
+        return prevState
+    }
+
+    protected fun getPrevSonarDistances(): Map<Angle, AngleData> {
+        return prevSonarDistances
+    }
+
+    private fun getAngles(): Array<Angle> {
         return requiredAngles
     }
 
-    protected abstract fun getCarState(anglesDistances: Map<Int, AngleData>): CarState?
+    protected abstract fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState?
-    protected abstract fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest
+    protected abstract fun getCommand(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest
     protected abstract fun afterGetCommand(route: RouteMetricRequest)
 
 

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

@@ -1,69 +1,140 @@
 package algorithm
 
-import objects.Car
-import java.util.concurrent.Exchanger
 import RouteMetricRequest
+import algorithm.geometry.Angle
 import algorithm.geometry.AngleData
 import algorithm.geometry.Point
+import algorithm.geometry.Wall
+import objects.Car
+import java.util.concurrent.Exchanger
 
 class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
 
-    val points = arrayListOf<Point>()
+    // TODO: set to appropriate values
-    private val DISTANCE_TO_WALL_THRESHOLD = 60
+    override val THRESHOLD = 0
+    override val ATTEMPTS: Int = 3
+    override val SMOOTHING = SonarRequest.Smoothing.MEDIAN
+
+    private val MAX_DISTANCE_TO_WALL_AHEAD = 60
+    private val ISOSCALENESS_THRESHOLD = 10
+    private val DISTANCE_TO_WALL_UPPER_BOUND = 50
+    private val DISTANCE_TO_WALL_LOWER_BOUND = 25
 
     var carX = 0
     var carY = 0
     var carAngle = 0
 
-    override fun getCarState(anglesDistances: Map<Int, AngleData>): CarState {
+    override fun getCarState(anglesDistances: Map<Angle, AngleData>): CarState {
         return CarState.WALL
     }
 
-    override fun getCommand(anglesDistances: Map<Int, AngleData>, state: CarState): RouteMetricRequest {
+    private fun noParallelWallFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
-        val dist0 = anglesDistances[0]
-        val dist60 = anglesDistances[60]
-        val dist90 = anglesDistances[90]
-        val dist120 = anglesDistances[120]
         val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
-        if (dist90 == null) {
+        resultBuilder.setDirections(getIntArray(FORWARD))
-            resultBuilder.setDirections(getIntArray(RIGHT))
+        resultBuilder.setDistances(getIntArray(10))
-            resultBuilder.setDistances(getIntArray(10))
+        return resultBuilder.build()
-            return resultBuilder.build()
+    }
+
+    private fun noOrthogonalMeasurementFound(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
+        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
+        resultBuilder.setDirections(getIntArray(RIGHT))
+        resultBuilder.setDistances(getIntArray(10))
+        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.setDistances(getIntArray(calculateAngle(anglesDistances, state).degs()))
+        addWall()
+        return resultBuilder.build()
+    }
+
+    private fun alignParallelToWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
+        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
+
+        // we're sure that we have 120 and 60 degs measurements as they were checked before
+        val rotationDirection = if (anglesDistances[Angle(120)]!!.distance > anglesDistances[Angle(60)]!!.distance) LEFT else RIGHT
+
+        resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
+        resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
+        return resultBuilder.build()
+    }
+
+    private fun correctDistanceToParallelWall(anglesDistances: Map<Angle, AngleData>, state: CarState): RouteMetricRequest {
+        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
+
+        val rotationDirection = if (anglesDistances[Angle(90)]!!.distance > 50) RIGHT else LEFT
+
+        resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
+        resultBuilder.setDistances(getIntArray(10, 10))
+        return resultBuilder.build()
+    }
+
+    private fun addWall() {
+        RoomModel.updateWalls()
+
+        if (RoomModel.walls.last() == RoomModel.walls.first() && RoomModel.walls.size > 1) {
+            println("YEAH!")
+            RoomModel.finished = true
+            RoomModel.walls.removeAt(RoomModel.walls.size - 1)
         }
+        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 dist90 = anglesDistances[Angle(90)]
+        val dist120 = anglesDistances[Angle(120)]
         val sonarAngle = carAngle - 90
-        points.add(Point(
+
-                x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
+        // Check most basic measurements: 60/90/120
-                y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
+        if (dist90 == null) {
-        ))
+            return noOrthogonalMeasurementFound(anglesDistances, state)
-        if (dist120 == null || dist60 == null) {
-            resultBuilder.setDirections(getIntArray(FORWARD))
-            resultBuilder.setDistances(getIntArray(10))
-            return resultBuilder.build()
-        }
-        if (dist0 != null && dist0.distance < DISTANCE_TO_WALL_THRESHOLD) {
-            resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
-            resultBuilder.setDistances(getIntArray(20, 10))
-            return resultBuilder.build()
-        }
-        val abs = Math.abs(dist120.distance - dist60.distance)
-        if (abs > 10) {
-            val rotationDirection = if (dist120.distance > dist60.distance) LEFT else RIGHT
-            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
-            resultBuilder.setDistances(getIntArray(10, 7))//todo calibrate
-            return resultBuilder.build()
-        }
-        if (dist90.distance > 50 || dist90.distance < 25) {
-            val rotationDirection = if (dist90.distance > 50) RIGHT else LEFT
-            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
-            resultBuilder.setDistances(getIntArray(10, 10))
-            return resultBuilder.build()
         }
 
+        if (dist120 == null || dist60 == null) {
+            return noParallelWallFound(anglesDistances, state)
+        }
+
+        // Add point to room map
+        val point = Point(
+                x = carX + dist90.distance * Math.cos(degreesToRadian(sonarAngle)),
+                y = carY + dist90.distance * Math.sin(degreesToRadian(sonarAngle))
+        )
+        RoomModel.walls.last().addPoint(point)
+
+        // Check if corner reached
+        if (dist0 != null && dist0.distance < MAX_DISTANCE_TO_WALL_AHEAD) {
+            return wallAheadFound(anglesDistances, state)
+        }
+
+        // Checks for paralleleness
+
+        // Check if wall is not parallel
+        val diff = Math.abs(dist120.distance - dist60.distance)
+        if (diff > ISOSCALENESS_THRESHOLD) {
+            return alignParallelToWall(anglesDistances, state)
+        }
+
+        // Check if wall is too close or too far
+        if (dist90.distance > 50 || dist90.distance < 25) {
+            return correctDistanceToParallelWall(anglesDistances, state)
+        }
+
+        // default case: everything is ok, just move forward
+        val resultBuilder = RouteMetricRequest.BuilderRouteMetricRequest(IntArray(0), IntArray(0))
         resultBuilder.setDirections(getIntArray(FORWARD))
         resultBuilder.setDistances(getIntArray(20))
         return resultBuilder.build()
     }
 
+    private fun calculateAngle(anglesDistances: Map<Angle, AngleData>, state: CarState): Angle {
+        // TODO: stub here, make proper angle calculation
+        return Angle(90)
+    }
+
     private fun getIntArray(vararg args: Int): IntArray {
         return args
     }

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

@@ -4,11 +4,16 @@ import DebugClInterface
 import DebugResponse
 import Waypoints
 import algorithm.geometry.Line
+import algorithm.geometry.Point
+import algorithm.geometry.Wall
 import objects.Car
+import java.util.*
 
 object RoomModel {
 
     val lines = arrayListOf<Line>()
+    val walls = arrayListOf<Wall>(Wall())
+    var finished = false
 
     val linesModel = listOf(Line(0.0, 1.0, -300.0),
             Line(1.0, 0.0, 150.0),
@@ -23,21 +28,17 @@ object RoomModel {
     var currentPosition_y = 0
     fun getUpdate(): Waypoints {
 
-        val algorithm = DebugClInterface.algorithmImpl
+        val points = getWallsPoints()
-        if (algorithm == null || algorithm !is RoomBypassingAlgorithm) {
-            val emptyArr = IntArray(0)
-            return Waypoints.BuilderWaypoints(emptyArr, emptyArr, emptyArr, emptyArr, false).build()
 
-        }
+        val begin_x = IntArray(points.size)
-        val begin_x = IntArray(algorithm.points.size)
+        val begin_y = IntArray(points.size)
-        val begin_y = IntArray(algorithm.points.size)
+        val end_x = IntArray(points.size)
-        val end_x = IntArray(algorithm.points.size)
+        val end_y = IntArray(points.size)
-        val end_y = IntArray(algorithm.points.size)
 
 
-        for (i in 0..algorithm.points.size - 2) {
+        for (i in 0..points.size - 2) {
-            val curPoint = algorithm.points[i]
+            val curPoint = points[i]
-            val nextPoint = algorithm.points[i + 1]
+            val nextPoint = points[i + 1]
             begin_x[i] = (curPoint.x + 0.5).toInt()
             begin_y[i] = (curPoint.y + 0.5).toInt()
 
@@ -47,6 +48,22 @@ object RoomModel {
         return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build()
     }
 
+    private fun getWallsPoints(): Array<Point> {
+        return walls.flatMap({ it.points }).toTypedArray()
+    }
+
+    fun updateWalls() {
+        if (walls.size < 2) {
+            // no walls to intersect
+            return
+        }
+        val line1 = walls.last().line
+        val line2 = walls[walls.size - 2].line
+
+        val intersection: Point = line1.intersect(line2)
+        walls[walls.size - 2].addPoint(intersection)
+    }
+
     fun getDebugInfo(): DebugResponse {
 
         val aRef = IntArray(linesModel.size)

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

@@ -0,0 +1,22 @@
+package algorithm.geometry
+
+data class Angle(private val degrees: Int) {
+
+    constructor(radians: Double) : this((180.0 * radians / Math.PI).toInt())
+
+    fun rads(): Double {
+        return Math.PI * degrees / 180.0
+    }
+
+    fun degs(): Int {
+        return degrees
+    }
+
+    operator fun minus(other: Angle): Angle {
+        return Angle(degrees - other.degrees)
+    }
+
+    operator fun plus(other: Angle): Angle {
+        return Angle(degrees + other.degrees)
+    }
+}

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

@@ -1,14 +1,14 @@
 package algorithm.geometry
 
 
-class AngleData(val angle: Int, val distance: Int) {
+class AngleData(val angle: Angle, val distance: Int) {
 
 
-    fun toPoint(carAngleOX: Int): Point {
+    fun toPoint(carAngleOX: Angle): Point {
 
         //convert to global coordinate system
         val angle = carAngleOX - angle
-        val radianAngle = Util.degreesToRadian(angle)
+        val radianAngle = angle.rads()
 
         return Point(
                 x = Math.cos(radianAngle) * distance,

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

@@ -0,0 +1,13 @@
+package algorithm.geometry
+
+fun Double.lt(other: Double): Boolean {
+    return this - other < -Util.eps
+}
+
+fun Double.gt(other: Double): Boolean {
+    return this - other > Util.eps
+}
+
+fun Double.eq(other: Double): Boolean {
+    return Math.abs(this - other) < Util.eps;
+}

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

@@ -1,21 +1,58 @@
 package algorithm.geometry
 
-class Line(val A: Double, val B: Double, var C: Double) {
+class Line(var A: Double, var B: Double, var C: Double) {
+    val COMPARISON_THRESHOLD = 0.5
 
+    init {
+        normalize()
+    }
 
-    fun getIntersectionPoint(lineTwo: Line): Pair<Double, Double> {
+    fun intersect(lineTwo: Line): Point {
         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)
+        return Point(xIntersection, yIntersection)
     }
 
-    override fun toString(): String{
+    override fun toString(): String {
         return "Line(A=$A, B=$B, C=$C)"
     }
 
+    fun normalize() {
+        val div = Math.sqrt(A * A + B * B);
+        A /= div;
+        B /= div;
+        C /= div;
 
+        if (A.eq(0.0)) {
+            if (B.lt(0.0)) {
+                A *= -1
+                B *= -1
+                C *= -1
+            }
+        }
+        else
+        {
+            if (A.lt(0.0)){
+                A *= -1
+                B *= -1
+                C *= -1
+            }
+        }
+    }
+
+    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
+
+        return metricDist(other) < COMPARISON_THRESHOLD
+    }
 }

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

@@ -1,4 +1,6 @@
 package algorithm.geometry
 
-class Point(val x: Double, val y: Double) {
+data class Point(val x: Double, val y: Double) {
+
+
 }

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

@@ -2,13 +2,5 @@ package algorithm.geometry
 
 object Util {
 
-
+    val eps = 1e-2
-    fun degreesToRadian(angleDegrees: Int): Double {
-        return Math.PI * angleDegrees / 180.0
-    }
-
-    fun radianToDegrees(angleRad: Double): Int {
-        return (180.0 * angleRad / Math.PI).toInt()
-    }
-
 }

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

@@ -0,0 +1,39 @@
+package algorithm.geometry
+
+import java.util.*
+
+data class Wall(val points: ArrayList<Point> = arrayListOf<Point>(),
+                var line: Line = Line(0.0, 0.0, 0.0)) {
+
+
+    fun addPoint(point: Point) {
+        points.add(point)
+        line = approximatePointsByLine()
+    }
+
+    private fun approximatePointsByLine(): Line {
+
+        val sumX = points.sumByDouble { it.x }
+        val sumXQuad = points.sumByDouble { it.x * it.x }
+        val sumY = points.sumByDouble { it.y }
+        val sumXY = points.sumByDouble { it.y * it.x }
+
+        val pointsCount = points.size
+        val den = pointsCount * sumXQuad - sumX * sumX
+
+        if (Math.abs(den) < 0.001) {
+            return Line(1.0, 0.0, -points.first().x)
+        }
+
+        val k = (pointsCount * sumXY - sumX * sumY) / (pointsCount * sumXQuad - sumX * sumX)
+        val b = (sumY - sumX * k) / pointsCount
+
+        return Line(-k, 1.0, -b)
+    }
+
+    override fun equals(other: Any?): Boolean {
+        if (other == null) return false
+        if (!(other is Wall)) return false
+        return line.equals(other.line)
+    }
+}