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;
        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();
                }
            }
        }
    }
}
