From adac66432ca05e48529cc38b44f344149651c5e4 Mon Sep 17 00:00:00 2001
From: Yash Karandikar <nerdstep710@gmail.com>
Date: Fri, 29 Sep 2023 10:25:49 -0500
Subject: [PATCH] Fix rectangle colors, attempt to draw rectangle on section

---
 .../freesight/vision/FreeSightPipeline.java   | 67 +++++++++++--------
 1 file changed, 40 insertions(+), 27 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/freesight/vision/FreeSightPipeline.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/freesight/vision/FreeSightPipeline.java
index 8dd269a..c872a6c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/freesight/vision/FreeSightPipeline.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/freesight/vision/FreeSightPipeline.java
@@ -97,39 +97,51 @@ public class FreeSightPipeline extends OpenCvPipeline {
         Imgproc.findContours(scaledThresh, contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_NONE);
 
 
-
-        int index = 0;
-        int area = 0;
-        for (int i = 0; i < contours.size(); i++)
-        {
-            MatOfPoint bar = contours.get(i);
-            int foo = bar.width() * bar.height();
-            if(foo > area)
+        Mat threshRGB = new Mat();
+        Imgproc.cvtColor(threshold, threshRGB, Imgproc.COLOR_GRAY2BGR);
+        if (contours.size() > 0) {
+            int index = 0;
+            int area = 0;
+            for (int i = 0; i < contours.size(); i++)
             {
-                index = i;
-                area = foo;
+                MatOfPoint bar = contours.get(i);
+                int foo = bar.width() * bar.height();
+                if(foo > area)
+                {
+                    index = i;
+                    area = foo;
+                }
             }
-        }
 
-        MatOfPoint contour = contours.get(index);
+            MatOfPoint contour = contours.get(index);
 
-        Rect boundingRect = Imgproc.boundingRect(contour);
-        // center is ( x + w ) / 2
+            Rect boundingRect = Imgproc.boundingRect(contour);
+            // center is ( x + w ) / 2
 
-        int point = (boundingRect.x + boundingRect.width) / 2;
+            int point = (boundingRect.x + boundingRect.width) / 2;
 
-        Imgproc.rectangle(
-                threshold,
-                boundingRect,
-                outline
-        );
+            Imgproc.rectangle(
+                    threshRGB,
+                    boundingRect,
+                    outline
+            );
 
-        if(point < width / 3)
-            positionState = Side.LEFT;
-        else if(point > width / 1.5)
-            positionState = Side.RIGHT;
-        else
-            positionState = Side.MIDDLE;
+            int bigX;
+            if(point < width / 3) {
+                positionState = Side.LEFT;
+                bigX = 0;
+            }
+            else if(point > width / 1.5) {
+                positionState = Side.RIGHT;
+                bigX = width * 2 / 3;
+            }
+            else {
+                positionState = Side.MIDDLE;
+                bigX = width / 3;
+            }
+
+        Imgproc.rectangle(threshRGB, new Rect(bigX, 0, width / 3, height), outline);
+    }
 
         //list of frames to reduce inconsistency, not too many so that it is still real-time, change the number from 5 if you want
         if (frameList.size() > 5) {
@@ -142,7 +154,8 @@ public class FreeSightPipeline extends OpenCvPipeline {
         scaledMask.release();
         mat.release();
         masked.release();
-        return threshold;
+        threshold.release();
+        return threshRGB;
 
     }
 }