From 904ae96d279a8a3a441bbd90837ca3fcd385c8cb Mon Sep 17 00:00:00 2001
From: Vadim Tkachenko <vt@homeclimatecontrol.com>
Date: Sun, 4 Aug 2024 00:26:38 -0700
Subject: [PATCH] Draft: adjusted the pipeline to take setpoint changes into
 account (#322)

---
 .../java/net/sf/dz3r/model/Thermostat.java    | 50 +++++++++++++++++--
 1 file changed, 46 insertions(+), 4 deletions(-)

diff --git a/modules/hcc-model/src/main/java/net/sf/dz3r/model/Thermostat.java b/modules/hcc-model/src/main/java/net/sf/dz3r/model/Thermostat.java
index 891d1d50..24468dbd 100644
--- a/modules/hcc-model/src/main/java/net/sf/dz3r/model/Thermostat.java
+++ b/modules/hcc-model/src/main/java/net/sf/dz3r/model/Thermostat.java
@@ -81,6 +81,12 @@ public class Thermostat implements Addressable<String> {
     private final Sinks.Many<Signal<Double, Status<Double>>> raiseSink = Sinks.many().unicast().onBackpressureBuffer();
     private final Flux<Signal<Double, Status<Double>>> raiseFlux = raiseSink.asFlux();
 
+    /**
+     * Sink to accept setpoints to feed to {@link #sensitivityController}.
+     */
+    private final Sinks.Many<Signal<Double, Void>> setpointSink = Sinks.many().unicast().onBackpressureBuffer();
+    private final Flux<Signal<Double, Void>> setpointFlux = setpointSink.asFlux();
+
     /**
      * Create a thermostat with a default 10C..40C setpoint range, specified setpoint and PID values, and no sensitivity adjustment.
      *
@@ -162,7 +168,6 @@ public Double getSetpoint() {
      *
      * @see net.sf.dz3r.device.actuator.economizer.v2.PidEconomizer#computeDeviceState(Flux)
      */
-//    @Override
     public Flux<Signal<Status<CallingStatus>, Void>> compute(Flux<Signal<Double, Void>> pv) {
 
         // Compute the control signal to feed to the renderer.
@@ -170,24 +175,61 @@ public Flux<Signal<Status<CallingStatus>, Void>> compute(Flux<Signal<Double, Voi
         var stage1 = controller
                 .compute(pv)
                 .doOnNext(e -> logger.trace("controller/{}: {}", name, e))
-                .doOnComplete(raiseSink::tryEmitComplete); // or it will hang forever
+
+                // or it will hang forever
+                .doOnComplete(raiseSink::tryEmitComplete)
+                .doOnComplete(setpointSink::tryEmitComplete);
 
         // Discard things the renderer doesn't understand.
         // The PID controller output value becomes the extra payload to pass to the zone controller to calculate demand.
-        Flux<Signal<Double, Status<Double>>> stage2 = stage1
+        var stage2 = stage1
                 .map(s -> new Signal<>(s.timestamp, s.getValue().signal, s.getValue(), s.status, s.error));
 
         // Inject signals from raise(), if any
         var stage3 = Flux.merge(stage2, raiseFlux);
 
+        // Broadcast setpoint to sensitivityController
+        var stage4 = stage3.doOnNext(this::watchSetpoint);
+
+        // Adjust the PID controller signal with spikes on setpoint changes
+        var setpointAdjustment = sensitivityController.compute(setpointFlux);
+        var stage5 = Flux.zip(stage4, setpointAdjustment, this::triggerHappy);
+
         // Deliver the signal
         // Might want to expose this as well
         return signalRenderer
-                .compute(stage3)
+                .compute(stage5)
                 .doOnNext(e -> logger.trace("renderer/{}: {}", name, e))
                 .map(this::mapOutput);
     }
 
+    /**
+     * Adjust the {@link #controller PID controller} signal with the sensitivity adjustment from {@link #sensitivityController}.
+     *
+     * @param pid PID controller signal.
+     * @param setpoint Setpoint change adjustments
+     */
+    private Signal<Double, Status<Double>> triggerHappy(Signal<Double, Status<Double>> pid, Signal<Status<Double>, Void> setpoint) {
+        ThreadContext.push("triggerHappy");
+
+        try {
+
+            logger.trace("{}: pid: {}", name, pid);
+            logger.trace("{}: setpoint: {}", name, setpoint);
+
+            // VT: FIXME: Adjust it here; need a data structure to represent both PID and HalfLife controller status
+
+            return pid;
+
+        } finally {
+            ThreadContext.pop();
+        }
+    }
+
+    private void watchSetpoint(Signal<Double, Status<Double>> source) {
+        setpointSink.tryEmitNext(new Signal<>(source.timestamp, source.payload.setpoint));
+    }
+
     private Signal<Status<CallingStatus>, Void> mapOutput(Signal<Status<Double>, Status<Double>> source) {
 
         var sample = source.getValue() instanceof HysteresisController.HysteresisStatus value