server: room scaner algorithm boilerplate

server/src/main/java/DebugClInterface.kt

@@ -1,13 +1,13 @@
+
 import Exceptions.InactiveCarException
 import algorithm.AbstractAlgorithm
 import algorithm.RoomBypassingAlgorithm
-import algorithm.RoomModel
-import algorithm.RoomTest
 import car.client.Client
 import io.netty.buffer.Unpooled
 import io.netty.handler.codec.http.*
 import objects.Car
 import java.util.concurrent.Exchanger
+import algorithm.RoomTest
 
 object DebugClInterface {
 
@@ -268,7 +268,7 @@ object DebugClInterface {
     }
 
     private fun getRouteMessage(): RouteRequest? {
-        println("print way points in polar coordinate als [distance] [rotation angle] in metres and degrees." +
+        println("print way points in polar coordinate als [distance] [rotation target] in metres and degrees." +
                 "after enter all points print \"done\". for reset route print \"reset\"")
         println("e.g. for move from (x,y) to (x+1,y) and back to (x,y) you need enter:" +
                 "1 0[enter] 1 180[enter] done")
@@ -292,7 +292,7 @@ object DebugClInterface {
                         distances.add(distance)
                         angles.add(angle)
                     } catch (e: NumberFormatException) {
-                        println("error in converting angle or distance to int. try again")
+                        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")
                     }

server/src/main/java/RoomScanner/CarController.kt

@@ -0,0 +1,50 @@
+package RoomScanner
+
+import objects.Car
+
+class CarController(var car: Car) {
+    enum class Direction(val id: Int) {
+        FORWARD(0),
+        BACKWARD(1),
+        LEFT(2),
+        RIGHT(3);
+    }
+
+    val WALL_DISTANCE = 50
+
+    var position = Pair(0.0, 0.0)
+        private set
+
+    private var angle = 0.0
+
+    fun moveTo(to: Pair<Double, Double>) {
+        val oldAngle = angle
+        rotateOn(estimateAngle(position, to))
+        drive(Direction.FORWARD, Math.max(0.0, distance(position, to)).toInt() - WALL_DISTANCE)
+        rotateOn(oldAngle)
+    }
+
+
+    fun rotateOn(target: Double) {
+        val rotateAngle = angleDistance(angle, target)
+        val direction = if (rotateAngle > 0) Direction.RIGHT else Direction.LEFT
+        drive(direction, rotateAngle.toInt())
+
+        angle = rotateAngle
+    }
+
+    private fun drive(direction: Direction, distance: Int) {
+        when (direction) {
+            Direction.BACKWARD -> {}
+            Direction.FORWARD -> {}
+            Direction.LEFT -> {}
+            Direction.RIGHT -> {}
+        }
+    }
+
+    fun scan(angles: IntArray): MutableList<Pair<Double, Double>> {
+        throw UnsupportedOperationException()
+    }
+
+
+}

server/src/main/java/RoomScanner/RoomScanner.kt

@@ -0,0 +1,30 @@
+package RoomScanner
+
+class RoomScanner(val controller: CarController) {
+    private val points = mutableListOf<Pair<Double, Double>>()
+
+    fun run() {
+        while (true) {
+            scan()
+            println("[${points.joinToString { "[${it.first}, ${it.second}]" }}}]")
+        }
+    }
+
+    fun scan() {
+        val horizon = IntArray(180 / 5, { it * 5 })
+
+        controller.rotateOn(0.0)
+        val iterationPoints = controller.scan(horizon)
+
+        controller.rotateOn(180.0)
+        iterationPoints.addAll(controller.scan(horizon))
+
+        points.addAll(iterationPoints)
+
+        val target = iterationPoints.maxBy { distance(controller.position, it) }
+        target ?: return
+
+        controller.moveTo(target)
+    }
+
+}

server/src/main/java/RoomScanner/utils.kt

@@ -0,0 +1,21 @@
+package RoomScanner
+
+fun distance(first: Pair<Double, Double>, second: Pair<Double, Double>): Double {
+    val xDistance = first.first - second.first
+    val yDistance = first.second - second.second
+
+    return Math.sqrt(xDistance * xDistance + yDistance * yDistance)
+}
+
+fun estimateAngle(from: Pair<Double, Double>, to: Pair<Double, Double>): Double =
+        Math.atan2(to.first - from.first, to.second - from.second)
+
+fun angleDistance(from: Double, to: Double): Double {
+    val min = Math.min(from, to)
+    val max = Math.max(from, to)
+
+    val up = max - min
+    val down = 360 - max + min
+
+    if (Math.abs(up) < Math.abs(down)) return up else return down
+}

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

@@ -4,7 +4,6 @@ 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) {
@@ -26,7 +25,6 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
         val dist0 = anglesDistances[0]
         val dist90 = anglesDistances[90]
         if (dist90 == null || dist90 > 85) {
-            //best analysis of outer angle is check minimum in values 60,70,80,90,100,...
             requiredAngles = getIntArray(0, 60, 90, 120, 180)
             return CarState.OUTER
         }
@@ -169,7 +167,7 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                 return resultBuilder.build()
             }
             CarState.OUTER -> {
-                //todo calculate angle
+                //todo calculate target
                 resultBuilder.setDirections(getIntArray(RIGHT, FORWARD, RIGHT))
                 resultBuilder.setTimes(getIntArray((45 / ROTATION_VELOCITY).toInt(),
                         (40 / MOVE_VELOCITY).toInt(), (45 / ROTATION_VELOCITY).toInt()))

server/src/main/java/objects/Car.kt

@@ -24,6 +24,6 @@ class Car constructor(uid: Int, host: String, port: Int) {
     }
 
     override fun toString(): String {
-        return "$uid ; x:$x; y:$y; angle:$angle"
+        return "$uid ; x:$x; y:$y; target:$angle"
     }
 }

server/src/main/java/web/server/Handler.kt

@@ -43,7 +43,7 @@ class Handler : SimpleChannelInboundHandler<Any>() {
                 response.headers().setInt(HttpHeaderNames.CONTENT_LENGTH, response.content().readableBytes())
                 response.headers().add("Access-Control-Allow-Origin", "*");
                 response.headers().add("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,OPTIONS");
-                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Type, Content-Length");
+                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Direction, Content-Length");
                 ctx.writeAndFlush(response).addListener(io.netty.channel.ChannelFutureListener.CLOSE)
             }
             Constants.getWaypointsURL -> {
@@ -63,7 +63,7 @@ class Handler : SimpleChannelInboundHandler<Any>() {
                 response.headers().setInt(HttpHeaderNames.CONTENT_LENGTH, response.content().readableBytes())
                 response.headers().add("Access-Control-Allow-Origin", "*");
                 response.headers().add("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,OPTIONS");
-                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Type, Content-Length");
+                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Direction, Content-Length");
                 ctx.writeAndFlush(response).addListener(io.netty.channel.ChannelFutureListener.CLOSE)
             }
             Constants.directionOrderURL -> {
@@ -94,7 +94,7 @@ class Handler : SimpleChannelInboundHandler<Any>() {
                 response.headers().setInt(HttpHeaderNames.CONTENT_LENGTH, response.content().readableBytes())
                 response.headers().add("Access-Control-Allow-Origin", "*");
                 response.headers().add("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,OPTIONS");
-                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Type, Content-Length");
+                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Direction, Content-Length");
                 ctx.writeAndFlush(response).addListener(io.netty.channel.ChannelFutureListener.CLOSE)
             }
             Constants.getDebug -> {
@@ -110,7 +110,7 @@ class Handler : SimpleChannelInboundHandler<Any>() {
                 response.headers().setInt(HttpHeaderNames.CONTENT_LENGTH, response.content().readableBytes())
                 response.headers().add("Access-Control-Allow-Origin", "*");
                 response.headers().add("Access-Control-Allow-Methods", "GET,POST,PUT,DELETE,OPTIONS");
-                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Type, Content-Length");
+                response.headers().add("Access-Control-Allow-Headers", "X-Requested-With, Content-Direction, Content-Length");
                 ctx.writeAndFlush(response).addListener(io.netty.channel.ChannelFutureListener.CLOSE)
             }
         }