make new algorithm. based on car position

server/src/main/java/DebugClInterface.kt

@@ -2,6 +2,7 @@ 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.*
@@ -64,6 +65,10 @@ object DebugClInterface {
                 val tmp = algorithmImpl
                 if (tmp is RoomBypassingAlgorithm) {
                     println("length: ${tmp.wallLength} angleOX: ${tmp.wallAngleWithOX}")
+                    println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}")
+                } else if (tmp is RoomTest) {
+                    println("points: ${tmp.points}")
+                    println("x: ${tmp.carX} y: ${tmp.carY} angle:${tmp.carAngle}")
                 }
             }
             "alg" -> executeAlg(readString)
@@ -81,7 +86,8 @@ object DebugClInterface {
                 } else 1
 
         if (algorithmImpl == null) {
-            algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
+            algorithmImpl = RoomTest(environment.map.values.last(), exchanger)
+//            algorithmImpl = RoomBypassingAlgorithm(environment.map.values.last(), exchanger)
         }
         while (count > 0) {
             count--

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

@@ -49,7 +49,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
         }
 
         try {
-            val distances = exchanger.exchange(IntArray(0), 20, TimeUnit.SECONDS)
+            val distances = exchanger.exchange(IntArray(0), 300, TimeUnit.SECONDS)
             val result = DoubleArray(angles.size)
             for (i in 0..result.size - 1) {
                 val distancesOnCurrentAngle = distances.drop(i * copyElemCount).take(copyElemCount)
@@ -60,7 +60,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
                     }
                 }
                 if (sum != 0) {
-                    result[i] = (sum.toDouble()) / (copyElemCount*1000000)
+                    result[i] = (sum.toDouble()) / (copyElemCount)
                 }
             }
             return result
@@ -113,6 +113,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
             return
         }
         val command = getCommand(anglesDistances, state)
+        afterGetCommand(command)
         println(Arrays.toString(command.directions))
         println(Arrays.toString(command.times))
 
@@ -137,6 +138,7 @@ abstract class AbstractAlgorithm(val thisCar: Car, val exchanger: Exchanger<IntA
 
     protected abstract fun getCarState(anglesDistances: Map<Int, Double>): CarState?
     protected abstract fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest
+    protected abstract fun afterGetCommand(route:RouteRequest)
 
 
     private fun getDefaultHttpRequest(host: String, url: String, bytes: ByteArray): DefaultFullHttpRequest {

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

@@ -16,6 +16,9 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
     public var wallLength = 0.0//sm
     var errorCount = 0
 
+    var carX = 0
+    var carY = 0
+    var carAngle = 0
 
     private var prevPoint = Pair(0.0, 0.0)
 
@@ -40,6 +43,27 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
         return args
     }
 
+    override fun afterGetCommand(route: RouteRequest) {
+        route.directions.forEachIndexed { idx, direction ->
+            when (direction) {
+                FORWARD -> {
+                    carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
+                    carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
+                }
+                BACKWARD -> {
+                    carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
+                    carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
+                }
+                LEFT -> {
+                    carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
+                }
+                RIGHT -> {
+                    carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
+                }
+            }
+        }
+    }
+
     override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
         val dist0 = anglesDistances[0]
         val dist60 = anglesDistances[60]
@@ -74,8 +98,8 @@ class RoomBypassingAlgorithm(thisCar: Car, exchanger: Exchanger<IntArray>) : Abs
                 if (dist90 > 40 || dist90 < 20) {
                     val rotationDirection = if (dist90 > 40) RIGHT else LEFT
                     resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
-                    resultBuilder.setTimes(getIntArray((30.0 / ROTATION_VELOCITY).toInt(),
+                    resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(),
-                            (25.0 / MOVE_VELOCITY).toInt()))
+                            (35.0 / MOVE_VELOCITY).toInt()))
                     return resultBuilder.build()
                 }
 

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

@@ -1,8 +1,9 @@
 package algorithm
 
+import DebugClInterface
+import DebugResponse
 import Waypoints
 import algorithm.geometry.Line
-import DebugResponse
 import objects.Car
 
 object RoomModel {
@@ -21,27 +22,37 @@ object RoomModel {
     var currentPosition_x = 0
     var currentPosition_y = 0
     fun getUpdate(): Waypoints {
-        if (iter == 5) {
+
-            web.server.Server.changeMode(web.server.Server.ServerMode.IDLE)
+        val algorithm = DebugClInterface.algorithmImpl
-            return Waypoints.BuilderWaypoints(IntArray(0), IntArray(0), IntArray(0), IntArray(0), true).build()
+        if (algorithm == null || !(algorithm is RoomTest)) {
+            val emptyArr = IntArray(0)
+            return Waypoints.BuilderWaypoints(emptyArr, emptyArr, emptyArr, emptyArr, false).build()
+
         }
+        val begin_x = IntArray(algorithm.points.size)
+        val begin_y = IntArray(algorithm.points.size)
+        val end_x = IntArray(algorithm.points.size)
+        val end_y = IntArray(algorithm.points.size)
 
-        val begin_x = IntArray(10)
-        val begin_y = IntArray(10)
-        val end_x = IntArray(10)
-        val end_y = IntArray(10)
 
-        for (i in 0..9) {
+        for (i in 0..algorithm.points.size - 1) {
-            begin_x[i] = currentPosition_x
+            val curPoint = algorithm.points[i]
-            begin_y[i] = currentPosition_y
+            if (algorithm.points.size - 1 == i) {
+//                val nextPoint = algorithm.points[0]
+//                begin_x[i] = curPoint.first.toInt()
+//                begin_y[i] = curPoint.second.toInt()
+//
+//                end_x[i] = nextPoint.first.toInt()
+//                end_y[i] = nextPoint.second.toInt()
+                continue
+            }
+            val nextPoint = algorithm.points[i + 1]
+            begin_x[i] = curPoint.first.toInt()
+            begin_y[i] = curPoint.second.toInt()
 
-            currentPosition_x += rng.nextInt(3)
+            end_x[i] = nextPoint.first.toInt()
-            currentPosition_y += rng.nextInt(3)
+            end_y[i] = nextPoint.second.toInt()
-
-            end_x[i] = currentPosition_x
-            end_y[i] = currentPosition_y
         }
-        iter++
         return Waypoints.BuilderWaypoints(begin_x, begin_y, end_x, end_y, false).build()
     }
 

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

@@ -0,0 +1,96 @@
+package algorithm
+
+import objects.Car
+import java.util.concurrent.Exchanger
+import RouteRequest
+
+class RoomTest(thisCar: Car, exchanger: Exchanger<IntArray>) : AbstractAlgorithm(thisCar, exchanger) {
+
+    private val MOVE_VELOCITY = 0.05//sm/ms
+    private val ROTATION_VELOCITY = 0.05//degrees/ms
+
+    val points = arrayListOf<Pair<Double, Double>>()
+
+
+    var carX = 0
+    var carY = 0
+    var carAngle = 0
+
+    override fun getCarState(anglesDistances: Map<Int, Double>): CarState {
+        return CarState.WALL
+    }
+
+    override fun getCommand(anglesDistances: Map<Int, Double>, state: CarState): RouteRequest {
+        val dist0 = anglesDistances[0]
+        val dist60 = anglesDistances[60]
+        val dist90 = anglesDistances[90]
+        val dist120 = anglesDistances[120]
+        val resultBuilder = RouteRequest.BuilderRouteRequest(IntArray(0), IntArray(0))
+        if (dist90 == null) {
+            resultBuilder.setDirections(getIntArray(RIGHT))
+            resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt()))
+            return resultBuilder.build()
+        }
+        val sonarAngle = carAngle - 90
+        points.add(Pair(carX + dist90 * Math.cos(degreesToRadian(sonarAngle)),
+                carY + dist90 * Math.sin(degreesToRadian(sonarAngle))))
+        if (dist120 == null || dist60 == null) {
+            //hmm
+            resultBuilder.setDirections(getIntArray(FORWARD))
+            resultBuilder.setTimes(getIntArray((10 / MOVE_VELOCITY).toInt()))
+            return resultBuilder.build()
+        }
+        if (dist0 != null && dist0 < 60) {
+            resultBuilder.setDirections(getIntArray(LEFT, FORWARD))
+            resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
+            return resultBuilder.build()
+        }
+        val abs = Math.abs(dist120 - dist60)
+        if (abs > 10) {
+            val rotationDirection = if (dist120 > dist60) LEFT else RIGHT
+            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
+            resultBuilder.setTimes(getIntArray((10.0 / ROTATION_VELOCITY).toInt(), (7 / MOVE_VELOCITY).toInt()))//todo calibrate
+            return resultBuilder.build()
+        }
+        if (dist90 > 50 || dist90 < 25) {
+            val rotationDirection = if (dist90 > 50) RIGHT else LEFT
+            resultBuilder.setDirections(getIntArray(rotationDirection, FORWARD))
+            resultBuilder.setTimes(getIntArray((10 / ROTATION_VELOCITY).toInt(), (10 / MOVE_VELOCITY).toInt()))
+            return resultBuilder.build()
+        }
+
+        resultBuilder.setDirections(getIntArray(FORWARD))
+        resultBuilder.setTimes(getIntArray((20 / ROTATION_VELOCITY).toInt()))
+        return resultBuilder.build()
+    }
+
+    private fun getIntArray(vararg args: Int): IntArray {
+        return args
+    }
+
+    override fun afterGetCommand(route: RouteRequest) {
+        route.directions.forEachIndexed { idx, direction ->
+            when (direction) {
+                FORWARD -> {
+                    carX += (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
+                    carY += (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
+                }
+                BACKWARD -> {
+                    carX -= (MOVE_VELOCITY * route.times[idx] * Math.cos(degreesToRadian(carAngle.toInt()))).toInt()
+                    carY -= (MOVE_VELOCITY * route.times[idx] * Math.sin(degreesToRadian(carAngle.toInt()))).toInt()
+                }
+                LEFT -> {
+                    carAngle += (ROTATION_VELOCITY * route.times[idx]).toInt()
+                }
+                RIGHT -> {
+                    carAngle -= (ROTATION_VELOCITY * route.times[idx]).toInt()
+                }
+            }
+        }
+    }
+
+    private fun degreesToRadian(angle: Int): Double {
+        return Math.PI * angle / 180
+    }
+
+}