From 6a88b3fa566394342cb20356135136619f9561f7 Mon Sep 17 00:00:00 2001
From: Imbus <>
Date: Thu, 7 Nov 2024 01:54:37 +0100
Subject: [PATCH] ControllerWater

---
 wash/src/wash/control/ControllerWater.java | 143 ++++++++++++---------
 1 file changed, 84 insertions(+), 59 deletions(-)

diff --git a/wash/src/wash/control/ControllerWater.java b/wash/src/wash/control/ControllerWater.java
index 8dafdef..a01c97b 100644
--- a/wash/src/wash/control/ControllerWater.java
+++ b/wash/src/wash/control/ControllerWater.java
@@ -1,78 +1,103 @@
 package wash.control;
 
 import actor.ActorThread;
+import wash.control.WashingMessage.Order;
 import wash.io.WashingIO;
+import java.util.LinkedList;
+import java.util.Queue;
 
 public class ControllerWater extends ActorThread<WashingMessage> {
 
     WashingMessage.Order o;
     WashingMessage m;
     WashingIO io;
+    WaterPid waterpid;
 
     public ControllerWater(WashingIO io) {
         this.io = io;
-    }
-
-    @Override
-    public void run() {
-
-        boolean firstRun = true;
-
-        while (true) {
-            try {
-                WashingMessage n = receiveWithTimeout(60000 / Settings.SPEEDUP);
-
-                if (n != null) {
-                    if (o != null && o != n.order())
-                        firstRun = true;
-                    m = n;
-                    o = m.order();
-                }
-
-                if (o != null) {
-                    switch (o) {
-                        case WATER_DRAIN -> {
-                            if (io.getWaterLevel() > 0.0) {
-                                io.fill(false);
-                                io.drain(true);
-                            }
-                            else {
-                                //io.drain(false);
-                                if (firstRun)
-                                    //sendAck();
-                                firstRun = false;
-                            }
-                        }
-
-                        case WATER_FILL -> {
-                            if (io.getWaterLevel() < 10.0) {
-                                io.drain(false);
-                                io.fill(true);
-                            } else {
-                                io.fill(false);
-                                if (firstRun)
-                                    //sendAck();
-                                firstRun = false;
-                            }
-                        }
-
-                        case WATER_IDLE -> {
-                            io.drain(false);
-                            io.fill(false);
-                        }
-
-                    }
-                }
-
-            } catch (InterruptedException e) {
-                throw new RuntimeException(e);
-            }
-
-            // if m is null, it means a minute passed and no message was received
-        }
+        this.waterpid = new WaterPid(0, 1);
     }
 
     public void sendAck() {
         m.sendAck(this);
     }
+
+    @Override
+    public void run() {
+        waterpid.start();
+        while (true) {
+            m = poll(60000 / Settings.SPEEDUP).orElse(new WashingMessage(this, Order.NOOP));
+
+            // io.getWaterLevel();
+            switch (m.order()) {
+                case Order.WATER_DRAIN -> waterpid.setTarget(0);
+                case Order.WATER_FILL -> waterpid.setTarget(20);
+                case Order.WATER_IDLE -> waterpid.setIdle();
+                default -> {
+                    continue;
+                }
+            }
+        }
+    }
+
+    public final class WaterPid extends Thread {
+        private double target;
+        private final double tolerance; // Acceptable range around target
+        private boolean hover = false; // Indicates if we're close enough to the target
+        private boolean hasAcknowledged = true; // Tracks if ack has been sent
+
+        public WaterPid(double targetLiter, double tolerance) {
+            this.target = targetLiter;
+            this.tolerance = tolerance;
+        }
+
+        /** Set the target level */
+        public synchronized void setTarget(double targetLiter) {
+            this.target = targetLiter;
+            this.hover = false;
+            this.hasAcknowledged = false; // Reset ack when target changes
+        }
+
+        /** Just coast at current level */
+        public synchronized void setIdle() {
+            this.setTarget(io.getWaterLevel());
+        }
+
+        /** Main control loop */
+        @Override
+        public void run() {
+            while (!isInterrupted()) {
+                double currentLevel = io.getWaterLevel(); // Get current water level
+                double error = target - currentLevel;
+                hover = Math.abs(error) <= tolerance;
+
+                // Open or close drain/fill based on current water level
+                if (!hover) {
+                    if (error > 0) {
+                        io.fill(true); // Open tap to increase water level
+                        io.drain(false); // Ensure drain is closed
+                    } else {
+                        io.drain(true); // Open drain to decrease water level
+                        io.fill(false); // Ensure tap is closed
+                    }
+                } else {
+                    // Stop adjustments if hovering near target
+                    io.fill(false);
+                    io.drain(false);
+
+                    // Send acknowledgment if target is reached for the first time
+                    if (!hasAcknowledged) {
+                        sendAck();
+                        hasAcknowledged = true;
+                    }
+                }
+
+                try {
+                    Thread.sleep(100);
+                } catch (InterruptedException e) {
+                    Thread.currentThread().interrupt();
+                }
+            }
+        }
+    }
 }