From 55d5224c1c0f0bf11f68b943debc9f8a805cc2aa Mon Sep 17 00:00:00 2001
From: e5l <leonid.stashevsky@gmail.com>
Date: Tue, 30 Aug 2016 13:08:55 +0300
Subject: [PATCH] car_fmw: sonar explore angle feature

---
 car_fmw/src/Control.kt                        | 12 ++++++++++
 car_fmw/src/ProtoRW.kt                        | 11 +++++++++
 car_fmw/src/include/Sonar.kt                  | 23 +++++++++----------
 car_fmw/src/include/utils.kt                  |  8 +++++++
 .../protofiles_sources/server_car/sonar.proto |  9 ++++++++
 .../protofiles_sources/server_car/task.proto  |  1 +
 6 files changed, 52 insertions(+), 12 deletions(-)

diff --git a/car_fmw/src/Control.kt b/car_fmw/src/Control.kt
index 9c2e5dc7f02..b686b11ff8e 100644
--- a/car_fmw/src/Control.kt
+++ b/car_fmw/src/Control.kt
@@ -16,6 +16,7 @@ object Control {
             TaskRequest.Type.ROUTE.id -> routeTask()
             TaskRequest.Type.ROUTE_METRIC.id -> routeMetricTask()
             TaskRequest.Type.SONAR.id -> sonarTask()
+            TaskRequest.Type.SONAR_EXPLORE.id -> sonarExploreTask()
         }
     }
 
@@ -83,6 +84,17 @@ object Control {
         Writer.writeSonar(response)
     }
 
+    private fun sonarExploreTask() {
+        val request = Reader.readSonarExplore()
+
+        val angle = request.angle
+        val window = request.windowSize
+        val result = Sonar.getRange(max(angle - window, 0), min(angle + window, 360))
+
+        val response = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(result).build()
+        Writer.writeSonarExplore(response)
+    }
+
     private fun sonarMeasure(smoothing: SonarRequest.Smoothing, attempts: Int, angle: Int, windowSize: Int): Int {
         val data = IntArray(attempts)
         var i = 0
diff --git a/car_fmw/src/ProtoRW.kt b/car_fmw/src/ProtoRW.kt
index edf7fff34bc..a0af0ec691e 100644
--- a/car_fmw/src/ProtoRW.kt
+++ b/car_fmw/src/ProtoRW.kt
@@ -24,6 +24,11 @@ object Reader {
         return SonarRequest.BuilderSonarRequest(IntArray(0), IntArray(0), 0, SonarRequest.Smoothing.NONE).parseFrom(stream).build()
     }
 
+    fun readSonarExplore(): SonarExploreAngleRequest {
+        val stream = getInputStream()
+        return SonarExploreAngleRequest.BuilderSonarExploreAngleRequest(0, 0, 0).parseFrom(stream).build()
+    }
+
     private fun getInputStream(): CodedInputStream {
         val buffer = Connection.receiveByteArray()
         return CodedInputStream(buffer)
@@ -49,6 +54,12 @@ object Writer {
         Connection.sendByteArray(stream.buffer)
     }
 
+    fun writeSonarExplore(response: SonarExploreAngleResponse) {
+        val stream = getOutputStream(response.getSizeNoTag())
+        response.writeTo(stream)
+        Connection.sendByteArray(stream.buffer)
+    }
+
     private fun getOutputStream(size: Int): CodedOutputStream {
         val buffer = ByteArray(size)
         return CodedOutputStream(buffer)
diff --git a/car_fmw/src/include/Sonar.kt b/car_fmw/src/include/Sonar.kt
index 221b69a605b..847542b0d3e 100644
--- a/car_fmw/src/include/Sonar.kt
+++ b/car_fmw/src/include/Sonar.kt
@@ -42,6 +42,17 @@ object Sonar {
         return -1
     }
 
+    fun getRange(start: Int, stop: Int): IntArray {
+        val result = IntArray(stop - start + 1)
+        var i = 0
+        while (i < stop - start + 1) {
+            result[i] = getDistance(start + i)
+            i++
+        }
+
+        return result
+    }
+
     private fun getOneOfRange(start: Int, stop: Int): Int {
         var i = start
         var distance = getDistance(start)
@@ -58,18 +69,6 @@ object Sonar {
         return distance
     }
 
-    private fun getRange(start: Int, stop: Int): IntArray {
-        val result = IntArray(stop - start + 1)
-        var i = 0
-        while (i < stop - start + 1) {
-            result[i] = getDistance(start + i)
-            i++
-        }
-
-        return result
-    }
-
-
     fun calibrate() {
         getDistance(180)
     }
diff --git a/car_fmw/src/include/utils.kt b/car_fmw/src/include/utils.kt
index 75a1bcf4409..78c31dbe375 100644
--- a/car_fmw/src/include/utils.kt
+++ b/car_fmw/src/include/utils.kt
@@ -2,3 +2,11 @@
 fun positive(i: Int): Boolean {
     return i > 0
 }
+
+fun min(first: Int, second: Int): Int {
+    if (first < second) return first else return second
+}
+
+fun max(first: Int, second: Int): Int {
+    if (first > second) return first else return second
+}
diff --git a/proto/protofiles_sources/server_car/sonar.proto b/proto/protofiles_sources/server_car/sonar.proto
index f94d31ba849..c4a37eb7e98 100644
--- a/proto/protofiles_sources/server_car/sonar.proto
+++ b/proto/protofiles_sources/server_car/sonar.proto
@@ -16,3 +16,12 @@ message SonarRequest {
 message SonarResponse {
     repeated int32 distances = 1;
 }
+
+message SonarExploreAngleRequest {
+    int32 angle = 1;
+    int32 windowSize = 2;
+}
+
+message SonarExploreAngleResponse {
+    repeated int32 distances = 1;
+}
diff --git a/proto/protofiles_sources/server_car/task.proto b/proto/protofiles_sources/server_car/task.proto
index 8e6384bbb73..a96ab2347f5 100644
--- a/proto/protofiles_sources/server_car/task.proto
+++ b/proto/protofiles_sources/server_car/task.proto
@@ -10,5 +10,6 @@ message TaskRequest {
         ROUTE = 1;
         SONAR = 2;
         ROUTE_METRIC = 3;
+        SONAR_EXPLORE = 4;
     }
 }