From a93159df90840901c64050c8fc1f309d3c4c662c Mon Sep 17 00:00:00 2001
From: Cyrille Nofficial <cynoffic@cyrilix.fr>
Date: Mon, 22 Aug 2022 19:51:44 +0200
Subject: [PATCH] WIP

---
 pkg/steering/bbox_test.go |  6 ++--
 pkg/steering/corrector.go | 70 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 73 insertions(+), 3 deletions(-)

diff --git a/pkg/steering/bbox_test.go b/pkg/steering/bbox_test.go
index af58d91..b85e106 100644
--- a/pkg/steering/bbox_test.go
+++ b/pkg/steering/bbox_test.go
@@ -35,7 +35,7 @@ func init() {
 	dataImages = make(map[string]*gocv.Mat, len(dataNames))
 
 	for _, dataName := range dataNames {
-		img, bb, err := load_data(dataName)
+		img, bb, err := loadData(dataName)
 		if err != nil {
 			zap.S().Panicf("unable to load data test: %v", err)
 		}
@@ -61,7 +61,7 @@ func (bb *BBox) toRect(imgWidth, imgHeight int) image.Rectangle {
 	)
 }
 
-func load_data(dataName string) (*gocv.Mat, []BBox, error) {
+func loadData(dataName string) (*gocv.Mat, []BBox, error) {
 	contentBBoxes, err := os.ReadFile(fmt.Sprintf("test_data/bboxes-%s.json", dataName))
 	if err != nil {
 		return nil, []BBox{}, fmt.Errorf("unable to load json file for bbox of '%v': %w", dataName, err)
@@ -126,7 +126,7 @@ func saveImage(name string, img *gocv.Mat) error {
 }
 
 func DisplayImageAndBBoxes(dataName string) error {
-	img, bboxes, err := load_data(dataName)
+	img, bboxes, err := loadData(dataName)
 	if err != nil {
 		return fmt.Errorf("unable to load image and bboxes: %w", err)
 	}
diff --git a/pkg/steering/corrector.go b/pkg/steering/corrector.go
index 734245a..22f2436 100644
--- a/pkg/steering/corrector.go
+++ b/pkg/steering/corrector.go
@@ -7,8 +7,37 @@ import (
 )
 
 type Corrector struct {
+	fixValues FixesTable
 }
 
+/*
+FixFromObjectPosition modify steering value according object positions
+
+ 1. To compute steering correction, split in image in zones and define correction value for each zone
+
+    Steering computed
+    :  -1   -0.66 -0.33   0    0.33  0.66   1
+    0%  |-----|-----|-----|-----|-----|-----|
+    :   |  0  |  0  |  0  |  0  |  0  |  0  |
+    20% |-----|-----|-----|-----|-----|-----|
+    :   |  0  |  0  |  0  |  0  |  0  |  0  |
+    40% |-----|-----|-----|-----|-----|-----|
+    :   |  0  |  0  | 0.25|-0.25|  0  |  0  |
+    60% |-----|-----|-----|-----|-----|-----|
+    :   |  0  | 0.25| 0.5 |-0.5 |-0.25|  0  |
+    80% |-----|-----|-----|-----|-----|-----|
+    :   |-0.25|-0.5 |  1  |  -1 |-0.5 | 0.25|
+    100%|-----|-----|-----|-----|-----|-----|
+
+ 2. For straight (current steering near of 0), search nearest object and if:
+
+    * left and right values < 0: use correction from right value according image splitting
+    * left and right values > 0: use correction from left value according image splitting
+    * left < 0 and right values > 0: use (right + (right - left) / 2) value
+
+ 3. If current steering != 0 (turn on left or right), shift right and left values proportionnaly to current steering and
+    apply 2.
+*/
 func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*events.Object) float64 {
 	// TODO, group rectangle
 
@@ -22,6 +51,22 @@ func (c *Corrector) FixFromObjectPosition(currentSteering float64, objects []*ev
 		return currentSteering
 	}
 
+	if currentSteering > 0.1 && currentSteering < 0.1 {
+
+		if nearest.Bottom < 0.4 {
+			// object is far, so no need to fix steering now
+			return currentSteering
+		}
+
+		if nearest.Left < 0 && nearest.Right < 0 {
+			return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Right)
+		}
+		if nearest.Left > 0 && nearest.Right > 0 {
+			return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left)
+		}
+		return currentSteering + c.fixValues.ValueOf(currentSteering, nearest.Left+(nearest.Right-nearest.Left)/2.)
+	}
+
 	// Search if current steering is near of Right or Left
 
 	return currentSteering
@@ -44,3 +89,28 @@ func (c *Corrector) nearObject(objects []*events.Object) (*events.Object, error)
 	}
 	return result, nil
 }
+
+type FixesTable struct {
+	values map[string]map[int]float64
+}
+
+func (f *FixesTable) ValueOf(steering float64, distance float32) float64 {
+	var key string
+	if steering < -0.66 {
+		key = "<-0.66"
+	} else if steering < -0.33 {
+		key = "-0.66:-0.33"
+	} else if steering < 0 {
+		key = "-0.33:0"
+	} else if steering < 0.33 {
+		key = "0:0.33"
+	} else if steering < 0.66 {
+		key = "0.33:0.66"
+	} else {
+		key = ">= 0.66"
+	}
+
+	keyDistance := int(distance / 20)
+	return f.values[key][keyDistance]
+
+}